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

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

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

• 最高阶7阶,共8个参数
  $$\begin{array}{rl}p(t)& =p_0+p_{1}t+p_2{t}^2...+p_n{t}^n=\sum _{i=0}^{n=7}{p}_i{t}^i\\ v(t)& =p^{\prime }(t)=\sum _{i⩾1}^{n=7}i\cdot p_i{t}^{i-1}\\ a(t)& =p^{\prime \prime }(t)=\sum _{i⩾2}^{n=7}\frac{i!}{(i-2)!}\cdot p_i{t}^{i-2}\\ jerk(t)& =p^{(3)}(t)=\sum _{i⩾3}^{n=7}\frac{i!}{(i-3)!}\cdot p_i{t}^{i-3}\\ snap(t)& =p^{(4)}(t)=\sum _{i⩾4}^{n=7}\frac{i!}{(i-4)!}\cdot p_i{t}^{i-4}\end{array}$$
• 目标函数为$J=\min {\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]}^T\left[\begin{array}{cccc}{Q}_1& & & \\ & Q_2& & \\ & & \ddots & \\ & & & Q_k\end{array}\right]\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]$
• 等式约束$A_{eq}\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]=d_{eq}$

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

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

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));

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矩阵

代价函数为
$$\begin{array}{rl}J& =\min {\int }_0^T(p^{(4)}(t){)}^{2}dt\\ & =\min \sum _{i=1}^k{\int }_{t_{i-1}}^{t_i}(p^{(4)}(t){)}^{2}dt\\ & =\min \sum _{i=1}^k{p}_i^T{Q}_i{p}_i\\ & =\min p^{T}Qp\end{array}$$

• 其中$Q_i$为${\left[\begin{array}{cc}{0}_{4\times 4}& 0_{4\times (n-3)}\\ 0_{(n-3)\times 4}& \frac{r!}{(r-4)!}\frac{c!}{(c-4)!}\frac{1}{(r-4)+(c-4)+1}(t_i^{(r+c-7)}-t_{i-1}^{(r+c-7)})\end{array}\right]}_{(n+1)\times (n+1)}$
• $Q$为${\left[\begin{array}{cccc}{Q}_1& & & \\ & Q_2& & \\ & & \ddots & \\ & & & Q_k\end{array}\right]}_{k(n+1)\times k(n+1)}$
• $p_i$为$[p_{i,0},p_{i,1},\cdots ,p_{i,7}{]}^T$共7个参数，列向量
• $p$为 ${\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]}_{k(n+1)\times 1}$

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
  $$\begin{array}{rl}{p}_1(0)& =\sum _{i=0}^{n=7}{p}_{1,i}\ast 0^i\\ v_1(0)& =p_1^{\prime }(0)=\sum _{i⩾1}^{n=7}i\cdot p_{1,i}\ast 0^{i-1}\\ a_1(0)& =p_1^{\prime \prime }(0)=\sum _{i⩾2}^{n=7}\frac{i!}{(i-2)!}\cdot p_{1,i}\ast 0^{i-2}\\ jer{k}_1(0)& =p_1^{(3)}(0)=\sum _{i⩾3}^{n=7}\frac{i!}{(i-3)!}\cdot p_{1,i}\ast 0^{i-3}\end{array}$$
• 终点位置、速度、加速度、jerk限制，时间为T
  $$\begin{array}{rl}{p}_k(T)& =\sum _{i=0}^{n=7}{p}_{k,i}{T}^i\\ v_k(T)& =p_k^{\prime }(T)=\sum _{i⩾1}^{n=7}i\cdot p_{k,i}{T}^{i-1}\\ a_k(T)& =p_k^{\prime \prime }(T)=\sum _{i⩾2}^{n=7}\frac{i!}{(i-2)!}\cdot p_{k,i}{T}^{i-2}\\ jer{k}_k(T)& =p_k^{(3)}(T)=\sum _{i⩾3}^{n=7}\frac{i!}{(i-3)!}\cdot p_{k,i}{T}^{i-3}\end{array}$$
• 前后段连接点的位置限制
• 前后段连接点高阶项连续,第$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

最后

以上就是虚心小鸽子为你收集整理的Minimum snap matlab 代码（一）- 用优化solver求解参数的全部内容，希望文章能够帮你解决Minimum snap matlab 代码（一）- 用优化solver求解参数所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。