ROS四旋翼无人机快速上手指南(4)：阿木实验室PX4功能包飞行控制分析与讲解(重点章节)

成就更好的自己

这一章详细讲解一下阿木实验室(AMOV)的开源项目px4_command功能包，此功能包通过mavlink协议直接控制烧录px4固件的自驾仪，还融合了来自各个传感器的位姿，距离等信息，此外，功能包提供的接口向上对接应用层算法级的程序例如SLAM，kfc目标检测引导，路径规划等算法，因此，在整个无人机控制中起到了承上启下至关重要的作用。

用到的文件及工具包链接如下：

AMOV实验室源码如下：

https://gitee.com/theroadofengineers/px4_command

例程项目工程文件：

https://gitee.com/theroadofengineers/ros-uav

文件结构与功能包讲解见系列上一篇文章：

https://blog.csdn.net/qq_36098477/article/details/115489986

AMOV实验室官方上手指南wiki文档：

https://amov-wiki.readthedocs.io/zh_CN/latest/docs/%E9%A1%B9%E7%9B%AE%E4%BB%8B%E7%BB%8D.html

---

目录

MAVlink，MAVROS与px4_command功能包

PX4_command功能包文件结构

Include文件夹

src文件夹

Config文件夹

Msg文件夹

Px4_command节点数据流讲解

---

MAVlink，MAVROS与px4_command功能包

机载处理器与飞行控制器之间依靠 mavlink 通讯协议进行通信。

MavLink（Micro Air Vehicle Link）是一种用于小型无人载具的通信协议，于 2009 年首 次发布。该协议广泛应用于地面站（Ground Control Station，GCS）与无人载具（Unmanned vehicles）之间的通信，同时也应用在载具内部子系统的内部通信中，协议以消息库的形式 定义了参数传输的规则。MavLink 协议支持无人固定翼飞行器、无人旋翼飞行器、无人车辆 等多种载具。而 mavros 则是连接 ros 与 mavlink 之间的桥梁，是将 mavlink 协议翻译成 ros 可以读懂 的 topic 的“翻译官”。

Mavros 包可以读取飞控反馈回来的各种数据，发送给 Ros 其他节点使用，也可以发布 位置控制，速度控制，加速度控制指令，下面来介绍一下 mavros 控制指令。

 

位置控制

主题名称：/mavros/setpoint_position/local

发布坐标系：NED 坐标系

参数：geometry_msgs/PoseStamped Message

Geometry_msgs/PoseStamped 包含的内容：

其中 Point 是指无人机目标位置，Quaternion 是指无人机目标姿态

Point 含义：

x-------本地坐标系下前后位置，前为正，后为负 y-------本地坐标系下左右位置，左为正，右为负 z-------本地坐标系下高度位置，上为正，下为负；

Quaternion 含义：

这是一个四元数，x-----俯仰，y-----滚转，z，w-----两个参数共同组成偏航角

 

速度控制：

主题名称：/mavros/setpoint_velocity/cmd_vel_unstamped

发布坐标系：NED坐标系

参数：geometry_msgs/Twist

Twist 包含内容：

linear 含义：

x-------本地坐标系下前后速度，前为正，后为负 y-------本地坐标系下左右速度，左为正，右为负 z-------本地坐标系下高度速度，上为正，下为负

angular:

z-------本地坐标系下旋转速度，左为正，右为负

 

加速度控制：

主题名称：/mavros/setpoint——accel/accel 发布坐标系：NED 坐标系

参数：geometry_msgs/Vector3Stamped

x-------本地坐标系下前后加速度，前为正，后为负

y-------本地坐标系下左右加速度，左为正，右为负

z-------本地坐标系下高度加速度，上为正，下为负

 

为了更直观的控制和使用，px4_command自定义了一套通信协议与 mavros 进行通讯，自定义协议结构体如下图所示：

comid---指令的 id，用于区分指令序列 command---指令类别

Move_ENU：ENU 坐标系下运动

Move_Body：机体坐标系下运动

Hold：悬停指令

Land：降落指令

Disarm：上锁指令

Failsafe_land：紧急降落

Custom：自定义

 

Sub_mode---子命令，指明该指令是速度还是位置指令

Pose_sp---位置指令

0：x 方向

1：y 方向

2：z 方向

Vel_sp---速度指令

0：x 方向

1：y 方向

2：z 方向

Yaw_sp---航向位置指令

Yaw_rate_sp---转向速度指令

控制架构图如下所示

这样，开发者就可以自由生成位置，速度，加速度指令，控制无人机运动而不用管它内部 是怎么实现的，专注于任务和感知层面的研究，更加详细的解读请看文章开头处链接。

PX4_command功能包文件结构

Config文件夹：存放不同程序运行时所需的各种Parameter，一般在launch文件中进行载入，放入ROS Parameter Service中供程序运行时进行调用。

Include文件夹：存放着C++源程序所需的部分头文件，供src文件夹内的C++文件调用，其中有个command_to_mavros.h文件和pos_controller_PID.h两个头文件极其重要。

Launch文件夹：存放着launch文件，用来调用config文件夹中的yaml文件中的Parameter参数和有规则的启动节点。

Msg：存放着px4_command功能包用到的自定义消息类型文件。

Scripts：存放着python编写的节点文件。

Src：存放着最为主要的px4_command功能包中数据融合处理，位姿检测，位姿设定，飞行控制等C++源文件。

Include文件夹

文件夹下的这两个头文件极其重要，上文所说的px4_command功能包自定义的命令类型与格式就是在command_to_mavros.h中定义的，文件中使用原来MAVROS中繁琐的命令进行了二次封装，所以说，这个文件就成为了此工具包中的根本中的根本。

而pos_controller_PID.h文件使用PID和串级PID算法，被底层解析控制命令的源文件调用从而进行计算。

src文件夹

Src文件夹下面有4个C++源文件，跟基础飞行有关的只有两个px4_vel_controller.cpp和px4_pos_estimator.cpp文件。

px4_vel_controller.cpp的内容是生成一个叫/px4_vel_controller的节点用来对px4_command功能包中command_to_mavros.h定义的command命令进行解析，结合pos_controller_PID.h文件定义的计算函数将命令与数值转化成MAVROS中的数据(单轴速度，加速度等)，向下对接MAVROS节点，向上对接自己设计的算法控制程序节点。

px4_pos_estimator.cpp的内容是生成一个叫/px4_pos_estimator的节点，内部订阅了大量的传感器信息，通过将各种信息进行融合，用来给自驾仪和上层算法程序提供一个精确的位姿信息，是整机位姿信息的核心文件。

Application文件夹则存放的是应用级的文件，通过使用px4_command自定义的命令格式和内容，将每次的命令与数值发布出去，控制无人机进行运动。

Config文件夹

其中比较重要的是Parameter_for_control.yaml和collision_avoidance.yaml

Parameter_for_control.yaml：存放着有关于飞行中位姿矫正的反馈参数，水平速度环，水平位置环，高度位置环，高度速度环，起飞高度，最大水平速度，最大垂直粗度等等。

collision_avoidance.yaml：这个文件主要对应的任务是避障算法，存放着有关避障应用级别控制的参数，最小安全距离，航点，避障速度限幅，航点位置等等参数。

Msg文件夹

Msg存放px4_command功能包中自定义的消息类型，其中最要的是command.msg，它定义了px4_command自定义的命令格式，这个命令直接面向应用级算法程序，对于命令格式有疑惑的可以看一下这个文件。

 

Px4_command节点数据流讲解

启动shell文件夹下middle中的takeoff.sh文件，这个是仅仅控制飞机起飞后悬停在室内某一高度的最简单最简洁项目，借此讲解一下节点中数据流的走向与来源。

'/home/bcsh/shell/middle/auto_takeoff.sh'

使用rqt_graph工具查看节点图与数据流：

rqt_graph

整张图可以这样看：

紫框圈出的是MAVROS节点与其发布和订阅的一些消息，位于整个系统中最底层的位置，通俗一点，MAVROS节点订阅的是根据融合后的位姿和MAVROS的命令，发布的是自身的位姿和状态信息参数。

橙色圈出的是传感器数据传出节点，分别有激光雷达/rplidarNode，T265相机/camera/realsense2_camera，自驾仪(自驾仪直接与MAVROS通过串口通信)/mavros；

绿框圈出的是位姿分析节点/px4_pos_estimator，订阅大量传感器信息进行融合，将融合后的信息通过/mavros/vision_pose/pose发布出去给自驾仪；

注:由于自驾仪设置自身位置选择的数据来源是vision模式，而MAVROS发布的/mavros/local_position/pose的信息是自驾仪设置自身位置，因此可以在消息内容上可以理解/mavros/vision_pose/pose=/mavros/local_position/pose，详见文章开头处链接。

蓝框圈出的的命令解析节点与数据计算/px4_vel_controller节点，根据图中能明显的看到此节点左侧订阅了上层节点发布的/px4/command消息，右侧发布了三个消息/mavros/setpoint_raw/local，/mavros/setpoint_position/local，/mavros/actuator_control直接通过MAVROS控制自驾仪；

最后，红框圈出的是算法应用级别的节点，只用发布command_to_mavros.h定义好的飞行控制命令就可以精确地控制飞机。

 

上文只是分析一下最简单的飞行情况，实际上，要完成一次复杂的飞行还需要加上很多节点与消息，假如现在要实现一些复杂的飞行，你觉得应该加到哪些位置呢？

下面这个数据流图是在飞行中进行yolo目标检测并引导飞行的，很显然，所有算法实质上只需要最后落实到发布/px4/command命令消息即可，至于剩下的怎么执行，啥都不用管，这就是px4_command功能包，就是这么方便！