企业官网建设流程全解析

参考资料：
A computationally efficient motion primitive for quadrocopter trajectory generation
移动机器人运动规划

一、背景

Boundary value problem (BVP)：给出机器人在起始点与终止点的状态，设计出一条状态转移的轨迹。
Optimal boundary value problem (OBVP)：按某种原则设计出一条最优轨迹。

二、模型建模

1. BVP

我们假设一条 5阶运动轨迹进行参数化
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_5t^5+c_4t^4+c_3t^3+c_2t^2+c_1t+c_0x(t)=c5​t5+c4​t4+c3​t3+c2​t2+c1​t+c0​
其边界条件如下：

位置	速度	加速度
起始时刻 (t=0)	a	0	0
终止时刻 (t=T)	b	0	0

对此求解，可以得到如下方程：
[ 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 ] \left[ \begin{matrix} a \\ b \\0\\0\\0\\0 \end{matrix} \right]= \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} c_5 \\ c_4 \\c_3\\c_2\\c_1\\c_0 \end{matrix} \right]​ab0000​​=​0T505T4020T3​0T404T3012T2​0T303T206T​0T202T22​0T1100​110000​​​c5​c4​c3​c2​c1​c0​​​
由此可以解的多条符合条件的轨迹，而为了获得最优轨迹则是 OBVP 来解决的。

2. OBVP

对于二维/三维空间中的机器人，通常在每个维度上分别进行轨迹设计。以三维空间中运动的无人机为例，考察其中一个轴k kk的运动。

无人机状态S k = ( p k , v k , a k ) S_k=(p_k, v_k, a_k)Sk​=(pk​,vk​,ak​)\qquad控制输入：u k ( t ) = j k ( t ) u_k(t) = j_k(t)uk​(t)=jk​(t)
状态方程：S ˙ k = f s ( S k , u k ) = ( v k , a k , j k ) \dot{S}_k=f_s(S_k,u_k)=(v_k,a_k,j_k)S˙k​=fs​(Sk​,uk​)=(vk​,ak​,jk​)
求解目标：最小化jerk二次方的积分，即
J Σ = Σ k = 1 3 J k min ⁡ J k = ∫ 0 T g ( s ( t ) , u ( t ) ) ⋅ d t = 1 T ∫ 0 T j k ( t ) 2 d t J_{\Sigma}= \mathop{\Sigma}\limits_{k=1}^3J_k \qquad \min J_k =\int_{0}^{T} g(s(t), u(t)) \cdot d t= \frac{1}{T}\int_{0}^{T}j_k(t)^{2}dtJΣ​=k=1Σ3​Jk​minJk​=∫0T​g(s(t),u(t))⋅dt=T1​∫0T​jk​(t)2dt

待求解的量是u k ( t ) u_k(t)uk​(t)，使得从初状态t = 0 t=0t=0到末状态t = T t=Tt=T过程中 对j k 2 j_k^2jk2​积分最小。

三、构建汉密尔方程求解

寻找最优轨迹的广义极小化代价函数如下：
J = h ( s ( T ) ) + ∫ 0 T g ( s ( t ) , u ( t ) ) ⋅ d t J=h(s(T))+\int_{0}^{T} g(s(t), u(t)) \cdot d tJ=h(s(T))+∫0T​g(s(t),u(t))⋅dt
其中，第一项h ( s ( T ) h(s(T)h(s(T)反映了末状态与理想状态的误差惩罚项，第二项∫ 0 T g ( s ( t ) , u ( t ) ) ⋅ d t \int_{0}^{T} g(s(t), u(t)) \cdot d t∫0T​g(s(t),u(t))⋅dt反映了系统运行过程中状态转移的代价 (transition cost)。

为求解最优的u ( t ) u(t)u(t)即最优j jj），可以使用庞特里亚金极小值原理：引入协态costateλ = ( λ 1 , λ 2 , λ 3 ) \lambda=(\lambda_1,\lambda_2,\lambda_3)λ=(λ1​,λ2​,λ3​)，（注意不是常量，而是λ ( t ) = ( λ 1 ( t ) , λ 2 ( t ) , λ 3 ( t ) ) \lambda(t)=(\lambda_1(t),\lambda_2(t),\lambda_3(t))λ(t)=(λ1​(t),λ2​(t),λ3​(t))的简写）,构建Hamiltonian funciton
H ( s , j , λ ) = g ( s , u ) + λ T f s ( s , j ) = 1 T j 2 + λ T f s ( s , j ) = 1 T j 2 + λ 1 v + λ 2 a + λ 3 j H(s,j,λ)=g(s,u)+λ^Tf_s(s,j)=\frac{1}{T}j^2+λ^Tf_s(s,j)=\frac{1}{T}j^2+λ_1v+λ_2a+λ_3jH(s,j,λ)=g(s,u)+λTfs​(s,j)=T1​j2+λTfs​(s,j)=T1​j2+λ1​v+λ2​a+λ3​j

构建汉密尔方程思路：
汉密尔方程是关于s , j , λ s,j,λs,j,λ联合构成的函数，由状态转移代价被积项g ( s ( t ) , u ( t ) ) g(s(t),u(t))g(s(t),u(t))和 协态λ T \lambda^TλT*f(S_k,u_k) 构成。

接着，先介绍庞特里亚金极小值原理 (Pontryagin’s minimum principle)：

庞特里亚金极小值原理(Pontryagin’s minimum principle) 是最优控制范畴里的概念。
对于初始状态给定的问题，
λ ˙ ( t ) = − ∇ s H ( s ∗ ( t ) , u ∗ ( t ) , λ ( t ) ) \dot{\lambda}(t)=-\nabla_{s} H\left(s^{*}(t), u^{*}(t), \lambda(t)\right)λ˙(t)=−∇s​H(s∗(t),u∗(t),λ(t))
说明：上式含义：λ 导数 = − ( H 对 p 求导 , H 对 v 求导 , H 对 a 求导 ) λ导数=−(H对p求导,H对v求导,H对a求导)λ导数=−(H对p求导,H对v求导,H对a求导)
最优控制输入为
u ∗ ( t ) = a r g m i n u ( t ) H ( s ∗ ( t ) , u ( t ) , λ ( t ) ) u^{*}(t)= \underset{u(t)}{argmin} H\left(s^{*}(t), u(t), \lambda(t)\right)u∗(t)=u(t)argmin​H(s∗(t),u(t),λ(t))
如果h ( s ( T ) ) h(s(T))h(s(T))是连续的，还有terminal boundary condition成立:
λ ( T ) = − ∇ h ( s ∗ ( T ) ) \lambda(T)=-\nabla h\left(s^{*}(T)\right)λ(T)=−∇h(s∗(T))

下面开始求解，由庞特里亚金极小值原理：
λ ˙ = − ∇ s H ( s ∗ , j ∗ , λ ) = ( 0 , − λ 1 , − λ 2 ) \dot{\lambda}=-\nabla_{s} H\left(s^{*}, j^{*}, \lambda\right)=\left(0,-\lambda_{1},-\lambda_{2}\right)λ˙=−∇s​H(s∗,j∗,λ)=(0,−λ1​,−λ2​)

说明上式意义：λ ( t ) \lambda(t)λ(t)这一向量各分量 第一项对p pp求导，第二项对v vv求导，第三项对a aa求导 (注意协态求导等式右侧有负号)。

引入待定系数α , β , γ \alpha,\beta,\gammaα,β,γ，该微分方程的一组解如下：
λ ( t ) = 1 T [ − 2 α 2 α t + 2 β − α t 2 − 2 β t − 2 γ ] \lambda(t)=\frac{1}{T}\left[ \begin{matrix} −2α \\ 2αt+2β \\−αt^2−2βt−2γ \end{matrix} \right]λ(t)=T1​​−2α2αt+2β−αt2−2βt−2γ​​

该组解的形式是为了后续推导简洁方便，也可以是其他形式的一组解如：
λ ( t ) = [ α − α t + β 1 2 α t 2 + β t + γ ] \lambda(t)=\left[ \begin{matrix} α \\ -αt+β \\ \frac{1}{2}αt^2+βt+γ \end{matrix} \right]λ(t)=​α−αt+β21​αt2+βt+γ​​

进而，可得最优 jerk

j ∗ ( t ) = a r g m i n j ( t ) H ( s ∗ ( t ) , j ∗ ( t ) , λ ( t ) ) = a r g m i n j ( t ) [ 1 T j 2 + λ 1 v + λ 2 a + λ 3 j ] = a r g m i n j ( t ) [ 1 T j 2 + λ 3 j ] = a r g m i n j ( t ) [ 1 T j 2 + 1 T ( − α t 2 − 2 β t − 2 γ ) j ] = 1 2 α t 2 + β t + γ \begin{aligned} j^*(t)&= \underset{j(t)}{argmin}H(s^*(t),j^*(t),λ(t))\\[2ex] &= \underset{j(t)}{argmin}[\frac{1}{T}j^2+\lambda_1 v+\lambda_2 a+\lambda_3 j]\\[2ex] &= \underset{j(t)}{argmin}[\frac{1}{T}j^2+\lambda_3 j]\\[2ex] &= \underset{j(t)}{argmin}[\frac{1}{T}j^2+\frac{1}{T}(−αt^2−2βt−2γ)j]\\[2ex] &=\frac{1}{2}αt^2+βt+γ \end{aligned}j∗(t)​=j(t)argmin​H(s∗(t),j∗(t),λ(t))=j(t)argmin​[T1​j2+λ1​v+λ2​a+λ3​j]=j(t)argmin​[T1​j2+λ3​j]=j(t)argmin​[T1​j2+T1​(−αt2−2βt−2γ)j]=21​αt2+βt+γ​
说明：当H HH取极值时，即此时与s ss相关的p , v , a p,v,ap,v,a取到极值，此时 H取最小的 j 即：j ∗ ( t ) = 1 T j 2 + λ 3 j = 1 2 α t 2 + β t + γ j^*(t)=\frac{1}{T}j^2+\lambda_3 j=\frac{1}{2}αt^2+βt+γj∗(t)=T1​j2+λ3​j=21​αt2+βt+γ。

通过对 jerk 的三次积分，可得最优的s ( t ) s(t)s(t)
s ∗ ( t ) = [ p ∗ v ∗ a ∗ ] = [ α 120 t 5 + β 24 t 4 + γ 6 t 3 + a 0 2 t 2 + v 0 t + p 0 α 24 t 4 + β 6 t 3 + γ 2 t 2 + a 0 t + v 0 α 6 t 3 + β 2 t 2 + γ t + a 0 ] s^{*}(t)=\left[ \begin{matrix} p^* \\[2ex] v^*\\[2ex] a^* \\ \end{matrix} \right] =\left[ \begin{matrix} \frac{α}{120}t^5+\frac{β}{24}t^4+\frac{γ}{6}t^3+\frac{a_0}2t^2+v_0t+p_0 \\[2ex] \frac{α}{24}t^4+\frac{β}{6}t^3+\frac{γ}{2}t^2+a_0t+v_0\\[2ex] \frac{α}{6}t^3+\frac{β}{2}t^2+γt+a0\\ \end{matrix} \right]s∗(t)=​p∗v∗a∗​​=​120α​t5+24β​t4+6γ​t3+2a0​​t2+v0​t+p0​24α​t4+6β​t3+2γ​t2+a0​t+v0​6α​t3+2β​t2+γt+a0​​
按对末状态的要求，分三种情况讨论。
情况一：Fully Defined End Translational State
要求末状态 s ( T ) s(T) s(T)的每个分量严格等于给定值

此时
h ( s ( T ) ) = { 0 , i f s = s ( T ) ∞ , o t h e r w i s e \begin{equation} h ( s ( T ) ) = \left\{ \begin{array}{} 0 ,\quad if \ s = s ( T ) \notag \\ ∞,\quad otherwise \\ \end{array} \right . \end{equation}h(s(T))={0,ifs=s(T)∞,otherwise​​
h ( s ( T ) ) h(s(T))h(s(T))不连续，属于硬约束(hard constraint)。不可使用庞特里亚金极小值原理的terminal boundary condition。

设期望的末状态s ( T ) = ( p f , v f , a f ) s(T)=(p_f,v_f,a_f)s(T)=(pf​,vf​,af​)，以s ( T ) s(T)s(T)第一维为例，有
s ∗ ( T ) − s ( 0 ) = α 120 T 5 + β 24 T 4 + γ 6 T 3 + a 0 2 T 2 + v 0 T + p 0 − p 0 = p f − p 0 \begin{aligned} s^{*}(T)-s(0)&=\frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{\gamma}{6}T^3+\frac{a_0}{2}T^2+v_{0}T+p_0-p_0=p_f-p_0\end{aligned}s∗(T)−s(0)​=120α​T5+24β​T4+6γ​T3+2a0​​T2+v0​T+p0​−p0​=pf​−p0​​

记Δ p = p f − p 0 − v 0 T − 1 2 a 0 T 2 \Delta p=p_{f}-p_{0}-v_{0} T-\frac{1}{2} a_{0} T^{2}Δp=pf​−p0​−v0​T−21​a0​T2，整理得
α 120 T 5 + β 24 T 4 + γ 6 T 3 = Δ p \frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{\gamma}{6}T^3=\Delta p120α​T5+24β​T4+6γ​T3=Δp

类似，写出s ( T ) s(T)s(T)所有维度上的改变量，并写作矩阵形式，有
[ 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 ] [ α β γ ] = [ Δ p Δ v Δ a ] \left[ \begin{matrix} \frac{1}{120}T^5 &\frac{1}{24}T^4 & \frac{1}{6}T^3 \\[2ex] \frac{1}{24}T^4 &\frac{1}{6}T^3 & \frac{1}{2}T^2\\[2ex] \frac{1}{6}T^3 &\frac{1}{2}T^2 & T \\ \end{matrix} \right] \left[ \begin{matrix} \alpha \\[2ex] \beta\\[2ex] \gamma \\ \end{matrix} \right]= \left[ \begin{matrix} \Delta p \\[2ex] \Delta v\\[2ex] \Delta a \\ \end{matrix} \right]​1201​T5241​T461​T3​241​T461​T321​T2​61​T321​T2T​​​αβγ​​=​ΔpΔvΔa​​

其中，
[ Δ p Δ v Δ a ] = [ p f − p 0 − v 0 T − 1 2 a 0 T 2 v f − v 0 − a 0 T a f − a 0 ] \left[ \begin{matrix} \Delta p \\[2ex] \Delta v\\[2ex] \Delta a \\ \end{matrix} \right] =\left[ \begin{matrix} p_f-p_0-v_0T-\frac{1}{2}a_0T^2 \\[2ex] v_f-v_0-a_0T\\[2ex] a_f-a_0 \\ \end{matrix} \right]​ΔpΔvΔa​​=​pf​−p0​−v0​T−21​a0​T2vf​−v0​−a0​Taf​−a0​​​

从而解出
[ α β γ ] = 1 T 5 [ 720 − 360 T 60 T 2 − 360 T 168 T 2 − 24 T 3 60 T 2 − 24 T 3 3 T 4 ] [ Δ p Δ v Δ a ] \left[ \begin{matrix} \alpha \\[2ex] \beta\\[2ex] \gamma \\ \end{matrix} \right]=\frac{1}{T^5} \left[ \begin{matrix} 720&−360T&60T^2 \\[2ex] −360T&168T^2&−24T^3\\[2ex] 60T^2&−24T^3&3T^4 \\ \end{matrix} \right] \left[ \begin{matrix} \Delta p \\[2ex] \Delta v\\[2ex] \Delta a \\ \end{matrix} \right]​αβγ​​=T51​​720−360T60T2​−360T168T2−24T3​60T2−24T33T4​​​ΔpΔvΔa​​
情况二：Partially Defined End Translational State
要求末状态 s ( T ) s(T) s(T)的部分分量等于给定值

对于s ( T ) s(T)s(T)第i ii维的的分量s i ( T ) s_{i}(T)si​(T)，若给定s i ( T ) s_{i}(T)si​(T)的值，则该维度上受积极约束，并称下标 i i i属于积极集：i ∈ A i\in \mathcal{A}i∈A

此时，由庞特里亚金极小值原理，对于所有自由分量 s j ( T ) s_{j}(T) sj​(T)，其对应的
λ j ( T ) = ∂ h ( s ∗ ( T ) ) ∂ s j , for j ∉ A \lambda_{j}(T)=\frac{\partial h\left(s^{*}(T)\right)}{\partial s_{j}}, \text { for } j \notin \mathcal{A}λj​(T)=∂sj​∂h(s∗(T))​,forj∈/A

由上文s ∗ ( t ) s^{*}(t)s∗(t)的表达式可知，s ∗ ( T ) s^{*}(T)s∗(T)是一个只与T TT有关的函数，因此
λ j ( T ) = ∂ h ( s ∗ ( T ) ) ∂ s j = 0 , for j ∉ A \lambda_{j}(T)=\frac{\partial h\left(s^{*}(T)\right)}{\partial s_{j}}=0, \text { for } j \notin \mathcal{A}λj​(T)=∂sj​∂h(s∗(T))​=0,forj∈/A

即对于末状态的所有自由分量，其相应的λ j ( T ) = 0 \lambda_{j}(T)=0λj​(T)=0

下面举两个例子。

Example1：固定终点的p pp与v vv，对a aa不做要求，求最优j e r k jerkjerk

由p pp与v vv的末状态，有
[ 1 120 T 5 1 24 T 4 1 6 T 3 1 24 T 4 1 6 T 3 1 2 T 2 ] [ α β ] = [ Δ p Δ v ] \left[ \begin{matrix} \frac{1}{120}T^5 &\frac{1}{24}T^4 & \frac{1}{6}T^3 \\[2ex] \frac{1}{24}T^4 &\frac{1}{6}T^3 & \frac{1}{2}T^2 \end{matrix} \right] \left[ \begin{matrix} \alpha \\[2ex] \beta \end{matrix} \right]= \left[ \begin{matrix} \Delta p \\[2ex] \Delta v \end{matrix} \right][1201​T5241​T4​241​T461​T3​61​T321​T2​][αβ​]=[ΔpΔv​]

由于a aa是自由分量，因此λ ( T ) \lambda(T)λ(T)的第三个分量为0，即
− α T − 2 β − 2 T γ = 0 -\alpha T-2 \beta -\frac{2}{T} \gamma=0−αT−2β−T2​γ=0

以上两式联立，有
[ 1 120 T 5 1 24 T 4 1 6 T 3 1 24 T 4 1 6 T 3 1 2 T 2 T 2 2 T ] [ α β γ ] = [ Δ p Δ v 0 ] \left[ \begin{matrix} \frac{1}{120}T^5 &\frac{1}{24}T^4 & \frac{1}{6}T^3 \\[2ex] \frac{1}{24}T^4 &\frac{1}{6}T^3 & \frac{1}{2}T^2\\[2ex] T &2 & \frac{2}{T} \\ \end{matrix} \right] \left[ \begin{matrix} \alpha \\[2ex] \beta\\[2ex] \gamma \\ \end{matrix} \right]= \left[ \begin{matrix} \Delta p \\[2ex] \Delta v\\[2ex] 0 \\ \end{matrix} \right]​1201​T5241​T4T​241​T461​T32​61​T321​T2T2​​​​αβγ​​=​ΔpΔv0​​

解得
[ α β γ ] = 1 T 5 [ 320 − 120 T − 200 T 72 T 2 40 T 2 − 12 T 3 ] [ Δ p Δ v ] \left[ \begin{matrix} \alpha \\[2ex] \beta\\[2ex] \gamma \\ \end{matrix} \right]=\frac{1}{T^5} \left[ \begin{matrix} 320&−120T\\[2ex] −200T&72T^2\\[2ex] 40T^2&−12T^3 \\ \end{matrix} \right] \left[ \begin{matrix} \Delta p \\[2ex] \Delta v \end{matrix} \right]​αβγ​​=T51​​320−200T40T2​−120T72T2−12T3​​[ΔpΔv​]

Example2：固定终点的p pp，对v vv,a aa不做要求，求最优j e r k jerkjerk

由p pp的末状态，有
α 120 T 5 + β 24 T 4 + r 6 T 3 = Δ p \frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{r}{6}T^3=\Delta p120α​T5+24β​T4+6r​T3=Δp

由于v vv,a aa是自由分量，因此λ ( T ) \lambda(T)λ(T)的第二、三个分量为0，即
α + 2 T β = 0 − α T − 2 β − 2 T γ = 0 \begin{aligned} \alpha+2T\beta&=0\\[2ex] -\alpha T-2 \beta -\frac{2}{T} \gamma&=0 \end{aligned}α+2Tβ−αT−2β−T2​γ​=0=0​
以上三式联立，有
[ 1 120 T 5 1 24 T 4 1 6 T 3 2 2 T 0 T 2 2 T ] [ α β γ ] = [ Δ p 0 0 ] \left[ \begin{matrix} \frac{1}{120}T^5 &\frac{1}{24}T^4 & \frac{1}{6}T^3 \\[2ex] 2 &\frac{2}{T} & 0\\[2ex] T &2 & \frac{2}{T} \\ \end{matrix} \right] \left[ \begin{matrix} \alpha \\[2ex] \beta\\[2ex] \gamma \\ \end{matrix} \right]= \left[ \begin{matrix} \Delta p \\[2ex] 0\\[2ex] 0 \\ \end{matrix} \right]​1201​T52T​241​T4T2​2​61​T30T2​​​​αβγ​​=​Δp00​​

解得
[ α β γ ] = 1 T 5 [ 20 − 20 T 10 T 2 ] Δ p \left[ \begin{matrix} \alpha \\[2ex] \beta\\[2ex] \gamma \\ \end{matrix} \right]=\frac{1}{T^5} \left[ \begin{matrix} 20\\[2ex] −20T\\[2ex] 10T^2 \\ \end{matrix} \right] \Delta p​αβγ​​=T51​​20−20T10T2​​Δp

类似地，还可以解出固定p pp与a aa、固定v vv与a aa等共计6种末状态部分固定的问题。
情况三：Motion Primitive Cost

未指定末状态的任意分量

简言之，一个motion primitive就是一套【机器人初始状态、运动时间、p , v , a p , v , ap,v,a的某种组合构成的末状态的组合。

这一类情况不是要求解出具体的 α , β , γ \alpha,\beta,\gamma α,β,γ，而是计算出评估一个motion primitive的代价函数的具体表达式：
J = 1 T ∫ 0 T j ( t ) 2 d t = 1 T ∫ 0 T ( 1 2 α t 2 + β t + γ ) 2 d t = 1 20 α 2 T 4 + 1 4 α β T 3 + 1 3 ( α γ + β 2 ) T 2 + β γ T + γ 2 \begin{aligned} J&=\frac{1}{T}\int^T_0j(t)^2dt\\[2ex] &=\frac{1}{T}\int^T_0(\frac{1}{2}αt2+βt+γ)^2dt\\[2ex] &=\frac{1}{20}α^2T^4+\frac{1}{4}αβT^3+\frac{1}{3}(αγ+β^2)T^2+βγT+γ^2 \end{aligned}J​=T1​∫0T​j(t)2dt=T1​∫0T​(21​αt2+βt+γ)2dt=201​α2T4+41​αβT3+31​(αγ+β2)T2+βγT+γ2​

回归到sample in state space的state lattice planning，这一代价函数可以用于评估不同的候选motion primitive。

四、总结

前两种情况是对具体任务的求解，第三种情况是对motion primitive的评价——在运动规划过程中，按某种规则生成一系列候选motion primitive，使用情况三中代价函数进行评价，筛选出某个（些）合适的motion primitive。