苏州专业做网站公司电话,网站建设之婚礼摄影网站设计,logo网站素材,北京门户企业网站建设本系列文章用于回顾学习记录Fast-Planner规划算法的相关内容#xff0c;【本系列博客写于2023年9月#xff0c;共包含四篇文章#xff0c;现在进行补发第一篇#xff0c;其余几篇文章将在近期补发】 一、Fast Planner前端 Fast Planner的轨迹规划部分一共分为三个模块… 本系列文章用于回顾学习记录Fast-Planner规划算法的相关内容【本系列博客写于2023年9月共包含四篇文章现在进行补发第一篇其余几篇文章将在近期补发】 一、Fast Planner前端 Fast Planner的轨迹规划部分一共分为三个模块前端采用Hybrid A* 算法生成比较粗糙的路径\轨迹然后采用后端对前端生成的路径进行更加细致的处理和优化。 本部分内容对前端进行介绍Hybrid A* 算法的原理在前面的文章中已经介绍过了这里主要介绍在Fast Planner中具体使用的一些流程和细节。 Fast Planner中 Hybrid A* 算法的主要流程如下 在每轮循环中首先会从优先队列中拿出新的节点 n c n_c nc并判断该节点是否为终点以及从该节点能不能直接生成到终点的路径比如采用Reeds sheep算法若是则规划结束返回路径若不是则继续进行本轮循环 在该新节点 n c n_c nc处向外拓展生成一些小的轨迹这些小轨迹称为primitive它们的末端就是拓展出来的节点然后进行剪枝操作如果有多个节点落在同一个栅格中仅保留一个讲这些新拓展出来的节点存放在nodes中。 然后对nodes中的每个节点的质量进行评估伪代码8-16行对于每个节点首先检查其是否已经在已经拓展过的闭集合中以及是否是可行节点(在边界范围内、不与障碍物碰撞等) 若在闭集合或不是可行节点则结束对这个节点的评估继续进行下一个节点的评估。若不在闭集合中且为可行节点则继续进行该节点的评估计算该节点的g值即其父节点 n c n_c nc的g值加上该节点对应的小轨迹的代价值。然后判断该节点是否在待拓展的开集合中若不在则将其加入到开集合中记录该节点的g值以及父节点并计算该节点的f值该节点的评估结束若已经在开集合中了则判断该节点上面算出的新g值是否大于原有的g值若是则不需要进行处理该节点原有的方案更优继续评估下一个节点若不是则说明该节点的现有方案更优将该节点的父节点更新为 n c n_c ncg值更新为新的g值并更新计算该节点的总代价f值。本节点评估结束继续评估下一个节点。 下面来详细看一下上面流程中的一些具体细节 【注Hybrid A * 算法每个具体执行过程的实现都有很多种方法在前面的文章中我们给出了 zhm_real/MotionPlanning运动规划库中的实现方法及细节这里给出Fast Planner中的具体实现细节】 1、如何由节点 n c n_c nc拓展生成小轨迹primitive——对应Expend函数 将xy、z轴的轨迹用三个独立的多项式来表示比如用二次多项式 p x ( t ) a 0 a 1 t a 2 t 2 p_x(t)a_0a_1ta_2t^2 px(t)a0a1ta2t2自变量是时间t则状态量和控制输入量可表示成以下的形式 x ( t ) : [ p ( t ) T , p ˙ ( t ) T , . . . , p ( n − 1 ) ( t ) T ] T ⊂ R 3 n u ( t ) : p ( n ) ( t ) ∈ U : [ − u m a x , u m a x ] 3 ⊂ R 3 \begin{aligned}\mathbf{x}(t):\boxed{\left[\mathbf{p}(t)^{\mathrm{T}}, \mathbf{\dot p}(t)^{\mathrm{T}},...,\mathbf{p}^{(n-1)}(t)^{\mathrm{T}}\right]^{\mathrm{T}}}\subset\mathbb{R}^{3n}\\\mathbf{u}(t):\mathbf{p}^{(n)}(t)\in\mathcal{U}:[-u_{max},u_{max}]^3\subset\mathbb{R}^3\end{aligned} x(t):[p(t)T,p˙(t)T,...,p(n−1)(t)T]T⊂R3nu(t):p(n)(t)∈U:[−umax,umax]3⊂R3 然后我们就可以写出状态空间方程 x ˙ A x B u A [ 0 I 3 0 ⋯ 0 0 0 I 3 ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 ⋯ ⋯ 0 I 3 0 ⋯ ⋯ 0 0 ] , B [ 0 0 ⋮ 0 I 3 ] \begin{gathered}\dot{\mathbf{x}}\mathbf{A}\mathbf{x}\mathbf{B}\mathbf{u}\\\mathbf{A}\begin{bmatrix}0\mathbf{I}_3\mathbf{0}\cdots\mathbf{0}\\\mathbf{0}\mathbf{0}\mathbf{I}_3\cdots\mathbf{0}\\\varvdots\varvdots\varvdots\ddots\varvdots\\\mathbf{0}\cdots\cdots\mathbf{0}\mathbf{I}_3\\\mathbf{0}\cdots\cdots\mathbf{0}\mathbf{0}\end{bmatrix},\mathbf{B}\begin{bmatrix}\mathbf{0}\\\mathbf{0}\\\varvdots\\\mathbf{0}\\\mathbf{I}_3\end{bmatrix}\end{gathered} x˙AxBuA 00⋮00I30⋮⋯⋯0I3⋮⋯⋯⋯⋯⋱0000⋮I30 ,B 00⋮0I3 这样我们给定一个初始状态和一段时间内的控制输入里以后就可以利用下式计算整条小轨迹上任意时刻的状态 x ( t ) e A t x ( 0 ) ∫ 0 t e A ( t − τ ) B u ( τ ) d τ initial state control input \begin{aligned}\mathbf{x}(t)e^{\mathbf{A}t}\color{red}{\boxed{\mathbf{x}(0)}}\color{black}\int_0^te^{\mathbf{A}(t-\tau)}\mathbf{B}\color{red}{\boxed{\mathbf{u}(\tau)}}\color{black} d\tau\\\color{red}{\text{initial state}}\color{red}{\text{control input}}\end{aligned} x(t)eAtx(0)∫0teA(t−τ)Binitial stateu(τ)dτcontrol input 实际使用时会对控制量在上下界范围内进行平均的离散采样得到多组控制输入量从而得到多条小轨迹 在Fast-Planner中n取值为2即状态选取为位置和速度输入为加速度 2、如何计算每段小轨迹的代价值——对应EdgeCost函数 对于一条小轨迹在Fast-Planner中我们比较在意的是这条轨迹的执行时间和控制量所以定义如下的代价函数不同的需求和实际应用场景可以选择不同的代价函数 T ( T ) ∫ 0 T ∥ u ( t ) ∥ 2 d t ρ T T(T)\int_{0}^{T}\|\mathbf{u}(t)\|^{2}dt\rho T T(T)∫0T∥u(t)∥2dtρT 其中 T T T表示这一段小轨迹的时间 u u u是控制量 ρ \rho ρ表示对时间惩罚项的权重参数对于一条小轨迹而言在0~T时间内它的控制输入是固定的常量每条小轨迹的总时间T也是固定的 所以我们并不需要计算上面的积分它等效于使用下式来计算小轨迹的代价值 τ \tau τ即为小轨迹的持续时间 e c ( ∥ u d ∥ 2 ρ ) τ e_{c}(\|\mathbf{u}_{d}\|^{2}\rho)\tau ec(∥ud∥2ρ)τ 然后我们把从起点开始的到当前节点的所有小轨迹的代价加起来就得到了从起点到当前节点的代价值也就是该节点的g值如下所示 g c ∑ j 1 J ( ∥ ( u d ) j ∥ 2 ρ ) τ g_c\sum_{j1}^J\left(\left\|(\mathbf{u}_d)_j\right\|^2\rho\right)\tau gcj1∑J(∥(ud)j∥2ρ)τ 3、如何评估一个节点的启发代价值——对应Heuristic函数 基于庞特里亚金最小原理设计了一条三阶的多项式轨迹从当前状态出发终止于目标点轨迹的总时长已知给定当前点和目标点的位置和速度如下所示 p ( t ) a 3 t 3 a 2 t 2 a 1 t a 0 p ( 0 ) p μ c , p ( 0 ˙ ) v μ c p ( T ) p μ g , p ( T ˙ ) v μ g \begin{aligned}p(t)a_3t^3a_2t^2a_1ta_0\\p(0)p_{\mu c},\quad p(\dot{0})v_{\mu c}\\p(T)p_{\mu g},\quad p(\dot{T})v_{\mu g}\end{aligned} p(t)p(0)p(T)a3t3a2t2a1ta0pμc,p(0˙)vμcpμg,p(T˙)vμg 因此可以得到该多项式系数满足下式 [ 0 0 0 1 0 0 1 0 T 3 T 2 T 1 3 T 2 2 T 1 0 ] [ a 3 a 2 a 1 a 0 ] [ p μ c v μ c p μ g v μ g ] \begin{bmatrix}0001\\0010\\T^3T^2T1\\3T^22T10\end{bmatrix}\begin{bmatrix}a_3\\a_2\\a_1\\a_0\end{bmatrix}\begin{bmatrix}p_{\mu c}\\v_{\mu c}\\p_{\mu g}\\v_{\mu g}\end{bmatrix} 00T33T200T22T01T11010 a3a2a1a0 pμcvμcpμgvμg 然后基于庞特里亚金最小原理得到最优的轨迹表达式如下 p μ ∗ ( t ) 1 6 α μ t 3 1 2 β μ t 2 v μ c t p μ c [ α μ β μ ] 1 T 3 [ − 12 6 T 6 T − 2 T 2 ] [ p μ g − p μ c − v μ c T v μ g − v μ c ] \begin{aligned}p_\mu^*(t)\frac{1}{6}\alpha_\mu t^3\frac{1}{2}\beta_\mu t^2v_{\mu c}tp_{\mu c}\\\\\begin{bmatrix}\alpha_\mu\\\beta_\mu\end{bmatrix}\frac{1}{T^3}\begin{bmatrix}-126T\\6T-2T^2\end{bmatrix}\begin{bmatrix}p_{\mu g}-p_{\mu c}-v_{\mu c}T\\v_{\mu g}-v_{\mu c}\end{bmatrix}\end{aligned} pμ∗(t)[αμβμ]61αμt321βμt2vμctpμcT31[−126T6T−2T2][pμg−pμc−vμcTvμg−vμc] 得到上面的位置轨迹后求两阶导数可以得到加速度轨迹在取位置和速度为状态量时加速度也即控制输入 a μ ∗ ( t ) α μ t β μ u ( t ) : [ a x ( t ) , a y ( t ) , a z ( t ) ] T \begin{aligned}a_\mu^*(t)\alpha_\mu t\beta_\mu\\\\\mathbf{u}(t):[a_x(t),a_y(t),a_z(t)]^\mathrm{T}\end{aligned} aμ∗(t)αμtβμu(t):[ax(t),ay(t),az(t)]T 得到了这样一条轨迹后就可以利用上面介绍的求小轨迹的代价的方式求出这条轨迹的代价作为考虑运动学但不考虑障碍物的启发式代价函数。 T ∗ ( T ) ∫ 0 l ∥ u ( t ) ∥ 2 d t ρ T ∑ μ ∈ { x , y , z } ( 1 3 α μ 2 T 3 1 2 α μ β μ T 2 β μ 2 T ) ρ T \begin{aligned} \mathcal{T}^{*}(T) \int_{0}^{l}\|\mathbf{u}(t)\|^{2}dt\rho T \\ \sum_{\mu\in\{x,y,z\}}\left(\frac{1}{3}{\alpha_{\mu}}^{2}T^{3}\frac{1}{2}\alpha_{\mu}\beta_{\mu}T^{2}{\beta_{\mu}}^{2}T\right)\rho T \end{aligned} T∗(T)∫0l∥u(t)∥2dtρTμ∈{x,y,z}∑(31αμ2T321αμβμT2βμ2T)ρT 上面我们假设时间T是人为给定的、已知的但是很明显我们并不清楚从当前点到目标点合适的T应该如何选择。所以我们可以将上式对T进行求导等于0即 ∂ T ∗ ( T ) ∂ T 0 \frac{\partial\mathcal{T}^*(T)}{\partial T}0 ∂T∂T∗(T)0来求取最合适的 T h T_h Th进而得到代价值 参考资料 1、[深蓝学院—移动机器人运动规划] 链接放不了了感兴趣的小伙伴自行查找吧
