ardupilot-3.6.10——航点导航WPNav之样条曲线(Spline Navigation)

本文深入解析ArduCopter中的样条曲线导航算法,介绍其核心算法——两点三次埃尔米特插值,探讨源点与目标点速度选取策略,以及飞行过程中的速度控制机制。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

参考:慕离巷《ArduCopter——ArduPilot——航点导航WPNav(二)——Spline Navigation》三石の四夕《三次Hermite插值》zhangyue_lala《一些插值法及理解》

目录

前言

核心算法

软件仿真 

源点、目标点速度选取及飞行中的速度控制

结束语


 

前言

    样条曲线,是一种曲线。所谓样条本来是工程设计中使用的一种绘图工具,它是富有弹性的细木条或细金属条。绘图员利用它把一些已知点连接成一条光滑曲线(样条曲线),并使连接点处有连续的曲率。

核心算法

    ardupilot样条曲线导航的核心算法为两点三次埃尔米特(hermite)插值算法。插值法应该是数值分析中讲的内容,大致意思就是,给定N个点的坐标,可以得到一个N+1阶多项式,这个多项式过给定的每个点。实际上就是通过离散(有限个)数据,估计得到一个表示这些数据分布规律的函数。也就是通过多项式来给定函数,一般来说给的点越多,多项式的阶数越高,拟合越准确。但有时也会出现龙格现象。埃尔米特插值就是其中的一种插值方法。

不少实际的插值问题不但要求在节点上的函数值相等,而且还要求对应的导数值也相等,甚至要求高阶导数也相等,满足这种要求的插值多项式就是埃尔米特插值多项式。                                                                                      ——来自百度百科

    插值是要拟合一个函数。但在样条曲线导航里,实际上没有给定的函数。样条曲线导航是利用埃尔米特插值多项式的导数是连续的这一特点,用到多旋翼上,就是轨迹是曲线,速度是连续变化的(位置的导数是速度),这样多旋翼不会有剧烈的姿态变化,飞行比较顺滑。

    埃尔米特插值算法的公式推导我就不再这儿列举了,我认为工科对于这些理论要先知道是什么,怎么用,具体是怎么来的可以在熟悉了之后再去了解,一上来就粘贴些理论推导,容易看得一头雾水,也丧失了看下去的信心。

    两点三次埃尔米特(hermite)插值算法,给定两点坐标和两点处导数,可以得到一个3阶多项式。

p = p0*(2*t^3-3*t^2+1) + v0*(t^3-2*t^2+t) + p1*(-2*t^3+3*t^2) + v1*(t^3-t^2)

     这个函数p就是样条曲线导航的轨迹方程。其中p0为源点坐标,m0为源点速度,p1为目标点坐标,m1为目标点速度。假设t等于0时多旋翼在源点开始向目标点移动,t等于1时,到达目标点。令y0等于p0,y1等于p1,m0等于v0,m1等于v1,x等于t,x0等于0,x1等于1,代入下面公式,化简后得到上式。

    轨迹函数得到后,如何确定两点坐标及其导数是关键。尤其是导数的确定,和你的第一想法会有很大的不同。 

软件仿真 

    通过ardupilot软件仿真,规划含有样条曲线的航线,飞行轨迹如下。 其中航点4、6、8、11、13为样条曲线航点。其他航点为正常航点,悬停时间设置为0。9点到11点之间的最大飞行速度设置为1m/s。

    通过打印调试信息,找到航点9、11的坐标位置(以飞机home点为原点)依次为[14.28151;-36.7041]、[12.1338;-48.3227]。速度标量按照1m/s,矢量为[0.991302;-0.131600923]、[-0.992780826;0.11994262]。得到matlab仿真图如下

    发现和ardupilot软件仿真轨迹严重不一致。 通过打印调试信息发现,源点和目标点的速度并不是设置的最大速度,再次使用matlab仿真后,发现这次结果相同。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值