南京营销网站开发制作报价wordpress文章导航

南京营销网站开发制作报价,wordpress文章导航,做靓号网站,备案名称和网站名称文章目录一、背景二、模型建模1. BVP2. OBVP三、构建汉密尔方程求解四、总结参考资料#xff1a;A computationally efficient motion primitive for quadrocopter trajectory generation移动机器人运动规划一、背景 Boundary value problem (BVP)#xff1a;给出机器人在起…文章目录一、背景二、模型建模1. BVP2. OBVP三、构建汉密尔方程求解四、总结参考资料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^5c_4t^4c_3t^3c_2t^2c_1tc_0x(t)c5​t5c4​t4c3​t3c2​t2c1​tc0​其边界条件如下位置速度加速度起始时刻 (t0)a00终止时刻 (tT)b00对此求解可以得到如下方程[ 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 00001 \\ T^5 T^4T^3T^2T1\\ 000010\\ 5T^44T^33T^22T10\\ 000200\\ 20T^312T^26T200 \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}_kf_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_{k1}^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Σ​k1Σ3​Jk​minJk​∫0T​g(s(t),u(t))⋅dtT1​∫0T​jk​(t)2dt待求解的量是u k ( t ) u_k(t)uk​(t)使得从初状态t 0 t0t0到末状态t T tTtT过程中 对j k 2 j_k^2jk2​积分最小。三、构建汉密尔方程求解寻找最优轨迹的广义极小化代价函数如下J h ( s ( T ) ) ∫ 0 T g ( s ( t ) , u ( t ) ) ⋅ d t Jh(s(T))\int_{0}^{T} g(s(t), u(t)) \cdot d tJh(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 funcitonH ( 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αt2β \\−αt^2−2βt−2γ \end{matrix} \right]λ(t)T1​​−2α2αt2β−α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γ​​进而可得最优 jerkj ∗ ( 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​j2T1​(−α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​j21​α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^2v_0tp_0 \\[2ex] \frac{α}{24}t^4\frac{β}{6}t^3\frac{γ}{2}t^2a_0tv_0\\[2ex] \frac{α}{6}t^3\frac{β}{2}t^2γta0\\ \end{matrix} \right]s∗(t)​p∗v∗a∗​​​120α​t524β​t46γ​t32a0​​t2v0​tp0​24α​t46β​t32γ​t2a0​tv0​6α​t32β​t2γta0​​按对末状态的要求分三种情况讨论。情况一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,ifss(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^2v_{0}Tp_0-p_0p_f-p_0\end{aligned}s∗(T)−s(0)​120α​T524β​T46γ​T32a0​​T2v0​Tp0​−p0​pf​−p0​​记Δ p p f − p 0 − v 0 T − 1 2 a 0 T 2 \Delta pp_{f}-p_{0}-v_{0} T-\frac{1}{2} a_{0} T^{2}Δppf​−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α​T524β​T46γ​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−360T60T^2 \\[2ex] −360T168T^2−24T^3\\[2ex] 60T^2−24T^33T^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} \gamma0−α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] −200T72T^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α​T524β​T46r​T3Δp由于v vv,a aa是自由分量因此λ ( T ) \lambda(T)λ(T)的第二、三个分量为0即α 2 T β 0 − α T − 2 β − 2 T γ 0 \begin{aligned} \alpha2T\beta0\\[2ex] -\alpha T-2 \beta -\frac{2}{T} \gamma0 \end{aligned}α2Tβ−αT−2β−T2​γ​00​以上三式联立有[ 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)2dtT1​∫0T​(21​αt2βtγ)2dt201​α2T441​αβT331​(αγβ2)T2βγTγ2​回归到sample in state space的state lattice planning这一代价函数可以用于评估不同的候选motion primitive。四、总结前两种情况是对具体任务的求解第三种情况是对motion primitive的评价——在运动规划过程中按某种规则生成一系列候选motion primitive使用情况三中代价函数进行评价筛选出某个些合适的motion primitive。