环境感知与规划专题（三）——一种高效的运动规划算法（JLT）（上）

0. 前言

  无人机在飞行过程中经常会遇到许多不可预见的突发情况或者障碍物，因此，寻求一种高效稳定的运动规划算法是十分必要的。
  本文介绍一种可运行于嵌入式mcu的运动规划算法——Jerk Limited Trajectory（JLT），该方法为BVP方法的其中一种解法，计算量小，实时性强。且常与非线性MPC相结合，应用于解决复杂环境的运动规划问题。
  该算法能够实时地根据变化的环境，在满足动力学约束的前提下，快速重规划一条最优路径，使得无人机能够安全可靠地执行飞行任务。

1. Jerk Limited Trajectory（目标速度确定的情况）

  JLT（这里指目标速度确定的情况）问题的求解本质上是一个分类讨论的过程，其关键在于确定 $\Delta T_{1}$, $\Delta T_{2}$, $\Delta T_{3}$ 的值，然后通过分段函数对三阶积分器模型进行递推，从而得到从初始状态平滑过度到目标状态的轨迹，包括这个过程中每一时刻的 p, v,a,j 。
三阶积分器模型的JLT求解示意图：

  如上图所示为三阶积分器模型的JLT求解示意图，该问题的求解流程图如下所示：

主要步骤为：
  初始化状态量 $a_{0}$, $v_{0}$, $p_{0}$

state(1).p = 10;
state(1).v = 1;
state(1).a = 0;
state(1).j = 0;

  更新约束条件 $j_{max}$, $a_{max}$, $v_{max}$

state(1).v_max = 5.0;
state(1).a_max = 5.0;
state(1).j_max = 10.0;

  更新设定值 $v_{sp}$

state(1).p_sp = 0.0;
state(1).v_sp = 3;
state(1).a_sp = 0.0;

更新每段轨迹的最小所需时间 $\Delta T_{1}$, $\Delta T_{2}$, $\Delta T_{3}$

function [ T1 ] = computeT1_1( a0, v3, j_max, a_max )
    delta = 2 * a0 * a0 + 4 * j_max * v3;
    
    if delta < 0
        T1 = 0;
        return;
    end
    
    sqrt_delta = sqrt(delta);
    T1_plus = (-a0 + 0.5 * sqrt_delta) / j_max;
    T1_minus = (-a0 - 0.5 * sqrt_delta) / j_max;
    
    T3_plus = a0 / j_max + T1_plus;
    T3_minus = a0 / j_max + T1_minus;
    
    T1 = 0;
    
    if (T1_plus >= 0 && T3_plus >= 0) 
		T1 = T1_plus;
    else
        if (T1_minus >= 0 && T3_minus >= 0)
            T1 = T1_minus;
        else
            
        end    
    end
    
    T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
    
    T1 = max(T1, 0);

end

function [ T1 ] = computeT1_2( T123, a0, v3, j_max, a_max )
    a = -j_max;
    b = j_max * T123 - a0;
    delta = T123 * T123 * j_max * j_max + 2 * T123 * a0 * j_max - a0 * a0 - 4 * j_max * v3;
    
    if delta < 0
        T1 = 0;
        return;
    end
    
    sqrt_delta = sqrt(delta);
    denominator_inv = 1 / (2 * a);
    T1_plus = max((-b + sqrt_delta) * denominator_inv, 0);
    T1_minus = max((-b - sqrt_delta) * denominator_inv, 0);
    
    T3_plus = a0 / j_max + T1_plus;
    T3_minus = a0 / j_max + T1_minus;
    
    T13_plus = T1_plus + T3_plus;
    T13_minus = T1_minus + T3_minus;
    
    T1 = 0;
    
    if (T13_plus > T123) 
		T1 = T1_minus;
    else
        if (T13_minus > T123) 
		T1 = T1_plus;
        end
    end
    T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
end

function [ T2 ] = computeT2_2( T123, T1, T3 )
	T2 = T123 - T1 - T3;
	T2 = max(T2, 0);
end

function [ T3 ] = computeT3( T1, a0, j_max )
	T3 = a0 / j_max + T1;
	T3 = max(T3, 0);
end

function [ T1, T2, T3 ] = updateDurationsMinimizeTotalTime( direction, max_jerk, max_accel, vel_sp, state_a, state_v )
    jerk_max_T1 = direction * max_jerk;
    delta_v = vel_sp - state_v;
    
    T1 = computeT1_1(state_a, delta_v, jerk_max_T1, max_accel);
    T3 = computeT3(T1, state_a, jerk_max_T1);
    T2 = computeT2_1(T1, T3, state_a, delta_v, jerk_max_T1);
    
end

由于不同方向规划的时间无法完全同步，因此，较快达到设定值的规划方向需要对较慢达到设定值的规划方向“妥协”，这里采用统一给定所有规划方向中最长时间作为统一的最长时间进行重规划的方式进行时间同步。

function [state] = timeSynchronization( state, n_traj )
    desired_time = 0;
    longest_traj_index = 0;
    
    for i = 1 : n_traj
        T123 = state(i).T1 + state(i).T2 + state(i).T3;
        if T123 > desired_time
            desired_time = T123;
            longest_traj_index = i;
        end
    end
    
    if desired_time > 0
        for i = 1 : n_traj
            if i ~= longest_traj_index
                [state(i).T1, state(i).T2, state(i).T3] = updateDurationsGivenTotalTime(desired_time, state(i).direction, state(i).j_max, state(i).a_max, state(i).v_sp, state(i).a, state(i).v);
            end
        end
    end

end

function [ T1, T2, T3 ] = updateDurationsGivenTotalTime( T123, direction, max_jerk, max_accel, vel_sp, state_a, state_v )
    jerk_max_T1 = direction * max_jerk;
    delta_v = vel_sp - state_v;
    
    T1 = computeT1_2(T123, state_a, delta_v, jerk_max_T1, max_accel);
    T3 = computeT3(T1, state_a, jerk_max_T1);
    T2 = computeT2_2(T123, T1, T3);
    
end

若规划值达到设定值附近，则结束规划算法，否则更新初始状态量，重新进行规划，直到规划值达到设定值。

% 模拟三角波摇杆输入量
for n = 1 : n_sample_stick + 1
    if n <= n_sample_stick / 2 
        stick(1).value = 2 * (n - 1) / n_sample_stick * state(1).v_max;
        stick(2).value = 2 * (n - 1) / n_sample_stick * state(2).v_max;
        stick(3).value = 2 * (n - 1) / n_sample_stick * state(3).v_max;
    else
        stick(1).value = (1 + (n_sample_stick / 2 - (n - 1)) * 2 / n_sample_stick) * state(1).v_max;
        stick(2).value = (1 + (n_sample_stick / 2 - (n - 1)) * 2 / n_sample_stick) * state(2).v_max;
        stick(3).value = (1 + (n_sample_stick / 2 - (n - 1)) * 2 / n_sample_stick) * state(3).v_max;
    end
    log_stick(1, n) = stick(1).value;
    log_stick(2, n) = stick(2).value;
    log_stick(3, n) = stick(3).value;
end

for i = 1 : 3
    [state(i).T1, state(i).T2, state(i).T3, local_time, state(i).direction ] = updateDurations_Velocity_Setpoint( state(i).v_sp, state(i).a, state(i).v, state(i).j_max, state(i).a_max, state(i).v_max);
end
state = timeSynchronization(state, 2);
t123 = state(1).T1 + state(1).T2 + state(1).T3;
n_steps = ceil(t123 / dt);
n_steps = ceil(t123 / dt) + (n_sample_stick + 1) * 8 + 400;

for i = 1 : n_steps
    for k = 1 : 3  
        [state(k).p, state(k).v, state(k).a, state(k).j] = updateTraj( dt, time_stretch, local_time, state(k).T1, state(k).T2, state(k).T3, state(k).a, state(k).v, state(k).p, state(k).direction, state(k).j_max );
        log_p(k, n_index) = state(k).p;
        log_v(k, n_index) = state(k).v;
        log_a(k, n_index) = state(k).a;
        log_j(k, n_index) = state(k).j;
        log_t(k, n_index) = n_index * dt;
        log_v_sp(k, n_index) = state(k).v_sp;
        [state(k).T1, state(k).T2, state(k).T3, local_time, state(k).direction ] = updateDurations_Velocity_Setpoint( state(k).v_sp, state(k).a, state(k).v, state(k).j_max, state(k).a_max, state(k).v_max);
    end
    state = timeSynchronization(state, 2);
    n_index = n_index + 1;
    absolute_time = absolute_time + dt;
%定周期改变速度期望设定值     
    if mod(n_index , 8) == 0 && n_stick <= (n_sample_stick + 1)
        state(1).v_sp = log_stick(1, n_stick);
        state(2).v_sp = log_stick(2, n_stick);
        state(3).v_sp = log_stick(3, n_stick);
        [state(k).T1, state(k).T2, state(k).T3, local_time, state(k).direction ] = updateDurations_Velocity_Setpoint( state(k).v_sp, state(k).a, state(k).v, state(k).j_max, state(k).a_max, state(k).v_max);
        state = timeSynchronization(state, 2);
        t123 = state(1).T1 + state(1).T2 + state(1).T3;
        n_stick = n_stick + 1;
    end 
end

figure(1);
plot(log_t(1, :), log_p(1, :), 'r');
hold on;
plot(log_t(1, :), log_v(1, :), 'g');
hold on;
plot(log_t(1, :), log_a(1, :), 'b');
hold on;
plot(log_t(1, :), log_j(1, :), 'm');
% hold on;
% plot(log_t(1, :), log_v_sp(1, :), 'k');
legend('pos_x','vel_x','acc_x','jerk_x');
grid on;

figure(2);
plot(log_t(2, :), log_p(2, :), 'r');
hold on;
plot(log_t(2, :), log_v(2, :), 'g');
hold on;
plot(log_t(2, :), log_a(2, :), 'b');
hold on;
plot(log_t(2, :), log_j(2, :), 'm');
% hold on;
% plot(log_t(2, :), log_v_sp(2, :), 'k');
legend('pos_y','vel_y','acc_y','jerk_y');
grid on;

figure(3);
plot(log_t(3, :), log_p(3, :), 'r');
hold on;
plot(log_t(3, :), log_v(3, :), 'g');
hold on;
plot(log_t(3, :), log_a(3, :), 'b');
hold on;
plot(log_t(3, :), log_j(3, :), 'm');
% hold on;
% plot(log_t(3, :), log_v_sp(3, :), 'k');
legend('pos_z','vel_z','acc_z','jerk_z');
grid on;

figure(4)
plot(log_stick(1, :), 'r');
grid on;

不同的设定值存在不同的轨迹情况，主要分为以下几类：

• 加速度曲线为三角形，且达到最大值
  
  加速度曲线为三角形，但未达到最大值
  
  加速度曲线为梯形
  

2 总结

  本篇主要阐述了速度设定情况的JLT解法，该方法适用于解决多旋翼无人机的以下问题：

• 适用于手动模式下的运动规划问题（生成最优轨迹）
• 也适用于自动模式下，通过航点得到速度设定值的运动规划问题

  下一篇将阐述位置设定情况的JLT解法，该解法更为复杂，适用范围更广。本篇中只贴出了部分代码，具体的Matlab仿真代码可参考我的Github相关项目，有疑问请留言，欢迎交流。
https://github.com/DistantUtopia/JerkLimitedTrajectory

---

作者简介： 一个被Coding耽误的无人机算法工程师，控制、导航略懂一二，热衷技术，喜欢乒乓、音乐、电影，欢迎交流。

知乎： @遥远的乌托邦

GitHub: https://github.com/DistantUtopia

微信公众号： @遥远的乌托邦