参考：慕离巷的《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;
结束语
感谢您耐心的阅读。
生命不息，学习不止。唯有前进！前进！前进！
7721
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
