淘宝上找网站建设好吗,wordpress推荐配置,免费做网站公司推荐,手机端网站开发流程文章目录 前言相关代码整理:相关文章#xff1a; 介绍什么是kinodynamic#xff1f;为什么需要kinodynamic#xff1f;模型示例unicycle model#xff08;独轮车模型#xff09;differential model#xff08;两轮差速模型#xff09;Simplified car model (简化车辆模型… 文章目录 前言相关代码整理:相关文章 介绍什么是kinodynamic为什么需要kinodynamic模型示例unicycle model独轮车模型differential model两轮差速模型Simplified car model (简化车辆模型) State Lattice PlanningSample in control space示例1示例2 Sample in state space示例1示例2 比较Boundary Value Problem (BVP)Optimal Boundary Value Problem (OBVP)Pontryagin’s minimum principle应用 Trajectory library轨迹库Heuristic design启发式设计Planning in Frenet-serret Frame 参考文献 前言 本文部分内容参考了深蓝学院的移动机器人运动规划依此做相关的笔记与整理。 相关代码整理: https://gitee.com/lxyclara/motion-plan-homework/https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/masterhttps://gitee.com/aries-wu/Motion-plan/blob/main/链接: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwdrhtp 提取码: rhtp 相关文章 自动驾驶路径规划——轨迹规划详解插值法 介绍
什么是kinodynamic K i n o d y n a m i c : K i n e m a t i c D y n a m i c Kinodynamic : Kinematic Dynamic Kinodynamic:KinematicDynamic 运动学规划问题是综合机器人的运动同时受到运动学约束如避开障碍物和动力学约束如速度、加速度和力的模量边界。动力学解是从时间到广义力或加速度的映射。 • Differentially constrained(微分约束) • Up to force (acceleration)考虑到力加速度
为什么需要kinodynamic
之前几节介绍的规划算法往往没有考虑到系统的微分约束状态点之间的连接形成的路线不能直接被用作无人车、无人机的轨迹。
这里可能又会产生新的疑惑传统的Pipeline通过先规划出一条路径在通过后端轨迹优化的方式同样也能得到适用于无人车的轨迹。但是若在前端不考虑运动学/动力学的约束完全交由后端的轨迹优化进行那么后端的负担就会比较大。 规划通常需要以下几点 • Coarse-to-fine process流程从粗到细 • Trajectory only optimizes locally 轨迹优化基于局部的状况 • Infeasible path means nothing to nonholonomic system在非完整系统中一些不可行路径将会毫无意义
基于kinodynamic的规划算法可以在path finding这一步就可以得到比较理想的轨迹再在trajectory optimization部分优化时任务相对轻松一些。
如下图右图无人机规划图所示若采用单纯的前端不考虑运动学/动力学的规划后端轨迹优化的方式规划出的路径是紫色实线很显然无人机依据这个路径得到最终的轨迹会是紫色虚线部分相比右侧考虑了kinodynamic的规划轨迹质量更差需要无人机通过减速、转向等操作才能进行轨迹跟踪。
同样的从左图的泊车规划示意图中可以看到轨迹是考虑了车辆的运动学/动力学模型的车辆无法进行侧向移动暂时不考虑全向麦轮若按传统方式进行规划则毫无意义。
模型示例
详细部分可以参考《PLANNING ALGORITHM》这本书
unicycle model独轮车模型 状态方程如下 ( x ˙ y ˙ θ ˙ ) ( c o s θ s i n θ 0 ) ⋅ ν ( 0 0 1 ) ⋅ ω \begin{pmatrix}\dot{x}\\\dot{y}\\\dot{\theta}\end{pmatrix}\begin{pmatrix}cos\theta\\sin\theta\\0\end{pmatrix}\cdot\nu\begin{pmatrix}0\\0\\1\end{pmatrix}\cdot\omega x˙y˙θ˙ cosθsinθ0 ⋅ν 001 ⋅ω
独轮车模型对于速度 v v v、角速度 ω \omega ω有以下约束 ∣ v ∣ ≤ ν max ∣ ω ∣ ≤ ω max \begin{aligned}|v|\leq\nu_{\max}\\|\omega|\leq\omega_{\max}\end{aligned} ∣v∣∣ω∣≤νmax≤ωmax
differential model两轮差速模型 状态方程如下 ( x ˙ y ˙ θ ˙ ) ( r 2 ( ω l ω r ) c o s θ r 2 ( ω l ω r ) s i n θ r L ( ω r − ω l ) ) \begin{pmatrix}\dot{x}\\\dot{y}\\\dot{\theta}\end{pmatrix}\begin{pmatrix}\dfrac{r}{2}(\omega_l\omega_r)cos\theta\\\dfrac{r}{2}(\omega_l\omega_r)sin\theta\\\dfrac{r}{L}(\omega_r-\omega_l)\end{pmatrix} x˙y˙θ˙ 2r(ωlωr)cosθ2r(ωlωr)sinθLr(ωr−ωl) v r 2 ( ω l ω r ) c o s θ v\frac r2(\omega_l\omega_r)cos\theta v2r(ωlωr)cosθ ω r L ( ω r − ω l ) \begin{aligned}\omega\frac{r}{L}(\omega_r-\omega_l)\end{aligned} ωLr(ωr−ωl)
其左右轮角速度有以下限制 ∣ ω l ∣ ≤ ω l , m a x ∣ ω r ∣ ≤ ω r , m a x \begin{aligned}|\omega_l|\leq\omega_{l,max}\\|\omega_r|\leq\omega_{r,max}\end{aligned} ∣ωl∣∣ωr∣≤ωl,max≤ωr,max
Simplified car model (简化车辆模型) 状态方程如下 ( x ˙ y ˙ θ ˙ ) ( v c o s θ v s i n θ r L t a n ϕ ) \begin{pmatrix}\dot{x}\\\dot{y}\\\dot{\theta}\end{pmatrix}\begin{pmatrix}v cos\theta\\v sin\theta\\\frac{r}{L}tan\phi\end{pmatrix} x˙y˙θ˙ vcosθvsinθLrtanϕ
模型约束Simple car modelReeds Shepp’s carDubin’s car
State Lattice Planning
相比较之前的搜索算法我们需要图中的连接是可行的、符合车辆动力学约束的。
同时对于图的建立包括正向连接和反向连接。正向的方法将机器人的控制空间进行离散化反向的方向则是对周围的状态空间进行离散化。
具体到小车/机器人的控制可以进行以下描述
对于一个机器人模型 s ˙ f ( s , u ) \dot s f(s,u) s˙f(s,u)假设我已知机器人的初始位置 s 0 s_0 s0我们可以用以下两种方式对机器人接下来可行的运动进行描述
选择控制量 u u u确定经历的时间 T T T可以用数值积分的方式正向仿真出机器人接下来的位置。其具有以下的特点
正向仿真确定的 u , T u,T u,T比较容易实现缺乏任务的引导没有目的性规划效率比较率比较低
选定一个状态量 s f s_f sf确定出从 s 0 s_0 s0到 s f s_f sf之间的轨迹。
后向的计算需要计算 u , T u,T u,T良好的任务引导性比较难以去实现规划效率比较高
Sample in control space
示例1
以无人机的模型为例模型状态变量为 s s s控制输入量为 u u u。 State: s ( x y z x ˙ y ˙ z ˙ ) Input: u ( x ¨ y ¨ z ¨ ) \textbf{State: s}\begin{pmatrix}x\\y\\z\\\dot{x}\\\dot{y}\\\dot{z}\end{pmatrix}\quad\text{Input: }u\begin{pmatrix}\ddot{x}\\\ddot{y}\\\ddot{z}\end{pmatrix} State: s xyzx˙y˙z˙ Input: u x¨y¨z¨
系统的状态方程为 s ˙ A ⋅ s B ⋅ u \dot s A\cdot s B\cdot u s˙A⋅sB⋅u , A [ 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] B [ 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 0 1 ] A\begin{bmatrix}000100\\000010\\000001\\000000\\000000\\000000\end{bmatrix}\quad B\begin{bmatrix}000\\000\\000\\100\\010\\001\end{bmatrix} A 000000000000000000100000010000001000 B 000100000010000001
高维推广之后可以得到如下矩阵 A [ 0 I 3 0 ⋯ 0 0 0 I 3 ⋯ 0 ⋮ ⋱ ⋱ ⋱ ⋮ 0 ⋯ ⋯ 0 I 3 0 ⋯ ⋯ 0 0 ] B [ 0 0 ⋮ 0 I 3 ] A\begin{bmatrix}0I_30\cdots0\\00I_3\cdots0\\\vdots\ddots\ddots\ddots\vdots\\0\cdots\cdots0I_3\\0\cdots\cdots00\end{bmatrix}\quad B\begin{bmatrix}0\\0\\\vdots\\0\\I_3\end{bmatrix} A 00⋮00I30⋱⋯⋯0I3⋱⋯⋯⋯⋯⋱0000⋮I30 B 00⋮0I3
矩阵A是幂零矩阵。
下图是一个基于加速度和jerk的九个平面运动单元从初始点到目标点的移动状态。 上述模型在线性时不变系统中可以如下表述 所以当A为零幂矩阵时状态转移矩阵将可以变得更简洁后面的高阶项会为0。 If matrix A ∈ R n × n is nilpotent,i.e. A n 0 , e A t has a closed-form expression in the form of an ( n − 1 ) degree matrix polynomial in t . \begin{array}{l}\text{If matrix A}\in R^{n\times n}\text{is nilpotent,i.e.}A^n0,\quad e^{At}\text{has a closed-form}\\\text{expression in the form of an}(n-1)\text{degree matrix polynomial in t}.\end{array} If matrix A∈Rn×nis nilpotent,i.e.An0,eAthas a closed-formexpression in the form of an(n−1)degree matrix polynomial in t. PS在搜索时只有在有必要的时刻才会进行建图只有当真正发现节点时才会建立相应的节点和边这种带有目的性/启发性的方式 可以减少计算量。
示例2
以简化后的车辆模型为例 State: s ( x y θ ) Input: u ( v ∅ ) System equation: ( x ˙ y ˙ θ ˙ ) ( v ⋅ c o s θ v ⋅ s i n θ r L ⋅ t a n ϕ ) \begin{aligned}\text{State: s}\begin{pmatrix}x\\y\\\theta\end{pmatrix}\quad\text{Input: }u\begin{pmatrix}v\\\emptyset\end{pmatrix}\\\text{System equation:}\begin{pmatrix}\dot{x}\\\dot{y}\\\dot{\theta}\end{pmatrix}\begin{pmatrix}v\cdot cos\theta\\v\cdot sin\theta\\\frac rL\cdot tan\phi\end{pmatrix}\end{aligned} State: s xyθ Input: u(v∅)System equation: x˙y˙θ˙ v⋅cosθv⋅sinθLr⋅tanϕ 主要有以下步骤
对于每一个从搜索树中可以找到的状态 s s s选取一定的控制量 u u u对其在一定的时间段内进行数值积分若得到的边遇到了障碍物则排除若没有则不排除
参考论文Search-based Motion Planning for Quadrotors using Linear Quadratic Minimum Time ControlSikang Liu, Nikolay Atanasov, Kartik Mohta, and Vijay Kumar
Sample in state space
示例1
基于Reeds-Shepp Car Model完成状态空间离散化根据给定的状态计算控制参数。
参考论文Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State SpacesMihail Pivtoraiko and Alonzo Kelly
示例2
建立两层的lattice graph第一层根据车辆的初始状态以及第一层离散化后的状态进行计算得到轨迹第二次再次离散化状态空间再次计算。
参考论文Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly
比较
控制空间采样的方式难以得到有效的轨迹同时自由度有限状态空间采样的方式计算难度大。
参考论文State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments, Thomas M. Howard, Colin J. Green, and Alonzo Kelly
Boundary Value Problem (BVP)
BVP是状态空间采样的基础通常没有通用的解决方案一般需要复杂的数值优化方法去解决。
首先介绍一下常用的五次多项式插值方法这部分还可以参考 自动驾驶路径规划——轨迹规划详解插值法这篇博文。
首先可以依据起始点、终点状态设置轨迹
之后利用待定系数法构造多项式依据边界条件位置、速度、加速建立方程组进行求解。 Optimal Boundary Value Problem (OBVP)
论文A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, Mark W. Mueller, Markus Hehn, and Raffaello D’Andrea Dynamic Programming and Optimal Control, D. P. Bertsekas 相关博客庞特里亚金最小值原理解最优控制问题
通过BVP的方式能求得轨迹的解但通常不是最优的因此需要用到OBVP。
首先构建目标函数对jerk的平方的积分求取最小值 J Σ ∑ k 1 3 J k , J k 1 T ∫ 0 T j k ( t ) 2 d t . \begin{aligned}J_{\Sigma}\sum_{k1}^{3}J_{k},J_{k}\frac{1}{T}\int_{0}^{T}j_{k}(t)^2dt.\end{aligned} JΣk1∑3Jk,JkT1∫0Tjk(t)2dt.系统的状态变量是 s k ( p k , v k , a k ) s_k (p_k,v_k,a_k) sk(pk,vk,ak),系统的输入是 u k j k u_k j_k ukjk 系统模型为 s ˙ f s ( s k , u k ) ( v k , a k , j k ) \dot s f_s(s_k,u_k)(v_k,a_k,j_k) s˙fs(sk,uk)(vk,ak,jk), k k k代表了维度。
之后通过Pontryagin’s minimum principle庞特里亚金最小值原理进行求解。
对于Pontryagin’s minimum principle首先简要介绍一下costate协态 λ \lambda λ λ ( λ 1 , λ 2 , λ 3 ) \lambda (\lambda_1,\lambda_2,\lambda_3) λ(λ1,λ2,λ3),可以简单理解为有多少状态量就有多少costate。定义Hamiltonian(汉密尔顿)函数 H ( s , u , λ ) 1 T j 2 λ T f s ( s , u ) 1 T j 2 λ 1 v λ 2 a λ 3 j \begin{aligned} H\left(s,u,\lambda\right) \begin{aligned}\frac1Tj^2\lambda^Tf_s(s,u)\end{aligned} \\ \frac1Tj^2\lambda_1v\lambda_2a\lambda_3j \end{aligned} H(s,u,λ)T1j2λTfs(s,u)T1j2λ1vλ2aλ3j
Pontryagin’s minimum principle
通常来说最有问题的目标函数是由终末状态的惩罚项(final state)和从0到T时刻都能量损耗(transition cost)。
接下来写出系统的汉密尔顿函数。它由状态量 s s s, 控制量 u u u,以及协态costate λ \lambda λ组成 H ( s , u , λ ) g ( s , u ) λ T f ( s , u ) λ ( λ 1 , λ 2 , λ 3 ) \begin{aligned}H(s,u,\lambda)g(s,u)\lambda^Tf(s,u)\\\lambda(\lambda_1,\lambda_2,\lambda_3)\end{aligned} H(s,u,λ)λg(s,u)λTf(s,u)(λ1,λ2,λ3)最后求解出最优的状态函数 s ∗ s^* s∗,表示轨迹以及最优的控制量 u ∗ u^* u∗。 λ ( t ) \lambda(t) λ(t)是如下方程的解 λ ˙ ( t ) − ∇ s H ( s ∗ ( t ) , u ∗ ( t ) , λ ( t ) ) \dot{\lambda}(t)-\nabla_sH(s^*(t),u^*(t),\lambda(t)) λ˙(t)−∇sH(s∗(t),u∗(t),λ(t))具有如下边界条件 λ ( T ) − ∇ h ( s ∗ ( T ) ) \lambda(T)-\nabla h(s^*(T)) λ(T)−∇h(s∗(T))h被定义成终末状态的函数。这个公式代表终末状态时间 T T T的时候协态与终末状态中最优的自由量的梯度的和等于0我们要求上面的问题中的终末状态的p,v,a均达到指定状态那么这个公式就没了因为终末状态没有任何自由度。 最后最优的控制是具有最优状态的汉密尔顿函数的最小值 u ∗ ( t ) arg min u ( t ) H ( s ∗ ( t ) , u ( t ) , λ ( t ) ) \begin{aligned}u^*(t)\arg\min_{u(t)}H(s^*(t),u(t),\lambda(t))\end{aligned} u∗(t)argu(t)minH(s∗(t),u(t),λ(t))
应用
为了方便设定 k 1 k1 k1则系统模型可以如下表述 s ˙ f s ( s , u ) ( v , a , j ) \dot s f_s(s,u)(v,a,j) s˙fs(s,u)(v,a,j)其汉密尔顿函数为 H ( s , u , λ ) 1 T j 2 λ T f s ( s , u ) 1 T j 2 λ 1 v λ 2 a λ 3 j \begin{aligned} H\left(s,u,\lambda\right) \begin{aligned}\frac1Tj^2\lambda^Tf_s(s,u)\end{aligned} \\ \frac1Tj^2\lambda_1v\lambda_2a\lambda_3j \end{aligned} H(s,u,λ)T1j2λTfs(s,u)T1j2λ1vλ2aλ3j对 s s s的每个分量 ( p , v , a ) (p,v,a) (p,v,a)进行求得可得 λ ˙ − ∇ s H ( s ∗ , u ∗ , λ ) ( 0 , − λ 1 , − λ 2 ) \dot{\lambda}-\nabla_sH\left(s^*,u^*,\lambda\right)\left(0,-\lambda_1,-\lambda_2\right) λ˙−∇sH(s∗,u∗,λ)(0,−λ1,−λ2) λ 1 \lambda_1 λ1的导数是 λ 2 \lambda_2 λ2 λ 2 \lambda_2 λ2的导数是 λ 3 \lambda_3 λ3易得常微分方程组的解为 λ ( t ) [ α − α t β 0.5 α t 2 − β t γ ] \lambda(t)\begin{bmatrix}\alpha\\-\alpha t\beta\\0.5\alpha t^2-\beta t\gamma\end{bmatrix} λ(t) α−αtβ0.5αt2−βtγ 配凑系数后可得 λ ( t ) 1 T [ − 2 α 2 α t 2 β − α t 2 − 2 β t − 2 γ ] \lambda(t)\frac1T\begin{bmatrix}-2\alpha\\2\alpha t2\beta\\-\alpha t^2-2\beta t-2\gamma\end{bmatrix} λ(t)T1 −2α2αt2β−αt2−2βt−2γ 接下来求解 u u u的最优解由 u ∗ ( t ) j ∗ ( t ) arg min j ( t ) H ( s ∗ ( t ) , j ( t ) , λ ( t ) ) u^*(t)j^*(t)\arg\min_{j(t)}H(s^*(t),j(t),\lambda(t)) u∗(t)j∗(t)argminj(t)H(s∗(t),j(t),λ(t))可知 u u u的最优解与 s ∗ ( t ) s^*(t) s∗(t)有关。 ∵ s ∗ ( t ) ( p ∗ , v ∗ , a ∗ ) s^*(t)(p^*,v^*,a^*) s∗(t)(p∗,v∗,a∗) ∴汉密尔顿函数可以改写为 H ( s , u , λ ) 1 T j 2 λ T f s ( s , u ) 1 T j 2 λ 1 v ∗ λ 2 a ∗ λ 3 j \begin{aligned} H(s,u,\lambda) \frac1Tj^2\lambda^Tf_s(s,u) \\ \frac1Tj^2\lambda_1v^*\lambda_2a^*\lambda_3j \end{aligned} H(s,u,λ)T1j2λTfs(s,u)T1j2λ1v∗λ2a∗λ3j此时可得 H H H目前只剩下 j j j一个变量只需对其求得便可求得控制量 u u u: u ∗ ( t ) j ∗ ( t ) arg min j ( t ) H ( s ∗ ( t ) , j ( t ) , λ ( t ) ) 1 2 α t 2 β t γ \begin{aligned} u^{*}(t) j^*(t)\arg\min_{j(t)}H\left(s^*(t),j(t),\lambda(t)\right) \\ \frac12\alpha t^2\beta t\gamma \end{aligned} u∗(t)j∗(t)argj(t)minH(s∗(t),j(t),λ(t))21αt2βtγ现在有了最优的控制表达式进行一次积分获得 a a a,在积分获得 v v v,在积分获得 p p p于是 s ∗ ( t ) [ α 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 ] Initial state: s ( 0 ) ( p 0 , v 0 , a 0 ) \begin{aligned}\mathrm{s}^*(t)\left[\begin{aligned}\frac{\alpha}{120}t^5\frac{\beta}{24}t^4\frac{\gamma}{6}t^3\frac{a_0}{2}t^2v_0tp_0\\\frac{\alpha}{24}t^4\frac{\beta}{6}t^3\frac{\gamma}{2}t^2a_0tv_0\\\frac{\alpha}{6}t^3\frac{\beta}{2}t^2\gamma ta_0\end{aligned}\right]\\\text{Initial state:}\quad s(0)(p_0,v_0,a_0)\end{aligned} s∗(t) 120αt524βt46γt32a0t2v0tp024αt46βt32γt2a0tv06αt32βt2γta0 Initial state:s(0)(p0,v0,a0)现在 s ∗ ( t ) s^*(t) s∗(t)中的 α 、 β 、 γ \alpha、\beta、\gamma α、β、γ还未确定可以用末尾时刻的边界条件进行确定。令 t T tT tT同时代入末尾状态 s f s_f sf时的 p , v , a p,v,a p,v,a的值可得 [ 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 ] [ Δ 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 ] \begin{gathered} \begin{bmatrix}\dfrac{1}{120}T^5\dfrac{1}{24}T^4\dfrac{1}{6}T^3\\\dfrac{1}{24}T^4\dfrac{1}{6}T^3\dfrac{1}{2}T^2\\\dfrac{1}{6}T^3\dfrac{1}{2}T^2T\end{bmatrix}\begin{bmatrix}\alpha\\\beta\\\gamma\end{bmatrix}\begin{bmatrix}\Delta p\\\Delta v\\\Delta a\end{bmatrix} \\ \begin{bmatrix}\Delta p\\\Delta v\\\Delta a\end{bmatrix}\begin{bmatrix}p_f-p_0-\boldsymbol{v}_0T-\frac12\boldsymbol{a}_0T^2\\\boldsymbol{v}_f-\boldsymbol{v}_0-\boldsymbol{a}_0T\\\boldsymbol{a}_f-\boldsymbol{a}_0\end{bmatrix} \end{gathered} 1201T5241T461T3241T461T321T261T321T2T αβγ ΔpΔvΔa ΔpΔvΔa pf−p0−v0T−21a0T2vf−v0−a0Taf−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 ] \begin{bmatrix}\alpha\\\beta\\\gamma\end{bmatrix}\frac1{T^5}\begin{bmatrix}720-360T60T^2\\-360T168T^2-24T^3\\60T^2-24T^33T^4\end{bmatrix}\begin{bmatrix}\Delta p\\\Delta v\\\Delta a\end{bmatrix} αβγ T51 720−360T60T2−360T168T2−24T360T2−24T33T4 ΔpΔvΔa
将结果回代目标函数可得 J γ 2 β γ T 1 3 β 2 T 2 1 3 α γ T 2 1 4 α β T 3 1 20 α 2 T 4 \begin{aligned}J\gamma^2\beta\gamma T\frac13\beta^2T^2\frac13\alpha\gamma T^2\frac14\alpha\beta T^3\frac1{20}\alpha^2T^4\end{aligned} Jγ2βγT31β2T231αγT241αβT3201α2T4可见因为 α 、 β 、 γ \alpha、\beta、\gamma α、β、γ都与 T T T相关显然可得目标函数 J J J只与 T T T相关。对 J J J求导就可以求得最优解。
另外 若在末尾状态时不完全指定 p , v , a p,v,a p,v,a时仍然能利用pontryagin’s minimum princple进行计算。利用边界条件 λ ( t ) − ∇ h ( s ∗ ( t ) ) \lambda(t)-\nabla h(s^*(t)) λ(t)−∇h(s∗(t)), 对于末尾条件确定的情况 h ( s ( T ) ) { 0 , i f s s ( T ) ∞ , o t h e r w i s e h(s(T))\begin{cases}0,\quad if\quad ss(T)\\\infty,\quad otherwise\end{cases} h(s(T)){0,∞,ifss(T)otherwise并不可微。
对于末尾条件不完全确定的情况则对未给定的量利用边界条件求偏导进行计算。 g i v e n s i ( T ) , i ∈ I given\quad s_i(T),\mathrm{~}i\in I givensi(T), i∈I λ j ( T ) ∂ h ( s ∗ ( T ) ) ∂ s j , f o r j ≠ i \lambda_j(T)\frac{\partial h(s^*(T))}{\partial s_j},~for~j\neq i λj(T)∂sj∂h(s∗(T)), for ji
论文的附录也给出了相应的情况 其他相关论文Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly
Trajectory library轨迹库
利用轨迹库使用cost函数进行评价、打分作局部路径规划 相关论文Maximum Likelihood Path Planning for Fast Aerial Maneuvers and Collision Avoidance Ji Zhang, Chen Hu, Rushat Gupta Chadha, and Sanjiv Singh
Heuristic design启发式设计
通过启发函数加速搜索。结合不考虑障碍物的估计方法与不考虑动力学的估计方法。
论文Planning Long Dynamically Feasible Maneuvers for Autonomous Vehicles, Maxim Likhachev, Dave Ferguson 论文Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, James Diebel
Planning in Frenet-serret Frame PS1:横纵向解耦 PS2:构造横纵方向上的五次多项式确定起始条件和终止条件车道线跟踪时的终止条件要注意
论文Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun 论文Optimal trajectories for time-critical street scenarios using discretized terminal manifolds, Moritz Werling, Sören Kammel, Julius Ziegler and Lutz Gröll
参考文献
[1] Search-based Motion Planning for Quadrotors using Linear Quadratic Minimum Time ControlSikang Liu, Nikolay Atanasov, Kartik Mohta, and Vijay Kumar [2] Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State SpacesMihail Pivtoraiko and Alonzo Kelly [3] Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly [4] State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments, Thomas M. Howard, Colin J. Green, and Alonzo Kelly [5] A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, Mark W. Mueller, Markus Hehn, and Raffaello D’Andrea Dynamic Programming and Optimal Control, D. P. Bertsekas [6] 庞特里亚金最小值原理解最优控制问题 [7] Maximum Likelihood Path Planning for Fast Aerial Maneuvers and Collision Avoidance Ji Zhang, Chen Hu, Rushat Gupta Chadha, and Sanjiv Singh [8] Planning Long Dynamically Feasible Maneuvers for Autonomous Vehicles, Maxim Likhachev, Dave Ferguson [9] Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, James Diebel [10] Optimal trajectories for time-critical street scenarios using discretized terminal manifolds, Moritz Werling, Sören Kammel, Julius Ziegler and Lutz Gröll [11] Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun
