随着生活水平的提高，旅游已经成为国民生活中不可或缺的一部分。我国旅游资源丰富，无论是自然景观还是人文景观都遍布全国各地。然而在旅游资源丰厚的前提下,我国的旅游景区却面临资源如季节性紧缺的情况，这就容易导致旅游景区的环境超载和污染高发。例如每到假期,泸沽湖、九寨沟、香格里拉古城等旅游胜地都将迎接来自全国数以百万计的游客。超负荷的游客量带来的不仅是轰动全国的数千游客滞留景区事件,还带来了严重的旅游垃圾污染问题。新京报报道,2013年国庆“黄金周”期间八达岭长城6天清理109吨垃圾；据凤凰网报道,2014年国庆假期鼓浪屿日产垃圾70吨等等,激增的垃圾对景区环境造成了极大破坏,甚至可能无法完全恢复。

[外链图片转存失败(img-pE2skL04-1564595804829)(media/1c701aab02d993f7bfaccc09802a3ecf.png)]

图1.1 华山清洁工人深入悬崖下清理垃圾

由于景区的地理环境较为复杂，景区垃圾大多需要依靠人工清理。在处理悬崖、深水区等区域的垃圾时，不仅要投入大量人力，还要面临巨大的风险。此外，在风及水流的作用下，景区垃圾分布较广，使得人工清理的效率大大降低。

近年来，小型多旋翼无人机技术日趋成熟，随着自动控制、物联网、大数据等技术的快速发展，与无人机有关的环境感知技术、通信导航技术等也得到广泛的应用。

英国较早将无人机应用于大面积区域及危险区域的垃圾处理。然而我国在无人机清理高危地带垃圾方面才刚刚起步，巨大的市场和飞速发展的技术使得无人机清理高危地带垃圾具有广阔的市场前景。

利用体积小巧，机动灵活，飞行稳定的多旋翼无人机代替人工处理景区垃圾成为了一套行之有效的方法。本产品正是从这种时代背景切入，提供模块化设计的小型无人机垃圾处理系统，且整体框架设计灵活，便于针对不同的景区环境投放。

[外链图片转存失败(img-LnUba49C-1564595804831)(media/928d7b19cddd89537015c631ff155f74.jpg)]

图1.2 日本福岛核泄漏造成千里荒无人烟

在经历了日本福岛核事故沉重打击后核电正在逐步走上复苏之路。核电作为安全可靠、技术成熟的清洁能源，并且，核电作为当前唯一可大规模替代化石燃料的清洁能源，越来越受到世界各国的重视 。

核废料是核物质在核反应堆(原子炉)内燃烧后余留下来的核灰烬，具有极强烈的放射性，而且其半衰期长达数千年甚至几十万年。也就是说，在几十万年后，这些核废料还能伤害人类和环境。所以如何安全、永久地处理核废料是科学家们一个重大的课题。

核废料只能靠放射性核素自身的衰变而减少，核废料放出的射线通过物质时，发生电离和激发作用，对生物体会引起辐射损伤。核废料中放射性核素通过衰变放出能量，当放射性核素含量较高时，释放的热能会导致核废料的 温度不断上升，甚至使溶液自行沸腾，固体自行熔融。由于核废料的这些特征，使人工处理核废料具有极高的操作难度和安全隐患，同时随着无人机等行业的快速发展和机械化的普及，无人机等智能机械成为处理核废料的一种高效安全的方案。

灾害事件发生突然，救援活动紧急，救援力量往往无法及时了解情况并快速到达现场，无人机可以在非正常状态下及时快速有效到达现场，第一时间收集到实时数据及时向指挥部报告监测情况，为事故检测、救援指挥与决策提供科学依据。

无人机技术用途越来越广泛，具有体积小、机动灵活、使用方便等优点，在救援方面开始大显身手，汶川地震。雅安地震等突发灾害事件救援中，均表现出不可替代的作用。

国外无人机在救援方面应用较早，2011年日本福岛核泄漏事件中，美国军方派出全球鹰无人机侦查反应堆，美国使用“牵牛星”、“捕食者”参与森林大火救援，还使用无人机进行飓风监测和灾后救援。针对救援情况，国外还研制了防碰撞、防水，运送医疗药品等类型无人机。

国家政府在四川汶川地震、青海玉树地震、四川雅安地震均派出无人机前往灾区进行救援工作。电商加速普及的无人机技术也正在应用到抢险救灾中，在今年汶川抗震救灾十周年纪念日当天，京东成立了首个全国性公益无人机救援队，以应对未来可能发生的地震等重大灾害。2018年7月17日，在北京市政府和北京市应急办的指挥下，京东抽调了两架Y3货运无人机和两架侦查无人机，前往密云区和怀柔区的受灾地区，协助进行应急物资运输以及灾区勘测工作，京东无人机运输物资占当天物资的30%。

[外链图片转存失败(img-rwzg2rED-1564595804832)(media/37a1fbaf1dcc066c965551dca9e0f1e6.png)]

图1.3 无人机用于抗震救灾

救援无人机可用于对灾区进行指挥和调度、物资补给等多种用途，我们研发的这款智能空中作业平台可运用精准的GPS实现灾区检测传感器的精准放置和救灾物资的投放，运用高精度摄像头和视觉算法实现灾区的地貌测绘，运用传感器进行受灾群众识别，实现灾区救援的功能。

本方案采用一个六旋翼无人机作为平台，该无人机可载重5kg到6kg。无人机主体结构采用碳纤维板材切割而成，强度高，密度小，有利于减轻无人机总重，连接部分采用铝合金外刷漆防锈制成，结构强度高。机臂部分采用六根20mm的碳纤管，末端连接电机和电调，既对称美观又不易在飞行过程中偏坠。无人机最上方安装有一个云台和激光雷达，用于生成障碍物点云图从而感知周围障碍物。无人机前下方倒置一个三轴云台和运动摄像机，用于操纵者观察正前方视频画面。无人机后部安装有机载电脑和机械臂舵机控制板，机载电脑用于对协助飞控对飞行过程中的各种传感器数据进行储存和分析，从而控制其他机载设备。舵机控制板采用arduino mega 2560，通过USB串口与机载电脑连接。机械臂部分采用双机械臂设计，由一个三自由度和六自由度的机械臂组成，三自由度机械臂采用遥控器远程控制，六自由度机械臂采用机载电脑自动化控制，并且六自由度机械臂具有环境感知的功能，可自动感知周围障碍物并规划运动路线。

创新点：

1.本项目解决了景区垃圾、危险核废料难处理的行业痛点问题，正如之前背景部分所描述的，自然风景区中山区、深水区等危险区域乃至核污染区域垃圾废料处理的难题一直耗费大量的人力物力成本。本项目基于无人机、机器学习、计算机视觉、自动控制等技术，有效解决了这一难题，具有较高的市场价值和行业前景。

2.多项创新和技术融合，综合应用了精准无人机定位、无人机自动控制等技术。采用强化学习中的Q学习算法和cartographer算法实现了稳定性高的精准避障。基于机械臂控制方面，应用了学术界最新的ikFast算法，极大的提高了机械臂运动速度和工作效率。

3.本项目采用了MANIFOLD 妙算机载电脑，拥有PC独立显卡级别的绘图能力，可以充分带动图像识别、自动控制、避障算法，高效准确地完成自动化、半自动化作业。

4.本项目自动化程度高，清理效率高。本项目基于强化学习、计算机视觉、无人机自动控制等技术，拥有较高的自动化程度。可以灵活高效地完成核废料清理险情抢救时关键设备布局等任务。

5.采用3+6自由度双机械臂配合作业，具有环境感知的功能，可自动感知周围障碍物并规划运动路线。

妙算为DJI第一代On-board SDK开发平台，配备了NVIDIA公司的低功耗性能嵌入式芯片Tegra K1 作为核心处理器，具备最高达到326GFLOPS的计算能力。妙算与DJI经纬M100飞行平台高度集成，可以为用户提供便捷的开发途径。并且妙算支持用户自己最大化性能。有UART 2/UART 3串口以连接经纬M100飞行平台上的UART串口。

1.云台控制板

云台控制板采用stm32f103的最小系统板，基于arm cortex-m3微处理器，主频可达到168MHz，它具有64K字节的FLASH闪存器，最大64K字节的SRAM，功耗较低，拥有共12个DMA通道，以及多达15个通信接口，包括6个USART，3个SPI，2个CAN，以及一个SDIO接口，原理图如下图所示

[外链图片转存失败(img-nlyceDKB-1564595804834)(media/668acd3c9f9d9e5b3c2f50caa5c90111.png)]

图2.1 云台控制板原理图

2.云台电机

该三轴云台具有三个小功率微型精密减速电机，主要结构由传动齿轮，无刷直流电机构成，具有相应快速，起动转矩较大等特点，在水平，横滚，俯仰三个轴，通过stm32控制板的基于经典PID的闭环控制理论对云台进行增稳控制。

本无人机使用了两个模拟信号图传，分别是机载电脑图传与摄像头图传

1.机载电脑图传

舵机控制是使用stm32f103实现的，其最高72MHz工作频率，在存储器的0等待周期访问时可达1.25DMips/MHZ。其运行速率可以满足本产品对机械臂底层驱动的要求，实现对机械臂稳定安全的驱动。

本项目无人机机架采用碳纤维材料，无人机自上而下共有四层碳纤维切割而成的板，碳纤板与碳纤管的连接采用金属件刷上防锈漆。电机座使用激光切割机切出形状后用铣刀钻出M4螺丝孔。本项目采用铝合金连接件固定电池板，电池板下倒置双机械臂。中心碳纤板上使用尼龙柱固定主分电板和24V降压模块，飞控和PMU固定于中心碳纤板后方，并通过铝合金连接件连接上方碳纤板。上方碳纤板固定机载电脑，OSD，图传等模块。最上方的板上装有单线激光雷达，周围无障碍物，可保证激光雷达测障的准确度。

由于我们开发的带臂无人机目前有两种设计用途，一种为对某些物件的夹取搬运，另一种为高空作业、设备的简单检修，因此机械臂的设计中分为两类：夹取搬运类与空中作业类，每一类则可根据物件大小更换不同的“手指”型号，两类机械臂在闲置时都被折叠于机腹，在需要使用时才被调用张开。目前暂定夹取搬运类为两个展开状态长为80cm的4自由度机械臂，空中作业类则为一个展开长度为80cm的3自由度机械臂与展开长度为70cm的6自由度带摄像头机械臂。

在执行的任务为夹取搬运的时候，考虑到两只手一起搬运东西会更为稳健，同时仅一只手容易导致重心不平衡的问题，故采用双臂方案。同样地，机械臂的末端“手部”同样采取了模块化设计，根据需要夹持的目标物体大小可以快速地更换模块，以适应不同的抓取需求。既可以是抓取拥有把手的较大物体时使用的“双括号型”（<>），亦可以是捏取小型物体的“ll型”。“双括号型”在抓取时会扣住其把手，尽可能防止晃动，如果把手过于光滑而滑落，作为补救措施其亦不会掉出机械臂“手指”所构成的环。“ll型”亦如其名，为两根平行移动的机械手指，用于精确地捏取目标物体，或与另一只机械臂一同紧抓其两端将其取走。在机械臂的右臂上装有小型摄像头，以提供精准的物体定位服务，反馈至控制板以更精准地进行抓取。

执行任务为高空作业类时，机器人所搭载的机械臂为一长一短，自由度为3（长）+6（短）。考虑到空中风向风速多变、高空作业对于悬停稳定性要求较高，而目前无人机的悬停能力远达不到可以用于高空作业的稳定性，因而需使用长机械臂紧紧地抱住作业物体，稳定机身，再进行任务要求的作业。此机械臂末端手部全部采用类似于上文的“双括号型”设计，可以根据需要更换大小，当固定物为柱形时可以将其抱住，但考虑到有可能机械臂仅能固定在较小的长条形或柱形物体上，“手指”部位进行了改进，再指根部增加了小型的平口夹，用以紧紧夹住上述物体。用于执行动作作业的机械臂要求足够灵活，故采用6自由度机械臂设计，此机械臂上同样装备了摄像头，以便于足够精准地进行各个作业动作。其所装备的“手指”部分同样可以根据任务需求更换大小、型号。考虑到可能会有类似于拧螺丝的动作，其手指部位具有旋转270°的能力，以便于快速地完成需要转动某些部件的操作。

云台采用simpleBGC俄制三轴云台，具有三个小功率微型精密减速电机，含有水平（YAW），横滚（ROLL），俯仰（PITCH）三个轴，每个轴都由无刷直流电机组成，响应速度较快。其示意图如下：

[外链图片转存失败(img-dV6Tod0J-1564595804836)(media/d7be4f5dcd05909d6a512c610e82994e.png)]

图3.1 三周云台设计示意图

ROS是一个用于编写机器人软件的灵活框架，它集成了大量的工具，库，协议，提供了类似操作系统所提供的功能，包括硬件抽象描述，底层驱动管理，消息传递等等。ROS被设计成为一种分布式结构，在这个框架中的每个功能模块都可以被单独设计，编译，运行时以松散耦合的方式结合在一起，而且ROS中的功能模块都封装于独立的功能包（Package）中，便于整理和编译。ROS的多语言特性使其能够支持多种语言，例如Java，Python，Lisp，C/C++等语言，使机器人的开发更为灵活便捷，并且ROS软件集成度较高，基于节点的程序设计，让模块的代码移植，复用更加便利。

ROS的架构如图所示，它并不是一个传统意义上的操作系统，而是需要依托在开源Linux系统上。Linux是一个通用系统，本身并没有针对机器人开发特殊的中间件，因此ROS在中间层做了大量工作，最重要的就是TCPROS /UDPROS的通信系统。

本项目采用的ROS搭载于ubuntu mate 16.04开源操作系统上，ROS版本为kinetic。

[外链图片转存失败(img-EKb8BRf0-1564595804845)(media/ace2e7cc5def7932ce31f39e5866215e.png)]

图4.2 SLAM激光雷达整体流程图

激光雷达的必要性：相比相机激光雷达的优点在于，其探测距离较远，而且能够准确获取物体的三维信息另外它的稳定性相当高，鲁棒性好。

本项目采用强化学习中的Q学习算法和RBPF的方法，效果较为稳定，根据激光雷达和里程计的信息，对环境地图进行感知构建，并实时对自身状态进行调整和估计。所以即使无人机在环境复杂的山区和灾区，也能够安全得进行飞行和空中作业。

无人机的自主化工作需要无人机对外界的环境有清晰的感知，需要利用小型机载激光雷达实现。本项目通过单线的激光雷达的实现辅助控制。并使用基于cartographer的图优化算法实现导航和SLAM即时建图等功能。

[外链图片转存失败(img-R1r5DnHl-1564595804846)(media/c0cfdb1f05b07bdd11dbd7763be2405e.png)]

图4.3 cartographer图优化算法

激光雷达采用三角测距技术，配合视觉采集处理机构，它可进行每秒 4000 次的测距动作。每次测量过程中，激光雷达发射经过调制的红外激光信号，该激光信号经目标物体漫反射后被激光雷达的视觉采集系统接收，然后激光雷达内部的 DSP 处理器实时数据处理，计算出目标物体到激光雷达的距离以及当前的夹角，通过UART通讯接口转USB输出给机载电脑。

雷达在直流无刷电机机构的驱动下，激光雷达测距核心将顺时针旋转，从而实现对周围环境的 360°全方位的扫描，从而得到所在空间的平面点云地图信息。本项目采用国产Delta-2B 激光雷达，旋转频率为 4～10hz (360rpm)， 在典型旋转频率下可以实现 0.3 ～ 0.8°的角度分辨率，从而实现在 2D 平面的 2m ~8m 半径范围内进行 360°全方位扫描，采样频率5KHz并产生所在空间的平面点云地图信息。这些点云地图信息被用于无人机定位导航,同时采用基于carographer的开源SLAM库以及IMU模块实现2D和3D的室外SLAM建图,辅助机器人实现对未知环境,障碍物的感知能力。

由于仅仅依赖前端（里程计）的话有积累误差，并且很难修正。所以项目采用Cartographer理论模型来消除积累误差。

[外链图片转存失败(img-ZhJQIexX-1564595804848)(media/008acb73a409fae653fd27be89b71e44.png)]

图4.4 激光雷达整体流程处理图

Cartographer主要依赖激光雷达以及惯性导航仪(IMU)实现二维和三维的SLAM建图,相对于gmapping来讲优势在于实现了相对较低的资源消耗,提高了实时优化速度,有利于无人机在高速运行时提高建图速度与质量.其算法关键点在于将分支限界算法(branch and bound)引入到loop closure的过程中,加快了闭环的查找,提高与之前submap比较的效率.

Cartographer的实现并没有采用以往的滤波算法,而是采用了类似图优化的模型进行姿态解算,最后通过loop closing非线性纠偏来消除这些误差,完成闭环反馈的控制.相对于karto来讲,两者都采取图优化框架,但采取的优化库不一致,karto采取的是spa或g2o,而cartographer采用的是google的ceres构建优化,采用4线程的后端优化相对于前者的单线程同时进行前后端的优化无论是速度还是准确度,都有明显的提升。

Cartographer采取双搜索的方式进行,先用一次三维窗口遍历寻优,再构建优化等式,利用ceres求解,并采用了submap的概念,依据一定熟练的扫描初始化一个submap,依据窗口大小,插入新的扫描数据,并更新submap,有地图缓存.相对来讲,karto无submap的概念,全部以扫描的方式储存在sensorManager.无地图缓存,但每次计算地图有计算消耗。

Delta-2B 激光雷达是通过 UART TTL 电平与外部设备通信的，仅支持单工通讯(即激光雷达主动发数据帧到机载电脑的ROS系统中)，ROS系统只需从数据帧中提取有效数据即可，不需要做任何回应，通讯帧中的所有数据都是 16 进制格式数据。本项目利用激光雷达的单工通讯，在机载电脑端生成点云图，再由点云图，通过Onboard SDK 的接口辅助无人机完成对障碍物的规避。同时依赖Cartographer完成对环境地图的构建。此后，通过ROS的代码控制无人机完成和目标物体的距离控制。

任务切换：可以通过远端发送指令或在信号丢失的情况下，切换不同任务模式，从避障切换到标定静止时，在下一次任务之前可由机载电脑完全控制飞行。

通过以上的设计，最终实现对环境的感知还有无人机的自主控制。SLAM建图和和无人机的自主控制的设计，能保证无人机的安全飞行，即使在遥控信号丢失的情况下，依然能通过自主的控制保证飞行器的安全。

Moveit！是ROS中一个重要的集成化开发平台，支持众多的机器人硬件平台，本项目基于ROS的Moveit！开发平台进行二次开发。本项目采用solidwork软件搭建六轴机械臂及三轴机械臂立体模型，并制作出1：1真实机械臂。本项目在这两个模型基础上，添加坐标系与转轴，设置关节运动范围，并利用solidwork导出各个组件到URDF文件中。导出后创建一个类似ROS的包，并移植到ROS机器人系统上面进行仿真与测试。

Gazebo是一个3D的动态仿真软件，基于ROS机器人系统。本项目使用gazebo仿真环境对机械臂进行物理模拟仿真，可以通过仿真各个传感器，执行器的特性，最大程度上做到仿真结果的准确。

[外链图片转存失败(img-M2oKj8YC-1564595804851)(media/b73031799b5085b99f7a788a0c8af899.png)]

图4.6 Gazebo框架

4.2.3 机械臂硬件驱动接口

ROS的moveit软件通过硬件抽象层来直接与机械臂驱动板实现双向同步通信。对于舵机手臂的位姿，我们通过编写相关功能包，直接发布规划路径的各个点数据就是在goalCB内把数据取出来，再用publisher 发布出去，实现对实体机械臂的驱动。

1.实体机械臂:STM32嵌入式控制器使用位置PID闭环算法来计算由硬件抽象层通过串口通信方式发过来的关节数据，并将计算好的数据直接发送给电机对其进行控制。

2.硬件抽象层:硬件抽象层和硬件直接打交道，通过write和read方法来完成对硬件的相关操作。硬件抽象层跟上面官方提供（红色的部分）的有一些不太一样的地方在于我并没有使用ROS Control提供的Transmissions（数据转换）和Joint Limits(关节限位）的API。

Transmissions就是机器人每个运动关节和电机之间的数学映射。因为机械臂关节结构的不同，会导致机器人上层规划所使用的Joint与Actuator数据之间存在明显的偏差。比如说有简单齿轮和同步带驱动的，有锥齿轮差动机构，四连杆机构等。Transmissions提供的接口中包含有解决上面这些结构进行数据转换的映射公式。

Joint Limits主要是维护了一个关节限位的数据结构，里面可以包含的数据种类不仅仅是常用的关节位置、速度、力矩、加速度等方面的限位，还可以储存具有安全作用的位置软限位、位置边界和速度边界等。

控制器管理器提供一种通用的接口来管理不同的ROS Controllers，它可以加载、开始运行、停止运行、卸载不同的Controller，并且提供了多种工具来完成这些操作。Controller Manager的输入就是ROS上层应用的输出。在这里面，我用到了Joint Command Controller和Joint State Controller，它们分别可以完成对每个关节的控制以及读取硬件接口中关节的状态。

Simulation和Hardware之上的Hardware Resource和Controller Manager是一样的，这很清晰地体现了ROS Control的底层无关性，即无论使用的是抽象的仿真还是具体的硬件，只要程序能继承RobotHW硬件抽象层的基类来做到数据接口的统一，Controller Manager就可以对相应的资源进行管理。

对于Simulation和Hardware来说，它们内部架构相似，但配置以及使用方式是不一样的。Simulation的RobotHW部分，Gazebo官方已经将其实现，并且提供了相应的ROS Control插件来从机械臂的URDF文件中载入所需的数据。用户只需写好URDF和YAML文件，并使用ROS Launch将其整合到一起就万事大吉了。

而对于Hardware这部分来说，除了上面说到的配置之外，我们还需要自己编写C++代码来继承RobotHW基类，并在里面分别使用命令和状态硬件接口句柄对相应的关节数据进行注册，然后再将不同的硬件接口注册到RobotHW上。最后，我们还要自己编写函数完成对关节和电机数据的相互转换，并且根据指定的通信协议，实现read和write函数。

仿真部分:使用gazebo仿真环境并启动Motion Control测试,运行后可使用键盘按键来控制机械臂每个关节的位姿.

硬件部分：

（1）创建子类继承RobotHW父类，并且声明了一些函数和变量。

（2）初始化关节和电机数据，并使用Hardware Interface对相应的数据进行注册，最后初始化与串口通信有关的Topic。

（3）根据机械臂公式，实现关节和电机数据之间的互相转换。

（4）实现定制的read和write函数。

（5）加载关节名字到ROS的参数服务器中。

以上就是硬件接口层的全部内容了。作为整个机械臂软件架构最底层的部分，它的重要性不言而喻。根据我之前开发机械臂的经验，只有编写出稳定且鲁棒的的硬件接口层，才能为之上的运动规划提供强有力的保证。

在Rviz中，本项目使用末端执行器上的交互式Marker来移动机械臂到目标位置。接着，你可以在Planning Library的下拉式菜单中选择OMPL库中的某个特定规划算法(KDL数值解较差,本项目采用ikfast来代替)。点击Execute按钮，MoveIt!会将上一步规划出来的机械臂关节轨迹通过FollowJointTrajectoryAction接口发送给Gazebo中对应类型的Controller，使得Gazebo中的机械臂可以移动到目标位置(驱动真实机械臂时要将launch文件中对应gazebo接口更换成hardware接口)。

[外链图片转存失败(img-8IicwdqK-1564595804852)(media/ad0825ad537248d4002001dd9c83f6e9.gif)]

图4.7 机械臂驱动控制板

stm32f103是一款较为常用的单片机，它能满足本产品的要求。本产品决定使用PWM实现对舵机的驱动。

一般而言，舵机的基准信号都是周期为20ms，宽度为1.5ms。这个基准信号定义的位置为中间位置。其中间位置的脉冲宽度是一定的，那就是1.5ms。角度是由来自控制线的持续的脉冲所产生。这种控制方法叫做脉冲调制。脉冲的长短决定舵机转动多大角度。当舵机接收到一个小于1.5ms的脉冲，同品牌，甚至同一品牌的不同舵机，都会有不同的最大值和最小值。一般而言，最小脉冲为1ms，最输出轴会以中间位置为标准，逆时针旋转一定角度。接收到的脉冲大于1.5ms情况相反。不大脉冲为2ms。

本项目决定使用c++在stm32f103上进行代码编译实现从而实现对舵机的控制。首先要有一个与真实机械臂相一致的机器人模型文件。使用moveit配置助手生成配置文件。上一步生成的配置文件默认控制的是虚拟机械臂，需要修改部分文件，来告诉moveit！我们想要控制的是真实机械臂。首先demo.launch文件中参数fake_execution的值改为false。然后move_group.launch文件中，moveit_controller_manager在选择参数值时，“unless”前面那个value值要修改，写一个自己机器人名称作为前缀，例如“aubo_i5”。按照原先的设置，本应该启动fake_moveit_controller_manager.launch.xml，你可以去moveit配置文件夹下去找，这个文件是存在的，而现在，一个叫做aubo_i5_moveit_controller_manager.launch.xml将会被启动，而这个文件moveit也应该已经帮我们创建好了，当我们使用先前的moveit向导加载机器人模型时，机器人模型中写明的机器人名称（属性），就会作为前缀写入这个文件的文件名，所以，“aubo_i5”这个名称源自于你模型文件里写明的机器人名称，需要前后保持统一，理解各个文件之间的调用关系。

下一步修改aubo_i5_moveit_controller_manager.launch.xml文件。最后创建controllers.yaml配置文件。经过上述操作代码最后一句指向了config目录下一个叫做controllers.yaml的文件，这个文件很重要，决定了我们所使用的moveit控制器的基本参数。我们打开配置文件夹中的config目录，发现只有fake_controllers.yaml,所以，现在要做的就是复制一份这个文件，然后将名字改为controllers.yaml，现在要做的就是复制一份这个文件，然后将名字改为controllers.yaml，打开这个新文件，将文件修改。在这时候需要对一个文件进行修改，解决机械臂轨迹执行时超时的问题，经过我们的优化可以是机械臂稳定的运动。再通过action的配置，将action客户端的指令转化为机械臂能执行的控制命令。这样就可以实现对机械臂的硬件驱动。

4.2.4 六自由度机械臂运动路径规划

一．OMPL简介

OMPL（Open Motion Planning Library）是一个开源的运动路径规划库。OMPL由于其模块化设计，使得较复杂的多自由度运动路径规划问题简化。

OMPL是一个大类,里面有不同的算法.OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。

二.OMPL运动规划层设计

运动规划层在机械臂的自主抓取中扮演了非常重要的角色。而对于运动规划本身来说，里面涉及了非常多的专业知识，比如运动学正逆解算、碰撞检测算法、3D环境感知、动作规划算法等。本项目采用逆运动学求解的方式，把终端位姿变成关节角度,给定终端位姿计算关节角度，q=IK§。p是终端位姿(xyz)，q是关节角度。概括来说，MoveIt!是ROS中与移动操作相关的组件包的集合。它包含了运动规划、操作控制、3D感知，运动学，碰撞检测等。当然，最重要的是MoveIt!提供了非常友好的配置和调试界面。

[外链图片转存失败(img-XTEuKzal-1564595804853)(media/8bbc471af28b8edc86f7140516a03925.png)]

图4.8 move_group架构图

move_group是MoveIT!最核心的部分,本质上还是一个ROS节点,使用参数服务器来获取以下三种信息.

URDF： move_group需要机械臂的URDF文件来进行运动规划。

SRDF： move_group在启动时会寻找机械臂的SRDF文件，它可以通过使用MoveIt! Setup Assistant自动生成。

MoveIt!配置： move_group在启动时会加载机械臂的关节限位、动力学、运动规划、感知以及其他相关信息。所有以上的配置信息都可以通过使用MoveIt! Setup Assistant自动生成。

move_group使用ROS中的Topic和Action两种机制来与机械臂进行数据通信。它可以获取当前机械臂的位置信息，点云数据以及其他传感器数据，并且发送命令给机械臂的Controller.

关节状态信息： move_group会监听机械臂的/joint_states主题来获取当前的状态信息。注意：move_group只管监听，你需要自己给机械臂配置好Joint State Controller。

坐标转换信息： move_group可以订阅机械臂的TF主题来确定机械臂内部各关节之间的位置变换关系。跟上面一样，你需要自己运行Robot State Publier节点来发布坐标转换。

控制器接口： move_group使用Follow Joint Trajectory类型的Action接口来与Controller进行数据通信。move_group自己是不带Action接口的，它是使用了一个特殊的插件来发布上述Follow Joint Trajectory类型的Action，而对于机械臂来说，你依然需要自己配置上述类型的Controller来订阅机械臂的数据。

规划场景： Planning Scene指的是机械臂本身以及其周围环境的表示(在Gazebo中显示)。

扩展能力： move_group的所有组件都是以独立插件的形式实现的，而且这些插件可以通过使用ROS的参数文件或插件库来进行配置，这使得move_group拥有了强大的定制以及可扩展能力。

三．Motion Planning介绍

在MoveIt!中，运动规划算法是由运动规划器算出来的。当然，运动规划算法有很多，每一个运动规划器都是MoveIt的一个插件，可以根据需求选用不同的规划算法。MoveIt!默认使用的是OMPL。OMPL(Open Motion Planning Library)是开源运动规划库的简写，它提供基于随机化的运动规划器。

运动规划请求：

在让运动规划器进行运动规划之前，我们要先发送一个运动规划的请求。这个请求可以是新的机械臂或末端执行器的位置。为了让运动规划器规划出来的轨迹符合要求，我们需要指定一些约束条件：

位置约束：约束机械臂Link的位置。

方向约束：约束机械臂Link的方向。

可见性约束：约束Link上的某点在某些区域的可见性。

关节约束：约束Joint的运动范围(实体机械臂取决于舵机可旋转角度,理论上小于等于机械臂可动角度)。

自定义约束：使用自定义的回调函数来指定约束条件。

四．运动规划结果

move_group节点最终将会根据上面的运动规划请求，生成一条运动轨迹。这条轨迹可以使机械臂移动到预想的目标位置。请注意：move_group输出的是一条轨迹，而不是路径。对于机械臂来说，路径是使末端执行器移动到目标位置的过程中，中间所经历的一系列独立的位置点。而轨迹则是在路径的基础上，通过加入速度、加速度约束以及时间参数来使机械臂运动的更加平滑。

五．规划请求适配器

在运动规划器的输入输出端分别有两个规划请求适配器。它们的作用分别是对规划请求和规划结果进行预处理和后期处理。MoveIt!提供了几种默认的适配器来完成一些特定的功能。

FixStartStateBounds:

当机械臂的一个或多个关节的初始状态稍微超出了URDF文件中所定义的Joint Limits后，为了能让运动规划器可以运行，FixStartStateBounds适配器会通过将关节状态移动到Joint Limits处来解决这个问题。不过，如果机械臂关节的偏差很大的话，这种靠软件方式修正的方式就不适用了。

以下为OMPL采用的适配器：

FixWorkspaceBounds:

这个适配器会默认地生成一个10x10x10立方米的机械臂规划空间。

FixStartStateCollision:

如果已有的关节配置文件会导致碰撞，这个适配器可以采样新的配置文件，并根据摇摆因子来修改已有的配置文件，从而保证新的机械臂不会发生碰撞(三维环境感知与此结合,实时采样周围障碍物信息)。

FixStartStatePathConstraints:

如果机械臂的初始姿态不满足路径约束，这个适配器可以找到附近满足约束的姿态作为机械臂的初始姿态。

AddTimeParameterization:

[外链图片转存失败(img-pe5J0mNu-1564595804855)(media/4986000b222f93bd44be4400bd8a6b78.png)]

这个适配器非常重要。它把从运动规划器中输出的空间路径按等距离进行划分，并在其中添加加速度、加速度约束，以及时间戳等必要信息。

图4.9

Planning Scene用来表示机械臂周围的外部世界并且保存机械臂自己本身的状态。它通过监听对应的Topic来获取关节状态信息、传感器信息。并可以根据传感器信息和用户的输入，生成机器人周围3D世界空间的表示。

除此之外，本项目还采用了基于3D Perception的技术，使用插件来获取点云和深度图像数据，并据此生成OctoMap，为之后机械臂的碰撞检测提供基础。

本项目使用OMPL规划机械臂的运动轨迹。对于本项目的六自由度机械臂，采用OMPL进行自动路径规划，在程序中设定终端移动到的目标，通过OMPL即可计算出一个最适合的机械臂运动轨迹路线，包含多个数组，每个数组的长度恰好等于机械臂的自由度（在这里也就是六自由度）。通过计算好的这个轨迹，依次移动关节，最终机械臂终端（手部）将移动到设定的目标。采用ikfast运动学求解器对机械臂运动过程进行求解，因为ikfast是基于解析解的，求解速度以及准确度远大于KDL，同时我们也将配合3D Perception环境感知对路径进行优化，从而做到不与环境中的任何物体发生碰撞。

[外链图片转存失败(img-r3cgxC2W-1564595804857)(media/e0283c85c255977646f93a7e8048643b.png)]

图4.10 基于ikfast运动学求解器计算的机械臂运动路径

五．规划群

规划群(Planning Groups)是moveit的核心之一。

Kinematic Solver: 运动学求解工具，这个就是负责求解正向运动学(Forward Kinematics)和逆运动学(IK, 见1.3节)的。 一般我们选用KDL， The Kinematics and Dynamics Library。这是一个运动学与动力学的库，可以很好的解决6自由度以上的单链机械结构的正逆运动学问题。当然你也可以用其他IK Solver, 比如SRV或者IK_FAST，甚至你可以自己开发新的Solver然后插入进来，如果有空，我以后会发帖讲解如何创建新的运动学求解库并插入到MoveIt。

每一次移动标记,就运行了一次逆运动学IK求解过程.

因为OMPL是采样算法，由于其随机采样的特性，每次的路径是不同的，而且有可能失败。(因此运动学求解工具是 OMPL(开源路径规划库)的一部分,这也就相当于SIRF和opencv的关系)

Search Resolution: 关节空间的采样密度

Search TImeout: 求解时间

Solver Attempts: 求解失败尝试次数，一般来说这三项使用默认值就可以。你也可以根据具体需要做出适当调整。

机器人预设位姿(Robot Poses),默认位姿应该为直立。

终端控制器(End Effectors)就是机械臂的手,在工作环境中直接控制的部分。

被动关节(Passive Joints)不配置电机的关节。

moveit的配置文件为SRDF,配合URDF使用。

4.2.5 机械臂防碰撞系统设计

MoveIt!使用CollisionWorld对象进行碰撞检测，采用FCL(Flexible Collision Library)功能包。碰撞检测是运动规划中最耗时的运算，往往会占用90%左右的时间，为了减少计算量，可以通过设置ACM(Allowed Collision Matrix)来进行优化。

1. 碰撞免检矩阵

碰撞免检矩阵(Avoid Collision Matrix, ACM)是一个不会随环境变化而改变的检测算法.碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说，机械结构复杂，肢体多，碰撞检测 需要涉及很多的空间几何计算。但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。这里生成的 ACM就是告诉我们，这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中，我们就可以略过对这些肢体之间的检测，以提高检测效率。

1. 三维环境感知

三维感知方式采用pointcloud2点云方式，生成一种octomap的方格三维地图，感知后的环境可以作为环境障碍物，机械臂规划路径时会主动避开有octomap地图的地方。Octomap以八叉树的的形式储存地图，地图由多个小方块构成。当分辨率较高的时候，方块很小，当分辨率较低的时候，方块较大。每个方块表示该格被占据的概率。通过查询某个方块或点“是否可以通过”，从而实现三维的路径规划。

OctoMap：一种基于八叉树的占据网格型地图，像是很多个小方块组成，可以在查询某一点能否通过时切换分辨率(方块大小)从而实现不同层次的导航，其设计思路具有以下几个优点

1.完整的3D模型：octomap不需要任何预设就可以为任意环境搭建模型。该模型同时包含了被占据空间和自由空间。即使是环境中的未知区域也包含在地图之中。我们知道，在机器人进行自主环境探索时未知空间的信息是非常重要的

2.可实时更新：octomap可以随时增加新信息或者传感器。建模和更新是以概率的方式进行的。这样可以解决由于物体运动引起的测量问题。同时，octomap还支持多个机器人共建一个地图，而以前记录的地图在探索新区域时可以进行扩展

3.灵活性：在简介中提到过，octomap支持实时切换分辨率实现不同层次导航，例如，高级规划可以使用粗映射，而本地规划则可以使用细分辨率操作，随时在粗分辨率视图和细分辨率特写之间切换

4.紧凑：octomap文件可以在内存和硬盘中高效存储，可以生成压缩文件。

本项目在Gazebo中给机器人的头部添加了深度传感器的插件，所以当障碍物放到深度相机前方的时候，MoveIt!可以立马从点云Topic中获取物体的信息，并在Rviz中生成可视化的OctoMap。在下一次做运动规划的时候，MoveIt!会将由正方体组成的OctoMap看成障碍物并考虑在内。

[外链图片转存失败(img-13jclfjg-1564595804858)(media/39d6914914939a41e1b389d4a3fd4413.jpg)]

图4.11 基于octomap的方格三维地图

OpenCV是 一个基于BSD许可（开源）发行的跨平台计算机视觉库，可用于Linux，Windows以及Mac OS等等操作系统上。它轻量级且高效，提供了MATLAB，Python等多种语言的接口，实现了图像处理和计算机视觉方面的很多统用算法。本项目基于ROS机器人系统并采用Opencv 4.0.0最新版本，通过guidance sdk以及Moveit等软件的API实现了无人机的目标图像识别，特征物体云台追踪以及三维环境感知等功能。