QP解决Minimum Snap轨迹优化问题
大佬的代码: https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots
本文代码: https://github.com/tgj-maker/QP_Minimumn_Snap(gitee无法使用,转战GitHub)
1. 多项式的次数确定
对于单段轨迹,多项式的次数 N N N确定如下
- Minimum Jerk: 假如优化变量为 ( p , v , a ) (p,v,a) (p,v,a),那么 N = 2 ∗ 3 ( j e r k ) − 1 = 5 N=2*3(jerk)-1=5 N=2∗3(jerk)−1=5
- Minimum Snap:假如优化变量为 ( p , v , a , j ) (p,v,a,j) (p,v,a,j),那么 N = 2 ∗ 4 ( s n a p ) − 1 = 7 N=2*4(snap)-1=7 N=2∗4(snap)−1=7
**注意:**上述只是假设Minimum Snap的优化变量为 ( p , v , a , j ) (p,v,a,j) (p,v,a,j) 4个变量,并不是说Minimum Snap一定只能是 ( p , v , a , j ) (p,v,a,j) (p,v,a,j),它们之间是相互独立的。
对于多段轨迹,多项式次数 N N N的确定如下
-
Minimum Jerk:假如优化变量为 ( p , v , a ) (p,v,a) (p,v,a),那么对于 k k k段轨迹,存在一个起点、一个终点、 k − 1 k-1 k−1个中间点的位置,那么固定的变量个数:
3 ( j e r k 起 点 ) + 3 ( j e r k 终 点 ) + ( k − 1 ) ∗ 1 ( 中 间 点 只 知 道 位 置 ) = k + 5 3(jerk \quad起点)+3(jerk \quad 终点)+(k-1)*1(中间点只知道位置)=k+5 3(jerk起点)+3(jerk终点)+(k−1)∗1(中间点只知道位置)=k+5
对于 k k k段轨迹, N N N次多项式至少需要
( N + 1 ) ∗ k (N+1)*k (N+1)∗k
因此
( N + 1 ) ∗ k ≥ k + 5 ⇒ N ≥ 5 k (N+1)*k \ge k+5 \\ \Rightarrow N \ge \frac{5}{k} (N+1)∗k≥k+5⇒N≥k5 -
Minimum Snap 推导过程如上,本文就不赘述。
注意:实际编程过程中,为了简单起见,多段轨迹的每一段多项式次数均相同,因此常采用单段轨迹的多项式次数作为多段轨迹的多项式次数。
2. Minimum Snap案例分析
2.1 轨迹的多项式表达
f ( t ) = { f 1 ( t ) = ∑ i = 0 N p 1 , i t i 0 ≤ t ≤ T 1 − T 0 = Δ T 1 f 2 ( t ) = ∑ i = 0 N p 2 , i t i 0 ≤ t ≤ T 2 − T 1 = Δ T 2 ⋮ f M ( t ) = ∑ i = 0 N p M , i t i 0 ≤ t ≤ T M − T M − 1 = Δ T M f(t) = \left\{ {\begin{matrix} {{f_1}(t) = \sum\nolimits_{i = 0}^N {{p_{1,i}}{t^i}0 \le t \le {T_1} - {T_0} = \Delta {T_1}} }\\ {{f_2}(t) = \sum\nolimits_{i = 0}^N {{p_{2,i}}{t^i}0 \le t \le {T_2} - {T_1} = \Delta {T_2}} }\\ \vdots \\ {{f_M}(t) = \sum\nolimits_{i = 0}^N {{p_{M,i}}{t^i}0 \le t \le {T_M} - {T_{M - 1}} = \Delta {T_M}} } \end{matrix}} \right. f(t)=⎩⎪⎪⎪⎨⎪⎪⎪⎧f1(t)=∑i=0Np1,iti0≤t≤T1−T0=ΔT1f2(t)=∑i=0Np2,iti0≤t≤T2−T1=ΔT2⋮fM(t)=∑i=0NpM,iti0≤t≤TM−TM−1=ΔTM
其中变量说明如下
M:表示总共
M
M
M段轨迹
p
M
,
i
p_{M,i}
pM,i:表示在第
M
M
M段轨迹中,多项式的第
i
i
i个参数
t:表示该段轨迹时间,注意在实际编程中,为了增加数据的稳定性,时间常采用
[
0
,
T
]
[0,T]
[0,T]的格式,这点和之前的理论部分略微不同。
2.2 约束条件确定
导数约束
规定起始状态 p s t a r t = ( x s t a r t , v s t a r t , a s t a r t , j s t a r t ) p_{start}=(x_{start},v_{start},a_{start},j_{start}) pstart=(xstart,vstart,astart,jstart),终止状态 p s t o p = ( x s t o p , v s t o p , a s t o p , j s t o p ) p_{stop}=(x_{stop},v_{stop},a_{stop},j_{stop}) pstop=(xstop,vstop,astop,jstop),中间点 p p p的位置。
连续性约束
规定为前后两段轨迹交点处状态 ( p , v , a , j ) (p,v,a,j) (p,v,a,j)连续。
通过第一节和该节的说明,此时多段多项式的系数均为7。
3 Minimum Snap算法运行过程
通过前章得出求解的方程
m
i
n
J
=
P
T
Q
P
=
m
i
n
[
p
1
⋮
p
M
]
T
[
Q
1
0
0
0
⋱
0
0
0
Q
M
]
[
p
1
⋮
p
M
]
s
.
t
.
A
e
q
[
p
1
⋮
p
M
]
=
d
e
q
min \quad J = {{\bf{P}}^T}{\bf{QP}} = min \quad {\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ \vdots \\ {{{\bf{p}}_M}} \end{matrix}} \right]^T}\left[ {\begin{matrix} {{{\bf{Q}}_1}}&0&0\\ 0& \ddots &0\\ 0&0&{{{\bf{Q}}_M}} \end{matrix}} \right]\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ \vdots \\ {{{\bf{p}}_M}} \end{matrix}} \right] \\ s.t.{{\bf{A}}_{eq}}\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ \vdots \\ {{{\bf{p}}_M}} \end{matrix}} \right] = {{\bf{d}}_{eq}}
minJ=PTQP=min⎣⎢⎡p1⋮pM⎦⎥⎤T⎣⎡Q1000⋱000QM⎦⎤⎣⎢⎡p1⋮pM⎦⎥⎤s.t.Aeq⎣⎢⎡p1⋮pM⎦⎥⎤=deq
注意:这里的
p
1
,
p
2
,
.
.
.
p
M
\bf p_1,\bf p_2,...\bf p_M
p1,p2,...pM为向量,均由该段轨迹的多项式系数组成,例如
p
M
\bf {p_M}
pM=
[
p
M
,
1
,
p
M
,
2
,
p
M
,
3
,
p
M
,
4
,
p
M
,
5
,
p
M
,
6
,
p
M
,
7
,
p
M
,
8
]
[p_{M,1},p_{M,2},p_{M,3},p_{M,4},p_{M,5},p_{M,6},p_{M,7},p_{M,8}]
[pM,1,pM,2,pM,3,pM,4,pM,5,pM,6,pM,7,pM,8]
3.1 准备工作
- 多项式系数确定
- 通过交互信息获取起点、中间点和终点的位置坐标
- 确定每段轨迹的时间这里通过轨迹两端的距离分配总时间T=25s,当然为了简单起见,也可以将每段时间设置一样。
- 调用函数MinimumSnapQPSolver进行QP求解MInimum Snap问题。代码将X与Y轴单独分离开来,进行Minimum Snap算法,这也是该算法的一大优势。
主函数 (hw1_1.m):
clc;clear;close all;
path = ginput() * 100.0;
n_order = 7;% 多项式最高次幂
n_seg = size(path,1)-1;% 路段个数
n_poly_perseg = (n_order+1); %多项式未知系数的个数
ts = zeros(n_seg, 1); %各路段所需要的时间,均是以0-ts
%根据两点之间的距离按比例计算时间分布
dist = zeros(n_seg, 1);
dist_sum = 0;
T = 25;
t_sum = 0;
for i = 1:n_seg
dist(i) = sqrt((path(i+1, 1)-path(i, 1))^2 + (path(i+1, 2) - path(i, 2))^2);
dist_sum = dist_sum+dist(i);
end
for i = 1:n_seg-1
ts(i) = dist(i)/dist_sum*T;
t_sum = t_sum+ts(i);
end
ts(n_seg) = T - t_sum;
% 简单化,时间间隔均设置为1
% for i = 1:n_seg
% ts(i) = 1.0;
% end
%x与y轴单独处理
poly_coef_x = MinimumSnapQPSolver(path(:, 1), ts, n_seg, n_order);
poly_coef_y = MinimumSnapQPSolver(path(:, 2), ts, n_seg, n_order);
3.2 MinimumSnapQPSolver函数
- 规定起始状态: p s t a r t = ( x s t a r t , v s t a r t , a s t a r t , j s t a r t ) = ( x s t a r t , 0 , 0 , 0 ) p_{start}=(x_{start},v_{start},a_{start},j_{start})=(x_{start},0,0,0) pstart=(xstart,vstart,astart,jstart)=(xstart,0,0,0)
- 规定终止状态 p s t o p = ( x s t o p , v s t o p , a s t o p , j s t o p ) = ( x s t o p , 0 , 0 , 0 ) p_{stop}=(x_{stop},v_{stop},a_{stop},j_{stop})=(x_{stop},0,0,0) pstop=(xstop,vstop,astop,jstop)=(xstop,0,0,0)
- 通过调用getQ函数获取最小化目标函数的Q矩阵,通过getAbeq函数获取QP解法中的等式方程(仿射变换)的矩阵A和变量b
- 通过matlab中quadprog函数进行QP问题的求解。有关quadprog函数说明参考quadprog
function poly_coef = MinimumSnapQPSolver(waypoints, ts, n_seg, n_order)
start_cond = [waypoints(1), 0, 0, 0];
end_cond = [waypoints(end), 0, 0, 0];
%#####################################################
% STEP 1: compute Q of p'Qp
Q = getQ(n_seg, n_order, ts);
%#####################################################
% STEP 2: compute Aeq and beq
[Aeq, beq] = getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond);
f = zeros(size(Q,1),1);
poly_coef = quadprog(Q,f,[],[],Aeq, beq);
end
3.2.1 getQ函数
getQ函数目的是获取 Q Q Q矩阵,首先大矩阵 Q Q Q矩阵是对角矩阵,是由每一段的小矩阵 Q i Q_i Qi拼凑而成的,matlab代码如下
function Q = getQ(n_seg, n_order, T)
Q = [];
fac = @(x) x*(x-1)*(x-2)*(x-3);
for k = 1:n_seg
%#####################################################
% STEP 1.1: calculate Q_k of the k-th segment
Q_k=zeros(n_order+1,n_order+1);
for i=0:n_order
for l=0:n_order
if (i<4) || (l<4)
continue;
else
Q_k(i+1,l+1)=fac(i)*fac(l)/(i+l-7)*T(k)^(i+l-7);
end
end
end
Q = blkdiag(Q, Q_k);
end
end
-
其中函数参数说明如下
n_seg: 路段个数
n_order:多项式最高次幂
T:各个路段的时间总集 -
各个路段的小矩阵Q的求解公式如下,代码也是按照以下公式进行的,参考前文。观察该矩阵发现,如果 i = 0 , 1 , 2 , 3 i=0,1,2,3 i=0,1,2,3或者 l = 0 , 1 , 2 , 3 l=0,1,2,3 l=0,1,2,3,矩阵会为0,这也是代码中出现if (i<4) || (l<4) continue 的原因。
[ ⋮ ⋯ i ( i − 1 ) ( i − 2 ) ( i − 3 ) l ( l − 1 ) ( l − 2 ) ( l − 3 ) i + l − 7 T i + l − 7 ⋯ ⋮ ] \left[ {\begin{matrix} {}& \vdots &{}\\ \cdots &{\frac{{i(i - 1)(i - 2)(i - 3)l(l - 1)(l - 2)(l - 3)}}{{i + l - 7}}{T^{i + l - 7}}}& \cdots \\ {}& \vdots &{} \end{matrix}} \right] ⎣⎢⎢⎡⋯⋮i+l−7i(i−1)(i−2)(i−3)l(l−1)(l−2)(l−3)Ti+l−7⋮⋯⎦⎥⎥⎤
- 其中blkdig为matlab内置函数,主要功能用于拼接矩阵
b l k d i g ( a , b , c , d , . . . ) → [ a 0 0 0 0 0 b 0 0 0 0 0 c 0 0 0 0 0 d 0 0 0 0 0 . . . ] blkdig(a,b,c,d,...) \\ \to \left[ {\begin{matrix} a&0&0&0&0\\ 0&b&0&0&0\\ 0&0&c&0&0\\ 0&0&0&d&0\\ 0&0&0&0&{...} \end{matrix}} \right] blkdig(a,b,c,d,...)→⎣⎢⎢⎢⎢⎡a00000b00000c00000d00000...⎦⎥⎥⎥⎥⎤
3.2.2 getAbeq函数
代码如下,有点复杂,不过不要急,后面有相关代码解释和原理推导。
function [Aeq, beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
n_all_poly = n_seg*(n_order+1);
%#####################################################
% p,v,a,j constraint in start,
% Note: p = [p0, p1, p2,...pn-1]'
Aeq_start = zeros(4, n_all_poly);
beq_start = start_cond';
Aeq_start(1:4,1:8) = getCoeff(0);
%#####################################################
% p,v,a constraint in end
Aeq_end = zeros(4, n_all_poly);
beq_end = end_cond';
t = ts(end);
Aeq_end(1:4, end-7:end) = getCoeff(t);
%#####################################################
% position constrain in all middle waypoints
Aeq_wp = zeros(n_seg-1, n_all_poly);
beq_wp = zeros(n_seg-1, 1);
% STEP 2.3: write expression of Aeq_wp and beq_wp
for k = 0:1:n_seg-2 % here k is the index of segments
beq_wp(k+1, 1) = waypoints(k+2);
coeff = getCoeff(ts(k+1));
Aeq_wp(k+1, 1+k*8:8+k*8) = coeff(1, :);
end
%#####################################################
% position continuity constrain between each 2 segments
Aeq_con_p = zeros(n_seg-1, n_all_poly);
beq_con_p = zeros(n_seg-1, 1);
% velocity continuity constrain between each 2 segments
Aeq_con_v = zeros(n_seg-1, n_all_poly);
beq_con_v = zeros(n_seg-1, 1);
% acceleration continuity constrain between each 2 segments
Aeq_con_a = zeros(n_seg-1, n_all_poly);
beq_con_a = zeros(n_seg-1, 1);
% jerk continuity constrain between each 2 segments
Aeq_con_j = zeros(n_seg-1, n_all_poly);
beq_con_j = zeros(n_seg-1, 1);
Aeq_con = [Aeq_con_p; Aeq_con_v; Aeq_con_a; Aeq_con_j];
beq_con = [beq_con_p; beq_con_v; beq_con_a; beq_con_j];
% STEP 2.4: write expression of Aeq_con_p and beq_con_p
for k = 0:1:n_seg-2 % here k is the index of segments
Aeq_con(1+4*k:4+4*k,1+8*k:8+8*k) = getCoeff(ts(k+1));
Aeq_con(1+4*k:4+4*k,1+8*(k+1):8+8*(k+1)) = -getCoeff(0);
end
%#####################################################
% combine all components to form Aeq and beq:
Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
beq = [beq_start; beq_end; beq_wp; beq_con];
end
- 函数参数说明
n_seg: 路段个数
n_order:多项式最高次幂
waypoints:交互信息的所有点的坐标值,包括起点、终点、中间点
ts:各个路段的时间总集
start_cond:起点的开始状态(x, v,a,j)
end_cond:起点的开始状态(x, v,a,j)
- 其中getCoeff函数,命名为G(t),目的是获取t时刻下,机器人的状态
x
,
v
,
a
,
j
x,v,a,j
x,v,a,j,计算公式如下
X ( t ) = p 7 t 7 + p 6 t 6 + p 5 t 5 + p 4 t 4 + p 3 t 3 + p 2 t 2 + p 1 t 1 + p 0 [ 1 t t 2 t 3 t 4 t 5 t 6 t 7 0 1 2 t 3 t 2 4 t 3 5 t 4 6 t 5 7 t 6 0 0 2 6 t 12 t 2 20 t 3 30 t 4 42 t 5 0 0 0 6 24 t 60 t 2 120 t 3 210 t 4 ] ⏟ G ( t ) [ p 0 p 1 p 2 p 3 p 4 p 5 p 6 p 7 ] = [ X ( t ) X ( t ) ( 1 ) X ( t ) ( 2 ) X ( t ) ( 3 ) ] = [ x v a j ] X(t)=p_7t^7+p_6t^6+p_5t^5+p_4t^4+p_3t^3+p_2t^2+p_1t^1+p_0 \\ \underbrace {\left[ {\begin{matrix} 1&t&{{t^2}}&{{t^3}}&{{t^4}}&{{t^5}}&{{t^6}}&{{t^7}}\\ 0&1&{2t}&{3{t^2}}&{4{t^3}}&{5{t^4}}&{6{t^5}}&{7{t^6}}\\ 0&0&2&{6t}&{12{t^2}}&{20{t^3}}&{30{t^4}}&{42{t^5}}\\ 0&0&0&6&{24t}&{60{t^2}}&{120{t^3}}&{210{t^4}} \end{matrix}} \right]}_{G(t)}\left[ {\begin{matrix} {{p_0}}\\ {{p_1}}\\ {{p_2}}\\ {{p_3}}\\ {{p_4}}\\ {{p_5}}\\ {{p_6}}\\ {{p_7}} \end{matrix}} \right] = \left[ {\begin{matrix} {X(t)}\\ {X(t)^{(1)}}\\ { X(t)^{(2)}}\\ { X(t)^{(3)}} \end{matrix}} \right] = \left[ {\begin{matrix} x\\ v\\ a\\ j \end{matrix}} \right] X(t)=p7t7+p6t6+p5t5+p4t4+p3t3+p2t2+p1t1+p0G(t) ⎣⎢⎢⎡1000t100t22t20t33t26t6t44t312t224tt55t420t360t2t66t530t4120t3t77t642t5210t4⎦⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡p0p1p2p3p4p5p6p7⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎡X(t)X(t)(1)X(t)(2)X(t)(3)⎦⎥⎥⎤=⎣⎢⎢⎡xvaj⎦⎥⎥⎤
代码框架如下
function coeff = getCoeff(t)
% Get the coefficient for Aeq
coeff = [1, 1*t, 1*t^2, 1*t^3, 1*t^4, 1*t^5, 1*t^6, 1*t^7;
0, 1, 2*t, 3*t^2, 4*t^3, 5*t^4, 6*t^5, 7*t^6;
0, 0, 2, 6*t, 12*t^2, 20*t^3, 30*t^4, 42*t^5;
0, 0, 0, 6, 24*t, 60*t^2, 120*t^3,210*t^4];
end
为了方便描述,假设从起点到终点存在4个点
x
s
t
a
r
t
,
x
1
,
x
2
,
x
s
t
o
p
x_{start},x_1,x_2,x_{stop}
xstart,x1,x2,xstop,即3条路段
P
1
,
P
2
,
P
3
P_1,P_2,P_3
P1,P2,P3,如图所示,每段时间均是从0开始。
-
导数约束
由前文可知,导数约束满足 A j p j = d j {{\bf{A}}_j}{{\bf{p}}_j} = {{\bf{d}}_j} Ajpj=dj,并且初始条件为
{ x s t a r t , v s t a r t , a s t a r t , j s t a r t x s t o p , v s t o p , a s t o p , j s t o p x 1 , x 2 \left\{ {\begin{matrix} {{x_{start}},v_{start},a_{start},j_{start}}\\ {{x_{stop}},v_{stop},a_{stop},j_{stop}}\\ {{x_1},{x_2}} \end{matrix}} \right. ⎩⎨⎧xstart,vstart,astart,jstartxstop,vstop,astop,jstopx1,x2
若向量
d
j
\bf d_j
dj表示形式如下(当然表示形式并不唯一)
[
x
s
t
a
r
t
v
s
t
a
r
t
a
s
t
a
r
t
j
s
t
a
r
t
x
s
t
o
p
v
s
t
o
p
a
s
t
o
p
j
s
t
o
p
x
1
x
2
]
\left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}}\\ \hdashline {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}}\\ \hdashline {{x_1}}\\ {{x_2}} \end{matrix}} \right]
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xstartvstartastartjstartxstopvstopastopjstopx1x2⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
由于
G
(
0
)
∙
p
1
=
[
x
s
t
a
r
t
v
s
t
a
r
t
a
s
t
a
r
t
j
s
t
a
r
t
]
G(0) \bullet \bf p_1=\left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}} \end{matrix}} \right]
G(0)∙p1=⎣⎢⎢⎡xstartvstartastartjstart⎦⎥⎥⎤
G
(
T
)
∙
p
M
=
G
(
T
3
)
∙
p
3
=
[
x
s
t
o
p
v
s
t
o
p
a
s
t
o
p
j
s
t
o
p
]
G(T) \bullet \bf p_M=G(T_3) \bullet \bf p_3=\left[ {\begin{matrix} {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}} \end{matrix}} \right]
G(T)∙pM=G(T3)∙p3=⎣⎢⎢⎡xstopvstopastopjstop⎦⎥⎥⎤
G
_
0
(
T
1
)
∙
p
1
=
x
1
G
_
0
(
T
2
)
∙
p
2
=
x
2
G\_0(T_1) \bullet p_1=x_1 \\ G\_0(T_2) \bullet p_2=x_2
G_0(T1)∙p1=x1G_0(T2)∙p2=x2
其中,G(t)表示getCoeff函数,
G
_
0
(
t
)
G\_0(t)
G_0(t)表示
t
t
t时刻方程
G
(
t
)
G(t)
G(t)的第一行。
化成矩阵形式如下
[
G
(
0
)
4
×
8
0
4
×
8
0
4
×
8
0
4
×
8
0
4
×
8
G
(
T
3
)
4
×
8
G
_
0
(
T
1
)
1
×
8
0
1
×
8
0
1
×
8
0
1
×
8
G
_
0
(
T
2
)
1
×
8
0
1
×
8
]
[
p
1
,
0
⋮
p
1
,
7
p
2
,
0
⋮
p
2
,
7
p
3
,
0
⋮
p
3
,
7
]
=
[
x
s
t
a
r
t
v
s
t
a
r
t
a
s
t
a
r
t
j
s
t
a
r
t
x
s
t
o
p
v
s
t
o
p
a
s
t
o
p
j
s
t
o
p
x
1
x
2
]
\left[ {\begin{matrix} {G{{(0)}_{4 \times 8}}}&{\bf {0_{4 \times 8}}}&{\bf{0_{4 \times 8}}}\\ {\bf{0_{4 \times 8}}}&{\bf{0_{4 \times 8}}}&{G{{({T_3})}_{4 \times 8}}}\\ \hdashline {G\_0{{({T_1})}_{1 \times 8}}}&{\bf{0_{1 \times 8}}}&{\bf{0_{1 \times 8}}}\\ {\bf{0_{1 \times 8}}}&{G\_0{{({T_2})}_{1 \times 8}}}&{\bf{0_{1 \times 8}}} \end{matrix}} \right]\left[ {\begin{matrix} {{p_{1,0}}}\\ \vdots \\ {{p_{1,7}}}\\ \hdashline {{p_{2,0}}}\\ \vdots \\ {{p_{2,7}}}\\ \hdashline {{p_{3,0}}}\\ \vdots \\ {{p_{3,7}}} \end{matrix}} \right] = \left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}}\\ \hdashline {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}}\\ \hdashline {{x_1}}\\ {{x_2}} \end{matrix}} \right]
⎣⎢⎢⎡G(0)4×804×8G_0(T1)1×801×804×804×801×8G_0(T2)1×804×8G(T3)4×801×801×8⎦⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡p1,0⋮p1,7p2,0⋮p2,7p3,0⋮p3,7⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xstartvstartastartjstartxstopvstopastopjstopx1x2⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
- 连续性约束
由前章可知,连续性约束为
[
A
j
−
A
j
+
1
]
[
p
j
p
j
+
1
]
=
0
\left[ {\begin{matrix} {{{\bf{A}}_j}}&{ - {{\bf{A}}_{{\rm{j + }}1}}} \end{matrix}} \right]\left[ {\begin{matrix} {{{\bf{p}}_j}}\\ {{{\bf{p}}_{j + 1}}} \end{matrix}} \right] = 0
[Aj−Aj+1][pjpj+1]=0
以节点
x
1
x_1
x1为例,连续性约束表示在
p
1
p_1
p1末端(
x
1
x_1
x1的左端)的
v
,
a
,
j
v,a,j
v,a,j与
p
2
p_2
p2首端(
x
1
x_1
x1的右端)相等,数学表示如下
左
端
=
[
x
1
(
T
1
)
v
1
(
T
1
)
a
1
(
T
1
)
j
1
(
T
1
)
]
=
[
x
2
(
0
)
v
2
(
0
)
a
2
(
0
)
j
2
(
0
)
]
=
右
端
G
(
T
1
)
∙
p
1
=
G
(
0
)
∙
p
2
→
[
G
(
T
1
)
G
(
0
)
]
[
p
1
p
2
]
=
0
左端=\left[ {\begin{matrix} {{x_1}({T_1})}\\ {{v_1}({T_1})}\\ {{a_1}({T_1})}\\ {{j_1}({T_1})} \end{matrix}} \right] = \left[ {\begin{matrix} {{x_2}(0)}\\ {{v_2}(0)}\\ {{a_2}(0)}\\ {{j_2}(0)} \end{matrix}} \right]=右端 \\ G(T_1)\bullet \bf p_1=G(0) \bullet \bf p_2 \\ \to \left[ {\begin{matrix} {G({T_1})}&{G(0)} \end{matrix}} \right]\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ {{{\bf{p}}_2}} \end{matrix}} \right] = {\bf{0}}
左端=⎣⎢⎢⎡x1(T1)v1(T1)a1(T1)j1(T1)⎦⎥⎥⎤=⎣⎢⎢⎡x2(0)v2(0)a2(0)j2(0)⎦⎥⎥⎤=右端G(T1)∙p1=G(0)∙p2→[G(T1)G(0)][p1p2]=0
化为矩阵形式为
[
G
(
T
1
)
4
×
8
−
G
(
0
)
4
×
8
0
4
×
8
0
4
×
8
G
(
T
2
)
4
×
8
−
G
(
0
)
4
×
8
]
[
p
1
,
0
⋮
p
1
,
7
p
2
,
0
⋮
p
2
,
7
p
3
,
0
⋮
p
3
,
7
]
=
[
0
(
4
×
2
)
×
1
]
\left[ {\begin{matrix} {G{{({T_1})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}\\ {{{\bf{0}}_{4 \times 8}}}&{G{{({T_2})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}\\ \end{matrix}} \right]\left[ {\begin{matrix} {{p_{1,0}}}\\ \vdots \\ {{p_{1,7}}}\\ \hdashline {{p_{2,0}}}\\ \vdots \\ {{p_{2,7}}}\\ \hdashline {{p_{3,0}}}\\ \vdots \\ {{p_{3,7}}} \end{matrix}} \right] = \left[ {\bf{0}_{(4\times2)\times1}} \right]
[G(T1)4×804×8−G(0)4×8G(T2)4×804×8−G(0)4×8]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡p1,0⋮p1,7p2,0⋮p2,7p3,0⋮p3,7⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=[0(4×2)×1]
- 合并导数与连续性约束
由前文可知,Minimum Snap下QP的等式约束是导数约束与和连续性约束的合并,得到
A
e
q
=
[
G
(
0
)
4
×
8
0
4
×
8
0
4
×
8
0
4
×
8
0
4
×
8
G
(
T
3
)
4
×
8
G
_
0
(
T
1
)
1
×
8
0
1
×
8
0
1
×
8
0
1
×
8
G
_
0
(
T
2
)
1
×
8
0
1
×
8
G
(
T
1
)
4
×
8
−
G
(
0
)
4
×
8
0
4
×
8
0
4
×
8
G
(
T
2
)
4
×
8
−
G
(
0
)
4
×
8
]
[
p
1
,
0
⋮
p
1
,
7
p
2
,
0
⋮
p
2
,
7
p
3
,
0
⋮
p
3
,
7
]
=
[
x
s
t
a
r
t
v
s
t
a
r
t
a
s
t
a
r
t
j
s
t
a
r
t
x
s
t
o
p
v
s
t
o
p
a
s
t
o
p
j
s
t
o
p
x
1
x
2
0
(
4
×
2
)
×
1
]
=
d
e
q
\bf A_{eq}=\left[ {\begin{matrix} {G{{(0)}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}\\ {{{\bf{0}}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}&{G{{({T_3})}_{4 \times 8}}}\\ \hdashline {G\_0{{({T_1})}_{1 \times 8}}}&{{{\bf{0}}_{1 \times 8}}}&{{{\bf{0}}_{1 \times 8}}}\\ {{{\bf{0}}_{1 \times 8}}}&{G\_0{{({T_2})}_{1 \times 8}}}&{{{\bf{0}}_{1 \times 8}}}\\ \hdashline {G{{({T_1})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}\\ {{{\bf{0}}_{4 \times 8}}}&{G{{({T_2})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}\\ \end{matrix}} \right]\left[ {\begin{matrix} {{p_{1,0}}}\\ \vdots \\ {{p_{1,7}}}\\ \hdashline {{p_{2,0}}}\\ \vdots \\ {{p_{2,7}}}\\ \hdashline {{p_{3,0}}}\\ \vdots \\ {{p_{3,7}}} \end{matrix}} \right] = \left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}}\\ \hdashline {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}}\\ \hdashline {{x_1}}\\ {{x_2}}\\ \hdashline {{{\bf{0}}_{(4 \times 2) \times 1}}} \end{matrix}} \right]=\bf d_{eq}
Aeq=⎣⎢⎢⎢⎢⎢⎢⎡G(0)4×804×8G_0(T1)1×801×8G(T1)4×804×804×804×801×8G_0(T2)1×8−G(0)4×8G(T2)4×804×8G(T3)4×801×801×804×8−G(0)4×8⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡p1,0⋮p1,7p2,0⋮p2,7p3,0⋮p3,7⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xstartvstartastartjstartxstopvstopastopjstopx1x20(4×2)×1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=deq
注意:上述描述仅表示3条轨迹、2个中间点的
A
e
q
,
d
e
q
\bf A_{eq},\bf d_{eq}
Aeq,deq的求解情况。对于任意的轨迹个数,矩阵
A
e
q
,
d
e
q
\bf A_{eq},\bf d_{eq}
Aeq,deq的计算过程类似,这里给出矩阵和向量的维数情况:
对
于
M
i
n
i
m
u
m
S
n
a
p
问
题
,
优
化
变
量
为
(
p
,
v
,
a
,
j
)
4
个
,
那
么
最
高
次
幂
为
7
。
对
于
M
段
轨
迹
,
那
么
矩
阵
A
e
q
的
行
数
为
:
4
(
s
n
a
p
)
+
4
(
s
n
a
p
)
+
(
M
−
1
)
+
4
×
(
M
−
1
)
=
5
×
M
+
3
矩
阵
A
e
q
的
列
数
为
:
(
7
+
1
)
×
M
=
8
×
M
向
量
d
e
q
的
行
数
为
:
5
×
M
+
3
对于Minimum Snap问题,优化变量为(p,v,a,j)4个,那么最高次幂为7。对于M段轨迹,那么\\ 矩阵{\bf A_{eq}}的行数为:4(snap)+4 (snap)+(M-1)+4 \times (M-1)=5\times M+3 \\ 矩阵{\bf A_{eq}}的列数为:(7+1)\times M=8\times M \\ 向量{\bf d_{eq}}的行数为:5\times M+3
对于MinimumSnap问题,优化变量为(p,v,a,j)4个,那么最高次幂为7。对于M段轨迹,那么矩阵Aeq的行数为:4(snap)+4(snap)+(M−1)+4×(M−1)=5×M+3矩阵Aeq的列数为:(7+1)×M=8×M向量deq的行数为:5×M+3
3.3 显示功能
主函数 hw1_1.m
% 显示轨迹
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:n_seg-1
%#####################################################
% STEP 3: 获得各路段的多项式系数
Pxi = poly_coef_x((n_order+1)*(i)+1:(n_order+1)*(i)+n_order+1);
Pyi = poly_coef_y((n_order+1)*(i)+1:(n_order+1)*(i)+n_order+1);
for t = 0:tstep:ts(i+1)
%flip表示翻转Pxi中的元素
%polyval([P0,P1,P2],t)生成P0t^2+P1^t+P2,因此需要翻转原始数据
X_n(k) = polyval(flip(Pxi), t);
Y_n(k) = polyval(flip(Pyi), t);
k = k + 1;
end
end
plot(X_n, Y_n , 'Color', [0 1.0 0], 'LineWidth', 2);
hold on
%绘制散点图
scatter(path(1:size(path, 1), 1), path(1:size(path, 1), 2));
这里需要注意的一点就是,matlab中的polyval函数所生成的多项式函数为 p 0 t 7 + p 1 t 6 + . . . + p 7 p_0t^7+p_1t^6+...+p_7 p0t7+p1t6+...+p7,这与本文多项式的表示( p 7 t 7 + p 6 t 6 + . . . + p 0 p_7t^7+p_6t^6+...+p_0 p7t7+p6t6+...+p0)不符,因此需要使用flip函数进行多项式系数翻转。
效果如下:
运行QP/hw1_1.m程序,通过鼠标在原图上点击几个关键点,然后通过键盘上的"Enter"键进行输入结束,稍等片刻,就会出现上述结果。文章来源:https://www.toymoban.com/news/detail-400322.html
|
|
|
|
|
|
|
|
注意:上述是看大佬代码并结合算法原理所得出自己的理解,可能出现错误,欢迎批评指证。码字不易,转载请注明出处,谢谢。,文章来源地址https://www.toymoban.com/news/detail-400322.html
到了这里,关于(四)路径规划算法---QP解决Minimum Snap轨迹优化问题的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!