1.用优化solver求解参数算法概况
具体看Minimum Snap轨迹规划详解(二)- 闭式求解
J
=
p
T
Q
p
=
[
d
F
d
P
]
T
C
T
M
−
T
Q
M
−
1
C
[
d
F
d
P
]
=
[
d
F
d
P
]
T
R
[
d
F
d
P
]
=
[
d
F
d
P
]
T
[
R
F
F
R
F
P
R
P
F
R
P
P
]
[
d
F
d
P
]
=
d
F
T
R
F
F
d
F
+
d
F
T
R
F
P
d
P
+
d
P
T
R
P
F
d
F
+
d
P
T
R
P
P
d
P
\begin{aligned} J&=p^TQp = \begin{bmatrix}d_F\\d_P\end{bmatrix}^TC^TM^{-T}QM^{-1}C\begin{bmatrix}d_F\\d_P\end{bmatrix} \\ &=\begin{bmatrix}d_F\\d_P\end{bmatrix}^TR\begin{bmatrix}d_F\\d_P\end{bmatrix} =\begin{bmatrix}d_F\\d_P\end{bmatrix}^T \begin{bmatrix}R_{FF} & R_{FP} \\R_{PF} & R_{PP} \end{bmatrix} \begin{bmatrix}d_F\\d_P\end{bmatrix}\\ &=d^T_FR_{FF} d_F + d^T_FR_{FP} d_P+d^T_PR_{PF} d_F+d^T_PR_{PP} d_P \end{aligned}
J=pTQp=[dFdP]TCTM−TQM−1C[dFdP]=[dFdP]TR[dFdP]=[dFdP]T[RFFRPFRFPRPP][dFdP]=dFTRFFdF+dFTRFPdP+dPTRPFdF+dPTRPPdP
求解
d
P
=
−
R
P
P
−
1
R
F
P
T
d
F
p
=
M
−
1
C
[
d
F
d
P
]
\begin{aligned} d_P &= -R^{-1}_{PP}R^T_{FP}d_F \\ p&=M^{-1}C\begin{bmatrix}d_F\\d_P\end{bmatrix} \end{aligned}
dPp=−RPP−1RFPTdF=M−1C[dFdP]
2.闭式解求解参数代码
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);
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;
poly_coef_x = MinimumSnapCloseformSolver(path(:, 1), ts, n_seg, n_order);
poly_coef_y = MinimumSnapCloseformSolver(path(:, 2), ts, n_seg, n_order);
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:n_seg-1
% 取第i段参数,参数为阶数从低到高排列
Pxi = poly_coef_x(i*(n_order+1)+1:(i+1)*(n_order+1));
Pxi = flipud(Pxi);
Pyi = poly_coef_y(i*(n_order+1)+1:(i+1)*(n_order+1));
Pyi = flipud(Pyi);
for t=0:tstep:ts(i+1)
X_n(k) = polyval(Pxi,t);
Y_n(k) = polyval(Pyi,t);
k = k+1;
end
end
plot(X_n, Y_n ,'Color',[0 1.0 0],'LineWidth',4);
hold on
scatter(path(1:size(path,1),1),path(1:size(path,1),2));
function poly_coef = MinimumSnapCloseformSolver(waypoints, ts, n_seg, n_order)
start_cond = [waypoints(1), 0, 0, 0];
end_cond = [waypoints(end), 0, 0, 0];
Q = getQ(n_seg, n_order, ts);
% 计算M矩阵
M = getM(n_seg, n_order, ts);
% 计算C矩阵
Ct = getCt(n_seg, n_order);
C = Ct';
% 计算R
R = C * inv(M)' * Q * inv(M) * Ct;
R_cell = mat2cell(R, [n_seg+7 3*(n_seg-1)], [n_seg+7 3*(n_seg-1)]);
R_pp = R_cell{2, 2};
R_fp = R_cell{1, 2};
% 已知参数,有起点终点的位置、速度、加速度、jerk;及中间点的位置
dF = zeros(8+n_seg-1,1);
%起点
dF(1:4)=start_cond;
%终点
dF(4+n_seg-1+1:8+n_seg-1)=end_cond;
%中间点 位置
if n_seg~=1
dF(4+1:4+n_seg-1)=waypoints(2:2+n_seg-2);
end
% 根据极值点求解未知数据,如中间点的速度、加速度、jerk
dP = -inv(R_pp) * R_fp' * dF;
% 反向求解所有轨迹段的参数
poly_coef = inv(M) * Ct * [dF;dP];
end
2.1 得到M矩阵
M
M
M矩阵为
M
p
=
d
Mp=d
Mp=d计算,
[
M
1
M
2
⋱
M
k
]
[
p
1
p
2
⋮
p
k
]
=
[
d
1
d
2
⋮
d
k
]
\begin{bmatrix}M_1&&&\\&M_2&&\\&&\ddots &\\&&& M_k\end{bmatrix} \begin{bmatrix}p_1\\p_2\\\vdots\\p_k\end{bmatrix}=\begin{bmatrix}d_1\\d_2\\\vdots\\d_k\end{bmatrix}
⎣⎢⎢⎡M1M2⋱Mk⎦⎥⎥⎤⎣⎢⎢⎢⎡p1p2⋮pk⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎡d1d2⋮dk⎦⎥⎥⎥⎤
对于每一段轨迹
M
j
=
[
1
0
0
2
0
3
⋯
0
7
0
1
2
!
1
!
∗
0
1
3
!
2
!
∗
0
2
⋯
7
!
6
!
∗
0
6
0
0
2
∗
0
0
3
!
1
!
∗
0
1
⋯
7
!
5
!
0
5
0
0
0
3
!
0
!
∗
0
0
⋯
7
!
4
!
0
4
1
T
T
2
T
3
⋯
T
7
0
1
2
!
1
!
T
3
!
2
!
T
2
⋯
7
!
6
!
T
6
0
0
2
3
!
1
!
T
⋯
7
!
5
!
T
5
0
0
0
3
!
0
!
⋯
7
!
4
!
T
4
]
[
p
1
p
2
⋮
p
k
]
=
[
p
j
(
0
)
v
j
(
0
)
a
j
(
0
)
j
j
(
0
)
p
j
(
T
)
v
j
(
T
)
a
j
(
T
)
j
j
(
T
)
]
M_j = \begin{bmatrix}1&0&0^2&0^3&\cdots&0^7\\ 0&1&\frac{2!}{1!}*0^1&\frac{3!}{2!}*0^2&\cdots&\frac{7!}{6!}*0^6\\ 0&0& 2*0^0&\frac{3!}{1!}*0^1&\cdots & \frac{7!}{5!}0^5\\ 0&0&0&\frac{3!}{0!}*0^0&\cdots&\frac{7!}{4!}0^4\\ 1&T&T^2&T^3&\cdots &T^7\\ 0&1&\frac{2!}{1!}T&\frac{3!}{2!}T^2&\cdots&\frac{7!}{6!}T^6\\ 0&0&2&\frac{3!}{1!}T&\cdots & \frac{7!}{5!}T^5\\ 0&0&0&\frac{3!}{0!}&\cdots&\frac{7!}{4!}T^4\\\end{bmatrix} \begin{bmatrix}p_1\\p_2\\\vdots\\p_k\end{bmatrix} = \begin{bmatrix}p_j(0) \\v_j(0)\\ a_j(0)\\j_j(0) \\ p_j(T) \\v_j(T)\\ a_j(T)\\j_j(T) \end{bmatrix}
Mj=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡100010000100T100021!2!∗012∗000T21!2!T20032!3!∗021!3!∗010!3!∗00T32!3!T21!3!T0!3!⋯⋯⋯⋯⋯⋯⋯⋯076!7!∗065!7!054!7!04T76!7!T65!7!T54!7!T4⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡p1p2⋮pk⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡pj(0)vj(0)aj(0)jj(0)pj(T)vj(T)aj(T)jj(T)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
function M = getM(n_seg, n_order, ts)
M = [];
for k = 1:n_seg
M_k = zeros(8, n_order+1);
%#####################################################
% STEP 1.1: calculate M_k of the k-th segment
for r=0:3
for c=r:n_order
M_k(r+1,c+1)=(factorial(c)/factorial(c-r))*0^(c-r);
M_k(r+1+4,c+1)=(factorial(c)/factorial(c-r))*ts(k)^(c-r);
end
end
M = blkdiag(M, M_k);
end
end
2.2计算C矩阵
C矩阵为 d = C [ d F d P ] d=C\begin{bmatrix}d_F\\d_P\end{bmatrix} d=C[dFdP];
- d为每段轨迹中起点终点的位置、速度、加速度、jerk,维度为 8 ∗ 段 数 × 1 8*段数\times 1 8∗段数×1;
- 而 d F d_F dF为已知数据,包括整体轨迹的起点终点的位置、速度、加速度、jerk,及中间点的位置,维度为 4 + 4 + ( 段 数 − 1 ) × 1 4+4+(段数-1) \times 1 4+4+(段数−1)×1
- d P d_P dP为未知数据,中间点的速度、加速度、jerk,维度为 ( 段 数 − 1 ) ∗ 3 × 1 (段数-1)*3\times 1 (段数−1)∗3×1
- C矩阵的维度为 ( 8 ∗ 段 数 ) × ( 8 + ( 段 数 − 1 ) ∗ 4 ) (8*段数) \times (8+(段数-1)*4) (8∗段数)×(8+(段数−1)∗4);且矩阵中数据为0/1,目的为选择 d F , d P d_F,d_P dF,dP中数据。
function Ct = getCt(n_seg, n_order)
% C矩阵维度为 (8*段数 )X (4+k-1+4+(k-1)*3)
Ct = zeros((n_order+1)*n_seg,8+(n_seg-1)*4);
% start,p,v,a,j,已知的起点数据
for i=0:3
Ct(i+1,i+1)=1;
end
% end,p,v,a,j,已知的终点数据
for i = 0:3
Ct(8*(n_seg-1)+4+i+1,4+(n_seg-1)+i+1)=1;
end
if n_seg~=1
for i=0:n_seg-2
% p,已知的位置
Ct(8*i+4+1,4+i+1)=1;
Ct(8*(i+1)+1,4+i+1)=1;
% v,未知的速度
Ct(8*i+4+2,8+(n_seg-1)+i*3+1)=1;
Ct(8*(i+1)+2,8+(n_seg-1)+i*3+1)=1;
% a,未知的加速度
Ct(8*i+4+3,8+(n_seg-1)+i*3+2)=1;
Ct(8*(i+1)+3,8+(n_seg-1)+i*3+2)=1;
% j,未知的jerk
Ct(8*i+4+4,8+(n_seg-1)+i*3+3)=1;
Ct(8*(i+1)+4,8+(n_seg-1)+i*3+3)=1;
end
end
end