如何使用外部定位设备作为无人机位置估计

adccarry

近很多小伙伴都提出了类似的问题，小编在这里解答一下大家的困惑，大家看不懂的地方，以及大家希望了解的技术细节，可以点开链接（http://amovauto.mikecrm.com/lThqMKF），通过填表来告诉我们您想了解什么技术。同时，我们也会开源一些P200科研无人机中的源码给大家使用。

准备工作

飞控固件及版本：PX4（建议用最新的版本）

其他要求：机载电脑（默认已经掌握：如何使用Mavros连接机载电脑和飞控）

Mavros源码链接：https://github.com/mavlink/mavros

旧的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消息）接口实现这个功能。

室内定位需要的信息

一般针对室内情况，需要通过外部设备获得无人机的位置及偏航角（这是由于飞控自身的磁力计在室内的太无力了）。

常见的定位方式

定位方式按照是否机载可以大致分为两大类：外部定位及机载定位。

外部定位：利用动作捕捉系统（motion capture
system，简称mocap）等设备测量无人机当前的位置及姿态。mocap目前主流的是两家国外牌子：Vicon和Optitrack。mocap设备价格较为昂贵（十几万到几百万不等，但是精度很高，可以达到厘米甚至毫米级别的定位，且刷新率很高。机载定位：这大概就是目前比较火的视觉SLAM和激光SLAM。笔者用过的有：HKUST开源的VINS，谷歌开源的cartographer。

如何在记载电脑中接受定位信息

如果是mocap的话，推荐使用vrpn_clinet_ros功能包来得到地面软件发出的定位信息（只需要将地面软件所在的电脑和机载电脑接入同一个局域网，IP设置正确即可。这一步不懂的可以留言，我后期可以补充具体步骤）。

主流的传感器和SLAM算法一般都有相应的ROS功能包，直接按照官方教程安装，运行起来就可以看到其发布的包含定位数据的话题了。

注意：距离单位！！尽管默认的单位都是米，也不排除有例外的。以及大部分姿态都是用四元数表示，这里可能涉及到四元数与欧拉角的转换关系。

小结：无论哪种定位方式，这一步你需要能够在机载电脑中接收到所需要的定位信息（位置xyz+偏航角yaw）。

飞控中如何设置

为了配合机载电脑中传来的定位信息，有两个和ekf2有关的参数需要修改。

EKF2_AID_MASK 设置为24（即勾选vision position fusion and vision yaw fusion）EKF2_HGT_MODE 设置为VISION （默认为气压计）

因为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不会订阅。订阅飞控返回的位置及姿态信息并打印出来，同时打印外部定位信数据，用于检查是否发送成功并确认方向。

具体消息流向

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以内，再差可能飞机就定不住发散了。