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！提供了非常友好的配置和调试界面。
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：
这个适配器非常重要。它把从运动规划器中输出的空间路径按等距离进行划分，并在其中添加加速度、加速度约束，以及时间戳等必要信息。
Planning
Scene用来表示机械臂周围的外部世界并且保存机械臂自己本身的状态。它通过监听对应的Topic来获取关节状态信息、传感器信息。并可以根据传感器信息和用户的输入，生成机器人周围3D世界空间的表示。
除此之外，本项目还采用了基于3D
Perception的技术，使用插件来获取点云和深度图像数据，并据此生成OctoMap，为之后机械臂的碰撞检测提供基础。
本项目使用OMPL规划机械臂的运动轨迹。对于本项目的六自由度机械臂，采用OMPL进行自动路径规划，在程序中设定终端移动到的目标，通过OMPL即可计算出一个最适合的机械臂运动轨迹路线，包含多个数组，每个数组的长度恰好等于机械臂的自由度（在这里也就是六自由度）。通过计算好的这个轨迹，依次移动关节，最终机械臂终端（手部）将移动到设定的目标。采用ikfast运动学求解器对机械臂运动过程进行求解，因为ikfast是基于解析解的，求解速度以及准确度远大于KDL，同时我们也将配合3D
Perception环境感知对路径进行优化，从而做到不与环境中的任何物体发生碰撞。
五．规划群
规划群（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）来进行优化。
碰撞免检矩阵
碰撞免检矩阵（Avoid Collision Matrix，
ACM）是一个不会随环境变化而改变的检测算法。碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说，机械结构复杂，肢体多，碰撞检测
需要涉及很多的空间几何计算。但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。这里生成的
ACM就是告诉我们，这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中，我们就可以略过对这些肢体之间的检测，以提高检测效率。
三维环境感知
三维感知方式采用pointcloud2点云方式，生成一种octomap的方格三维地图，感知后的环境可以作为环境障碍物，机械臂规划路径时会主动避开有octomap地图的地方。Octomap以八叉树的的形式储存地图，地图由多个小方块构成。当分辨率较高的时候，方块很小，当分辨率较低的时候，方块较大。每个方块表示该格被占据的概率。通过查询某个方块或点“是否可以通过”，从而实现三维的路径规划。
OctoMap：一种基于八叉树的占据网格型地图，像是很多个小方块组成，可以在查询某一点能否通过时切换分辨率（方块大小）从而实现不同层次的导航，其设计思路具有以下几个优点
1.完整的3D模型：octomap不需要任何预设就可以为任意环境搭建模型。该模型同时包含了被占据空间和自由空间。即使是环境中的未知区域也包含在地图之中。我们知道，在机器人进行自主环境探索时未知空间的信息是非常重要的
2.可实时更新：octomap可以随时增加新信息或者传感器。建模和更新是以概率的方式进行的。这样可以解决由于物体运动引起的测量问题。同时，octomap还支持多个机器人共建一个地图，而以前记录的地图在探索新区域时可以进行扩展
3.灵活性：在简介中提到过，octomap支持实时切换分辨率实现不同层次导航，例如，高级规划可以使用粗映射，而本地规划则可以使用细分辨率操作，随时在粗分辨率视图和细分辨率特写之间切换
4.紧凑：octomap文件可以在内存和硬盘中高效存储，可以生成压缩文件。
本项目在Gazebo中给机器人的头部添加了深度传感器的插件，所以当障碍物放到深度相机前方的时候，MoveIt！可以立马从点云Topic中获取物体的信息，并在Rviz中生成可视化的OctoMap。在下一次做运动规划的时候，MoveIt！会将由正方体组成的OctoMap看成障碍物并考虑在内。
4.3 基于OpenCV的无人机双目视觉
OpenCV是
一个基于BSD许可（开源）发行的跨平台计算机视觉库，可用于Linux，Windows以及Mac
OS等等操作系统上。它轻量级且高效，提供了MATLAB，Python等多种语言的接口，实现了图像处理和计算机视觉方面的很多统用算法。本项目基于ROS机器人系统并采用Opencv
4.0.0最新版本，通过guidance
sdk以及Moveit等软件的API实现了无人机的目标图像识别，特征物体云台追踪以及三维环境感知等功能。
4.3.1 guidance sdk开发
Guidance是一套视觉导航传感系统，可感知附近的障碍物，让飞行器主动躲避。它配备有五组超声双目视觉传感器，在guidance系统的控制下，可实现分别调用五组视觉深度数据以及超声波感知距离参数等等。Guidance采用的高精度立体视觉算法，近地面精度可达厘米级别，在飞行器高速飞行状态下仍能提供准确的障碍物深度信息，距离等参数，为无人机的避障，三维环境感知提供了强有力的工具。
通过Guidance SDK，可以从 Guidance
硬件上获取到各种输出数据，包括实时三维速度、IMU
数据，五向超声波距离等等。可实时输出的数据如下：
1.实时机体状态数据，包括机体坐标系下的飞机三维速度以及三维位置，实时输出由图像和超声波数据融合得到的与最近障碍物的距离。
2.Guidance模块传感器数据，包括机体坐标系下的IMU数据，以及三自由度加速度和三自由度的陀螺仪角度，实时输出机体坐标系下到最近5个方向的超声波距离
（0.1m - 8m）。
3.图像数据，包括输出双目摄像头的8比特灰度图，分辨率为320*240，以及输出双目摄像头的16比特深度图，分辨率为320*240。
4.相机参数，包括双目摄像头的标定参数，获取并设置双目摄像头的曝光模式以及曝光参数。
图4.12 Guidance视觉导航传感系统
Guidance模块通过UART和USB串口传输相关数据（若使用UART串口则传输除图像外的全部数据），该模块自带高精度视觉里程计，用于描述Guidance在环境中的运动，对速度的测量精度高达厘米级别。本项目的SDK开发基于这些数据提升空中机器人应用的精度，以更好地适应更多的空中作业应用领域。
由FPGA硬件加速模块和两个ARM
Cortex-A9核心构成了Guidance系统强大的计算平台，可实时处理多达10路摄像头数据，并且可以重新配置以便未来升级。Guidance模块通过CAN总线与manifold机载计算机连接，在Nvidia
TK1的GPU上面通过获取稠密深度图来实时进行三维环境感知与全向障碍物动态分析，从而引导飞控系统及时修正轨迹，避免发生碰撞事故，为无人机在严酷的环境作业提供了强有力的安全保障。
本项目将guidance处理器通过CAN总线连接N3飞控，并通过USB连接至妙算机载计算机，guidance处理器将结合多组超声和深度相机数据并通过CAN连接N3，在飞控实现避障的同时，guidance处理器读取四个方位的黑白深度相机数据并通过USB/UART串口发送给妙算机载电脑，在opencv中调取各个方向的深度摄像头数据完成三维环境感知与未知地形探索等功能。
图4.13 无人机guidance处理器3
4.3.2 目标物体图像识别
使用python语言编译，结合机器学习进行视觉识别。主要通过与环境的交互并在其中进行学习，用极大化的从环境获得的评价性反馈信号为学习目标。较为陌生的环境下仍然具有较好的自适应性和鲁棒性。处理器提取对摄像头的关键帧图像进行提取、识别与匹配，当与目标匹配率达到一定程度时，即认为检测到了目标物体（垃圾、核废料等）。
4.3.3 基于各类目标跟踪算法的物体追踪
目前OpenCV有几种追踪算法，我们对以下五种算法进行了比较。
由于不同算法在不同环境下的跟踪效果不同。由于本项目应用于景区垃圾、危险核废料难处理，对于部分遮挡的要求较高，并且空中识别的速度要求不高，因此本项目采用TLD追踪算法进行物体追踪，同时传回跟踪物体的坐标信息，便于机械臂进行路径规划。
4.4 无人机地面站开发
4.4.1 QGC地面站概述
本项目无人机地面站基于QGroundControl（以下简称为QGC）进行二次开发而成。该地面站可通过无线数传将机载计算机以及飞控的信息传回地面站PC/移动端，实现对无人机的精准降落以及航点规划等，也可通过地面站控制机载stm32控制板，进而控制云台实现目标追踪或机械臂精准控制，也可通过地面站搭配机载HDMI图传实现实时视频广播等等。
QGC地面站可用于为无人机自主规划航线，空中作业时可自动飞向目标地点指定高度，从而减轻飞手的任务，提高作业效率。如上图所示，该截图显示了一个任务规划，该计划从规划起始位置起飞，飞过指定三个航点，然后落在最后一个航路点。
QGC地面站功能丰富，主工具栏上面提供对不同应用程序试图的访问，以及无人机的状态信息，包括IMU信息，磁罗盘信息，电池信息，遥测信号强度等等。也可通过地面站上的一键返航按钮实现自动返回起飞点。右上角的仪表板在无人机起飞后会实时更新当前无人机信息。QGC也可通过无人机飞控的UDP通讯协议连接支持RTP和RTSP视频流接收器，同时接收多路视频信号，实现飞手，机械臂操作手以及地面分析人员各自享有一路视频，互不干扰，最大程度上提高无人机作业效率。
4.4.2 地面站航点规划
对于本项目使用的无人机，遥控器上映射一个通道用于无人机一键返航。当操作者触发此通道按键时，无人机将调用GPS确定当前位置，并将起飞点（地面站若开启返航点刷新则以地面站GPS位置为准）与当前位置进行比较从而确定返航方向并自动规划返航航线，也可通过设置返航高度，在高精度气压计的监测下实现无人机定高返航，同时配合guidance视觉避障模块实现无人机返航过程的安全。QGC地面站可通过终端命令的方式通过数传实时修改无人机的航线，实现精准规划作业轨迹，提高了智能空中作业机器人自动化作业效率。
终端可输入航路点和其他命令，也可以用地面站的功能映射创建任务（Auto
grid），飞机会来回在规划的任务区域飞行进行拍照收集照片，实现无人机的程序化自动控制。
Default
Alt（默认高度）：进入新航路点的默认高度，同事也是返航时候飞机的高度，返航时如果飞机没有达到这个高度，会先升高到这个默认的高度再返回，如果高过这个高度，飞机会保持当前高度飞到返航的位置，下降着陆。
Verify
height（验证高度）：就是地面站用谷歌地图拓扑数据调整你想期望的在每个航路点的高度去反映离地面的高度。因此如果你的航路点在一个小山上，你选择了验证高度在地面站上，设置的高度将会加上小山的高度，使用这种方式将会保证你的飞机不撞上山峰。
设置好你的航路点然后点击右边的“写入”，任务就写入了APM并寄存在存储器中，删除所有航路点，点击“读取”查看确定规划的航路点是否写入，如果写入，会在地图上显示你规划的路径。
4.4.3 Dronecode SDK开发
Dronecode
是一个开源无人机航空操作系统，基于Linux开发，可以根据VTOL，固定翼，直升机以及多旋翼等特性进行二次开发。QGC也可根据Dronecode进行定制型开发，也可通过MAVLink协议与无人机天空端通信。Dronecode框架与ROS类似，其模块化设计可经修改后作为ROS节点运行，同时辅助其他软件包（例如guidance
SDK，onboard SDK以及lidar
SDK等等）实现机载计算机全自动控制，也可将Dronecode在Gazebo仿真环境下进行仿真与测试，确定无人机各项功能是否有异常等等。
Dronecode源码主要语言为C++，可以使用插件扩展SDK，以添加任何其他所需的MAVLink
API，也可通过API扩展dronecode从而实现遥测等功能。
4.4.4 MAVLink协议
MAVLink是为微型飞行器MAV（Micro Air
Vehicle）设计的（LGPL）开源的通讯协议。是无人飞行器和地面站（Ground Control
Station ，GCS）之间，以及无人飞行器之间通讯常用的协议。APM、PIXHAWK飞控，Mission
Planner、QGroundControl地面站均使用了MAVLink协议进行通讯。
MAVLink传输时，以消息包作为基本单位，数据长度为8~263字节。消息数据包的结构如下：
MAVLink采用校验的功能，当两个通讯终端之间（飞行器和地面站，或飞行器和飞行器）使用不同版本的MAVLink协议时，双方计算得到的校验码会不同，则不同版本的MAVLink协议之间将无法通讯。MAVLINK_MESSAGE_CRCS中存储了每种消息包对应的MAVLINK_CRC_EXTRA。这个MAVLINK_CRC_EXTRA在用python生成MAVLink代码时在common.h头文件中自动生成。
MAVLink定义了一些通用消息集，是大多数地面控制站（包括QGC）和自动驾驶仪实现的参考消息集。该消息集分为两部分：MAVLink
Type Enumerations（MAVLink类型枚举 ）和MAVLink
Messages（MAVLink消息包）。这些消息在每个消息包的头文件#define中定义。
MAVLink采用心跳包的方式来判断该设备是否活跃，（一般以1Hz发送），消息接收端会根据是否及时收到了心跳包来判断是否和消息发送端失去了联系。
心跳包由6个数据成员组成，占用9个字节。
1、type：飞行器类型，表示了当前发消息的是什么飞行器，如四旋翼，直升机等。type的取值对应枚举类型MAV_TYPE（如四旋翼，对应数值2）。
2、autopilot：飞控类型，如apm，Pixhawk等，发送心跳包的飞行器的飞控类型。autopilot的取枚举类型MAV_AUTOPILOT。
3、base
mode（基本模式）：飞控现在所处的飞行模式，这个参数要看各个飞控自己的定义方式，会有不同的组合、计算方式。
4、custom
mode（用户模式）：飞控现在所处的飞行模式，这个参数要看各个飞控自己的定义方式，会有不同的组合、计算方式。
5、system status：系统状态，见MAV_STATE枚举变量。
6、mavlink version：消息发送端的MAVLink版本。
其余的消息也是类似的结构，基本消息的定义可以查看官方网页的说明（具体说明以各个飞控为准），也可查看各个消息包头文件的定义。
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！提供了非常友好的配置和调试界面。
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：
这个适配器非常重要。它把从运动规划器中输出的空间路径按等距离进行划分，并在其中添加加速度、加速度约束，以及时间戳等必要信息。
Planning
Scene用来表示机械臂周围的外部世界并且保存机械臂自己本身的状态。它通过监听对应的Topic来获取关节状态信息、传感器信息。并可以根据传感器信息和用户的输入，生成机器人周围3D世界空间的表示。
除此之外，本项目还采用了基于3D
Perception的技术，使用插件来获取点云和深度图像数据，并据此生成OctoMap，为之后机械臂的碰撞检测提供基础。
本项目使用OMPL规划机械臂的运动轨迹。对于本项目的六自由度机械臂，采用OMPL进行自动路径规划，在程序中设定终端移动到的目标，通过OMPL即可计算出一个最适合的机械臂运动轨迹路线，包含多个数组，每个数组的长度恰好等于机械臂的自由度（在这里也就是六自由度）。通过计算好的这个轨迹，依次移动关节，最终机械臂终端（手部）将移动到设定的目标。采用ikfast运动学求解器对机械臂运动过程进行求解，因为ikfast是基于解析解的，求解速度以及准确度远大于KDL，同时我们也将配合3D
Perception环境感知对路径进行优化，从而做到不与环境中的任何物体发生碰撞。
五．规划群
规划群（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）来进行优化。
碰撞免检矩阵
碰撞免检矩阵（Avoid Collision Matrix，
ACM）是一个不会随环境变化而改变的检测算法。碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说，机械结构复杂，肢体多，碰撞检测
需要涉及很多的空间几何计算。但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。这里生成的
ACM就是告诉我们，这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中，我们就可以略过对这些肢体之间的检测，以提高检测效率。
三维环境感知
三维感知方式采用pointcloud2点云方式，生成一种octomap的方格三维地图，感知后的环境可以作为环境障碍物，机械臂规划路径时会主动避开有octomap地图的地方。Octomap以八叉树的的形式储存地图，地图由多个小方块构成。当分辨率较高的时候，方块很小，当分辨率较低的时候，方块较大。每个方块表示该格被占据的概率。通过查询某个方块或点“是否可以通过”，从而实现三维的路径规划。
OctoMap：一种基于八叉树的占据网格型地图，像是很多个小方块组成，可以在查询某一点能否通过时切换分辨率（方块大小）从而实现不同层次的导航，其设计思路具有以下几个优点
1.完整的3D模型：octomap不需要任何预设就可以为任意环境搭建模型。该模型同时包含了被占据空间和自由空间。即使是环境中的未知区域也包含在地图之中。我们知道，在机器人进行自主环境探索时未知空间的信息是非常重要的
2.可实时更新：octomap可以随时增加新信息或者传感器。建模和更新是以概率的方式进行的。这样可以解决由于物体运动引起的测量问题。同时，octomap还支持多个机器人共建一个地图，而以前记录的地图在探索新区域时可以进行扩展
3.灵活性：在简介中提到过，octomap支持实时切换分辨率实现不同层次导航，例如，高级规划可以使用粗映射，而本地规划则可以使用细分辨率操作，随时在粗分辨率视图和细分辨率特写之间切换
4.紧凑：octomap文件可以在内存和硬盘中高效存储，可以生成压缩文件。
本项目在Gazebo中给机器人的头部添加了深度传感器的插件，所以当障碍物放到深度相机前方的时候，MoveIt！可以立马从点云Topic中获取物体的信息，并在Rviz中生成可视化的OctoMap。在下一次做运动规划的时候，MoveIt！会将由正方体组成的OctoMap看成障碍物并考虑在内。
4.3 基于OpenCV的无人机双目视觉
OpenCV是
一个基于BSD许可（开源）发行的跨平台计算机视觉库，可用于Linux，Windows以及Mac
OS等等操作系统上。它轻量级且高效，提供了MATLAB，Python等多种语言的接口，实现了图像处理和计算机视觉方面的很多统用算法。本项目基于ROS机器人系统并采用Opencv
4.0.0最新版本，通过guidance
sdk以及Moveit等软件的API实现了无人机的目标图像识别，特征物体云台追踪以及三维环境感知等功能。
4.3.1 guidance sdk开发
Guidance是一套视觉导航传感系统，可感知附近的障碍物，让飞行器主动躲避。它配备有五组超声双目视觉传感器，在guidance系统的控制下，可实现分别调用五组视觉深度数据以及超声波感知距离参数等等。Guidance采用的高精度立体视觉算法，近地面精度可达厘米级别，在飞行器高速飞行状态下仍能提供准确的障碍物深度信息，距离等参数，为无人机的避障，三维环境感知提供了强有力的工具。
通过Guidance SDK，可以从 Guidance
硬件上获取到各种输出数据，包括实时三维速度、IMU
数据，五向超声波距离等等。可实时输出的数据如下：
1.实时机体状态数据，包括机体坐标系下的飞机三维速度以及三维位置，实时输出由图像和超声波数据融合得到的与最近障碍物的距离。
2.Guidance模块传感器数据，包括机体坐标系下的IMU数据，以及三自由度加速度和三自由度的陀螺仪角度，实时输出机体坐标系下到最近5个方向的超声波距离
（0.1m - 8m）。
3.图像数据，包括输出双目摄像头的8比特灰度图，分辨率为320*240，以及输出双目摄像头的16比特深度图，分辨率为320*240。
4.相机参数，包括双目摄像头的标定参数，获取并设置双目摄像头的曝光模式以及曝光参数。
图4.12 Guidance视觉导航传感系统
Guidance模块通过UART和USB串口传输相关数据（若使用UART串口则传输除图像外的全部数据），该模块自带高精度视觉里程计，用于描述Guidance在环境中的运动，对速度的测量精度高达厘米级别。本项目的SDK开发基于这些数据提升空中机器人应用的精度，以更好地适应更多的空中作业应用领域。
由FPGA硬件加速模块和两个ARM
Cortex-A9核心构成了Guidance系统强大的计算平台，可实时处理多达10路摄像头数据，并且可以重新配置以便未来升级。Guidance模块通过CAN总线与manifold机载计算机连接，在Nvidia
TK1的GPU上面通过获取稠密深度图来实时进行三维环境感知与全向障碍物动态分析，从而引导飞控系统及时修正轨迹，避免发生碰撞事故，为无人机在严酷的环境作业提供了强有力的安全保障。
本项目将guidance处理器通过CAN总线连接N3飞控，并通过USB连接至妙算机载计算机，guidance处理器将结合多组超声和深度相机数据并通过CAN连接N3，在飞控实现避障的同时，guidance处理器读取四个方位的黑白深度相机数据并通过USB/UART串口发送给妙算机载电脑，在opencv中调取各个方向的深度摄像头数据完成三维环境感知与未知地形探索等功能。
图4.13 无人机guidance处理器3
4.3.2 目标物体图像识别
使用python语言编译，结合机器学习进行视觉识别。主要通过与环境的交互并在其中进行学习，用极大化的从环境获得的评价性反馈信号为学习目标。较为陌生的环境下仍然具有较好的自适应性和鲁棒性。处理器提取对摄像头的关键帧图像进行提取、识别与匹配，当与目标匹配率达到一定程度时，即认为检测到了目标物体（垃圾、核废料等）。
4.3.3 基于各类目标跟踪算法的物体追踪
目前OpenCV有几种追踪算法，我们对以下五种算法进行了比较。
由于不同算法在不同环境下的跟踪效果不同。由于本项目应用于景区垃圾、危险核废料难处理，对于部分遮挡的要求较高，并且空中识别的速度要求不高，因此本项目采用TLD追踪算法进行物体追踪，同时传回跟踪物体的坐标信息，便于机械臂进行路径规划。
4.4 无人机地面站开发
4.4.1 QGC地面站概述
本项目无人机地面站基于QGroundControl（以下简称为QGC）进行二次开发而成。该地面站可通过无线数传将机载计算机以及飞控的信息传回地面站PC/移动端，实现对无人机的精准降落以及航点规划等，也可通过地面站控制机载stm32控制板，进而控制云台实现目标追踪或机械臂精准控制，也可通过地面站搭配机载HDMI图传实现实时视频广播等等。
QGC地面站可用于为无人机自主规划航线，空中作业时可自动飞向目标地点指定高度，从而减轻飞手的任务，提高作业效率。如上图所示，该截图显示了一个任务规划，该计划从规划起始位置起飞，飞过指定三个航点，然后落在最后一个航路点。
QGC地面站功能丰富，主工具栏上面提供对不同应用程序试图的访问，以及无人机的状态信息，包括IMU信息，磁罗盘信息，电池信息，遥测信号强度等等。也可通过地面站上的一键返航按钮实现自动返回起飞点。右上角的仪表板在无人机起飞后会实时更新当前无人机信息。QGC也可通过无人机飞控的UDP通讯协议连接支持RTP和RTSP视频流接收器，同时接收多路视频信号，实现飞手，机械臂操作手以及地面分析人员各自享有一路视频，互不干扰，最大程度上提高无人机作业效率。
4.4.2 地面站航点规划
对于本项目使用的无人机，遥控器上映射一个通道用于无人机一键返航。当操作者触发此通道按键时，无人机将调用GPS确定当前位置，并将起飞点（地面站若开启返航点刷新则以地面站GPS位置为准）与当前位置进行比较从而确定返航方向并自动规划返航航线，也可通过设置返航高度，在高精度气压计的监测下实现无人机定高返航，同时配合guidance视觉避障模块实现无人机返航过程的安全。QGC地面站可通过终端命令的方式通过数传实时修改无人机的航线，实现精准规划作业轨迹，提高了智能空中作业机器人自动化作业效率。
终端可输入航路点和其他命令，也可以用地面站的功能映射创建任务（Auto
grid），飞机会来回在规划的任务区域飞行进行拍照收集照片，实现无人机的程序化自动控制。
Default
Alt（默认高度）：进入新航路点的默认高度，同事也是返航时候飞机的高度，返航时如果飞机没有达到这个高度，会先升高到这个默认的高度再返回，如果高过这个高度，飞机会保持当前高度飞到返航的位置，下降着陆。
Verify
height（验证高度）：就是地面站用谷歌地图拓扑数据调整你想期望的在每个航路点的高度去反映离地面的高度。因此如果你的航路点在一个小山上，你选择了验证高度在地面站上，设置的高度将会加上小山的高度，使用这种方式将会保证你的飞机不撞上山峰。
设置好你的航路点然后点击右边的“写入”，任务就写入了APM并寄存在存储器中，删除所有航路点，点击“读取”查看确定规划的航路点是否写入，如果写入，会在地图上显示你规划的路径。
4.4.3 Dronecode SDK开发
Dronecode
是一个开源无人机航空操作系统，基于Linux开发，可以根据VTOL，固定翼，直升机以及多旋翼等特性进行二次开发。QGC也可根据Dronecode进行定制型开发，也可通过MAVLink协议与无人机天空端通信。Dronecode框架与ROS类似，其模块化设计可经修改后作为ROS节点运行，同时辅助其他软件包（例如guidance
SDK，onboard SDK以及lidar
SDK等等）实现机载计算机全自动控制，也可将Dronecode在Gazebo仿真环境下进行仿真与测试，确定无人机各项功能是否有异常等等。
Dronecode源码主要语言为C++，可以使用插件扩展SDK，以添加任何其他所需的MAVLink
API，也可通过API扩展dronecode从而实现遥测等功能。
4.4.4 MAVLink协议
MAVLink是为微型飞行器MAV（Micro Air
Vehicle）设计的（LGPL）开源的通讯协议。是无人飞行器和地面站（Ground Control
Station ，GCS）之间，以及无人飞行器之间通讯常用的协议。APM、PIXHAWK飞控，Mission
Planner、QGroundControl地面站均使用了MAVLink协议进行通讯。
MAVLink传输时，以消息包作为基本单位，数据长度为8~263字节。消息数据包的结构如下：
MAVLink采用校验的功能，当两个通讯终端之间（飞行器和地面站，或飞行器和飞行器）使用不同版本的MAVLink协议时，双方计算得到的校验码会不同，则不同版本的MAVLink协议之间将无法通讯。MAVLINK_MESSAGE_CRCS中存储了每种消息包对应的MAVLINK_CRC_EXTRA。这个MAVLINK_CRC_EXTRA在用python生成MAVLink代码时在common.h头文件中自动生成。
MAVLink定义了一些通用消息集，是大多数地面控制站（包括QGC）和自动驾驶仪实现的参考消息集。该消息集分为两部分：MAVLink
Type Enumerations（MAVLink类型枚举 ）和MAVLink
Messages（MAVLink消息包）。这些消息在每个消息包的头文件#define中定义。
MAVLink采用心跳包的方式来判断该设备是否活跃，（一般以1Hz发送），消息接收端会根据是否及时收到了心跳包来判断是否和消息发送端失去了联系。
心跳包由6个数据成员组成，占用9个字节。
1、type：飞行器类型，表示了当前发消息的是什么飞行器，如四旋翼，直升机等。type的取值对应枚举类型MAV_TYPE（如四旋翼，对应数值2）。
2、autopilot：飞控类型，如apm，Pixhawk等，发送心跳包的飞行器的飞控类型。autopilot的取枚举类型MAV_AUTOPILOT。
3、base
mode（基本模式）：飞控现在所处的飞行模式，这个参数要看各个飞控自己的定义方式，会有不同的组合、计算方式。
4、custom
mode（用户模式）：飞控现在所处的飞行模式，这个参数要看各个飞控自己的定义方式，会有不同的组合、计算方式。
5、system status：系统状态，见MAV_STATE枚举变量。
6、mavlink version：消息发送端的MAVLink版本。
其余的消息也是类似的结构，基本消息的定义可以查看官方网页的说明（具体说明以各个飞控为准），也可查看各个消息包头文件的定义。
举报
举报
