0. 前言
无人机在飞行过程中经常会遇到许多不可预见的突发情况或者障碍物,因此,寻求一种高效稳定的运动规划算法是十分必要的。
本文介绍一种可运行于嵌入式mcu的运动规划算法——Jerk Limited Trajectory(JLT),该方法为BVP方法的其中一种解法,计算量小,实时性强。且常与非线性MPC相结合,应用于解决复杂环境的运动规划问题。
该算法能够实时地根据变化的环境,在满足动力学约束的前提下,快速重规划一条最优路径,使得无人机能够安全可靠地执行飞行任务。
1. Jerk Limited Trajectory(目标速度确定的情况)
JLT(这里指目标速度确定的情况)问题的求解本质上是一个分类讨论的过程,其关键在于确定
,
,
的值,然后通过分段函数对三阶积分器模型进行递推,从而得到从初始状态平滑过度到目标状态的轨迹,包括这个过程中每一时刻的 p, v,a,j 。
三阶积分器模型的JLT求解示意图:
如上图所示为三阶积分器模型的JLT求解示意图,该问题的求解流程图如下所示:
主要步骤为:
初始化状态量
,
,
state(1).p = 10;
state(1).v = 1;
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 = 0.0;
state(1).v_sp = 3;
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 [ 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
微信公众号: @遥远的乌托邦