基于彭特里亚金极小值原理求解OBVP问题以获得运动原语

Hugin&Munin

已于 2023-03-18 19:45:58 修改

阅读量300

点赞数 2

分类专栏：无人机路径规划文章标签：机器人算法

于 2023-03-18 17:57:08 首次发布

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接：https://blog.csdn.net/qq_29703971/article/details/129638988

版权

无人机路径规划专栏收录该内容

3 篇文章 1 订阅

订阅专栏

一、基本问题

通过多阶多项式可以去拟合一条无人机轨迹，通常可以使用五阶多项式，并给定初始和终末状态量通过求解方程组来计算多项式参数，从而拟合出一条轨迹。

然而，如果多项式的阶数很高，对应的自由度就会很高，需要很多条件来计算轨迹，当方程组数<自由度时，就会计算出很多条轨迹，那么如何去判定或者说计算出一条最优的轨迹，就需要通过求解OBVP问题（optimal boundary value problem）来获得一条最优轨迹。

二、基本思路

1. 设置无人机系统的状态，输入，系统模型，目标函数。

2. 设置hamilton 函数

$H(s,u,\lambda) = g(s,u) + \lambda^Tf(s,u)$

$\lambda = (\lambda_1,\lambda_2,\lambda_3)$

其中， $\lambda$ 的维度等于系统状态量的个数

3. 计算hamilton function对状态量的导数

$\dot{\lambda} = - \frac{\partial H(s^*(t),u^*(t),\lambda(t))}{\partial s}$

4. 得到的 $\dot{\lambda}$ 可以用来计算出 $\lambda(t)$

5. 计算最优的u，可以通过计算 $\underset{u(t)}{argmin}H(S^*(t),u(t),\lambda(t))$ ，直接将 $\lambda(t)$ 代入，对u(t)求导即可。

6. 计算最优的s，对前面计算的 $u^*$ 积分即可，得到的s就是轨迹的最优表达式，但是包含参数变量 $\alpha, \beta, \gamma$ 。

7. 通过起始和终末条件来计算出 $\alpha, \beta, \gamma$ ，将0和T代入s的表达式中，计算出 $\alpha, \beta, \gamma$ 。

8. 同时cost function J也变成了只与时间T相关的表达式。

9. 最后，对于给定初始和终末状态，同时给定人为希望的时间T，我们就可以计算出一条最优轨迹。

三、具体实施

1. 设置系统，（注意，这里是将系统划分为三个维度，单独解决问题，目标函数是三维的）

状态State: $s_k = (p_k,v_k,a_k)$

输入Input： $u_k = j_k$

系统模型model： $\dot{s}_k = f_s(s_k,u_k) = (v_k,a_k,j_k)$

目标函数，最小化Jerk的平方：

$J_\Sigma=\sum_{k=1}^{3}J_k,J_k = \frac{1}{T}\int_{0}^{T}j_k(t)^2dt$

2. 针对每一维，计算Hamilton 函数：

$H(s,u,\lambda) = g(s,u) + \lambda^Tf(s,u)$

$\lambda = (\lambda_1,\lambda_2,\lambda_3)$

最后，

$H(s,u,\lambda) = \frac{1}{T}j^2 +\lambda^Tf_s(s,u)$

$=\frac{1}{T}j^2+\lambda_1v+\lambda_2a+\lambda_3j$

3. 根据最小值原理，计算 $\dot{\lambda}$

$\dot{\lambda}(t) = -\bigtriangledown_sH(s^*,j^*,\lambda) = (0,-\lambda_1,-\lambda_2)$

4. 解微分方程，得到 $\lambda(t)$

$\lambda(t) = \frac{1}{T} \begin{bmatrix} -2\alpha \\ 2\alpha t+2\beta \\ -\alpha t^2-2\beta t - 2\gamma \end{bmatrix}$

5. 代入 $\lambda(t)$ 求解最优控制量u*

$H = \frac{1}{T}j^2+\begin{bmatrix} -2\alpha \\ 2\alpha t+2\beta \\ -\alpha t^2-2\beta t - 2\gamma \end{bmatrix} \begin{bmatrix} v \\a \\j \end{bmatrix}$

$\underset{u(t)}{argmin}H(s^*,u,\lambda) =\frac{1}{T}j^2+\lambda_1v+\lambda_2a+\lambda_3j$

求导

$\frac{2}{T}j+\lambda_3 = 0$

$j = -\frac{T}{2}\lambda_3$

$= -\frac{T}{2} \frac{1}{T}[-\alpha t^2 - 2\beta t - 2\gamma]$

$u^* = j=\frac{1}{2}\alpha t^2 +\beta t+\gamma$

6. 对u*积分，并且有初始状态 $s(0)= (p_0,v_0,a_0)$ ，可以得到最优状态轨迹表达式

7. 最后指定终末状态

得到

8.将参数代入到目标函数中可以得到J

这里的 $\alpha, \beta, \gamma$ 是可以被代掉的，但是终末状态的具体数值未知，因此用 $\alpha, \beta, \gamma$ 简化表示。

9. 对 $J(T)$ 进行求导，令其等于0，计算最优的T。将T代入就可以得到最优轨迹 $s^*(t)$ 。

四、极小值原理

五、求解方程

在求解 $\dot{J} = 0$ 的时候，会遇到解一元四次方程组的问题，具体解法看这篇博客。

(200条消息) 一元四次方程的求解_一元四次方程matlab 代码_古路的博客-CSDN博客

关注

2
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
基于彭特里亚金极小值原理求解OBVP问题以获得运动原语

通过多阶多项式可以去拟合一条无人机轨迹，通常可以使用五阶多项式，并给定初始和终末状态量通过求解方程组来计算多项式参数，从而拟合出一条轨迹。然而，
复制链接

扫一扫

专栏目录

评论

被折叠的条评论为什么被折叠?

到【灌水乐园】发言

查看更多评论

添加红包

成就一亿技术人!

hope_wisdom

发出的红包

实付元

使用余额支付

点击重新获取

扫码支付

钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。