0. 前言
上一篇中针对速度设定的JLT算法进行了详解,该算法能够解决一部分给定速度期望值,或者通过给定位置期望值以一定规则转化为给定速度期望值的问题。
该方法简单高效,能够实时运行于嵌入式MCU,解决多旋翼无人机的部分运动规划问题。本篇将探讨一种更直接的运动规划算法,该算法基于上一篇的速度设定问题的解法,做了一定扩展,其适用于解决更广泛的问题。
1. 位置设定问题的JerkLimitedTrajectory解法
首先还是回到问题本身,JLT解决的是对应三阶积分器数学模型的运动规划问题,本篇在上一篇的基础上,对该问题进行了延伸与扩展,其数学与几何描述如下所示:
位置设定JLT问题与速度设定JLT问题相似之处在于,它们都可分类为三大类情况进行分类讨论。不同之处则在于,本篇讨论的位置设定问题更为复杂,需要将JLT问题求解的各段时间
从至多3段扩展至至多7段。位置设定JLT问题的三种情况:
位置设定JLT的算法流程图如下所示,与速度设定JLT问题相比,该算法将分别计算上升阶段(acc),下降阶段(dec)以及巡航阶段(cruise)的时间分配情况。
-
上升阶段对应T1、T2、T3
-
巡航阶段对应T4
-
下降阶段对应T5、T6、T7
然而,在确定这些时间参数之前,我们需要首先通过位置设定值(
)与初始位置(
)、初始速度(
)、初始加速度(
)等参数,结合二分查找算法(BinarySearch)进行上升阶段的终止速度
的搜索。
如下图所示为详细的位置设定JLT问题的算法流程图。
主要步骤为:
- 初始化状态量 , ,
state(1).p = 0;
state(1).v = 0;
state(1).a = 0;
state(1).j = 0;
- 更新约束条件 , ,
state(1).v_max = 5.0;
state(1).a_max = 5.0;
state(1).j_max = 10.0;
- 更新设定值
state(1).p_sp = 5.0;
state(1).v_sp = 0.0;
state(1).a_sp = 0.0;
更新每段轨迹的最小所需时间 , ,
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
- 二分搜索上升阶段速度终止值 。
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问题需要通过二分查找算法寻找最优的上升阶段速度终止值
- 位置设定JLT问题需要将时间段扩展为7段
位置设定JLT问题的解法能够是适用于解决多旋翼无人机的运动规划问题,有兴趣的朋友可以关注我的GitHub相关工程,后续有时间会逐步完善。
https://github.com/DistantUtopia/JerkLimitedTrajectory
作者简介: 一个被Coding耽误的无人机算法工程师,控制、导航略懂一二,热衷技术,喜欢乒乓、音乐、电影,欢迎交流。
知乎: @遥远的乌托邦
GitHub: https://github.com/DistantUtopia
微信公众号: @遥远的乌托邦