问题描述

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)=c5​t5+c4​t4+c3​t3+c2​t2+c1​t+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​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎡​0T505T4020T3​0T404T3012T2​0T303T206T​0T202T22​0T1100​110000​⎦⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​c5​c4​c3​c2​c1​c0​​⎦⎥⎥⎥⎥⎥⎥⎤​

那么对于 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))+∫0T​g(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)=−▽s​H(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)argmin​H(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=∫0T​g(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α1​2α2​2α3​−2α1​t−2β1​−2α2​t−2β2​−2α3​t−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)argmin​H(x∗(t),u(t),λ(t))=⎣⎡​α1​t+β1​α2​t+β2​α3​t+β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​α1​t3+21​β1​t2+vx0​t+px0​61​α2​t3+21​β2​t2+vy0​t+py0​61​α3​t3+21​β3​t2+vz0​t+pz0​21​α1​t2+β1​t+vx0​21​α2​t2+β2​t+vy0​21​α3​t2+β3​t+vz0​​⎦⎥⎥⎥⎥⎥⎥⎤​, initial state:x(0)=⎣⎢⎢⎢⎢⎢⎢⎡​px0​py0​pz0​vx0​vy0​vz0​​⎦⎥⎥⎥⎥⎥⎥⎤​
  • 根据末状态边界条件可以求解 α , β \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} ⎣⎢⎢⎢⎢⎢⎢⎡​61​T30021​T200​061​T30021​T20​0061​T30021​T2​21​T200T00​021​T200T0​0021​T200T​⎦⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​α1​α2​α3​β1​β2​β3​​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎡​pxf​−px0​−vx0​Tpyf​−py0​−vy0​Tpzf​−pz0​−vz0​Tvxf​−vx0​vyf​−vy0​vzf​−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​​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎡​−T312​00T26​00​0−T312​00T26​0​00−T312​00T26​​T26​00−T2​00​0T26​00−T2​0​00T26​00−T2​​⎦⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​pxf​−px0​−vx0​Tpyf​−py0​−vy0​Tpzf​−pz0​−vz0​Tvxf​−vx0​vyf​−vy0​vzf​−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∗=⎣⎡​α1​t+β1​α2​t+β2​α3​t+β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​α12​T3+α1​β1​T2+β12​T)+(31​α22​T3+α2​β2​T2+β22​T)+(31​α32​T3+α3​β3​T2+β32​T)
  • 最终 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} ⎣⎡​61​T300​061​T30​0061​T3​21​T200​021​T20​0021​T2​⎦⎤​⎣⎢⎢⎢⎢⎢⎢⎡​α1​α2​α3​β1​β2​β3​​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎡​pxf​−px0​−vx0​Tpyf​−py0​−vy0​Tpzf​−pz0​−vz0​T​⎦⎤​
  • 现在需要将速度边界条件修改为自由边界条件，此时将用上这个条件 λ ( 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α1​T−2β1​−2α2​T−2β2​−2α3​T−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} ⎣⎢⎢⎢⎢⎢⎢⎡​61​T300T00​061​T300T0​0061​T300T​21​T200100​021​T20010​0021​T2001​⎦⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​α1​α2​α3​β1​β2​β3​​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎡​pxf​−px0​−vx0​Tpyf​−py0​−vy0​Tpzf​−pz0​−vz0​T000​⎦⎥⎥⎥⎥⎥⎥⎤​
    • [ α 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​​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎡​−T33​00T23​00​0−T33​00T23​0​00−T33​00T23​​2T3​00−21​00​02T3​00−21​0​002T3​00−21​​⎦⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​pxf​−px0​−vx0​Tpyf​−py0​−vy0​Tpzf​−pz0​−vz0​T000​⎦⎥⎥⎥⎥⎥⎥⎤​

• 计算代价（最优值）与上述过程一样，这里对最优值进行下深入计算

  • 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​α12​T3+α1​β1​T2+β12​T)+(31​α22​T3+α2​β2​T2+β22​T)+(31​α32​T3+α3​β3​T2+β32​T)
  • 由于 [ − 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α1​T−2β1​−2α2​T−2β2​−2α3​T−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​α12​T3+31​α22​T3+31​α32​T3
  • 记 Δ 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​−vx0​T)​T3−3(Δpy​−vy0​T)​T3−3(Δpz​−vz0​T)​​⎦⎥⎤​
  • 可得 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(Δpx​vx0​+Δpy​vy0​+Δpz​vz0​)T+(Δpx​2+Δpy​2+Δpz​2)]​
  • 记，
    • 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=Δpx​vx0​+Δpy​vy0​+Δpz​vz0​
    • c = Δ p x 2 + Δ p y 2 + Δ p z 2 c=\Delta{p_x}^2+\Delta{p_y}^2+\Delta{p_z}^2 c=Δpx​2+Δpy​2+Δpz​2
  • 求 ∂ 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。

两类边界条件的OBVP求解方法相关推荐

1. 几类常微分方程的matlab求解方法 | 刚性微分方程、隐式微分方程、微分代数方程

   目录 微分方程的转换 一.单个高阶常微分方程 二.高阶常微分方程组 刚性微分方程求解 隐式微分方程求解 微分代数方程求解 微分方程的转换 根据微分方程求解的标准型,要得到微分方程的数值解,应该先将该方 ...

2. 算法：最小公倍数的求解方法

   一 写在开头 1.1 本节内容 本文的主要内容是介绍一种两个数最小公倍数(Lowest Common Multiple)的求解方法. 二 最小公倍数求法 2.1 算法原理 两个数的公倍数可以是无限多个 ...

3. 创建两个Thread子类，第一个用run（）方法启动，并捕获第二个Thread对象的句柄，然后调用wait（）。第二个类的run（）方法几秒后为第一个线程调用notifAll(),使第一个线程打印消息

   创建两个Thread子类,第一个用run()方法启动,并捕获第二个Thread对象的句柄,然后调用wait().第二个类的run()方法几秒后为第一个线程调用notifAll(),使第一个线程打印消息 ...

4. 编写一个抽象类Shape，声明计算图形面积的抽象方法。再分别定义Shape的子类Circle（圆）和Rectangle（矩形），在两个子类中按照不同图形的面积计算公式，实现Shape类中计算面积的方法

   编写一个抽象类Shape,声明计算图形面积的抽象方法.再分别定义Shape的子类Circle(圆)和Rectangle(矩形),在两个子类中按照不同图形的面积计算公式,实现Shape类中计算面积的方法 ...

5. 数控编程方法可以分为两类：一类是手工编程，另一类是自动编程

   数控加工工作过程:如下图所示,在数控机床上加工零件时,要预先根据零件加工图样的要求确定零件加工的工艺过程.工艺参数和走刀运动数据,然后编制加工程序,传输给数控系统,在事先存入数控装置内部的控制软件支持 ...

6. 线性规划两阶段求解方法

   想快速掌握线性规划两阶段求解方法,强烈推荐博文<三言两语讲清楚线性规划单纯形方法>. 百度百科给了下面一个例子,感觉其解法不容易看明白原理,换一种解释方法,应该很容易看明白两阶段法的原理. ...

7. 《C语言程序设计：问题与求解方法》——3.8节不同类型数据之间的类型转换

   本节书摘来自华章社区<C语言程序设计:问题与求解方法>一书中的第3章,第3.8节不同类型数据之间的类型转换,作者:何 勤,更多章节内容可以访问云栖社区"华章社区"公众号 ...

8. 《C语言程序设计：问题与求解方法》——3.9节常见编程错误

   本节书摘来自华章社区<C语言程序设计:问题与求解方法>一书中的第3章,第3.9节常见编程错误,作者:何 勤,更多章节内容可以访问云栖社区"华章社区"公众号查看 3.9 ...

9. 重磅！一文读懂线性方程组的求解方法

   目录 1.AAA为方阵 直接法 迭代法 2.AAA为非方阵且A∈Rm×n,m>nA\in R^{m\times n},m>nA∈Rm×n,m>n 2.1. r(A)=n<mr( ...

最新文章

1. OpenAI推新程序包：GPU适应十倍大模型仅需增加20%训练时间
2. 项目小结：日立OA系统（Asp.net）
3. Spring Boot 2.0 新特性
4. Linux下配置FTP、SSH服务
5. ELK学习5_ELK文档资料:《ELK stack 权威指南/饶琛琳》推荐
6. dart系列之:dart类中的构造函数
7. php定义数据表类,phpwind中的数据库操作类
8. 2017年WorkApplication牛客网线上机试题
9. Js 日期 多少分钟前，多少秒前
10. 重做系统，出现invalid switch noid
11. 2017iOS开发最新的打包测试步骤（亲测）
12. SQL prompt无法激活跳转到127.0.0.1:22223的解决方案
13. 深信服vmp云桌面安装测试小结
14. ENSP实验五——三层交换机+二层交换机
15. centos usb转网口_CentOS 6.5安装qf9700 USB网卡驱动
16. android scheme 配置多个,Android Scheme URL 使用方法
17. 美元的阿拉伯数字转换为英文大写的格式
18. 服务器只识别2t硬盘,网吧用2008R2服务器系统不认2T以上单个硬盘？
19. Camera硬件结构组成（三）
20. 城市道路井盖安全监测系统 opencv

热门文章

1. 8.利用红外遥控信号控制LED灯的亮灭
2. 苹果第四财季净利润85亿美元 同比增长13%
3. 种春草肥禾，织数字天下
4. python可以这样学读书笔记_Python 编程：从入门到实战 读书笔记
5. chm文件的文件格式 (chm format)
6. 关于js关闭窗口的事件和用法
7. Win10 .chm文件无法打开解决方案
8. 什么是智慧社区 智慧社区解决方案概括
9. 对于c++面向对象的深刻认识和理解--哲学角度看问题（源生论）
10. [这是兴趣么](http://simplemind.info/blog/?paged=2m=201104)