通过闭式求解的方法求解只有等式约束没有不等式约束的Minimun snap路径轨迹生成问题
原理
解目标函数为Minimum Snap，多项式表达式为七次多项式
f
(
t
)
=
∑
i
p
i
t
i
=
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
+
p
0
f(t)=\sum\limits_{i}p_it^i=p_7t^7 + p_6t^6 + p_5t^5 + p_4t^4 + p_3t^3 + p_2t^2 + p_1t + p_0
f(t)=i∑piti=p7t7+p6t6+p5t5+p4t4+p3t3+p2t2+p1t+p0
直接求解p_0、p_1、p_2、p_3、p_4、p_5、p_6、p_7,这些系数也就是优化参数，若直接求解这些系数可能会导致数值求解不稳定，若把优化项替换成p,v,a,j这些具有实际物理意义的变量的话则求解结果会更稳定。
通过一个mapping矩阵把多项式系数映射到p,v,a,j（即f(t),f’(t),f’’(t),f’’’(t)）。
M
j
p
j
=
d
j
(1)
M_jp_j=d_j \tag{1}
Mjpj=dj(1)
Mj就这这个映射矩阵，则：
p
j
=
M
j
−
1
d
j
(2)
p_j=M_j^{-1}d_j \tag{2}
pj=Mj−1dj(2)
其转置为：
p
j
T
=
d
j
T
M
j
−
T
(3)
p_j^T=d_j^TM_j^{-T} \tag{3}
pjT=djTMj−T(3)
J
=
[
p
1
⋮
p
M
]
T
[
Q
1
0
0
0
⋱
0
0
0
Q
M
]
[
p
1
⋮
p
M
]
(4)
J=\left[ \begin{matrix} p_1 \\ \vdots\\ p_M \\ \end{matrix} \right]^T \left[ \begin{matrix} Q_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &Q_M \\ \end{matrix} \right] \left[ \begin{matrix} p_1 \\ \vdots\\ p_M \\ \end{matrix} \right] \tag{4}
J=⎣⎢⎡p1⋮pM⎦⎥⎤T⎣⎡Q1000⋱000QM⎦⎤⎣⎢⎡p1⋮pM⎦⎥⎤(4)
由把式(2)、(3)代入式(4)，则
J
=
[
d
1
⋮
d
M
]
T
[
M
1
0
0
0
⋱
0
0
0
M
M
]
−
T
[
Q
1
0
0
0
⋱
0
0
0
Q
M
]
[
M
1
0
0
0
⋱
0
0
0
M
M
]
−
1
[
d
1
⋮
d
M
]
(4)
J=\left[ \begin{matrix} d_1 \\ \vdots\\ d_M \\ \end{matrix} \right]^T \left[ \begin{matrix} M_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &M_M \\ \end{matrix} \right]^{-T} \left[ \begin{matrix} Q_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &Q_M \\ \end{matrix} \right] \left[ \begin{matrix} M_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &M_M \\ \end{matrix} \right]^{-1} \left[ \begin{matrix} d_1 \\ \vdots\\ d_M \\ \end{matrix} \right] \tag{4}
J=⎣⎢⎡d1⋮dM⎦⎥⎤T⎣⎡M1000⋱000MM⎦⎤−T⎣⎡Q1000⋱000QM⎦⎤⎣⎡M1000⋱000MM⎦⎤−1⎣⎢⎡d1⋮dM⎦⎥⎤(4)
映射矩阵Mj的求解方法
Mj代表第j段轨迹曲线所代表的多项式系数对应的映射矩阵。
即由这段轨迹p0、p1、p2、p3、p4、p5、p6、p7多项式系数可以找到一个M矩阵映射到该段轨迹首状态f(0)、f’(0)、f’’(0)、f’’’(0)和末状态f(t)、f’(t)、f’’(t)、f’’’(t)。若知道了首末状态也就能唯一确定多项式系数。
f
(
t
)
=
p
(
t
)
=
p
0
+
p
1
t
+
p
2
t
2
+
p
3
t
3
+
p
4
t
4
+
p
5
t
5
+
p
6
t
6
+
p
7
t
7
f(t)= p(t)=p_0 + p_1t +p_2t^2 + p_3t^3+ p_4t^4+ p_5t^5 + p_6t^6 +p_7t^7
f(t)=p(t)=p0+p1t+p2t2+p3t3+p4t4+p5t5+p6t6+p7t7
f
(
1
)
(
t
)
=
v
(
t
)
=
0
+
p
1
+
p
2
∗
2
t
+
p
3
∗
3
t
2
+
p
4
∗
4
t
3
+
p
5
∗
5
t
4
+
p
6
∗
6
t
5
+
p
7
∗
7
t
6
f^{(1)}(t)= v(t)=0 + p_1 +p_2*2t + p_3*3t^2+ p_4*4t^3+ p_5*5t^4 + p_6*6t^5 +p_7*7t^6
f(1)(t)=v(t)=0+p1+p2∗2t+p3∗3t2+p4∗4t3+p5∗5t4+p6∗6t5+p7∗7t6
f
(
2
)
(
t
)
=
a
(
t
)
=
0
+
0
+
p
2
∗
2
+
p
3
∗
6
t
+
p
4
∗
12
t
2
+
p
5
∗
20
t
3
+
p
6
∗
30
t
4
+
p
7
∗
42
t
5
f^{(2)}(t)= a(t)=0 + 0 +p_2*2 + p_3*6t+ p_4*12t^2+ p_5*20t^3 + p_6*30t^4 +p_7*42t^5
f(2)(t)=a(t)=0+0+p2∗2+p3∗6t+p4∗12t2+p5∗20t3+p6∗30t4+p7∗42t5
f
(
3
)
(
t
)
=
j
(
t
)
=
0
+
0
+
0
+
p
3
∗
6
+
p
4
∗
24
t
+
p
5
∗
60
t
2
+
p
6
∗
120
t
3
+
p
7
∗
210
t
4
f^{(3)}(t)= j(t)=0 + 0 +0 + p_3*6+ p_4*24t+ p_5*60t^2 + p_6*120t^3 +p_7*210t^4
f(3)(t)=j(t)=0+0+0+p3∗6+p4∗24t+p5∗60t2+p6∗120t3+p7∗210t4
d
j
=
[
f
(
0
)
f
(
1
)
(
0
)
f
(
2
)
(
0
)
f
(
3
)
(
0
)
f
(
t
j
e
n
d
)
f
(
1
)
(
t
j
e
n
d
)
f
(
2
)
(
t
j
e
n
d
)
f
(
3
)
(
t
j
e
n
d
)
]
d_j=\left[ \begin{matrix} f(0) \\ f^{(1)}(0) \\ f^{(2)}(0) \\ f^{(3)}(0) \\ f(t_{jend}) \\ f^{(1)}(t_{jend}) \\ f^{(2)}(t_{jend}) \\ f^{(3)}(t_{jend}) \\ \end{matrix} \right]
dj=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡f(0)f(1)(0)f(2)(0)f(3)(0)f(tjend)f(1)(tjend)f(2)(tjend)f(3)(tjend)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
则：
[
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
2
0
0
0
0
0
0
0
0
6
0
0
0
0
1
t
j
e
n
d
t
j
e
n
d
2
t
j
e
n
d
3
t
j
e
n
d
4
t
j
e
n
d
5
t
j
e
n
d
6
t
j
e
n
d
7
0
1
2
t
j
e
n
d
3
t
j
e
n
d
2
4
t
j
e
n
d
3
5
t
j
e
n
d
4
6
t
j
e
n
d
5
7
t
j
e
n
d
6
0
0
2
6
t
j
e
n
d
12
t
j
e
n
d
2
20
t
j
e
n
d
3
30
t
j
e
n
d
4
42
t
j
e
n
d
5
0
0
0
6
24
t
j
e
n
d
60
t
j
e
n
d
2
120
t
j
e
n
d
3
210
t
j
e
n
d
4
]
[
p
0
p
1
p
2
p
3
p
4
p
5
p
6
p
7
]
=
[
f
(
0
)
f
(
1
)
(
0
)
f
(
2
)
(
0
)
f
(
3
)
(
0
)
f
(
t
j
e
n
d
)
f
(
1
)
(
t
j
e
n
d
)
f
(
2
)
(
t
j
e
n
d
)
f
(
3
)
(
t
j
e
n
d
)
]
=
[
d
0
,
1
(
0
)
d
0
,
1
(
1
)
d
0
,
1
(
2
)
d
0
,
1
(
3
)
d
j
e
n
d
,
1
(
0
)
d
j
e
n
d
,
1
(
1
)
d
j
e
n
d
,
1
(
2
)
d
j
e
n
d
,
1
(
3
)
]
\left[ \begin{matrix} 1 &0 &0 &0 &0 &0 &0 &0 \\ 0 &1 &0 &0 &0 &0 &0 &0 \\ 0 &0 &2 &0 &0 &0 &0 &0 \\ 0 &0 &0 &6 &0 &0 &0 &0 \\ 1 &t_{jend} &t_{jend}^2 &t_{jend}^3 &t_{jend}^4 &t_{jend}^5 &t_{jend}^6 &t_{jend}^7 \\ 0 &1 &2t_{jend} &3t_{jend}^2 &4t_{jend}^3 &5t_{jend}^4 &6t_{jend}^5 &7t_{jend}^6 \\ 0 &0 &2 &6t_{jend} &12t_{jend}^2 &20t_{jend}^3 &30t_{jend}^4 &42t_{jend}^5 \\ 0 &0 &0 &6 &24t_{jend} &60t_{jend}^2 &120t_{jend}^3 &210t_{jend}^4 \\ \end{matrix} \right] \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} f(0) \\ f^{(1)}(0) \\ f^{(2)}(0) \\ f^{(3)}(0) \\ f(t_{jend}) \\ f^{(1)}(t_{jend}) \\ f^{(2)}(t_{jend}) \\ f^{(3)}(t_{jend}) \\ \end{matrix} \right]= \left[ \begin{matrix} d_{0,1}^{(0)} \\ d_{0,1}^{(1)} \\ d_{0,1}^{(2)} \\ d_{0,1}^{(3)} \\ d_{jend,1}^{(0)} \\ d_{jend,1}^{(1)} \\ d_{jend,1}^{(2)} \\ d_{jend,1}^{(3)} \\ \end{matrix} \right]
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡100010000100tjend1000020tjend22tjend200006tjend33tjend26tjend60000tjend44tjend312tjend224tjend0000tjend55tjend420tjend360tjend20000tjend66tjend530tjend4120tjend30000tjend77tjend642tjend5210tjend4⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡p0p1p2p3p4p5p6p7⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡f(0)f(1)(0)f(2)(0)f(3)(0)f(tjend)f(1)(tjend)f(2)(tjend)f(3)(tjend)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡d0,1(0)d0,1(1)d0,1(2)d0,1(3)djend,1(0)djend,1(1)djend,1(2)djend,1(3)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
其中：
M
j
=
[
1
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
2
0
0
0
0
0
0
0
0
6
0
0
0
0
1
t
j
e
n
d
t
j
e
n
d
2
t
j
e
n
d
3
t
j
e
n
d
4
t
j
e
n
d
5
t
j
e
n
d
6
t
j
e
n
d
7
0
1
2
t
j
e
n
d
3
t
j
e
n
d
2
4
t
j
e
n
d
3
5
t
j
e
n
d
4
6
t
j
e
n
d
5
7
t
j
e
n
d
6
0
0
2
6
t
j
e
n
d
12
t
j
e
n
d
2
20
t
j
e
n
d
3
30
t
j
e
n
d
4
42
t
j
e
n
d
5
0
0
0
6
24
t
j
e
n
d
60
t
j
e
n
d
2
120
t
j
e
n
d
3
210
t
j
e
n
d
4
]
M_j= \left[ \begin{matrix} 1 &0 &0 &0 &0 &0 &0 &0 \\ 0 &1 &0 &0 &0 &0 &0 &0 \\ 0 &0 &2 &0 &0 &0 &0 &0 \\ 0 &0 &0 &6 &0 &0 &0 &0 \\ 1 &t_{jend} &t_{jend}^2 &t_{jend}^3 &t_{jend}^4 &t_{jend}^5 &t_{jend}^6 &t_{jend}^7 \\ 0 &1 &2t_{jend} &3t_{jend}^2 &4t_{jend}^3 &5t_{jend}^4 &6t_{jend}^5 &7t_{jend}^6 \\ 0 &0 &2 &6t_{jend} &12t_{jend}^2 &20t_{jend}^3 &30t_{jend}^4 &42t_{jend}^5 \\ 0 &0 &0 &6 &24t_{jend} &60t_{jend}^2 &120t_{jend}^3 &210t_{jend}^4 \\ \end{matrix} \right]
Mj=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡100010000100tjend1000020tjend22tjend200006tjend33tjend26tjend60000tjend44tjend312tjend224tjend0000tjend55tjend420tjend360tjend20000tjend66tjend530tjend4120tjend30000tjend77tjend642tjend5210tjend4⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
若有多段轨迹则通过对角矩阵的形式组合
M
=
[
M
1
0
0
0
⋱
0
0
0
M
j
]
M= \left[ \begin{matrix} M_1 &0 &0 \\ 0 &\ddots &0\\ 0 &0 &M_j \\ \end{matrix} \right]
M=⎣⎡M1000⋱000Mj⎦⎤
以四个端点的，有三段轨迹的为例：
selection matrix（选择矩阵）C的求解
C
T
[
d
F
d
P
]
=
[
d
1
⋮
d
M
]
C^T \left[ \begin{matrix} d_F \\ d_P \end{matrix} \right]= \left[ \begin{matrix} d_1 \\ \vdots \\ d_M \end{matrix} \right]
CT[dFdP]=⎣⎢⎡d1⋮dM⎦⎥⎤
参考PPT内容：
d_P是真正要优化的项，中间点的v,a,j
d_F是被约束项：总轨迹的首末点的p、v、a、j和中间点的p。
把d_P和d_F分离开。
还以三段式的轨迹为例：
再往下推：
J
=
[
d
F
d
P
]
T
C
M
−
T
Q
M
−
1
C
T
[
d
F
d
P
]
J=\left[ \begin{matrix} d_F \\ d_P \end{matrix} \right]^TCM^{-T}QM^{-1}C^T\left[ \begin{matrix} d_F \\ d_P \end{matrix} \right]
J=[dFdP]TCM−TQM−1CT[dFdP]
令
R
=
C
M
−
T
Q
M
−
1
C
T
=
[
R
F
F
R
F
P
R
P
F
R
P
P
]
R=CM^{-T}QM^{-1}C^T= \left[ \begin{matrix} R_{FF} &R_{FP} \\ R_{PF} &R_{PP} \end{matrix} \right]
R=CM−TQM−1CT=[RFFRPFRFPRPP]
则：
J
=
[
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
J=\left[ \begin{matrix} d_F \\ d_P \end{matrix} \right]^T\left[ \begin{matrix} R_{FF} &R_{FP} \\ R_{PF} &R_{PP} \end{matrix} \right] \left[ \begin{matrix} d_F \\ d_P \end{matrix} \right]= d_F^TR_{FF}d_F+d_F^TR_{FP}d_P+d_P^TR_{PF}d_F+d_P^TR_{PP}d_P
J=[dFdP]T[RFFRPFRFPRPP][dFdP]=dFTRFFdF+dFTRFPdP+dPTRPFdF+dPTRPPdP
其中R矩阵中R_FF…的分割，和d_F的行数有关。R_FF为row(d_F)*row(d_F)矩阵，R_PP为row(d_P)*row(d_P)矩阵。
Q的求法见我写的另一篇文章，见链接: 求解Q的方法.
从而推导出：（应该是通过对d_P求导推出来的）
d
P
∗
=
−
R
P
P
−
1
R
F
P
T
d
F
d_P^*=-R_{PP}^{-1}R_{FP}^Td_F
dP∗=−RPP−1RFPTdF
则可以得出多项式的系数：
p
=
M
−
1
C
T
[
d
F
d
P
]
p=M^{-1}C^T \left[ \begin{matrix} d_F \\ d_P \end{matrix} \right]
p=M−1CT[dFdP]
分别独立求出x(t)和y(t)，就能得到y-x轨迹图。
代码分析
%hw1_2.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);
% calculate time distribution based on distance between 2 points
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;
% or you can simply average the time
% for i = 1:n_seg
% ts(i) = 1.0;
% end
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
%#####################################################
% STEP 4: get the coefficients of i-th segment of both x-axis
% and y-axis
Pxi = poly_coef_x((n_order+1)*(i)+1:(n_order+1)*(i)+n_order+1); % note (n_order+1) jump to another segment!
Pyi = poly_coef_y((n_order+1)*(i)+1:(n_order+1)*(i)+n_order+1);
for t=0:tstep:ts(i+1)
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));
function poly_coef = MinimumSnapCloseformSolver(waypoints, ts, n_seg, n_order)
start_cond = [waypoints(1), 0, 0, 0];
end_cond = [waypoints(end), 0, 0, 0];
%#####################################################
% you have already finished this function in hw1
Q = getQ(n_seg, n_order, ts);
%#####################################################
% STEP 1: compute M
M = getM(n_seg, n_order, ts);
%#####################################################
% STEP 2: compute Ct
Ct = getCt(n_seg, n_order);
C = Ct';
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};
%#####################################################
% STEP 3: compute dF
dF = zeros(8+(n_seg-1),1);
dF(1) = waypoints(1);%p0
% v0 a0 j0 are 0
for i=1:1:n_seg-1
dF(4+i) = waypoints(i+1); % p2,..pk-1
end
dF(end-3) = waypoints(end);% pk
dP = -inv(R_pp) * R_fp' * dF;
poly_coef = inv(M) * Ct * [dF;dP];
end
%getM.m
function M = getM(n_seg, n_order, ts)
M = [];
for k = 1:n_seg
M_k = [];
%#####################################################
% STEP 1.1: calculate M_k of the k-th segment
t = ts(k);
M_k(1:4,1:8) = MCoeff(0);
M_k(5:8,1:8) = MCoeff(t);
M = blkdiag(M, M_k);
end
end
function Mcoeff = MCoeff(t)
% Get the coefficient for Aeq
Mcoeff = [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
%getCt.m
function Ct = getCt(n_seg, n_order)
%#####################################################
% STEP 2.1: finish the expression of Ct
% CT矩阵的维度8k * (4k+4), k:p,v,a,j--4
d_order = 4;
ct_rows = d_order*2*n_seg;
ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
Ct = zeros(ct_rows, ct_cols);
% 构造dF部分的CT矩阵，首末的p v a j)
Ct(1:4,1:4) = diag([1,1,1,1]);
Ct(4+8*(n_seg-1)+1:4+8*(n_seg-1)+4,4+n_seg:7+n_seg) = diag([1,1,1,1]);
% 构造dP部分中中间点p约束
for k=0:n_seg-2
Ct(4+8*k+1,4+k+1) = 1;
Ct(4+8*k+5,4+k+1) = 1;
end
for k=0:n_seg-2
Ct(4+8*k+2:4+8*k+4,2*4+n_seg-1+3*k+1:2*4+n_seg-1+3*k+3)=diag([1,1,1]);
Ct(4+8*k+6:4+8*k+8,2*4+n_seg-1+3*k+1:2*4+n_seg-1+3*k+3)=diag([1,1,1]);
end
% 构造dP部分中连续性约束，中间点的v,a,j
end
最终结果
1268
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
