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仿真后，发现这次结果相同。

    通过上图发现，matab仿真出的速度标量在10～40m/s区间内，加速度在20~150m/s^2区间内,这明显和ardupilot软件仿真是冲突的。由此可以发现ardupilot样条曲线算法对速度的选取，轨迹跟踪过程中速度的控制，是有自己的逻辑的，依我个人的观点来看，这就是理论和工程实践的良好结合，不得不佩服ardupilot的那些核心维护人员是真厉害。

    仔细想想，不难看出，matlab仿真出的速度、加速度这么大的前提是，多旋翼飞行器用1秒钟的时间，飞完两点三次埃尔米特（hermite）插值算法生成的轨迹。通过ardupilot软件仿真发现，实际情况并不是这样。也就是说，ardupilot利用源点、目标点速度和1秒钟完成轨迹的这个假设生成轨迹方程，然后按照一种方法控制飞行速度，实现轨迹平滑的飞行。

    matlab源码如下

function Hermite_apm(N,p0,v0,p1,v1)
% 程序使用两点三次埃尔米特插值
% N代表节点个数，数组形式
% p0----origin
% m0----origin_vel
% p1----dest
% m1----dest_vel


syms t;
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);
v = diff(p);
a = diff(p,2);

p_m = [];
v_m = [];
a_m = [];

t1=linspace(0,1,N);%插值节点的确认

    for j=1:N
        sp = subs(p,t,t1(j));
        sv = subs(v,t,t1(j));
        sa = subs(a,t,t1(j));
        
        p_m = [p_m sp];
        v_m = [v_m sv];
        a_m = [a_m sa];
        
        %drawnow;
        pause(0.02);
        
        hold on;
        subplot(2,2,1);
        plot(sp(2,1),sp(1,1),'o')
        axis equal
        ylabel('north(m)')
        xlabel('east(m)')
        title('水平位置')
        
        hold on;
        subplot(2,2,2);
        plot(sv(2,1),sv(1,1),'o')
        axis equal
        ylabel('north(m)')
        xlabel('east(m)')
        title('速度矢量')
        
        hold on;
        subplot(2,2,3);
        plot(t1(j),norm(sv),'o')
        %axis equal
        ylabel('speed(m/s)')
        xlabel('time(s)')
        title('速度标量')
        
        hold on;
        subplot(2,2,4);
        plot(t1(j),norm(sa),'o')
        %axis equal
        ylabel('Acceleration(m/s^2)')
        xlabel('time(s)')
        title('加速度标量')
    end
    x = p_m(1,:);
    y = p_m(2,:);
    
    n = v_m(1,:);
    m = v_m(2,:);
    
    subplot(2,2,1);
    plot(y,x);%绘图
    axis equal
end

    将上述源码保存为 Hermite_apm.m文件，然后添加到搜索路径，在命令行窗口输入

Hermite_apm(100,[14.28151;-36.7041],[35.6694;-4.73531],[12.1338;-48.3227],[-11.1978;1.35286])

可以看到上述仿真结果。 

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

    先说结论，源点、目标点速度选取和航线长度有关，也和段类型和样条线段结束类型有关。AC_WPNav中有一个关于段类型的标志，表明这个段是直线或者曲线。

// segment types, either straight or spine
    enum SegmentType {
        SEGMENT_STRAIGHT = 0,
        SEGMENT_SPLINE = 1
    };

    还有一个标志来表明样条线段结束类型，及样条曲线这个点是最后一个点，或者这个点之后的端是直线或曲线。

// spline segment end types enum
    enum spline_segment_end_type {
        SEGMENT_END_STOP = 0,
        SEGMENT_END_STRAIGHT,
        SEGMENT_END_SPLINE
    };

    以下图为例

 段2-3的段类型为直线（SEGMENT_STRAIGHT），段3-4的段类型为样条曲线（SEGMENT_SPLINE），段4-5为直线。点4为样条曲线航点，它之后的点5为普通航点，所以点4的样条线段结束类型为直线（SEGMENT_END_STRAIGHT）。

    段类型的判断比较简单，普通航点（MAV_CMD_NAV_WAYPOINT） 就是直线类型，样条曲线（MAV_CMD_NAV_SPLINE_WAYPOINT）就是样条曲线类型。样条线段结束类型的判断依赖于下一个航点的类型，如果下一个航点是普通航点，就是直线类型（SEGMENT_END_STRAIGHT），如果下一个航点是样条曲线，就是样条曲线类型（SEGMENT_END_SPLINE），如果下一个航点不存在，就是停止类型（SEGMENT_END_STOP）。

    源点速度判断条件：

    1.如果样条曲线为第一个航点或运行AC_WPNav库的第一个航点或运行还没有到达目标点（例如在飞行下一个目标点之前激活样条曲线航点为目标点），使用（目标点-源点）*0.0025作为源点的速度矢量。代码在函数set_spline_origin_and_destination中，下面是代码片段

bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000));

// calculate spline velocity at origin
    if (stopped_at_start || !prev_segment_exists) {
    	// if vehicle is stopped at the origin, set origin velocity to 0.02 * distance vector from origin to destination
    	_spline_origin_vel = (destination - origin) * dt;
    	_spline_time = 0.0f;
    	_spline_vel_scaler = 0.0f;
    }

    2.如果上一段是直线，源点速度为上一段的（目标点-源点）

// look at previous segment to determine velocity at origin
        if (_flags.segment_type == SEGMENT_STRAIGHT) {
            // previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
            // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
            _spline_origin_vel = (_destination - _origin);
            _spline_time = 0.0f;	// To-Do: this should be set based on how much overrun there was from straight segment?
            _spline_vel_scaler = _pos_control.get_vel_target().length();    // start velocity target from current target velocity
        }

    3.如果上一段是样条曲线，则以上一段的目标点速度作为这一段的源点速度。

// look at previous segment to determine velocity at origin
        if (_flags.segment_type == SEGMENT_STRAIGHT) {
            // previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
            // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
            _spline_origin_vel = (_destination - _origin);
            _spline_time = 0.0f;	// To-Do: this should be set based on how much overrun there was from straight segment?
            _spline_vel_scaler = _pos_control.get_vel_target().length();    // start velocity target from current target velocity
        }else{
            // previous segment is splined, vehicle will fly through origin
            // we can use the previous segment's destination velocity as this segment's origin velocity
            // Note: previous segment will leave destination velocity parallel to position difference vector
            //       from previous segment's origin to this segment's destination)
            _spline_origin_vel = _spline_destination_vel;
            if (_spline_time > 1.0f && _spline_time < 1.1f) {    // To-Do: remove hard coded 1.1f
                _spline_time -= 1.0f;
            }else{
                _spline_time = 0.0f;
            }
            // Note: we leave _spline_vel_scaler as it was from end of previous segment
        }

    目标点速度判断条件 ：

    1.如果样条线段结束类型为SEGMENT_END_STOP，则目标点速度为（目标点-源点）*0.0025。

case SEGMENT_END_STOP:
        // if vehicle stops at the destination set destination velocity to 0.02 * distance vector from origin to destination
        _spline_destination_vel = (destination - origin) * dt;
        _flags.fast_waypoint = false;
        break;

    2.如果样条线段结束类型为SEGMENT_END_STRAIGHT，则目标点速度为（下一个航点-目标点）。

case SEGMENT_END_STRAIGHT:
        // if next segment is straight, vehicle's final velocity should face along the next segment's position
        _spline_destination_vel = (next_destination - destination);
        _flags.fast_waypoint = true;
        break;

    3.如果样条线段结束类型为SEGMENT_END_SPLINE，则目标点速度为（下一个航点-源点）。

case SEGMENT_END_SPLINE:
        // if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination
        _spline_destination_vel = (next_destination - origin);
        _flags.fast_waypoint = true;
        break;

    速度选择好后，把源点、目标点速度加起来和（目标点-源点）*4做比较，然后缩放相应的比例，变成最终轨迹方程中的源点、目标点速度。

// code below ensures we don't get too much overshoot when the next segment is short
    float vel_len = _spline_origin_vel.length() + _spline_destination_vel.length();
    float pos_len = (destination - origin).length() * 4.0f;
    if (vel_len > pos_len) {
        // if total start+stop velocity is more than twice position difference
        // use a scaled down start and stop velocityscale the  start and stop velocities down
        float vel_scaling = pos_len / vel_len;
        // update spline calculator
        update_spline_solution(origin, destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);
    }else{
        // update spline calculator
        update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
    }

    飞行中的速度控制：

    在函数advance_spline_target_along_track(float dt)中进行轨迹跟踪，飞行速度、位置都在这个函数控制中。

    下面函数计算3次多项式各个项的系数

/// update_spline_solution - recalculates hermite_spline_solution grid
///		relies on _spline_origin_vel, _spline_destination_vel and _origin and _destination
void AC_WPNav::update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel)
{
    _hermite_spline_solution[0] = origin;
    _hermite_spline_solution[1] = origin_vel;
    _hermite_spline_solution[2] = -origin*3.0f -origin_vel*2.0f + dest*3.0f - dest_vel;
    _hermite_spline_solution[3] = origin*2.0f + origin_vel -dest*2.0f + dest_vel;
 }

    下面函数把时间变量代入轨迹方程计算目标位置和速度（位置的微分是速度）

// calc_spline_pos_vel_accel - calculates target position, velocity and acceleration for the given "spline_time"
/// 	relies on update_spline_solution being called when the segment's origin and destination were set
void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector3f& velocity)
{
    float spline_time_sqrd = spline_time * spline_time;
    float spline_time_cubed = spline_time_sqrd * spline_time;

    position = _hermite_spline_solution[0] + \
               _hermite_spline_solution[1] * spline_time + \
               _hermite_spline_solution[2] * spline_time_sqrd + \
               _hermite_spline_solution[3] * spline_time_cubed;

    velocity = _hermite_spline_solution[1] + \
               _hermite_spline_solution[2] * 2.0f * spline_time + \
               _hermite_spline_solution[3] * 3.0f * spline_time_sqrd;
}

    上面两个函数所做的计算，和一开头给出的公式是一模一样的。

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) 

    如果严格按照时间假设 ，飞行所需要的速度、加速度是相当大的。所以apm做了十分聪明的速度限制，用时间来显示速度，及通过时间控制目标点位置，从而达到控制速度的目的。

// advance spline time to next step
_spline_time += _spline_time_scale*dt;

    _spline_time就是刚才所说的时间 ，运行频率400Hz，时间间隔（上述代码中的dt）0.0025，如果_spline_time_scale是1,飞行情况就会像是matlab仿真中的那样，但那个结果显然已经超出了飞行器的能力，所以apm通过控制_spline_time_scale，来实现限速。

    _spline_time_scale（时间比例） = 飞行的最大速度/_spline_time时刻通过公式算出的速度。

    不得不佩服这个设计真是十分的巧妙。下面附上速度控制的主要程序片段，完整程序请自行下载apm源代码。

// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);

// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
    _spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cmss);
}else if(_spline_vel_scaler < vel_limit) {
    // increase velocity using acceleration
    _spline_vel_scaler += _wp_accel_cmss * dt;
}

// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);

// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
_spline_time_scale = _spline_vel_scaler / target_vel_length;

结束语

    感谢您耐心的阅读。

    生命不息，学习不止。唯有前进！前进！前进！