庞特里亚金最小值原理求解能耗最小化的时间最优OBVP问题

数学理论 同时被 2 个专栏收录
27 篇文章 1 订阅
6 篇文章 2 订阅

本文依据论文A computationally efficient motion primitive for quadrocopter trajectory generation中所述的方法,整理出其中对OBVP问题的求解方法。所谓的OBVP即为optimal boundary value problem。

OBVP是特殊的BVP问题,最简单的BVP问题就是state sampled lattice planning,这其实也是一种lattice planner方法。常见的例子比如:给定初始位置和终点位置,求解多阶曲线方程。
在这里插入图片描述
如果这条曲线是个五阶多项式:
在这里插入图片描述
给定初始位置和终点位置a,b,则系数求解为:
在这里插入图片描述
现在讨论OBVP方法,我们给定初始位置,和重点位置的,再给定一个objective function, 比如我们论文中描述的,objective function只考虑jerk的平方。那么问题的状态和位置速度加速度相关,系统的输入为jerk = u, 状态空间即为速度,加速度,jerk的函数,总结如下:
在这里插入图片描述
对于这个问题,我们用庞特里亚金最小值原理求解:
最小值原理的基本内容:
在这里插入图片描述
先写出hamiltonian,这个g就是objective function中的transition cost, f 就是状态方程(上面的system model):
在这里插入图片描述
那么在本问题中,对应的g就是 j^2, f就是三个系统状态 a, v, j.写出如下的表达式:
在这里插入图片描述
根据上面的最小值原理基本内容:
在这里插入图片描述
我们计算lamda梯度(H分别对 p,v,a求导数,因为s是关于这三个参数的函数):
在这里插入图片描述
然后积分获得lamda表达式(这里对系数稍作了处理,是为了后面计算结果好看):
在这里插入图片描述
继续看最小值原理:
在这里插入图片描述
我们的lamda已经求解出来了,而且其中s是打星号(optimal)的,所以在H里面:
在这里插入图片描述
v和a都是知道一定是最优解的,那么只要算一下j是多少就行了。求一下一阶导数就能算出j的值,再带入H的表达式,得到:
在这里插入图片描述
对u做一次积分得到a, 在积分得到V,在积分得到p:
在这里插入图片描述

整理一下:
在这里插入图片描述
这里算逆矩阵可以自己验算一下:

%%
clear all; close all;
syms delta_p delta_v delta_a
syms T

Variants = [1/120*T^5 1/24*T^4 1/6*T^3;1/24*T^4 1/6*T^3 1/2*T^2;...
    1/6*T^3 1/2*T^2 T];

inv_Variants = inv(Variants)

结果没问题:

inv_Variants =
 
[  720/T^5, -360/T^4,  60/T^3]
[ -360/T^4,  168/T^3, -24/T^2]
[   60/T^3,  -24/T^2,     3/T]

然后得到alpha, beta, gamma后,带入objective function,
在这里插入图片描述
具体推导如下:
在这里插入图片描述

对cost function求一阶导数就知道T在什么时候可以取到minimum cost了,这里很有趣,objective function只和T相关。这也就是我们所谓的最小时间问题。

最后,还有其他终点的情况也要讨论一下,比如刚才的问题限制了终点的p,v,a,那么如果放开a的限制该如何求解。这里用到边界条件:
在这里插入图片描述
另外两个条件依旧不变,具体推导如下:
在这里插入图片描述
验证一下结果:

clear all; close all;
syms delta_p delta_v delta_a
syms T

Variants_2 = [1/120*T^5 1/24*T^4 1/6*T^3;1/24*T^4 1/6*T^3 1/2*T^2;...
    1/2*T^2 - T T-2 1 - 2/T];

inv_Variants_2 = inv(Variants_2)

结果如下:

inv_Variants_2 =
 
[  320/T^5, -120/T^4, -20/(3*(- T^2 + 2*T))]
[ -200/T^4,   72/T^3,        -8/(3*(T - 2))]
[   40/T^3,  -12/T^2,         T/(3*(T - 2))]

最后一项是0就不要管它了,所以解出表达式为:
在这里插入图片描述
其他的情况就不一一证明了:
在这里插入图片描述

  • 4
    点赞
  • 5
    评论
  • 27
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

相关推荐
©️2020 CSDN 皮肤主题: Age of Ai 设计师:meimeiellie 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值