在⽆⼈机空中过程中,会不会觉得简单的指点⻜⾏过于简单,⻜⾏轨迹过于直板,在本⽂中,尝试使⽤ mavros更多的接⼝让⽆⼈机的⻜⾏轨迹看起来⽐较丝滑,以绕⼋运动来体现此点。
话不多说,先看视频效果:
https://www.bilibili.com/video/BV1BL411N774?spm_id_from=333.999.0.0
qgc中的⻜机轨迹
数据点绘制曲线图
如果⽐较感兴趣可以继续深⼊阅读,了解其实现过程及思路分析,让你在你⾃⼰的电脑上也能实现同样的demo。
⼀、绕⼋运动
说起绕⼋运动,会不会想起 伯努利双纽线
伯努利双纽线的参数⽅程为:
二、MAVROS接口下代码实现
在mavros提供的接⼝中,有 mavros/setpoint_raw/local 话题,该话题就是对⽆⼈机的位置,速度,加 速度,航向⻆以及航向⻆速率控制的接⼝。
在px4 mavros offboard官⽅案例demo中提供的是简单的位置数据控制,多个位置点时候,⽆⼈机轨迹不够丝滑,在此我们要对这个 mavros/setpoint_raw/local 的所有数据进⾏控制
为了让每⼀个数据都有体现出来,利⽤伯努利双纽线的参数⽅程计算出,
position.x;position.y;velocity.x;velocity.y;afx;afy;yaw
在代码中的体现为
整个过程我们的⾼度⽅向上⾯的⾼度为指定⾼度,速度和加速度均为零。
完整代码为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个元素,结尾没有空格,时间以秒为单位)
我们使⽤ save_traj_as_tum.cpp 代码订阅
mavros/local_position/pose 话题,并保存⽂件为 /home/amov/test.txt 路径之下,完整代码⻅⽂末。
结果保存的 test.txt 实际数据效果如下:
3、生成轨迹图
上⾯保存到的数据 test.txt 使⽤如下指令⽣成轨迹图
evo_traj tum test.txt -p
四、总结
总体来说,实现的绕⼋运动不能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