问题描述
OBVP 即 optimal bundary value problem，是特殊的 BVP， BVP 问题其实就是解决 state sampled lattice planning 的基本操作方法。
常见的应用案例如：给定初始位置和终点位置，求连接两点的多阶曲线方程。
- 已知边界条件如下
| Position | Velocity | Acceleration | |
|---|---|---|---|
| t = 0 | a | 0 | 0 |
| t = T | b | 0 | 0 |
-
求五次多项式轨迹
- x ( t ) = c 5 t 5 + c 4 t 4 + c 3 t 3 + c 2 t 2 + c 1 t + c 0 x(t)=c_5 t^5 + c_4 t^4 + c_3 t^3 + c_2 t^2 + c_1 t + c_0 x(t)=c5t5+c4t4+c3t3+c2t2+c1t+c0
-
求解
- [ a b 0 0 0 0 ] = [ 0 0 0 0 0 1 T 5 T 4 T 3 T 2 T 1 0 0 0 0 1 0 5 T 4 4 T 3 3 T 2 2 T 1 0 0 0 0 2 0 0 20 T 3 12 T 2 6 T 2 0 0 ] [ c 5 c 4 c 3 c 2 c 1 c 0 ] \begin{bmatrix} a\\b\\0\\0\\0\\0 \end{bmatrix} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ T^5 & T^4 & T^3 & T^2 & T & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0\end{bmatrix} \begin{bmatrix} c_5 \\ c_4 \\ c_3 \\ c_2 \\ c_1 \\ c_0 \end{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡ab0000⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡0T505T4020T30T404T3012T20T303T206T0T202T220T1100110000⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡c5c4c3c2c1c0⎦⎥⎥⎥⎥⎥⎥⎤
那么对于 OBVP 问题，已经有成熟的解法，本文主要说明借助庞特里亚金最小值原理 (Pontryagin’s minimum principle) 求解 BVP 问题的方法。
对于如下所示的优化问题：
-
a
r
g
m
i
n
J
=
h
(
s
(
T
)
)
+
∫
0
T
g
(
s
(
t
)
,
u
(
t
)
)
⋅
d
t
arg\ min \ J=h(s(T))+\int_0^Tg(s(t),u(t))\cdot dt
arg min J=h(s(T))+∫0Tg(s(t),u(t))⋅dt
- 该优化问题的代价由两部分组成，分别为对最终状态的惩罚项和过程惩罚项
- 写出其 Hamiltonian 和 costate(协变量)
- H ( s , u , λ ) = g ( s , u ) + λ T f ( s , u ) H(s,u,\lambda)=g(s,u)+\lambda^Tf(s,u) H(s,u,λ)=g(s,u)+λTf(s,u)
- λ \lambda λ 为协变量，维数与系统模型 f ( s , u ) f(s,u) f(s,u) 相关
- 假设最优解
- s ∗ s^* s∗ ：最优状态
- u ∗ u^* u∗ ：最优控制（输入）
- 则满足以下最优性条件
- s ∗ ( t ) = f ( s ∗ ( t ) , u ∗ ( t ) ) , g i v e n : s ∗ ( 0 ) = s ( 0 ) s^*(t)=f(s^*(t),u^*(t)),given:s^*(0)=s(0) s∗(t)=f(s∗(t),u∗(t)),given:s∗(0)=s(0)
-
λ
(
t
)
\lambda(t)
λ(t) 是微分方程的解
- λ ˙ ( t ) = − ▽ s H ( s ∗ ( t ) , u ∗ ( t ) , λ ( t ) ) \dot\lambda(t)=-\triangledown_sH(s^*(t),u^*(t),\lambda(t)) λ˙(t)=−▽sH(s∗(t),u∗(t),λ(t))
- λ ( T ) = − ▽ h ( s ∗ ( T ) ) \lambda(T)=-\triangledown h(s^*(T)) λ(T)=−▽h(s∗(T))
- 最优控制
- u ∗ ( t ) = arg min u ( t ) H ( s ∗ ( t ) , u ( t ) , λ ( t ) ) u^*(t)=\mathop{\arg\min}\limits_{u(t)}H(s^*(t),u(t),\lambda(t)) u∗(t)=u(t)argminH(s∗(t),u(t),λ(t))
通常求解最优性条件即可得到最优解。但是在处理问题的过程中，会遇到不同的边界条件情况，比如固定边界条件和自由边界条件，下面将分别对这两类问题展开说明，并附上自由边界条件演示示意图
固定边界条件
- 建模
- 目标，最小化时间间隔和加速度
- J = ∫ 0 T g ( x , u ) d t = ∫ 0 T ( 1 + u T R u ) d t = ∫ 0 T ( 1 + a x 2 + a y 2 + a z 2 ) d t J=\int_0^Tg(x,u)dt=\int_0^T(1+u^TRu)dt=\int_0^T(1+a_x^2+a_y^2+a_z^2)dt J=∫0Tg(x,u)dt=∫0T(1+uTRu)dt=∫0T(1+ax2+ay2+az2)dt
- 系统方程
- x = ( p x , p y , p z , v x , v y , v z ) T x=(p_x,p_y,p_z,v_x,v_y,v_z)^T x=(px,py,pz,vx,vy,vz)T
- u = ( a x , a y , a z ) T u=(a_x,a_y,a_z)^T u=(ax,ay,az)T
- x ˙ = f ( x , u ) = ( v x , v y , v z , a x , a y , a z ) T \dot{x}=f(x,u)=(v_x,v_y,v_z,a_x,a_y,a_z)^T x˙=f(x,u)=(vx,vy,vz,ax,ay,az)T
- 目标，最小化时间间隔和加速度
- 求解
- 协变量 λ = ( λ 1 , λ 2 , λ 3 , λ 4 , λ 5 , λ 6 ) T \lambda=(\lambda_1,\lambda_2,\lambda_3,\lambda_4,\lambda_5,\lambda_6)^T λ=(λ1,λ2,λ3,λ4,λ5,λ6)T
- 定义 Hamiltonian 函数
- H ( x , u , λ ) = g ( x , u ) + λ T f ( x , u ) = ( 1 + a x 2 + a y 2 + a z 2 ) + λ T f ( x , u ) H(x,u,\lambda)=g(x,u)+\lambda^Tf(x,u)=(1+a_x^2+a_y^2+a_z^2)+\lambda^Tf(x,u) H(x,u,λ)=g(x,u)+λTf(x,u)=(1+ax2+ay2+az2)+λTf(x,u)
- λ ˙ = − ▽ H ( x ∗ , u ∗ , λ ) = ( 0 , 0 , 0 , − λ 1 , − λ 2 , − λ 3 ) T \dot{\lambda}=-\triangledown H(x^*,u^*,\lambda)=(0,0,0,-\lambda_1,-\lambda_2,-\lambda_3)^T λ˙=−▽H(x∗,u∗,λ)=(0,0,0,−λ1,−λ2,−λ3)T
- 则可以求解出协变量，引入常数
α
,
β
\alpha,\beta
α,β
- λ = [ 2 α 1 2 α 2 2 α 3 − 2 α 1 t − 2 β 1 − 2 α 2 t − 2 β 2 − 2 α 3 t − 2 β 3 ] \lambda=\begin{bmatrix} 2\alpha_1 \\ 2\alpha_2 \\ 2\alpha_3 \\ -2\alpha_1t-2\beta_1 \\ -2\alpha_2t-2\beta_2 \\ -2\alpha_3t-2\beta_3 \end{bmatrix} λ=⎣⎢⎢⎢⎢⎢⎢⎡2α12α22α3−2α1t−2β1−2α2t−2β2−2α3t−2β3⎦⎥⎥⎥⎥⎥⎥⎤
- 进一步的可以求解最优输入
- u ∗ = arg min a ( t ) H ( x ∗ ( t ) , u ( t ) , λ ( t ) ) = [ α 1 t + β 1 α 2 t + β 2 α 3 t + β 3 ] u^*=\mathop{\arg\min}\limits_{a(t)}H(x^*(t),u(t),\lambda(t))=\begin{bmatrix} \alpha_1t+\beta_1 \\ \alpha_2t+\beta_2 \\ \alpha_3t+\beta_3 \end{bmatrix} u∗=a(t)argminH(x∗(t),u(t),λ(t))=⎣⎡α1t+β1α2t+β2α3t+β3⎦⎤
- 那么最优状态（轨迹）如下
- x ∗ = [ 1 6 α 1 t 3 + 1 2 β 1 t 2 + v x 0 t + p x 0 1 6 α 2 t 3 + 1 2 β 2 t 2 + v y 0 t + p y 0 1 6 α 3 t 3 + 1 2 β 3 t 2 + v z 0 t + p z 0 1 2 α 1 t 2 + β 1 t + v x 0 1 2 α 2 t 2 + β 2 t + v y 0 1 2 α 3 t 2 + β 3 t + v z 0 ] , i n i t i a l s t a t e : x ( 0 ) = [ p x 0 p y 0 p z 0 v x 0 v y 0 v z 0 ] x^*=\begin{bmatrix} \frac{1}{6}\alpha_1t^3+\frac{1}{2}\beta_1t^2+v_{x0}t+p_{x0} \\ \frac{1}{6}\alpha_2t^3+\frac{1}{2}\beta_2t^2+v_{y0}t+p_{y0} \\ \frac{1}{6}\alpha_3t^3+\frac{1}{2}\beta_3t^2+v_{z0}t+p_{z0} \\ \frac{1}{2}\alpha_1t^2+\beta_1t+v_{x0} \\ \frac{1}{2}\alpha_2t^2+\beta_2t+v_{y0} \\ \frac{1}{2}\alpha_3t^2+\beta_3t+v_{z0} \end{bmatrix},\ initial\ state:x(0)=\begin{bmatrix} p_{x0} \\ p_{y0} \\ p_{z0} \\ v_{x0} \\ v_{y0} \\ v_{z0} \end{bmatrix} x∗=⎣⎢⎢⎢⎢⎢⎢⎡61α1t3+21β1t2+vx0t+px061α2t3+21β2t2+vy0t+py061α3t3+21β3t2+vz0t+pz021α1t2+β1t+vx021α2t2+β2t+vy021α3t2+β3t+vz0⎦⎥⎥⎥⎥⎥⎥⎤, initial state:x(0)=⎣⎢⎢⎢⎢⎢⎢⎡px0py0pz0vx0vy0vz0⎦⎥⎥⎥⎥⎥⎥⎤
- 根据末状态边界条件可以求解
α
,
β
\alpha,\beta
α,β
- [ 1 6 T 3 0 0 1 2 T 2 0 0 0 1 6 T 3 0 0 1 2 T 2 0 0 0 1 6 T 3 0 0 1 2 T 2 1 2 T 2 0 0 T 0 0 0 1 2 T 2 0 0 T 0 0 0 1 2 T 2 0 0 T ] [ α 1 α 2 α 3 β 1 β 2 β 3 ] = [ p x f − p x 0 − v x 0 T p y f − p y 0 − v y 0 T p z f − p z 0 − v z 0 T v x f − v x 0 v y f − v y 0 v z f − v z 0 ] \begin{bmatrix} \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{bmatrix} \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \beta_1 \\ \beta_2 \\ \beta_3 \end{bmatrix}=\begin{bmatrix} p_{xf}-p_{x0}-v_{x0}T \\ p_{yf}-p_{y0}-v_{y0}T \\ p_{zf}-p_{z0}-v_{z0}T \\ v_{xf}-v_{x0} \\ v_{yf}-v_{y0} \\ v_{zf}-v_{z0} \end{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡61T30021T200061T30021T200061T30021T221T200T00021T200T00021T200T⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡α1α2α3β1β2β3⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡pxf−px0−vx0Tpyf−py0−vy0Tpzf−pz0−vz0Tvxf−vx0vyf−vy0vzf−vz0⎦⎥⎥⎥⎥⎥⎥⎤
- [ α 1 α 2 α 3 β 1 β 2 β 3 ] = [ − 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 ] [ p x f − p x 0 − v x 0 T p y f − p y 0 − v y 0 T p z f − p z 0 − v z 0 T v x f − v x 0 v y f − v y 0 v z f − v z 0 ] \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \beta_1 \\ \beta_2 \\ \beta_3 \end{bmatrix}=\begin{bmatrix} -\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{bmatrix}\begin{bmatrix} p_{xf}-p_{x0}-v_{x0}T \\ p_{yf}-p_{y0}-v_{y0}T \\ p_{zf}-p_{z0}-v_{z0}T \\ v_{xf}-v_{x0} \\ v_{yf}-v_{y0} \\ v_{zf}-v_{z0} \end{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡α1α2α3β1β2β3⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡−T31200T26000−T31200T26000−T31200T26T2600−T2000T2600−T2000T2600−T2⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡pxf−px0−vx0Tpyf−py0−vy0Tpzf−pz0−vz0Tvxf−vx0vyf−vy0vzf−vz0⎦⎥⎥⎥⎥⎥⎥⎤
- 计算代价（最优值）
- J = ∫ 0 T ( 1 + a x 2 + a y 2 + a z 2 ) d t J=\int_0^T(1+a_x^2+a_y^2+a_z^2)dt J=∫0T(1+ax2+ay2+az2)dt
- 将求解的最优输入 a = u ∗ = [ α 1 t + β 1 α 2 t + β 2 α 3 t + β 3 ] a=u^*=\begin{bmatrix} \alpha_1t+\beta_1 \\ \alpha_2t+\beta_2 \\ \alpha_3t+\beta_3 \end{bmatrix} a=u∗=⎣⎡α1t+β1α2t+β2α3t+β3⎦⎤ 带入上式，得到
- J = T + ( 1 3 α 1 2 T 3 + α 1 β 1 T 2 + β 1 2 T ) + ( 1 3 α 2 2 T 3 + α 2 β 2 T 2 + β 2 2 T ) + ( 1 3 α 3 2 T 3 + α 3 β 3 T 2 + β 3 2 T ) J=T+(\frac{1}{3}\alpha_1^2T^3+\alpha_1\beta_1T^2+\beta_1^2T)+(\frac{1}{3}\alpha_2^2T^3+\alpha_2\beta_2T^2+\beta_2^2T)+(\frac{1}{3}\alpha_3^2T^3+\alpha_3\beta_3T^2+\beta_3^2T) J=T+(31α12T3+α1β1T2+β12T)+(31α22T3+α2β2T2+β22T)+(31α32T3+α3β3T2+β32T)
- 最终 J J J 的只和 T T T 相关，则可以求出最优的 T ∗ = arg min T J ( T ) T^*=\mathop{\arg\min}\limits_{T}\ J(T) T∗=Targmin J(T) ，然后可求出 α , β \alpha,\beta α,β，进一步的可以求出 x ∗ ( t ) , u ∗ ( t ) x^*(t),u^*(t) x∗(t),u∗(t)
自由边界条件
在上述求解过程中，我们已知了初始状态 x 0 x_0 x0 和末状态 x f x_f xf ，但是如果对于末状态，我们只约束位置，而不约束速度，即末状态条件为 x f = ( p x f , p y f , p z f ) T x_f=(p_{xf},p_{yf},p_{zf})^T xf=(pxf,pyf,pzf)T，那么上述问题又该如何求解呢？
-
其实在根据末状态边界条件可以求解 α , β \alpha,\beta α,β 之前的操作都是相同的，不在赘述，在求解 α , β \alpha,\beta α,β 时，位置条件不变，仍然如下：
- [ 1 6 T 3 0 0 1 2 T 2 0 0 0 1 6 T 3 0 0 1 2 T 2 0 0 0 1 6 T 3 0 0 1 2 T 2 ] [ α 1 α 2 α 3 β 1 β 2 β 3 ] = [ p x f − p x 0 − v x 0 T p y f − p y 0 − v y 0 T p z f − p z 0 − v z 0 T ] \begin{bmatrix} \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{bmatrix} \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \beta_1 \\ \beta_2 \\ \beta_3 \end{bmatrix}=\begin{bmatrix} p_{xf}-p_{x0}-v_{x0}T \\ p_{yf}-p_{y0}-v_{y0}T \\ p_{zf}-p_{z0}-v_{z0}T \end{bmatrix} ⎣⎡61T300061T300061T321T200021T200021T2⎦⎤⎣⎢⎢⎢⎢⎢⎢⎡α1α2α3β1β2β3⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎡pxf−px0−vx0Tpyf−py0−vy0Tpzf−pz0−vz0T⎦⎤
- 现在需要将速度边界条件修改为自由边界条件，此时将用上这个条件 λ ( T ) = − ▽ h ( s ∗ ( T ) ) \lambda(T)=-\triangledown h(s^*(T)) λ(T)=−▽h(s∗(T))
- 由于代价函数中不存在
h
(
s
(
T
)
)
h(s(T))
h(s(T)) 项，所以
- [ λ 4 ( T ) λ 5 ( T ) λ 6 ( T ) ] = [ − 2 α 1 T − 2 β 1 − 2 α 2 T − 2 β 2 − 2 α 3 T − 2 β 3 ] = 0 \begin{bmatrix} \lambda_4(T) \\ \lambda_5(T) \\ \lambda_6(T) \end{bmatrix}=\begin{bmatrix} -2\alpha_1T-2\beta_1 \\ -2\alpha_2T-2\beta_2 \\ -2\alpha_3T-2\beta_3 \end{bmatrix}=0 ⎣⎡λ4(T)λ5(T)λ6(T)⎦⎤=⎣⎡−2α1T−2β1−2α2T−2β2−2α3T−2β3⎦⎤=0
- 因此总的边界条件如下所示
- [ 1 6 T 3 0 0 1 2 T 2 0 0 0 1 6 T 3 0 0 1 2 T 2 0 0 0 1 6 T 3 0 0 1 2 T 2 T 0 0 1 0 0 0 T 0 0 1 0 0 0 T 0 0 1 ] [ α 1 α 2 α 3 β 1 β 2 β 3 ] = [ p x f − p x 0 − v x 0 T p y f − p y 0 − v y 0 T p z f − p z 0 − v z 0 T 0 0 0 ] \begin{bmatrix} \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{bmatrix} \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \beta_1 \\ \beta_2 \\ \beta_3 \end{bmatrix}=\begin{bmatrix} p_{xf}-p_{x0}-v_{x0}T \\ p_{yf}-p_{y0}-v_{y0}T \\ p_{zf}-p_{z0}-v_{z0}T \\ 0 \\ 0 \\ 0 \end{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡61T300T00061T300T00061T300T21T200100021T200100021T2001⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡α1α2α3β1β2β3⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡pxf−px0−vx0Tpyf−py0−vy0Tpzf−pz0−vz0T000⎦⎥⎥⎥⎥⎥⎥⎤
- [ α 1 α 2 α 3 β 1 β 2 β 3 ] = [ − 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 ] [ p x f − p x 0 − v x 0 T p y f − p y 0 − v y 0 T p z f − p z 0 − v z 0 T 0 0 0 ] \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \beta_1 \\ \beta_2 \\ \beta_3 \end{bmatrix}=\begin{bmatrix} -\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{bmatrix}\begin{bmatrix} p_{xf}-p_{x0}-v_{x0}T \\ p_{yf}-p_{y0}-v_{y0}T \\ p_{zf}-p_{z0}-v_{z0}T \\ 0 \\ 0 \\ 0 \end{bmatrix} ⎣⎢⎢⎢⎢⎢⎢⎡α1α2α3β1β2β3⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡−T3300T23000−T3300T23000−T3300T232T300−210002T300−210002T300−21⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡pxf−px0−vx0Tpyf−py0−vy0Tpzf−pz0−vz0T000⎦⎥⎥⎥⎥⎥⎥⎤
-
计算代价（最优值）与上述过程一样，这里对最优值进行下深入计算
- J = T + ( 1 3 α 1 2 T 3 + α 1 β 1 T 2 + β 1 2 T ) + ( 1 3 α 2 2 T 3 + α 2 β 2 T 2 + β 2 2 T ) + ( 1 3 α 3 2 T 3 + α 3 β 3 T 2 + β 3 2 T ) J=T+(\frac{1}{3}\alpha_1^2T^3+\alpha_1\beta_1T^2+\beta_1^2T)+(\frac{1}{3}\alpha_2^2T^3+\alpha_2\beta_2T^2+\beta_2^2T)+(\frac{1}{3}\alpha_3^2T^3+\alpha_3\beta_3T^2+\beta_3^2T) J=T+(31α12T3+α1β1T2+β12T)+(31α22T3+α2β2T2+β22T)+(31α32T3+α3β3T2+β32T)
- 由于 [ − 2 α 1 T − 2 β 1 − 2 α 2 T − 2 β 2 − 2 α 3 T − 2 β 3 ] = 0 \begin{bmatrix} -2\alpha_1T-2\beta_1 \\ -2\alpha_2T-2\beta_2 \\ -2\alpha_3T-2\beta_3 \end{bmatrix}=0 ⎣⎡−2α1T−2β1−2α2T−2β2−2α3T−2β3⎦⎤=0
- J = T + 1 3 α 1 2 T 3 + 1 3 α 2 2 T 3 + 1 3 α 3 2 T 3 J=T+\frac{1}{3}\alpha_1^2T^3+\frac{1}{3}\alpha_2^2T^3+\frac{1}{3}\alpha_3^2T^3 J=T+31α12T3+31α22T3+31α32T3
- 记 Δ p = p f − p 0 \Delta p=p_f-p_0 Δp=pf−p0 ，则有 [ α 1 α 2 α 3 ] = [ − 3 ( Δ p x − v x 0 T ) T 3 − 3 ( Δ p y − v y 0 T ) T 3 − 3 ( Δ p z − v z 0 T ) T 3 ] \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \end{bmatrix}=\begin{bmatrix} \frac{-3(\Delta p_x-v_{x0}T)}{T^3} \\ \frac{-3(\Delta p_y-v_{y0}T)}{T^3} \\ \frac{-3(\Delta p_z-v_{z0}T)}{T^3} \end{bmatrix} ⎣⎡α1α2α3⎦⎤=⎣⎢⎡T3−3(Δpx−vx0T)T3−3(Δpy−vy0T)T3−3(Δpz−vz0T)⎦⎥⎤
- 可得 J = T + 3 [ ( v x 0 2 + v y 0 2 + v z 0 2 ) T 2 − 2 ( Δ p x v x 0 + Δ p y v y 0 + Δ p z v z 0 ) T + ( Δ p x 2 + Δ p y 2 + Δ p z 2 ) ] T 3 J=T+\frac{3[(v_{x0}^2+v_{y0}^2+v_{z0}^2)T^2-2(\Delta{p_x}v_{x0}+\Delta{p_y}v_{y0}+\Delta{p_z}v_{z0})T+(\Delta{p_x}^2+\Delta{p_y}^2+\Delta{p_z}^2)]}{T^3} J=T+T33[(vx02+vy02+vz02)T2−2(Δpxvx0+Δpyvy0+Δpzvz0)T+(Δpx2+Δpy2+Δpz2)]
- 记，
- a = v x 0 2 + v y 0 2 + v z 0 2 a=v_{x0}^2+v_{y0}^2+v_{z0}^2 a=vx02+vy02+vz02
- b = Δ p x v x 0 + Δ p y v y 0 + Δ p z v z 0 b=\Delta{p_x}v_{x0}+\Delta{p_y}v_{y0}+\Delta{p_z}v_{z0} b=Δpxvx0+Δpyvy0+Δpzvz0
- c = Δ p x 2 + Δ p y 2 + Δ p z 2 c=\Delta{p_x}^2+\Delta{p_y}^2+\Delta{p_z}^2 c=Δpx2+Δpy2+Δpz2
- 求 ∂ J ∂ T = T 4 − 3 a T 2 + 12 b T − 9 c T 4 = 0 \frac{\partial{J}}{\partial{T}}=\frac{T^4-3aT^2+12bT-9c}{T^4}=0 ∂T∂J=T4T4−3aT2+12bT−9c=0 ，即求一元四次方程 T 4 − 3 a T 2 + 12 b T − 9 c = 0 T^4-3aT^2+12bT-9c=0 T4−3aT2+12bT−9c=0 的“正实根”，在c++中可使用“求多项式伴随矩阵特征值”的方法进行求解
-
c++代码示例
// 该函数用于计算三维空间中的 OBVP 问题 // 输入：起点的位置、速度，终点的位置 // 计算最优解 T，以及最优值 J // 最优解 T 用于计算最优轨迹 (s*(t), u*(t)) // 最优值 J 用于评估轨迹的总代价 double OBVPtool::OptimalBVP(Eigen::Vector3d _start_position, Eigen::Vector3d _start_velocity, Eigen::Vector3d _target_position, double* _T) { double optimal_cost = 100000; Vector3d _delta_pos = _target_position - _start_position; // solving T^4 - 3*(vx0^2 + vy0^2 + vz0^2) * T^2 + 12 * (dx * vx0 + dy * vy0 + dz * vz0) * T - 9 * (dx^2 + dy^2 +dz^2) double c0 = -9.0 * _delta_pos.squaredNorm(); double c1 = 12.0 * _delta_pos.dot(_start_velocity); double c2 = -3.0 * _start_velocity.squaredNorm(); double c3 = 0.0; Matrix<double, 4, 4> matrix_4x4; Matrix<complex<double>, Dynamic, Dynamic> matrix_eigenvalues; matrix_4x4 << 0, 0, 0, -c0, 1, 0, 0, -c1, 0, 1, 0, -c2, 0, 0, 1, -c3; matrix_eigenvalues = matrix_4x4.eigenvalues(); for (int i = 0; i < (int)matrix_eigenvalues.size(); i++) { double value_imag = imag(matrix_eigenvalues(i)); double value_real = real(matrix_eigenvalues(i)); if (std::abs(value_imag) >= 1e-6 || value_real <= 1e-6) { // costs.emplace_back(optimal_cost); continue; } double cost = value_real + 3.0 * _start_velocity.squaredNorm() / value_real - 6.0 * _delta_pos.dot(_start_velocity) / (value_real * value_real) + 3.0 * _delta_pos.squaredNorm() / (value_real * value_real * value_real); if (cost < optimal_cost) { optimal_cost = cost; *_T = value_real; } } return optimal_cost; } -
运行效果
绿色轨迹表示最优
蓝色轨迹表示无碰撞但不是最优
红色轨迹表示有碰撞
总结
从以上计算过程中可见自由边界问题与固定边界问题的处理过程中，最大的区别主要是对边界条件 λ ( T ) = − ▽ h ( s ∗ ( T ) ) \lambda(T)=-\triangledown h(s^*(T)) λ(T)=−▽h(s∗(T)) 的处理方法不同，在固定边界问题中，由于我们对系统的末状态施加了确切的约束，因此 h ( s ∗ ( T ) ) h(s^*(T)) h(s∗(T)) 是不可导的（且先不说其是否存在），可以认为此时该边界条件无意义；而在处理自由边界问题时，由于末状态速度自由，所以 h ( s ∗ ( T ) ) h(s^*(T)) h(s∗(T)) 对速度是可导的，但是由于在代价函数中未定义对系统末状态的约束，可认为 h ( s ∗ ( T ) ) = 0 h(s^*(T))=0 h(s∗(T))=0 ，所以其对速度的偏导也为0。
2562
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
