Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control
先讀部落格：軌迹生成】參數化最優控制 限制-控制-圖形參數
狀态空間
x ⃗ = ( x , y , θ , κ ) \vec{x}=(x,y,\theta ,\kappa ) x
=(x,y,θ,κ)
将狀态表示為關于弧長的表達式：
κ ( s ) = κ 0 + a s + b s 2 + c s 3 θ ( s ) = θ 0 + ∫ 0 κ ( s ) = θ 0 + κ 0 s + a s 2 2 + b s 3 3 + c s 4 4 x ( s ) = x 0 + ∫ 0 cos ( θ ( s ) ) d s y ( s ) = y 0 + ∫ 0 sin ( θ ( s ) ) d s \kappa \left( s \right) =\kappa _0+as+bs^2+cs^3 \\ \theta \left( s \right) =\theta _0+\int_0{\kappa \left( s \right) \,\,=\theta _0+\kappa _0s+\frac{as^2}{2}+\frac{bs^3}{3}+\frac{cs^4}{4}} \\ x\left( s \right) =x_0+\int_0{\cos \left( \theta \left( s \right) \right)}ds \\ y\left( s \right) =y_0+\int_0{\sin \left( \theta \left( s \right) \right) ds} κ(s)=κ0+as+bs2+cs3θ(s)=θ0+∫0κ(s)=θ0+κ0s+2as2+3bs3+4cs4x(s)=x0+∫0cos(θ(s))dsy(s)=y0+∫0sin(θ(s))ds
曲線節點參數化，統一量綱降低數值疊代的不穩定性，将曲線分為0、1/3、2/3、1，其中
k ( 0 ) = p 0 a ( p ) = p 0 k ( s G 3 ) = p 1 b ( p ) = − 11 p 0 − 18 p 1 + 9 p 2 − 2 p 3 2 s G k ( 2 s G 3 ) = p 2 c ( p ) = 9 ( 2 p 0 − 5 p 1 + 4 p 2 − p 3 ) 2 s G 2 k ( s G ) = p 3 d ( p ) = − 9 ( p 0 − 3 p 1 + 3 p 2 − p 3 ) 2 s G 3 k\left( 0 \right) =p0\quad a\left( p \right) =p0 \\ k\left( \frac{s_G}{3} \right) =p1\quad b\left( p \right) \,\,=\,\,-\frac{11p0-18p1+9p2-2p3}{2s_G} \\ k\left( \frac{2s_G}{3} \right) =p2 \quad c\left( p \right) =\frac{9\left( 2p0-5p1+4p2-p3 \right)}{2{s_G}^2} \\ k\left( s_G \right) =p3 \quad d\left( p \right) \,\,=\,\,-\frac{9\left( p0-3p1+3p2-p3 \right)}{2{s_G}^3} \\ k(0)=p0a(p)=p0k(3sG)=p1b(p)=−2sG11p0−18p1+9p2−2p3k(32sG)=p2c(p)=2sG29(2p0−5p1+4p2−p3)k(sG)=p3d(p)=−2sG39(p0−3p1+3p2−p3)
将參數空間[a b c d]轉換為 p = { p 0 , p 1 , p 2 , p 3 , s g } \boldsymbol{p}=\{p_0,p_1,p_2,p_3,s_g\} p={p0,p1,p2,p3,sg},由于 p 0 = k 0 , p 3 = k f p_0\,\,=\,\,k_0, p3=k_f\,\, p0=k0,p3=kf，則參數向量簡化為 p = p 1 , p 2 , s g \boldsymbol{p}=p_1,p_2,s_g p=p1,p2,sg
最優化求解
端點邊界值
q i n i t = [ x i , y i , θ i , k i ] q g o a l = [ x g , y g , θ g , k g ] q_{init}=\left[ x_i,y_i,\theta _i,k_i \right] \\ q_{goal}=\left[ x_g,y_g,\theta _g,k_g \right] qinit=[xi,yi,θi,ki]qgoal=[xg,yg,θg,kg]
參數初值估計
R = Δ x 2 + Δ y 2 S = R ( Δ θ 2 5 + 1 ) 将 k ( s ) 的 d 參數置 0 ，有 k 0 與 k f 求解 b 與 c 的解 a = 6 θ f s 2 − 4 k 0 s + 2 k f s b = 3 s 2 ( k 0 + k f ) + 6 θ f s 3 R\,\,=\,\,\sqrt{\Delta x^2+\Delta y^2} \\ S\,\,=\,\,R\left( \frac{\Delta \theta ^2}{5}+1 \right) \\ \text{将}k\left( s \right) \text{的}d\text{參數置}0\text{，有}k_0\text{與}k_f\text{求解}b\text{與}c\,\,\text{的解} \\ a=\frac{6\theta _f}{s^2}-\frac{4k_0}{s}+\frac{2k_f}{s} \\ b=\frac{3}{s^2}\left( k_0+k_f \right) +6\frac{\theta _f}{s^3} R=Δx2+Δy2
S=R(5Δθ2+1)将k(s)的d參數置0，有k0與kf求解b與c的解a=s26θf−s4k0+s2kfb=s23(k0+kf)+6s3θf
把a、b、c、d與S代入求取 p = p 1 , p 2 , s g \boldsymbol{p}=p_1,p_2,s_g p=p1,p2,sg;
matlab代碼：
%初值估計
S = (0.2*dt*dt+1)*sqrt(dx*dx+dy*dy); %疊代初始值估計，用弦長
bs = 6*(xe(ST)-xs(ST))/(S * S) - 4*xs(SK)/S - 2*xe(SK)/S; %先将k(s)=k0+a*s+b*s^2,求解出a b 作為初始值估計
cs = 3*(xs(SK) + xe(SK))/(S * S) - 6*(xe(ST)-xe(ST))/(S * S * S);
ps = solve(b-bs,c-cs,sf-S); %求解方程組
p = eval([ps.p1 ps.p2 ps.sf]); %初始化參數向量p
疊代求解
由于位置表達式沒有解析解，是以在求解過程中使用複化Simpson積分求解
matlab代碼如下：
%% 複化simpon公式，n是幾等分，f是被積函數，[a,b]是積分區間
% 可變參數 第五個參數指派則輸出積分過程
function varargout = SimpsonFun(fin,a,b,n,varargin)
format long
fN = 1;
if isa(fin, 'function_handle') %f是函數句柄還是符号函數
% f is a handle
f = fin;
else
% f is not a handle
fN = numel(formula(fin)); %判斷有多少個參數
f = matlabFunction(fin,'vars',{argnames(fin)}); %将參數化為向量形式輸入，轉為句柄函數
end
fa = f(a);
fb = f(b);
S = b-a;
h = S/(2*n); %步長
Sn = {};
simpath=zeros(n+1,fN);
if nargin == 4 %求解積分結果
sum1=0;
sum2=0;
for i=0:n-1
sum1=sum1 + f(a+(2*i+1).*h);
end
for j = 1:n-1
sum2=sum2 + f(a+2*j.*h);
end
Sn = h.*( fa + 4*sum1 + 2*sum2 + fb )/3;
if isa(Sn,'double') == 0
Sn = eval(Sn);
end
elseif nargin == 5 %求解積分過程
simpath(1,:) = fa;
for i = 1:n
simpath(i+1,:) = f(a+2*(i-1).*h)+4*f(a+(2*i-1).*h)+f(a+2*i.*h);
simpath(i+1,:) = simpath(i+1,:)+simpath(i,:);
end
for j=1:fN
simpath(:,j) = h(j).*simpath(:,j)/3;
end
end
if nargin == 5
varargout{1} = simpath;
elseif nargin == 4
varargout{1} = Sn;
end
end
牛頓疊代算法
由于牛頓疊代算法出現某些點無法收斂，疊代失敗，被放棄
matlab代碼：
for n=1:20
f(s)=deltaX(p(1),p(2),p(3),s);
x_initG= xs(1)+SimpsonFun(f,0,S,100);
f(s)=deltaY(p(1),p(2),p(3),s);
y_initG=xs(2)+SimpsonFun(f,0,S,100 );
theta_G=eval(theta(p(1),p(2),p(3),S));
k_initG=k(p(1),p(2),p(3),S);
state = [x_initG y_initG theta_G];
if find(state) < 0
fprintf('估計值錯誤');
state
break;
end
dq = xe(1:3)- state; %将符号結果變為常數
err = norm(dq);
fprintf('疊代次數：%d\t error:%f\n',n,err);
if err < 1e-7
fprintf('求解成功\n');
fprintf('參數解：p1:%f p2:%f sf:%f\n',p(1),p(2),p(3));
break;
end
for i=1:size(A,1)
for j=1:size(A,2)
fun(p1,p2,sf,s)=A(i,j);
f(s)=fun(p(1),p(2),p(3),s);
intA(i,j)=SimpsonFun(f,0,S,30);
end
end
intA(1,3) = deltaX(p(1),p(2),p(3),S);
intA(2,3) = deltaY(p(1),p(2),p(3),S);
intA(3,3) = k_initG;
dp = intA\dq';
p = p + dp';
S = p(3);
if find(p(3)<=0)
fprintf('出現弧長小于0的錯誤\n');
break;
end
end
nelder mead無梯度疊代算法
matlab代碼可以參考我的部落格，下面的用于積分形式的疊代代碼結果修改
% f 是函數句柄或者是符号函數，隻接受一個 N 維行矢量作為輸入變量， 并傳回一個函數值
% x0 是 N 維行矢量， xerr 是一個标量
% 可以輸入函數方程組，求方程組的最小值
% 第四個參數為積分函數（例如int函數），fin為導數，通過積分來求取函數值
% 其他的參數為積分函數（例如int函數）的輸入參數
function [xmin, fmin] = NelderMeadInt(fin, x0, goal,xerr,varargin)
N = numel(x0); % f 是 N 元函數
x = zeros(N+1,N); % 預指派
if isa(fin, 'function_handle') %f是函數句柄還是符号函數
% f is a handle
fN = 1;
fvar = fin;
else
% f is not a handle
fN = numel(formula(fin)); %判斷有多少個參數
fvar = fin;
% fvar = matlabFunction(fin,'vars',{argnames(fin)}); %将參數化為向量形式輸入，轉為句柄函數
end
farg=argnames(fin); %擷取輸入參數向量
varin = farg(1:N); %參數值
varS = farg(N+1); %被積變量
argN=0;
f = fvar;
if nargin > 4
argN = nargin-5;
fint = varargin{1};
end
% fint(varargin{2:end});
argint={};
for i=1:argN
argint{i} = varargin{i+1};
end
% varin={x0,varargin{:}};
y = zeros(N+1,fN);
% 計算 N+1 個初始點
x(1,:) = x0;
for ii = 1:N
x(ii+1,:) = x(1,:);
if x(1,ii) == 0
x(ii+1,ii) = 0.00025;
else
x(ii+1,ii) = 1.05 * x(1,ii);
end
end
% 主循環
for kk = 1:10000
y_order=zeros(1,size(y,1));
% 求值并排序
for ii = 1 : N + 1
fx=subs(f,varin,x(ii,:));
argint{2} = x(ii,3); %S積分要更新
fs(varS)=fx;
y(ii,:) = fint(fs,argint{:})-goal;
y_order(ii) = norm(y(ii,:));
end
[~, order] = sort(y_order); %按照範數大小重新排序
y=y(order,:);
x = x(order,:);
if norm(x(N+1,:) - x(1,:)) < xerr % 判斷誤差
break;
end
m = mean(x(1:N,:)); % 平均位置
r = 2*m - x(N+1,:); % 反射點
fx=subs(f,varin,r);
argint{2} = r(3); %S積分要更新
fs(varS)=fx;
f_r = fint(fs,argint{:})-goal;
if norm(y(1,:)) <= norm(f_r) && norm(f_r) < norm(y(N,:)) % 第 4 步
x(N+1,:) = r; continue;
elseif norm(f_r) < norm(y(1,:)) % 第 5 步
s = m + 2*(m - x(N+1,:));
fx=subs(f,varin,s);
argint{2} = s(3); %S積分要更新
fs(varS)=fx;
f_s = fint(fs,argint{:})-goal;
if norm(f_s) < norm(f_r)
x(N+1,:) = s;
else
x(N+1,:) = r;
end
continue;
elseif norm(f_r) < norm(y(N+1,:)) % 第 6 步
c1 = m + (r - m)*0.5;
fx=subs(f,varin,c1);
argint{2} = c1(3); %S積分要更新
fs(varS)=fx;
f_c1 = fint(fs,argint{:})-goal;
if norm(f_c1) < norm(f_r)
x(N+1,:) = c1; continue;
end
else % 第 7 步
c2 = m + (x(N+1,:) - m)*0.5;
fx=subs(f,varin,c2);
argint{2} = c2(3); %S積分要更新
fs(varS)=fx;
f_c2 = fint(fs,argint{:})-goal;
if norm(f_c2) < norm(y(N+1,:))
x(N+1,:) = c2; continue;
end
end
for jj = 2:N+1 % 第 8 步
x(jj,:) = x(1,:) + (x(jj,:) - x(1,:))*0.5;
end
end
% 輸出變量
xmin = x(1,:);
fx=subs(f,varin,xmin);
argint{2} = xmin(3); %S積分要更新
fs(varS)=fx;
fmin = fint(fs,argint{:})-goal;
end
代碼結果：
xs = [0 0 pi/2 0]; %始端狀态
xe = [10 10 pi/2 0]; %末端狀态
%求解參數
p=[p1,p2,sf];
運作結果：
軌迹規劃結果
matlab主函數全部代碼：
clear all
clc;
syms s sf p0 p1 p2 p3
SX=1;SY=2;ST=3;SK=4;
xs = [0 0 pi/2 0]; %始端狀态
xe = [10 10 pi/2 0]; %末端狀态
dx = xe(1)-xs(1);
dy = xe(2)-xs(2);
dt = xe(3)-xs(3);
dk = xe(4)-xs(4);
p0 = xs(4); %p0 = k(0);
p3 = xe(4); %p3 = k(sf);
p = [p1 p2 sf]; %p1=k(sf/3) p2 = k(2*sf/3)
%曲線參數節點初始化，統一量綱
a = p0;
b = -( (11*p0-18*p1+9*p2-2*p3)/(2*sf));
c = 9*(2*p0-5*p1+4*p2-p3)/(2*sf^2);
d = -9*(p0-3*p1+3*p2-p3)/(2*sf^3);
k(p,s) = a+b*s+c*s^2+d*s^3;
theta(p,s) = xs(ST) + int(k,s);
deltaX = cos(theta);
deltaY = sin(theta);
dstate = [deltaX deltaY k];
jp = [p1 p2]; %積分上限s也是變量，不能對被積函數求導後再積分
jacState = jacobian(dstate,jp);
%初值估計
S = (0.2*dt*dt+1)*sqrt(dx*dx+dy*dy); %疊代初始值估計，用弦長
bs = 6*(xe(ST)-xs(ST))/(S * S) - 4*xs(SK)/S - 2*xe(SK)/S; %先将k(s)=k0+a*s+b*s^2,求解出a b 作為初始值估計
cs = 3*(xs(SK) + xe(SK))/(S * S) - 6*(xe(ST)-xe(ST))/(S * S * S);
ps = solve(b-bs,c-cs,sf-S); %求解方程組
p = eval([ps.p1 ps.p2 ps.sf]); %初始化參數向量p
A = formula(jacState);
intA=zeros(size(A,1),size(A,2)+1);
solution = 2;
%% 牛頓疊代法 目前是無法收斂，疊代失敗
if solution == 1
for n=1:20
f(s)=deltaX(p(1),p(2),p(3),s);
x_initG= xs(1)+SimpsonFun(f,0,S,100);
f(s)=deltaY(p(1),p(2),p(3),s);
y_initG=xs(2)+SimpsonFun(f,0,S,100 );
theta_G=eval(theta(p(1),p(2),p(3),S));
k_initG=k(p(1),p(2),p(3),S);
state = [x_initG y_initG theta_G];
if find(state) < 0
fprintf('估計值錯誤');
state
break;
end
dq = xe(1:3)- state; %将符号結果變為常數
err = norm(dq);
fprintf('疊代次數：%d\t error:%f\n',n,err);
if err < 1e-7
fprintf('求解成功\n');
fprintf('參數解：p1:%f p2:%f sf:%f\n',p(1),p(2),p(3));
break;
end
for i=1:size(A,1)
for j=1:size(A,2)
fun(p1,p2,sf,s)=A(i,j);
f(s)=fun(p(1),p(2),p(3),s);
intA(i,j)=SimpsonFun(f,0,S,30);
end
end
intA(1,3) = deltaX(p(1),p(2),p(3),S);
intA(2,3) = deltaY(p(1),p(2),p(3),S);
intA(3,3) = k_initG;
dp = intA\dq';
p = p + dp';
S = p(3);
if find(p(3)<=0)
fprintf('出現弧長小于0的錯誤\n');
break;
end
end
%% nelderMead 無梯度疊代法
elseif solution == 2
[xmin,fmin] = NelderMeadInt(dstate,p,xe(1:3)-xs(1:3),1e-5,@SimpsonFun,0,S,100);
p=xmin;
end
fprintf('參數:\n');
disp(p);
dpx(s) = vpa(cos(theta(p(1),p(2),p(3),s)),8);
dpy(s) = vpa(sin(theta(p(1),p(2),p(3),s)),8);
curX = xs(1)+SimpsonFun(dpx,0,p(3),100,1);
curY = xs(2)+SimpsonFun(dpy,0,p(3),100,1);
plot(curX,curY);
title('軌迹規劃');
grid on
C++ 代碼實作
代碼已經放到gitee和github上面了，求解速度比matlab快很多。
gitee: https://gitee.com/liang_rongmin/spiralsTrajactory
github: https://github.com/lrm2017/SpiralsTrajactory
辛苦寫的代碼，歡迎大家來star一下