ardupilot-3.6.10——航点导航WPNav之样条曲线（Spline Navigation）

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