0. 前言

上一篇中针对速度设定的JLT算法进行了详解，该算法能够解决一部分给定速度期望值，或者通过给定位置期望值以一定规则转化为给定速度期望值的问题。
该方法简单高效，能够实时运行于嵌入式MCU，解决多旋翼无人机的部分运动规划问题。本篇将探讨一种更直接的运动规划算法，该算法基于上一篇的速度设定问题的解法，做了一定扩展，其适用于解决更广泛的问题。

1. 位置设定问题的JerkLimitedTrajectory解法

首先还是回到问题本身，JLT解决的是对应三阶积分器数学模型的运动规划问题，本篇在上一篇的基础上，对该问题进行了延伸与扩展，其数学与几何描述如下所示：在这里插入图片描述
位置设定JLT问题与速度设定JLT问题相似之处在于，它们都可分类为三大类情况进行分类讨论。不同之处则在于，本篇讨论的位置设定问题更为复杂，需要将JLT问题求解的各段时间 $T$ 从至多3段扩展至至多7段。位置设定JLT问题的三种情况:
在这里插入图片描述
位置设定JLT的算法流程图如下所示，与速度设定JLT问题相比，该算法将分别计算上升阶段（acc），下降阶段（dec）以及巡航阶段（cruise）的时间分配情况。

上升阶段对应T1、T2、T3
巡航阶段对应T4
下降阶段对应T5、T6、T7

然而，在确定这些时间参数之前，我们需要首先通过位置设定值（ $p_{sp}$ ）与初始位置（ $p_{0}$ ）、初始速度（ $v_{0}$ ）、初始加速度（ $a_{0}$ ）等参数，结合二分查找算法（BinarySearch）进行上升阶段的终止速度 $v_{m}$ 的搜索。
如下图所示为详细的位置设定JLT问题的算法流程图。
主要步骤为：

初始化状态量 $a_{0}$ , $v_{0}$ , $p_{0}$

state(1).p = 0;
state(1).v = 0;
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;

更新设定值 $p_{sp}$

state(1).p_sp = 5.0;
state(1).v_sp = 0.0;
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 [ T4 ] = computeT4( delt_p_acc, delt_p_dec, v_i, delt_p )
	T4 = (delt_p - delt_p_acc - delt_p_dec) / v_i;
	T4 = max(T4, 0);
end

function [ T5 ] = computeT5_1(  a0, v3, j_max, a_max  )
    delta = 2 * a0 * a0 + 4 * j_max * v3;
    
    if delta < 0
        T5 = 0;
        return;
    end
    
    sqrt_delta = sqrt(delta);
    T5_plus = (-a0 + 0.5 * sqrt_delta) / j_max;
    T5_minus = (-a0 - 0.5 * sqrt_delta) / j_max;
    
    T7_plus = a0 / j_max + T5_plus;
    T7_minus = a0 / j_max + T5_minus;
    
    T5 = 0;
    
    if (T5_plus >= 0 && T7_plus >= 0) 
		T5 = T5_plus;
    else
        if (T5_minus >= 0 && T7_minus >= 0)
            T5 = T5_minus;
        else
            
        end    
    end
    
    T5 = saturateT5ForAccel(a0, j_max, T5, a_max);
    
    T5 = max(T5, 0);
end

function [ T6 ] = computeT6_1( T5, T7, a0, v3, j_max )
    T6 = 0;
    den = a0 + j_max * T5;
    
    if(abs(den) > 0)
        T6 = (-0.5 * T5 * T5 * j_max - T5 * T7 * j_max - T5 * a0 + 0.5 * T7 * T7 * j_max - T7 * a0 + v3) / den;
    end

    T6 = max(T6, 0);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      
end

function [ T7 ] = computeT7( T5, a0, j_max )
    T7 = a0 / j_max + T5;
	T7 = max(T7, 0);
end


function [ T1, T2, T3, T4, T5, T6, T7, local_time, direction_acc, direction_dec, deltP ] = updateDurations_Position_Setpoint( vel_sp, state_a, state_v, max_jerk, max_accel, max_velocity, p0, pt )
    p_acc = 0; v_acc = 0; a_acc = 0; j_acc = 0; delt_p_acc = 0;
    p_dec = 0; v_dec = 0; a_dec = 0; j_dec = 0; delt_p_dec = 0;
    deltP = 0;    
    delt_p = pt - p0;
    if vel_sp > max_velocity
        vel_sp = max_velocity;
    end
    if vel_sp < - max_velocity
        vel_sp = - max_velocity;
    end
    
    local_time = 0.0;
    
    direction_acc = computeDirection(vel_sp, state_a, state_v, max_jerk); 
    if direction_acc ~= 0
        [T1, T2, T3, delt_p_acc, p_acc, v_acc, a_acc, j_acc] = updateDurationsMinimizeTotalTimeAccleration(direction_acc, max_jerk, max_accel, vel_sp, state_a, state_v, p0);
    else
        T1 = 0;
        T2 = 0;
        T3 = 0;
    end
    
    state_a = 0;
    state_v = vel_sp;
    vel_sp = 0;
    direction_dec = computeDirection(vel_sp, state_a, state_v, max_jerk); 
    if direction_dec ~= 0
        [T5, T6, T7, delt_p_dec, p_dec, v_dec, a_dec, j_dec] = updateDurationsMinimizeTotalTimeDecleration(direction_dec, max_jerk, max_accel, vel_sp, a_acc, v_acc, p_acc);
    else
        T5 = 0;
        T6 = 0;
        T7 = 0;
    end
    
    T4 = computeT4( delt_p_acc, delt_p_dec, state_v, delt_p );
    
    delt_p_cruise = 0;
    [p_cruise, v_cruise, a_cruise, j_cruise] = updateTrajCruise( T4, 1.0, local_time, T4, 0, v_acc, p_acc, 0, max_jerk );
    delt_p_cruise = p_cruise - p_acc;
    
    deltP = delt_p_acc + delt_p_dec + delt_p_cruise;
end

二分搜索上升阶段速度终止值 $v_{m}$ 。

function [ T1, T2, T3, T4, T5, T6, T7, local_time, direction_acc, direction_dec, v_m ] = binarySearchUpdateDurationsMinimizeTotalTime( j_max, a0, a_max, v0, v_max, p0, pt )
    %初始化速度、位置参数
    delt_p = pt - p0;
    v_now = sign(delt_p) * v_max;
    v_s = 0;
    v_e = sign(delt_p) * v_max;
    
    MeetTheSetPoint = 0;
    delt_p_thr = 0.000001;
    
    iterCounter = 1;
    
    while(MeetTheSetPoint ~= 1)
        %更新时间及位置增量
        [ T1, T2, T3, T4, T5, T6, T7, local_t, dir_acc, dir_dec, deltP ] = updateDurations_Position_Setpoint( v_now, a0, v0, j_max, a_max, v_max, p0, pt );
        direction_acc = dir_acc;
        direction_dec = dir_dec;
        local_time = local_t;
        if abs(deltP - delt_p) <= delt_p_thr || iterCounter > 100
            MeetTheSetPoint = 1;
            break;
        else
            MeetTheSetPoint = 0;
        end
        %更新v_now值重新进行时间计算
        if abs(deltP) > abs(delt_p)
            v_s = 0;
            v_e = v_now;
            v_now = (v_s + v_e) / 2;
        else
            if abs(deltP) < abs(delt_p)
                v_s = v_now;
                v_e = sign(delt_p) * v_max;
                v_now = (v_s + v_e) / 2;
            else

            end
        end
        iterCounter = iterCounter + 1;
    end
    v_m = v_now;
end


若规划值达到设定值附近，则结束规划算法，否则更新初始状态量，重新进行规划，直到规划值达到设定值。
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).total_time, ... 
                                                           state(k).T1, state(k).T2, state(k).T3, state(k).T4, state(k).T5, state(k).T6, state(k).T7, ...
                                                           state(k).a, state(k).v, state(k).p, state(k).direction_acc, state(k).direction_dec, state(k).j_max );
        state(k).log_p(n_index) = state(k).p;
        state(k).log_v(n_index) = state(k).v;
        state(k).log_a(n_index) = state(k).a;
        state(k).log_j(n_index) = state(k).j;
        state(k).log_t(n_index) = n_index * dt;
        state(k).log_v_sp(n_index) = state(k).v_sp;
        
        [state(k).T1, state(k).T2, state(k).T3, state(k).T4, state(k).T5, state(k).T6, state(k).T7, local_time, state(k).direction_acc, state(k).direction_dec, state(k).v_m ] = ...
                        binarySearchUpdateDurationsMinimizeTotalTime( state(k).j_max, state(k).a, state(k).a_max, state(k).v, state(k).v_max, state(k).p, state(k).p_sp );

    end
         state = timeSynchronization(state, 2);
    n_index = n_index + 1;
end

仿真结果图

在这里插入图片描述

总结

本篇详细阐述了位置设定JLT问题的解法，与速度设定JLT问题不同的是：

位置设定JLT问题需要通过二分查找算法寻找最优的上升阶段速度终止值 $v_{m}$
位置设定JLT问题需要将时间段扩展为7段

位置设定JLT问题的解法能够是适用于解决多旋翼无人机的运动规划问题，有兴趣的朋友可以关注我的GitHub相关工程，后续有时间会逐步完善。

https://github.com/DistantUtopia/JerkLimitedTrajectory

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

知乎： @遥远的乌托邦

GitHub: https://github.com/DistantUtopia

微信公众号： @遥远的乌托邦
在这里插入图片描述

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

0. 前言

1. 位置设定问题的JerkLimitedTrajectory解法

总结

猜你喜欢