px4+vio实现无人机室内定位

文章主要讲述px4 如何利用vins_fusion里程计数据实现在室内定位功能。

文章基于以下软、硬件展开。

硬件	软件
机载电脑： Intel NUC	系统：Ubuntu 20.04
相机： Intel Realsense D435i	ros：noetic
飞控：Pixhawk 2.4.8	固件：PX4 1.14.0

完整vins_to_mavros 功能包地址：
https://github.com/rotorcraftman/px4ctrl

随着slam开源技术的普及，px4 要实现室内定位，实现方式很多，如文章使用的vins-fusion等视觉里程计，激光里程计等。
本质上，要实现无人机的室内定位有两个方法：
1.提供室内位置信息
室外可以用gps提供位置信息，实现定位，而室内因为gps没有信号，所以朴素的想法是只要提供位置信息给飞控就可实现像室外gps定位的效果了。
思路：
vio提供里程计—>(/mavros/vision_posion/pose)mavros(mavlink)—>px4
可以看出mavros起到了连接vio和px4的桥梁作用。
2.offboard
外部计算机遵守照MAVLink 协议提供的位置，速度或姿态设定值。 设定值可以由机载计算机上运行的 MAVLink API（例如 MAVSDK (opens new window) 或 MAVROS (opens new window)）提供（通常通过串口或 wifi 连接）。
3.光流、UWB等。

接下来详细讲述第一种实现方式，vio以vins_fusion为例。

文章叙述展开方式默认已完成了px4飞控的建立、mavros的安装，未完成的同志可参考上一篇文章：
gazebo11+px4联合仿真测试

一、Pixhawk MAVLink Ports配置

配置Pixhawk Telem2作为与机载电脑数据交互的MAVLINK端口

MAV_1_CONFIG= TELEM 2
MAV_1_MODE = Onboard
SER_TEL2_BAUD = 921600 8N1

设置前参数里可能只有MAV_1_CONFIG，搜不到其他的参数，将MAV_1_CONFIG设置为TELEM 2，然后把飞控重启后其他参数就有了。
Pixhawk 2.4.8硬件，设置为102，参数对应关系如下。

详细参数介绍参见：https://docs.px4.io/main/en/advanced_config/parameter_reference.html

Pixhawk 2.4.8 TELEM1/TELEM2端口设置如下：

TELEM1	TELEM2
MAV_0_CONFIG = TELEM 1	MAV_1_CONFIG = TELEM 2
MAV_0_MODE = Normal	MAV_1_MODE = Onboard
MAV_0_RATE= 1200 Bytes/s	MAV_1_RATE= 0 (Half maximum)
MAV_0_FORWARD = True	MAV_1_FORWARD = Disabled
SER_TEL1_BAUD = 57600	SER_TEL2_BAUD = 921600

详细端口设置参见：https://docs.px4.io/main/en/peripherals/mavlink_peripherals.html

配置好端口后，需要做一根连接机载电脑和Pixhawk TELEM 2 端口的线，我这边直接用一个USB转TTL模块。
Pixhawk TELEM1 / TELEM2 端口线序图如下：

Pin	Signal	Volt
1 (red)	VCC	+5V
2 (blk)	TX (OUT)	+3.3V
3 (blk)	RX (IN)	+3.3V
4 (blk)	CTS (IN)	+3.3V
5 (blk)	RTS (OUT)	+3.3V
6 (blk)	GND	GND

其它端口详见：https://docs.px4.io/main/en/flight_controller/pixhawk.html#where-to-buy

展示一张做好的端子连接线如下：

在qgc上测试通信是否正常。

qgc-Application Settings-通讯连接-添加，设置如下。

正常情况下，就可通过TELEM 2连上QGC了。

二、在机载电脑上启动MAVROS

我这里用的是nuc的usb，设备名称：dev/ttyUSB0，按照自己实际情况配置。921600是波特率，就是前面设置的SER_TEL2_BAUD参数，改成设置值就行。

roslaunch mavros px4.launch fcu_url:=serial:=/dev/ttyUSB0:921600 gcs_url:=udp://@172.16.7.210

gcs_url：运行qgc主机的IP地址。
如果不想设置ip，可以设置为以下参数自动寻址。

roslaunch mavros px4.launch fcu_url:=serial:=/dev/ttyUSB0:921600 gcs_url:=udp-b://@

若出现报错
FCU: DeviceError:serial:open: Permission denied

解决方法是给对应的串口权限

sudo chmod 777 /dev/ttyUSB0

三、vins_fusion、mavros建立连接

思考vins_fusion的里程计数据如何发布给px4？
这也是实现室内定位的关键。实现这一步只需将vins_fusion里程计数据以话题 /mavros/vision_pose/pose 发布，mavros 收到/mavros/vision_pose/pose话题后，转化成mavlink通过TELEM 2传给飞控。于是就完成了vins_fusion和px4的连接。

接下来创建发布 /mavros/vision_pose/pose 话题的功能包过程了

1.创建工作空间px4ctrl

mkdir -p px4ctrl/src/
cd px4ctrl/src/

2.创建功能包vins_to_mavros

catkin_create_pkg vins_to_mavros roscpp std_msgs geometry_msgs mavros_msgs nav_msgs tf2_eigen tf

在px4ctrl/src/vins_to_mavros/src/ 目录下创建一个 vins_to_mavros 节点，主要功能：
（1）将 VINS-Fusion 的 body 坐标系在 world 坐标系下为位姿转化为 base_link 在 map 坐标系中的位姿；
（2）将转化后的位姿信息以话题 /mavros/vision_pose/pose 发布。

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
 
 
Eigen::Vector3d p_mav;
Eigen::Quaterniond q_mav;
 
 
void vins_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    if(msg->header.frame_id == "world")
    {
        p_mav = Eigen::Vector3d(msg->pose.pose.position.y, -msg->pose.pose.position.x, msg->pose.pose.position.z);
 
        q_mav = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Eigen::AngleAxisd roll(M_PI/2,Eigen::Vector3d::UnitX()); // 绕 x 轴旋转 pi / 2
        Eigen::AngleAxisd pitch(0,Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd yaw(0,Eigen::Vector3d::UnitZ());
 
        Eigen::Quaterniond _q_mav = roll * pitch * yaw;
        q_mav = q_mav * _q_mav;
    }
}
 
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_to_mavros");
    ros::NodeHandle nh("~");
 
    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("odom", 100, vins_callback);
 
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("vision_pose", 10);
 
 
    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);
 
    ros::Time last_request = ros::Time::now();
 
    while(ros::ok()) {
        geometry_msgs::PoseStamped vision;
 
        vision.pose.position.x = p_mav[0];
        vision.pose.position.y = p_mav[1];
        vision.pose.position.z = p_mav[2];
 
        vision.pose.orientation.x = q_mav.x();
        vision.pose.orientation.y = q_mav.y();
        vision.pose.orientation.z = q_mav.z();
        vision.pose.orientation.w = q_mav.w();
 
        vision.header.stamp = ros::Time::now();
        vision_pub.publish(vision);
 
        ROS_INFO("\nposition:\n   x: %.18f\n   y: %.18f\n   z: %.18f\norientation:\n   x: %.18f\n   y: %.18f\n   z: %.18f\n   w: %.18f", \
        p_mav[0],p_mav[1],p_mav[2],q_mav.x(),q_mav.y(),q_mav.z(),q_mav.w());
 
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}

3.配置 CMakeList.txt 文件

找到相应位置添加

add_executable(vins_to_mavros_node src/vins_to_mavros.cpp)
target_link_libraries(vins_to_mavros_node  ${catkin_LIBRARIES})

4.创建vins_to_mavros节点的launch文件

在目录px4ctrl/src/launch/ 创建vins_to_mavros.launch

<launch>
    <node pkg="vins_to_mavros"  type="vins_to_mavros_node" name="vins_to_mavros" output="screen">
        <remap from="~vision_pose" to="/mavros/vision_pose/pose" />
        <remap from="~odom" to="/vins_estimator/odometry" />
    </node>
</launch>

5.编译

cd px4ctrl
catkin_make
source devel/setup.bash

6.验证

启动vins_to_mavros节点

roslaunch vins_to_mavros vins_to_mavros.launch

查看话题

rostopic list

显示如下：

PS：此处坐标系转化适配的是Realsense D435i相机，如其它相机需要根据相机imu坐标系与px4坐标系进行相应的转换。

四、联调测试

联调测试的基础是：
1.vins_fusion里程计精度尚可，且具有一定的鲁棒性；
2.px4飞控在自稳模式下手动可控、达到可飞条件。

关于vins_fusion的相关调试参见系列文章：
https://blog.csdn.net/u010196944/article/details/127240169

1.px4飞控设置

将px4定位数据源设置为vinsion，参数EKF2_AID_MASK设置为24，具体如下：

2.在终端依次输入：

此时qgc已连上，可在qgc作如下验证。

Analyze Tools-MAVlink检测，出现了LOCAL_POSITION_NED数据，如下：

验证：
（1）前后左右移动飞机，看看位置是否正确。
（2）前后移动飞机后，放回原位置看位置数据偏差是否大。

验证没问题之后，就可以起飞，通过qgc或者遥控器切换定位模式了。

完结，希望你一切顺利，不“炸鸡”。

补充：

上一步验证过程中，很多同学可能会有疑问，vins-fusion、mavros和px4的坐标关系是如何的呢，以下做一个简要说明。
1.vins-fusion坐标系
在vins-fusion中，坐标系包括：world坐标系、body坐标系和camera坐标系，其中body为IMU的坐标系，camera为相机坐标系(如果双目是左目相机的坐标系)，world坐标系是在初始化，通过首帧关键帧和重力对齐完成。详细请参照代码，有疑问可讨论。
vins-fusion初始化完成后，word坐标系为：以d435i相机朝向为正东方(F)作为基准，x轴朝向正南方?，y轴朝向正东方（F），z轴朝向天(U)，故vins-fusion的参考坐标系为南东天SEU(RFU)。注意，这里所谓的正东方不是真实地理的东方，只是作为一种朝向基准。
2.mavros坐标系
mavros 的坐标变换关系主要有三种：东北天坐标系（ENU），北东地坐标系（NED），前左上坐标系（FLU），朴素的想法是只要发过来的里程计位姿对应这三种坐标系中的一种就可以。
测试发现：mavros的map坐标系原点是mavros收到里程计信息时飞控所在的位置，map坐标系为x轴朝东，y轴朝北，z轴朝天即东北天坐标系。
vins-fusion word坐标系和mavros map坐标系如下：

mavros map坐标系	vins-fusion word坐标系
X： E	X： S
Y： N	Y： E
Z： U	Z： U

图示如下：

因此：vins-fusion word坐标系转换到mavros map系只需要将x换成y轴，y轴换成-x轴，z轴已对齐，保持不变即可，若用四元数表示旋转则保持w不变，其它相应的变化即可。参考：https://docs.px4.io/main/zh/ros/external_position_estimation.html。
3.px4坐标系
px4 local 坐标系，从mavlink LOCAL_POSITION_NED也可以看出，px4是北东地坐标系（NED）。在vins-mavros-px4链路，mavros会自动转化成适配px4坐标系的里程计信息，前提是vins到mavros是准确的。
图示为px4 NED坐标系：

思考：如果vins-fusion启动时，相机机头并不朝向正东方(F)，将如何转换？
当然只需要根据vins-fusion启动时，画出相应的坐标系，对照mavros map系进行相应的变换即可。

参考

1.https://blog.csdn.net/u010196944/article/details/127240169
2.https://docs.px4.io/main/en/
3.https://zhuanlan.zhihu.com/p/364390798
4.https://blog.csdn.net/qq_44998513/article/details/133144421?spm=1001.2014.3001.5502