主要针对室内飞行，在无GPS定位的情况下，如何将外部设备或者机载SLAM系统的定位信息传递给飞控中的位置及姿态估计器，从而使得飞控中的估计器可以融合外部定位数据及自身IMU数据得到能够用于室内飞行的定位信息。
写在前面
近期论坛上很多小伙伴都提出了类似的问题，这里做一个系统性的解答。但是，一提笔总觉得很多相关内容都糅杂在一起，很难剥离出来说清楚。所以，这篇权当一次试水，大家看不懂的地方，以及大家希望了解的技术细节，可以在本帖下方留言。
其次，关于无人机、PX4二次开发、自主控制、室内导航、offboard模式、mavros、SLAM、视觉追踪等等笔者略知一二的东西都会陆陆续续整理出来，同时也会开源一些P200科研无人机中的源码给大家使用。
手写的文字及代码难免有错及笔误，烦请各位指正。
准备工作
飞控固件及版本：PX4（建议用教新的版本）
其他要求：机载电脑（默认已经掌握：如何使用Mavros连接机载电脑和飞控）
Mavros源码链接：Mavros Github
旧的PX4手册是没有这部分相关内容的详细说明的，最新版本的手册已经更新了这部分内容，与本文档内容略有重叠，但侧重不同，供大家参考。
官方教程链接：https://dev.px4.io/en/ros/external_position_estimation.html
飞控中的估计模块（滤波器）
状态估计包括位置估计与姿态估计。在旧版本的飞控中默认使用的是attitude_estimator_q（姿态估计）、position_estimator_inav（位置估计，最新的固件已经删除这个模块了）或local_position_estimator（位置估计）。新版本固件中的默认滤波器是ekf2模块（包含位置及姿态估计）。
本文就将利用ekf2模块中提供的vehicle_visual_odometry.msg（uORB消息）接口实现这个功能。（备注：吐槽一下官方，固件更新太快，1.9.0以前这个话题不是叫这个名字，应该是vehicle_vision_position.msg及vehicle_vision_attitude.msg）
室内定位需要哪些信息？
一般针对室内情况，需要通过外部设备获得无人机的位置及偏航角（这是由于飞控自身的磁力计在室内的太无力了）。
都有哪些常见的定位方式？
定位方式按照是否机载可以大致分为两大类：外部定位及机载定位。
- 外部定位：利用动作捕捉系统（motion capture
system，简称mocap）、UWB等设备测量无人机当前的位置及姿态。mocap目前主流的是两家国外牌子：Vicon和Optitrack。mocap设备价格较为昂贵（十几万到几百万不等），但是精度很高，可以达到厘米甚至毫米级别的定位，且刷新率很高。UWB没有深入的了解，从他人口中听闻，价格大概也需要几万块，精度5-10cm左右，刷新率肯定是不如MOCAP的。
- 机载定位：这大概就是目前比较火的视觉SLAM和激光SLAM。这里就不深入讲了，网上资料太多了。笔者用过的有：HKUST开源的VINS，谷歌开源的cartographer。（前两天在一个会议上见到HKUST的沈老师，以前一直以为是个老教授，结果是个很年轻的老师，比我大不了几岁。欸~）
如何在机载电脑中接收定位信息？
如果是mocap的话，推荐使用vrpn_clinet_ros功能包来得到地面软件发出的定位信息（只需要将地面软件所在的电脑和机载电脑接入同一个局域网，IP设置正确即可。这一步不懂的可以留言，我后期可以补充具体步骤）。
如果是UWB，一般来说是串口收发方式，将其提供的接收端插在机载电脑上，github上找一个基于ros的串口收发小程序对应改改就可以（按照其协议解码即可）。
主流的传感器和SLAM算法一般都有相应的ROS功能包，直接按照官方教程安装，运行起来就可以看到其发布的包含定位数据的话题了。
注意：距离单位！！ 尽管默认的单位都是米，也不排除有例外的。别问我怎么知道要提示你这一点的，都是泪。以及大部分姿态都是用四元数表示，这里可能涉及到四元数与欧拉角的转换关系。嘿嘿。别搞错了哦。
小结：无论哪种定位方式，这一步你需要能够在机载电脑中接收到所需要的定位信息（位置xyz+偏航角yaw）。
飞控中如何设置？
这里就不讲怎么连接机载电脑和飞控了，默认你已经会使用Mavros连接机载电脑了。
为了配合机载电脑中传来的定位信息，有两个和ekf2有关的参数需要修改。
因为ekf2中默认是不会融合这个外部的定位信息的，这里相当于在ekf2中启用这个功能。至于ekf2参数中还有很多和这个外部定位相关的滤波参数设置，就请参看官网教程吧。我没有改任何参数，但是ekf2代码的话，阿木团队优化过，具体修改笔者也不清楚。
其他说明：
- 做了以上修改之后，水平校准会遇到问题，所以要修改之前校准水平。
- 修改之后，自稳模式会变得有些奇怪，基本上只能使用位置闭环的控制模式了，也就是offboard模式。
如何将定位信息发送至飞控中？（重点内容）
话不多说，直接共享代码 px4_pos_estimator.cpp
px4_pos_estimator.cpp是阿木社区最新推出的P200科研无人机中px4_command功能包中的一个小程序，主要负责的功能就是将外部信息发送到飞控中，代码文件中有详尽的注释（因为是基于ros写的，所以需要一点点ros编程的基础）。该程序也只是提供一个示例及代码框架而已，照搬肯定是不行的，部分地方还是要按照你选择定位方式的具体情况来修改的。
我在这里大致提几点注意事项：
- 坐标系转换（对应的callback函数中修改）：需要将外部定位信息转换至ENU坐标系。mavros中默认使用的是ENU系。这里的ENU系是指：无人机前方为正，左方为正，上方为正，由上向下看飞机逆时针旋转为正。
/mavros/vision_pose/pose 这个话题发布的时候必须要加时间戳，不然mavros不会订阅。- 订阅飞控返回的位置及姿态信息并打印出来，同时打印外部定位信数据，用于检查是否发送成功并确认方向。
具体消息流向
就像我在Mavros培训课上讲得，只要梳理清楚每个消息的流向及其存在形式，那么就能很轻松的把握Mavros和PX4的代码了。
首先，理解ros话题、Mavlink消息、uORB消息这三者的概念。简单来说：ros话题是机载电脑中传递的消息载体，Mavlink是机载电脑与飞控传递的消息载体，uORB话题是飞控内部传递的消息载体。
这里针对外部定位位置（即一个包含xyz的数据类型）进行梳理：
- 首先是传感器测量得到，由机载电脑中的ros话题发布出来。
px4_pos_estimator.cpp订阅该话题，进行一定处理后，转存至另一个话题/mavros/vision_pose/pose 并发布。- Mavros功能包中vision_pose_estimate.cpp订阅
/mavros/vision_pose/pose话题，并将其封装成Mavlink消息，发送给飞控。
- 飞控接收到该Mavlink消息，通过mavlink_receiver.cpp接收并处理，转存为相应的uORB消息。
- 飞控中ekf2模块订阅该uORB消息，并进行相应计算处理，融合得到无人机的位置速度等状态。
实现步骤总结
- 在机载电脑中，利用相应驱动程序或者功能包获得外部定位数据，并以ros话题的形式发布出来。
- 利用px4_pos_estimator.cpp程序订阅该话题，做好相应坐标转换，然后转发该数据至飞控中。
- 打印相应信息，检查飞控是否收到定位信息。
- 前后左右移动飞机，确认方向正确。
调试建议
- 第四步很重要，基本上问题都出在这，如果实在搞不清方向转换的，就多试几次，因为位置的坐标转换只是正负号的问题，多试几次总能得到正确的结果。
- 基本上飞行的控制精度是取决于这里的定位精度的。理解以下几个概念：静态定位精度，动态定位精度，控制精度。以激光slam为例，静态精度1cm左右，动态精度可能只有5cm了，如果不其他（延时，控制参数等等）影响不显著时，控制精度较好时在正负10cm以内，较差的话正负20cm以内，再差可能飞机就定不住发散了。
关于作者
阿木社区技术负责人、Mavros培训课程主讲老师、北理工在读博士
对任何与四旋翼无人机有关的话题感兴趣，欢迎交流。
个人微信号：qyp0210
发表于 2019-6-7 21:47:02
31888 浏览 19 回复
