参考:慕离巷的《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仿真后,发现这次结果相同。