(三)路径规划算法---OBVP例子

最新推荐文章于 2024-03-31 11:53:32 发布

月夕花晨TS

最新推荐文章于 2024-03-31 11:53:32 发布

阅读量1.2k

点赞数 11

分类专栏：路径规划人工智能文章标签：算法线性代数矩阵

本文链接：https://blog.csdn.net/qq_45369294/article/details/124760515

版权

路径规划同时被 2 个专栏收录

10 篇文章 32 订阅

订阅专栏

人工智能

10 篇文章 14 订阅

订阅专栏

路径规划算法中的OBVP例子

文章目录

路径规划算法中的OBVP例子

通过上章的OBVP的原理讲解，大家想必对该算法的流程有了大致了解，现在通过路径规划课程里面的例子，加深对OBVP的了解。

1.已知量

1.1 目标函数

$J=\int_0^T {g(s,u)dt = \int_0^T {(1 + {u^T}Ru)dt = \int_0^T {(1 + a_x^2 + a_y^2 + a_z^2)} } } dt$

目标函数表示时间也会参与整个系统的损失计算，同时矩阵 $R$ 为权重矩阵，这里为单位矩阵，向量 $u$ 为输入变量

1.2 变量

状态变量： $x=(p_x,p_y,p_z,v_x,v_y,v_z)$

输入变量： $u=(a_x,a_y,a_z)$

1.3 状态方程

状态方程如下
$\dot x=f(s,u)=\left( {\begin{matrix} {{v_x}}\\ {{v_y}}\\ {{v_z}}\\ {{a_x}}\\ {{a_y}}\\ {{a_z}} \end{matrix}} \right)$

1.4 初始值

0时刻系统的状态： $x(0)=(p_{x0},p_{y0},p_{z0},v_{x0},v_{y0},v_{z0})^T$

T时刻系统的状态：根据是否确定机器人的最终状态，OBVP有两种解法

2.固定边界条件

设定机器人的最终状态 $x(T)=(p_{xf},p_{yf},p_{zf},v_{xf},v_{yf},v_{zf})^T$ ,那么OBVP求解步骤如下

2.1 构建系统的Hamiltonian矩阵 $H$ 和协变量 $\lambda$

构造如下
$\lambda=(\lambda_1,\lambda_2,\lambda_3,\lambda_4,\lambda_5,\lambda_6)\\ H(s,u,\lambda)=g(s,u)+\lambda^Tf(s,u)\\ \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad\quad \quad \quad \quad\quad \quad \quad =(1+a_x^2+a_y^2+a_z^2)+\lambda_1v_x+\lambda_2v_y+\lambda_3v_z+\lambda_4a_x+\lambda_5a_y+\lambda_6a_z$

2.2 通过Hamiltonian矩阵对协变量进行求导

求导如下
$\dot \lambda (t) = - {\nabla _s}H({s^*}(t),{u^*}(t),\lambda (t))=(0,0,0-\lambda_1,-\lambda_2,-\lambda_3)$
由常微分方程可得协方差
$\left\{ {\begin{matrix} {{{\dot \lambda }_1} = 0}\\ {{{\dot \lambda }_2} = 0}\\ {{{\dot \lambda }_3} = 0}\\ {{\lambda _4} = - {\lambda _1}}\\ {{\lambda _5} = - {\lambda _2}}\\ {{\lambda _6} = - {\lambda _3}} \end{matrix}} \right. \Rightarrow \left\{ {\begin{matrix} {{\lambda _1} = 2{\alpha _1}}\\ {{\lambda _2} = 2{\alpha _2}}\\ {{\lambda _3} = 2{\alpha _3}}\\ {{\lambda _4} = - 2{\alpha _1}t - 2{\beta _1}}\\ {{\lambda _5} = - 2{\alpha _2}t - 2{\beta _2}}\\ {{\lambda _6} = - 2{\alpha _3}t - 2{\beta _3}} \end{matrix}} \right.$

2.3 最小化输入变量

由于
$\quad {u^*}(t) = \arg \mathop {\min }\limits_{u(t)} H({s^*}(t),u(t),\lambda (t))$
因为
$H(s^*,u,\lambda)=(1+a_x^2+a_y^2+a_z^2)+\lambda_1v_x^*+\lambda_2v_y^*+\lambda_3v_z^*+\lambda_4a_x+\lambda_5a_y+\lambda_6a_z$
对上式方程 $H$ 关于 $u$ 进行求偏导
$\left\{ {\begin{matrix} {\frac{{\partial {u^*}}}{{\partial {a_x}}} = 2{a_x} + {\lambda _4} = 0}\\ {\frac{{\partial {u^*}}}{{\partial {a_y}}} = 2{a_y} + {\lambda _5} = 0}\\ {\frac{{\partial {u^*}}}{{\partial {a_z}}} = 2{a_z} + {\lambda _6} = 0} \end{matrix}} \right. \Rightarrow \left\{ {\begin{matrix} {{a_x} = - \frac{1}{2}{\lambda _4}}\\ {{a_y} = - \frac{1}{2}{\lambda _5}}\\ {{a_z} = - \frac{1}{2}{\lambda _6}} \end{matrix}} \right.$
所以
${u^*} = \left[ {\begin{matrix} { - \frac{1}{2}{\lambda _4}}\\ { - \frac{1}{2}{\lambda _5}}\\ { - \frac{1}{2}{\lambda _6}} \end{matrix}} \right] = \left[ {\begin{matrix} {{a_1}t + {\beta _1}}\\ {{a_2}t + {\beta _2}}\\ {{a_3}t + {\beta _3}} \end{matrix}} \right] = \left[ {\begin{matrix} {a_x^*}\\ {a_y^*}\\ {a_z^*} \end{matrix}} \right]$

2.4 通过积分求得最优轨迹 $s^*$

众所周知，对加速度关于时间的一次积分，表示速度；二次积分表示位置。那么
${s^*(t)} = \left[ {\begin{matrix} {p_x^*}\\ {p_y^*}\\ {p_z^*}\\ {v_x^*}\\ {v_y^*}\\ {v_z^*} \end{matrix}} \right] = \left[ {\begin{matrix} {\int {v_x^*dt + {p_{x0}}} }\\ {\int {v_y^*dt + {p_{y0}}} }\\ {\int {v_z^*dt + {p_{z0}}} }\\ {\int {a_x^*dt + {v_{x0}}} }\\ {\int {a_y^*dt + {v_{y0}}} }\\ {\int {a_z^*dt + {v_{z0}}} } \end{matrix}} \right] = \left[ {\begin{matrix} {\frac{1}{6}{\alpha _1}{t^3} + \frac{1}{2}{\beta _1}{t^2} + {v_{x0}}t + {p_{x0}}}\\ {\frac{1}{6}{\alpha _2}{t^3} + \frac{1}{2}{\beta _2}{t^2} + {v_{y0}}t + {p_{y0}}}\\ {\frac{1}{6}{\alpha _3}{t^3} + \frac{1}{2}{\beta _2}{t^2} + {v_{z0}}t + {p_{z0}}}\\ {\frac{1}{2}{\alpha _1}{t^2} + {\beta _1}t + {v_{x0}}}\\ {\frac{1}{2}{\alpha _2}{t^2} + {\beta _2}t + {v_{y0}}}\\ {\frac{1}{2}{\alpha _3}{t^2} + {\beta _3}t + {v_{z0}}} \end{matrix}} \right]$

2.5 最终状态确定最优轨迹的参数

通过 $s^*(T)=s_f$ ,可以得出最优轨迹中的未知数 $(\alpha_1,\alpha_2,\alpha_3,\beta_1,\beta_2,\beta_3)$
$\left[ {\begin{matrix} {\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}&0&0\\ 0&{\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}&0\\ 0&0&{\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}\\ {\frac{1}{2}{T^2}}&0&0&T&0&0\\ 0&{\frac{1}{2}{T^2}}&0&0&T&0\\ 0&0&{\frac{1}{2}{T^2}}&0&0&T \end{matrix}} \right]\left[ {\begin{matrix} {{\alpha _1}}\\ {{\alpha _2}}\\ {{\alpha _3}}\\ {{\beta _1}}\\ {{\beta _2}}\\ {{\beta _3}} \end{matrix}} \right] = \left[ {\begin{matrix} {{p_{xf}} - {p_{x0}} - {v_{x0T}}}\\ {{p_{yf}} - {p_{y0}} - {v_{y0T}}}\\ {{p_{zf}} - {p_{z0}} - {v_{z0T}}}\\ {{v_{xf}} - {v_{x0}}}\\ {{v_{yf}} - {v_{y0}}}\\ {{v_{zf}} - {v_{z0}}} \end{matrix}} \right]$
推导出
$\left[ {\begin{matrix} {{\alpha _1}}\\ {{\alpha _2}}\\ {{\alpha _3}}\\ {{\beta _1}}\\ {{\beta _2}}\\ {{\beta _3}} \end{matrix}} \right] = \left[ {\begin{matrix} { - \frac{{12}}{{{T^3}}}}&0&0&{\frac{6}{{{T^2}}}}&0&0\\ 0&{ - \frac{{12}}{{{T^3}}}}&0&0&{\frac{6}{{{T^2}}}}&0\\ 0&0&{ - \frac{{12}}{{{T^3}}}}&0&0&{\frac{6}{{{T^2}}}}\\ {\frac{6}{{{T^2}}}}&0&0&{ - \frac{2}{T}}&0&0\\ 0&{\frac{6}{{{T^2}}}}&0&0&{ - \frac{2}{T}}&0\\ 0&0&{\frac{6}{{{T^2}}}}&0&0&{ - \frac{2}{T}} \end{matrix}} \right]\left[ {\begin{matrix} {{p_{xf}} - {p_{x0}} - {v_{x0T}}}\\ {{p_{yf}} - {p_{y0}} - {v_{y0T}}}\\ {{p_{zf}} - {p_{z0}} - {v_{z0T}}}\\ {{v_{xf}} - {v_{x0}}}\\ {{v_{yf}} - {v_{y0}}}\\ {{v_{zf}} - {v_{z0}}} \end{matrix}} \right]$

2.6 最优状态下目标函数

将上述所得到的最优输入变量 $u^*$ 代入到目标函数中，那么
${J^*}(T) = \int_0^T {(1 + a_x^2 + a_y^2 + a_z^2)dt} \\ \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad=\int_0^T {[1 + {{({\alpha _1}t + {\beta _{}})}^2} + {{({\alpha _2}t + {\beta _2})}^2} + {{({\alpha _3}t + {\beta _3})}^2}]} dt \\ \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad = T + \int_0^T {{{({\alpha _1}t + {\beta _1})}^2}dt} + \int_0^T {{{({\alpha _1}t + {\beta _2})}^2}dt} + \int_0^T {{{({\alpha _1}t + {\beta _2})}^2}dt} \\ \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad= (\frac{1}{3}\alpha _1^2 + \frac{1}{3}\alpha _2^2 + \frac{1}{3}\alpha _3^2){T^3} + ({a_1}{\beta _1} + {a_2}{\beta _2} + {a_3}{\beta _3}){T^2} + (\beta _1^2 + \beta _2^2 + \beta _3^2)T$
将上式所得到 $(\alpha_1,\alpha_2,\alpha_3,\beta_1,\beta_2,\beta_3)$ 代入目标函数中，得知目标函数仅关于T有关，由于计算过程复杂，此时可有matlab代替人工完成

syms T
syms px0 py0 pz0 vx0 vy0 vz0 pxf pyf pzf vxf vyf vzf 

H=[-12/T^3      0        0     6/T^2     0        0;
     0        -12/T^3    0       0      6/T^2     0;
     0           0    -12/T^3    0       0      6/T^2 ;
   6/T^2         0       0      -2/T     0        0;
     0         6/T^2     0       0     -2/T       0;
     0           0     6/T^2     0       0       -2/T;];
DeltaPV=[pxf-vx0*T-px0;
         pyf-vy0*T-py0;
         pzf-vz0*T-pz0;
         vxf-vx0;
         vyf-vy0;
         vzf-vz0
        ];
Param=H*DeltaPV;

alpha1=Param(1);
alpha2=Param(2);
alpha3=Param(3);
beta1=Param(4);
beta2=Param(5);
beta3=Param(6);

J=T+(1/3*alpha1^2*T^3+alpha1*beta1*T^2+beta1^2*T) ...
   +(1/3*alpha2^2*T^3+alpha2*beta2*T^2+beta2^2*T) ...
   +(1/3*alpha3^2*T^3+alpha3*beta3*T^2+beta3^2*T);

disp("J方程：")
disp(J)

dJdT=diff(J,T);
disp("dJ/dT方程：")
disp(dJdT)

%latex()转化为mathtype格式
dJdT=simplify(dJdT);
dJdT=collect(dJdT,T);
disp("化简dJdT：")
disp(dJdT)

[I,D]=numden(dJdT);
disp("分子：")
I=collect(I,T);
disp(I)
disp("分母：")
disp(D)

matlab显示结果
在这里插入图片描述
其中图片中的dJdT表示目标函数对时间 $T$ 进行求导，然后根据上章的根据矩阵的特征值求解多项式根的方法，求解最优的时间T

3.自由边界条件

此时并不完全知道机器人最终的状态信息，假设我们只约束了位置信息，而不约束速度信息，即 $s_f=(p_{xf},p_{yf},p_{zf},?,?,?)^T$ ,那么可通过Pontrayagin’s 最小值的边界条件 $\lambda(T)=- {\nabla}h(s^{*}(T))$ 进行对未知参数的计算,但是本系统并没有设置最终状态的惩罚项 $h$ ，可认为 $h = 0$ ,因此

位置信息没有改变，依然如下：
$\left[ {\begin{matrix} {\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}&0&0\\ 0&{\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}&0\\ 0&0&{\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}\\ \end{matrix}} \right]\left[ {\begin{matrix} {{\alpha _1}}\\ {{\alpha _2}}\\ {{\alpha _3}}\\ \end{matrix}} \right] = \left[ {\begin{matrix} {{p_{xf}} - {p_{x0}} - {v_{x0T}}}\\ {{p_{yf}} - {p_{y0}} - {v_{y0T}}}\\ {{p_{zf}} - {p_{z0}} - {v_{z0T}}}\\ \end{matrix}} \right]$
速度信息发生变化，如下：
$\left[ {\begin{matrix} {{\lambda _4}}\\ {{\lambda _5}}\\ {{\lambda _6}} \end{matrix}} \right] = \left[ {\begin{matrix} { - 2{\alpha _1}T - 2{\beta _1}}\\ { - 2{\alpha _2}T - 2{\beta _2}}\\ { - 2{\alpha _3}T - 2{\beta _3}} \end{matrix}} \right] = 0$

因此，新的最优轨迹 $s^*$ 的未知参数表达式如下
$\left[ {\begin{matrix} {\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}&0&0\\ 0&{\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}&0\\ 0&0&{\frac{1}{6}{T^3}}&0&0&{\frac{1}{2}{T^2}}\\ T&0&0&1&0&0\\ 0&T&0&0&1&0\\ 0&0&T&0&0&1 \end{matrix}} \right]\left[ {\begin{matrix} {{\alpha _1}}\\ {{\alpha _2}}\\ {{\alpha _3}}\\ {{\beta _1}}\\ {{\beta _2}}\\ {{\beta _3}} \end{matrix}} \right] = \left[ {\begin{matrix} {{p_{xf}} - {p_{x0}} - {v_{x0T}}}\\ {{p_{yf}} - {p_{y0}} - {v_{y0T}}}\\ {{p_{zf}} - {p_{z0}} - {v_{z0T}}}\\ 0\\ 0\\ 0 \end{matrix}} \right]$
推导如下
$\left[ {\begin{matrix} {{\alpha _1}}\\ {{\alpha _2}}\\ {{\alpha _3}}\\ {{\beta _1}}\\ {{\beta _2}}\\ {{\beta _3}} \end{matrix}} \right] = \left[ {\begin{matrix} { - \frac{3}{{{T^3}}}}&0&0&{\frac{3}{{2T}}}&0&0\\ 0&{ - \frac{3}{{{T^3}}}}&0&0&{\frac{3}{{2T}}}&0\\ 0&0&{ - \frac{3}{{{T^3}}}}&0&0&{\frac{3}{{2T}}}\\ {\frac{3}{{{T^2}}}}&0&0&{ - \frac{1}{2}}&0&0\\ 0&{\frac{3}{{{T^2}}}}&0&0&{ - \frac{1}{2}}&0\\ 0&0&{\frac{3}{{{T^2}}}}&0&0&{ - \frac{1}{2}} \end{matrix}} \right]\left[ {\begin{matrix} {{p_{xf}} - {p_{x0}} - {v_{x0T}}}\\ {{p_{yf}} - {p_{y0}} - {v_{y0T}}}\\ {{p_{zf}} - {p_{z0}} - {v_{z0T}}}\\ 0\\ 0\\ 0 \end{matrix}} \right]$

matlab代码化简目标函数如下

syms T
syms px0 py0 pz0 vx0 vy0 vz0 pxf pyf pzf vxf vyf vzf 

H=[-3/T^3      0        0     3/(2*T)     0        0;
     0        -3/T^3    0       0      3/(2*T)     0;
     0           0    -3/T^3    0       0      3/(2*T) ;
   3/T^2         0       0      -1/2     0        0;
     0         3/T^2     0       0     -1/2       0;
     0           0     3/T^2     0       0       -1/2;];
DeltaPV=[pxf-px0-vx0*T;
         pyf-py0-vy0*T;
         pzf-pz0-vz0*T;
         0;
         0;
         0;
        ];
Param=H*DeltaPV;

alpha1=Param(1);
alpha2=Param(2);
alpha3=Param(3);
beta1=Param(4);
beta2=Param(5);
beta3=Param(6);

J=T+(1/3*alpha1^2*T^3+alpha1*beta1*T^2+beta1^2*T) ...
   +(1/3*alpha2^2*T^3+alpha2*beta2*T^2+beta2^2*T) ...
   +(1/3*alpha3^2*T^3+alpha3*beta3*T^2+beta3^2*T);
% J=(1/3*alpha1^2+1/3*alpha2^2+1/3*alpha3^2)*T^3 ...
%    +(alpha1*beta1+alpha1*beta1+alpha1*beta1)*T^2 ...
%    +(beta1^2+beta2^2+beta3^2+1)*T;
disp("J方程：")
disp(J)

dJdT=diff(J,T);
disp("dJ/dT方程：")
disp(dJdT)

%latex()转化为mathtype格式
dJdT=simplify(dJdT);
dJdT=collect(dJdT,T);
disp("化简dJdT：")
disp(dJdT)

[I,D]=numden(dJdT);
disp("分子：")
I=collect(I,T);
disp(I)
disp("分母：")
disp(D)

显示结果如下
在这里插入图片描述
其中图片中的dJdT表示目标函数对时间 $T$ 进行求导，然后根据上章的根据矩阵的特征值求解多项式根的方法，求解最优的时间T

4 .小结

OBVP基本步骤如下

1.确定系统的已知量，例如目标函数、系统变量、系统状态方程、以及初始值

2.根据初始值中的最终状态是否明确边界条件，可分为固定边界和自由边界

2.1 固定边界

(1) 先构建系统的Hamiltonian矩阵 $H$ 和协变量 $\lambda$

(2) 通过Hamiltonian矩阵对协变量进行求导

(3) 最小化输入变量

(4) 积分确定最优轨迹

(5) 最终状态信息确定系统的未知参数

(6) 求解目标函数的极值，以确定系统的最优时间

2.2 自由边界
（1) 先构建系统的Hamiltonian矩阵 $H$ 和协变量 $\lambda$

(2) 通过Hamiltonian矩阵对协变量进行求导

(3) 最小化输入变量

(4) 积分确定最优轨迹

(5) 通过Pontrayagin’s 最小值边界条件确定系统的未知参数

(6) 求解目标函数的极值，以确定系统的最优时间

本文参考：https://blog.csdn.net/m0_51238303/article/details/123384256
其中，matlab代码也是参考网上的一位博主的，不过我不知道他的链接在哪，尴尬，知道的小伙伴，可以告诉我一声。最后，码字不容易，转载请注明出处，谢谢。

月夕花晨TS

关注

11
点赞
踩
22

收藏

觉得还不错? 一键收藏
0
评论
(三)路径规划算法---OBVP例子

路径规划算法中的OBVP例子文章目录路径规划算法中的OBVP例子1.已知量1.1 目标函数1.2 变量1.3 状态方程1.4 初始值2.固定边界条件2.1 构建系统的Hamiltonian矩阵HHH和协变量λ\lambdaλ2.2 通过Hamiltonian矩阵对协变量进行求导2.3 最小化输入变量2.4 通过积分求得最优轨迹s∗s^*s∗2.5 最终状态确定最优轨迹的参数2.6 最优状态下目标函数3.自由边界条件4 .小结通过上章的OBVP的原理讲解，大家想必对该算法的流程有了大致了解，现在通过路径规
复制链接

扫一扫