Minimum snap matlab 代码（一）- 用优化solver求解参数

128 阅读 0 评论 85 点赞

我是靠谱客的博主虚心小鸽子，这篇文章主要介绍Minimum snap matlab 代码（一）- 用优化solver求解参数，现在分享给大家，希望可以做个参考。

文章目录

- - 1.用优化solver求解参数算法概况
  - 2.用优化solver求解参数代码
  - - 2.1 计算Q矩阵
    - 2.2 计算等式约束

1.用优化solver求解参数算法概况

具体看Minimum Snap轨迹规划详解（一）

最高阶7阶,共8个参数
$p(t)&=p_0+p_1t+p_2t^2...+p_nt^n=sum_{i=0}^{n=7}p_it^i\ v(t) &= p^prime(t) = sum_{igeqslant1}^{n=7}icdot p_it^{i-1}\ a(t) &= p^{prime prime}(t) =sum_{igeqslant2}^{n=7}frac{i!}{(i-2)!}cdot p_it^{i-2} \ jerk(t) &= p^{(3)}(t) =sum_{igeqslant3}^{n=7}frac{i!}{(i-3)!}cdot p_it^{i-3} \ snap(t) &= p^{(4)}(t) =sum_{igeqslant4}^{n=7}frac{i!}{(i-4)!}cdot p_it^{i-4} end{aligned}$
目标函数为 $begin{bmatrix}p_1\p_2\vdots\p_kend{bmatrix}^T begin{bmatrix} Q_1 &&& \ &Q_2 && \ && ddots &\ &&& Q_kend{bmatrix}begin{bmatrix}p_1\p_2\vdots\p_kend{bmatrix}$
等式约束 $A_{eq}begin{bmatrix}p_1\p_2\vdots\p_kend{bmatrix}=d_{eq}$

2.用优化solver求解参数代码

代码中时间按照相对坐标轴取值，以免数值计算不稳定。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
clc;clear;close all;
path = ginput() * 100.0;
% n次项
n_order
= 7;
% 段数
n_seg
= size(path,1)-1;
% 每段参数数目
n_poly_perseg = (n_order+1);
% 每段轨迹的时间，按距离均匀分配时间，或直接赋值
ts = zeros(n_seg, 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);
% display the trajectory
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:n_seg-1
% 取计算好的每段参数，此时参数根据阶数由低到高
Pxi = poly_coef_x(i*n_poly_perseg +1:(i+1)*n_poly_perseg );
% 阶数由高到低排列
Pxi = flipud(Pxi);
Pyi = poly_coef_y(i*n_poly_perseg +1:(i+1)*n_poly_perseg );
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', 2);
hold on
scatter(path(1:size(path, 1), 1), path(1:size(path, 1), 2));

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
function poly_coef = MinimumSnapQPSolver(waypoints, ts, n_seg, n_order)
% waypoints 点集
% ts，每段的时间，相对时间轴坐标系
% n_seg，段数
% n_order ,最高阶数
% 起点位置、速度、加速度、jerk 数值
start_cond = [waypoints(1), 0, 0, 0];
% 终点位置、速度、加速度、jerk 数值
start_cond = [waypoints(1), 0, 0, 0];
end_cond
= [waypoints(end), 0, 0, 0];
% 计算Q矩阵
Q = getQ(n_seg, n_order, ts);
% 计算等式约束
[Aeq, beq] = getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond);
% 用solver优化，得到优化后的所有参数poly_coef
f = zeros(size(Q,1),1);
poly_coef = quadprog(Q,f,[],[],Aeq, beq);
end

2.1 计算Q矩阵

代价函数为
$int^T_0(p^{(4)}(t))^2dt\ &=minsum_{i=1}^k int^{t_i}_{t_{i−1}}(p^{(4)}(t))^2dt \ &= minsum_{i=1}^kp_i^TQ_ip_i \ &=min p^TQp end{aligned}$

其中 $Q_i$ 为 $begin{bmatrix}0_{4times4}&0_{4times (n−3)}\ 0_{(n−3)times 4}&frac{r!}{(r−4)!}frac{c!}{(c−4)!} frac{1}{(r−4)+(c−4)+1}(t^{(r+c−7)}_i−t^{(r+c−7)}_{i−1})end{bmatrix}_{(n+1)times(n+1)}$
$Q$ 为 $Q_1 &&& \ &Q_2 && \ && ddots &\ &&& Q_kend{bmatrix}_{k(n+1)times k(n+1)}$
$p_i$ 为 $[p_{i,0},p_{i,1},cdots,p_{i,7}]^T$ 共7个参数，列向量
$p$ 为 $begin{bmatrix}p_1\p_2\vdots\p_kend{bmatrix}_{k(n+1)times 1}$

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
function Q = getQ(n_seg, n_order, ts)
% ts，每段的时间，相对时间轴坐标系
% n_seg，段数
% n_order ,最高次项
Q = [];
% 每段分别计算Q
for k = 1:n_seg
% 初始化，对于7次多项式，维度为8*8
Q_k = zeros(n_order+1, n_order+1);
%行
for r = 4:n_order
%列
for c= 4:n_order
% t_i为第i段的时间，t_{i-1}为第i段的起始时间，取相对时间轴，值为0
Q_k(r+1,c+1) = (factorial(r)/factorial(r-4))*(factorial(c)/factorial(c-4))*(1/(r+c-7))*(ts(k)^(r+c-7));
end
end
Q = blkdiag(Q, Q_k);
end
end

2.2 计算等式约束

起点位置、速度、加速度、jerk限制，时间为0
$p_1(0)&=sum_{i=0}^{n=7}p_{1,i}*0^i\ v_1(0) &= p_1^prime(0) = sum_{igeqslant1}^{n=7}icdot p_{1,i}*0^{i-1}\ a_1(0) &= p_1^{prime prime}(0) =sum_{igeqslant2}^{n=7}frac{i!}{(i-2)!}cdot p_{1,i}*0^{i-2} \ jerk_1(0) &= p_1^{(3)}(0) =sum_{igeqslant3}^{n=7}frac{i!}{(i-3)!}cdot p_{1,i}*0^{i-3} end{aligned}$
终点位置、速度、加速度、jerk限制，时间为T
$p_k(T)&=sum_{i=0}^{n=7}p_{k,i}T^i\ v_k(T) &= p_k^prime(T) = sum_{igeqslant1}^{n=7}icdot p_{k,i}T^{i-1}\ a_k(T) &= p_k^{prime prime}(T) =sum_{igeqslant2}^{n=7}frac{i!}{(i-2)!}cdot p_{k,i}T^{i-2} \ jerk_k(T) &= p_k^{(3)}(T) =sum_{igeqslant3}^{n=7}frac{i!}{(i-3)!}cdot p_{k,i}T^{i-3} end{aligned}$
前后段连接点的位置限制
前后段连接点高阶项连续,第 $j$ 段， $m$ 阶，在 $T_j$ 处的极限值，等于第 $j + 1$ 段， $m$ 阶，在 $j + 1$ 段初始时时间0处的极限值
$p_{j}^{(m)}(T_j) - p_{j+1}^{(m)}(0) =0$

复制代码

function [Aeq beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
% n_seg 段数
% n_order， 最高阶数
% waypoints，点集
% ts每段时间
% start_cond = [waypoints(1), 0, 0, 0];起始时位置，速度，加速度，jerk
% end_cond
= [waypoints(end), 0, 0, 0];终止时位置，速度，加速度，jerk
% 所有参数的个数
n_all_poly = n_seg*(n_order+1);
%#####################################################
% p,v,a,j constraint in start, 起点位置，速度，加速度，jerk的限制，时间为0，矩阵只剩4项
Aeq_start = zeros(4, n_all_poly);
beq_start = start_cond ;
for r=1:4
Aeq_start(r,r) = factorial(r-1);
end
%#####################################################
% p,v,a constraint in end，终点位置、速度、加速度、jerk的限制
Aeq_end = zeros(4, n_all_poly);
beq_end =end_cond;
for r = 0:3
for c=r:n_order
Aeq_end(r+1,(n_seg-1)*(n_order+1)+c+1) =
(factorial(c)/ factorial(c-r))*(ts(n_seg)^(c-r));
end
end;
%#####################################################
% position constrain in all middle waypoints，中间路标点对位置的限制
Aeq_wp = zeros(n_seg-1, n_all_poly);
beq_wp = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=0:n_order
Aeq_wp(r+1,r*(n_order+1)+c+1)= (ts(r+1)^c);
end
beq_wp(r+1) = waypoints(r+2);
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);
for r=0:n_seg-2
for c=0:n_order
%第r段的位置
Aeq_con_p(r+1,r*(n_order+1)+c+1)= ts(r+1)^c;
%第r+1段的位置
Aeq_con_p(r+1,(r+1)*(n_order+1)+c+1) = -0^c;
end
end
%#####################################################
% velocity continuity constrain between each 2 segments
% segments，前后两段对速度连续性的限制
Aeq_con_v = zeros(n_seg-1, n_all_poly);
beq_con_v = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=1:n_order
Aeq_con_v(r+1,r*(n_order+1)+c+1)= c*ts(r+1)^(c-1);
Aeq_con_v(r+1,(r+1)*(n_order+1)+c+1) = -c*0^(c-1);
end
end
%#####################################################
% acceleration continuity constrain between each 2 segments
% segments，前后两段对加速度连续性的限制
Aeq_con_a = zeros(n_seg-1, n_all_poly);
beq_con_a = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=2:n_order
Aeq_con_a(r+1,r*(n_order+1)+c+1)= c*(c-1)*ts(r+1)^(c-2);
Aeq_con_a(r+1,(r+1)*(n_order+1)+c+1) = -c*(c-1)*0^(c-2);
end
end
%#####################################################
% jerk continuity constrain between each 2 segments
% segments，前后两段对jerk连续性的限制
Aeq_con_j = zeros(n_seg-1, n_all_poly);
beq_con_j = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=3:n_order
Aeq_con_j(r+1,r*(n_order+1)+c+1)= c*(c-1)*(c-2)*ts(r+1)^(c-3);
Aeq_con_j(r+1,(r+1)*(n_order+1)+c+1) = -c*(c-1)*(c-2)*0^(c-3);
end
end
%#####################################################
% combine all components to form Aeq and beq
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];
Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
beq = [beq_start; beq_end; beq_wp; beq_con];
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
function [Aeq beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
% n_seg 段数
% n_order， 最高阶数
% waypoints，点集
% ts每段时间
% start_cond = [waypoints(1), 0, 0, 0];起始时位置，速度，加速度，jerk
% end_cond
= [waypoints(end), 0, 0, 0];终止时位置，速度，加速度，jerk
% 所有参数的个数
n_all_poly = n_seg*(n_order+1);
%#####################################################
% p,v,a,j constraint in start, 起点位置，速度，加速度，jerk的限制，时间为0，矩阵只剩4项
Aeq_start = zeros(4, n_all_poly);
beq_start = start_cond ;
for r=1:4
Aeq_start(r,r) = factorial(r-1);
end
%#####################################################
% p,v,a constraint in end，终点位置、速度、加速度、jerk的限制
Aeq_end = zeros(4, n_all_poly);
beq_end =end_cond;
for r = 0:3
for c=r:n_order
Aeq_end(r+1,(n_seg-1)*(n_order+1)+c+1) =
(factorial(c)/ factorial(c-r))*(ts(n_seg)^(c-r));
end
end;
%#####################################################
% position constrain in all middle waypoints，中间路标点对位置的限制
Aeq_wp = zeros(n_seg-1, n_all_poly);
beq_wp = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=0:n_order
Aeq_wp(r+1,r*(n_order+1)+c+1)= (ts(r+1)^c);
end
beq_wp(r+1) = waypoints(r+2);
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);
for r=0:n_seg-2
for c=0:n_order
%第r段的位置
Aeq_con_p(r+1,r*(n_order+1)+c+1)= ts(r+1)^c;
%第r+1段的位置
Aeq_con_p(r+1,(r+1)*(n_order+1)+c+1) = -0^c;
end
end
%#####################################################
% velocity continuity constrain between each 2 segments
% segments，前后两段对速度连续性的限制
Aeq_con_v = zeros(n_seg-1, n_all_poly);
beq_con_v = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=1:n_order
Aeq_con_v(r+1,r*(n_order+1)+c+1)= c*ts(r+1)^(c-1);
Aeq_con_v(r+1,(r+1)*(n_order+1)+c+1) = -c*0^(c-1);
end
end
%#####################################################
% acceleration continuity constrain between each 2 segments
% segments，前后两段对加速度连续性的限制
Aeq_con_a = zeros(n_seg-1, n_all_poly);
beq_con_a = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=2:n_order
Aeq_con_a(r+1,r*(n_order+1)+c+1)= c*(c-1)*ts(r+1)^(c-2);
Aeq_con_a(r+1,(r+1)*(n_order+1)+c+1) = -c*(c-1)*0^(c-2);
end
end
%#####################################################
% jerk continuity constrain between each 2 segments
% segments，前后两段对jerk连续性的限制
Aeq_con_j = zeros(n_seg-1, n_all_poly);
beq_con_j = zeros(n_seg-1, 1);
for r=0:n_seg-2
for c=3:n_order
Aeq_con_j(r+1,r*(n_order+1)+c+1)= c*(c-1)*(c-2)*ts(r+1)^(c-3);
Aeq_con_j(r+1,(r+1)*(n_order+1)+c+1) = -c*(c-1)*(c-2)*0^(c-3);
end
end
%#####################################################
% combine all components to form Aeq and beq
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];
Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
beq = [beq_start; beq_end; beq_wp; beq_con];
end