技术分享 | 四旋翼丝滑“绕⼋”⾃主轨迹运动

在⽆⼈机空中过程中，会不会觉得简单的指点⻜⾏过于简单，⻜⾏轨迹过于直板，在本⽂中，尝试使⽤ mavros更多的接⼝让⽆⼈机的⻜⾏轨迹看起来⽐较丝滑，以绕⼋运动来体现此点。

话不多说，先看视频效果：
https://www.bilibili.com/video/BV1BL411N774?spm_id_from=333.999.0.0

file

qgc中的⻜机轨迹

file

数据点绘制曲线图

⼀、绕⼋运动
**
说起绕⼋运动，会不会想起 伯努利双纽线

file
伯努利双纽线的参数⽅程为：
file
**
二、MAVROS接口下代码实现
file
在mavros提供的接⼝中，有 mavros/setpoint_raw/local 话题，该话题就是对⽆⼈机的位置，速度，加 速度，航向⻆以及航向⻆速率控制的接⼝。

在px4 mavros offboard官⽅案例demo中提供的是简单的位置数据控制，多个位置点时候，⽆⼈机轨迹不够丝滑，在此我们要对这个 mavros/setpoint_raw/local 的所有数据进⾏控制!

640.gif

为了让每⼀个数据都有体现出来，利⽤伯努利双纽线的参数⽅程计算出，

position.x；position.y；velocity.x；velocity.y；afx；afy；yaw

file
file
file
file
file

在代码中的体现为

file

整个过程我们的⾼度⽅向上⾯的⾼度为指定⾼度，速度和加速度均为零。

完整代码为figure_8_circle.cpp ，⻅⽂末。

**三、数据保存并生成轨迹图 **

上⾯基本已经实现了，绕⼋的运动控制代码，为了在ROS中对⽆⼈机数据的精确处理，⽣成类似于MATLAB上的轨迹图，我们获取mavros/local_position/pose 的话题数据，保存成tum格式的txt⽂档数据。
代码为 save_traj_as_tum.cpp

1、安装evo
我们使⽤快捷安装⽅式，直接安装最新的稳定发⾏版本：
pip install evo --upgrade --no-binary evo
注意：此安装过程中，⽹络不佳可导致安装报错

2、将轨迹保存为tum格式
tum的数据集格式为：timestamp tx ty tz qx qy qz qw（每⾏有8个元素，结尾没有空格，时间以秒为单位）

file

我们使⽤ save_traj_as_tum.cpp 代码订阅 mavros/local_position/pose 话题，并保存⽂件为 /home/amov/test.txt 路径之下，完整代码⻅⽂末。
结果保存的 test.txt 实际数据效果如下：

file

3、生成轨迹图
上⾯保存到的数据 test.txt 使⽤如下指令⽣成轨迹图
evo_traj tum test.txt -p

file
file
file
file

四、总结

总体来说，实现的绕⼋运动不能100%能够达到伯努利双纽线那样丝滑，这⾥也是给出控制⽆⼈机中的另外⼀条思路，不要只考虑使⽤纯位置控制，或者纯速度控制，多考虑⼀些复合控制，这样⽆⼈机才能丝滑流畅。px4下的mavros中有很多插件，也有很多功能，能在⽆⼈机地面站QGC上实现的功能，在机载计算机中亦能实现，“阿⽊实验室，让机器研发更⾼效！” ，希望给看完⽂章的你起到启发的想法。

查看评论区获取完整源码

五、参考链接

ROS轨迹保存为tum格式,并⽤evo轨迹绘制：
https://its301.com/article/just_do_it567/114672305

伯努利双纽线：
https://zh.wikipedia.org/wiki/%E4%BC%AF%E5%8A%AA%E5%88%A9%E5%8F%8C%E7%BA%BD%E7%BA%BF

数据类型：
http://docs.ros.org/en/api/mavros_msgs/html/msg/PositionTarget.html

作者：阿⽊实验室-新⼀

• End -

技术发展的日新月异，阿木实验室将紧跟技术的脚步，不断把机器人行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进，是我们培训最大的价值。如果你在机器人行业，就请关注我们的公众号，我们将持续发布机器人行业最有价值的信息和技术。
阿木实验室致力于为机器人研发提供开源软硬件工具和课程服务，让研发更高效！