南阳高质量建设大市网站,wordpress保存文件路径,软件开发培训机构出来的人社保是怎么交的,tamed wordpress插件汉化Motion Plan之搜索算法笔记 Motion Plan之基于采样的路径规划算法笔记
为什么要动力学规划#xff1a;
前面几章介绍的路径规划#xff0c;我们只是认为机器人是质点#xff0c;这节课要说的就是#xff0c;如何在考虑机器人的运动学模型下再去找一个安全可行的路径。考虑…Motion Plan之搜索算法笔记 Motion Plan之基于采样的路径规划算法笔记
为什么要动力学规划
前面几章介绍的路径规划我们只是认为机器人是质点这节课要说的就是如何在考虑机器人的运动学模型下再去找一个安全可行的路径。考虑了机械运动学模型以后路径规划方法仍然可以分为像前两课一样归为基于搜索的和基于采样的。KINODYNAMIC是运动学和动力学两部分组成的机器人不是简单的一个质点可以随意运动的一个质点它的高阶模型受到一个微分的约束。Kinodynamic Path Finding是一种在机器人和运动规划中的问题类别其中必须满足速度、加速度和力/扭矩界限以及如避免障碍物等运动约束。这是一种解决运动规划问题的方法它考虑了机器人的动力学约束。这意味着kinodynamic规划算法输出的引导轨迹不仅要对机器人的环境避免碰撞而且还要对机器人的连续时间动力学的代表模型可行。 传统规划算法生成的路径无法准确匹配机器人的运动约束,使得规划与控制脱节。例如汽车的非完全约束限制其侧方移动,但传统算法可能生成需要急转的最短路径,很难优化成可行解。 尤其是高速运动场景下,初态末态间的精确运动学建模和控制求解至关重要,这需要求解两点边界值问题。 混合A*等算法试图在规划层面加入约束,但仍面临启发函数设计、离散化、连续性等困难,说明了这是一个 layer 上的系统性问题,而不是简单扩展现有方法就能完美解决。 更准确地对运动学建模,直接在规划层面加入约束,不仅能生成更优路径,也能减轻后续轨迹优化负担,改善整体规划控制系统的协同性。由于系统的微分约束状态对之间的直线连接通常不是有效的轨迹。找到的路径越平滑优化它就越容易。 综上,动力学规划为使机器人运动更符合自身约束条件、实现更流畅高效的规划与控制提供了可能。与损失函数设计等其他问题一样,这也是实现复杂机器人功能必需解决的基本问题。 总计一下之所以要在Front end阶段就做KINODYNAMIC path find原因如下
传统规划方法只考虑避障,不能进行平滑运动实际机器人受运动学约束和动力学约束影响,不是质点模型必须在规划层面考虑这些约束,生成符合机器人运动能力的路径这是实现机器人自动控制和功能的基础使机器人运动更平滑自然,逼近人的运动减少后端轨迹优化的负担,提高整体规划和控制的协同性有利于我们理解和建模机器人系统,为更复杂控制算法打基础
前置知识
所谓的动力学建模其实就是在对移动机器人做刚体形状建模然后利用数学方程对建模出来的移动机器人刚体做动力学的建模。得到一个可以通过调整控制变量向量来控制移动机器人做状态变化的过程。
四旋翼无人机建模
建模流程 各部分介绍 KINODYNAMIC控制数学形式
位置动力学模型
牛顿第二定律 $m\dot{v}{e}G{e}T^{b} $ 式中 v ˙ \dot{v} v˙, 与G右上角的e代表这是地面坐标系下的向量T右上角的b代表这是机体坐标系下的向量。由于拉力是由螺旋桨产生的与四旋翼飞行器固连故在机体坐标系下表示。 等式两边同时除以m v ˙ e g e T b m \dot{v}^{e}g^{e}\frac{T^{b}}{m} v˙egemTb 将机体坐标系下的拉力转换到地面坐标系左乘旋转矩阵即可 $ot{v}{e}g{e}R_{b}{e}\frac{T{b}}{m} \$ 展开成矩阵形式其中g与T都是标量g为重力加速度T为四个螺旋桨产生的总升力 [ v ˙ x v ˙ y v ˙ z ] g [ 0 0 1 ] 1 m ∗ [ c o s θ c o s ψ ; c o s ψ s i n θ s i n ϕ − s i n ψ c o s ϕ c o s ψ s i n θ c o s ϕ s i n ψ s i n ϕ c o s θ s i n ψ ; s i n ψ s i n θ s i n ϕ c o s ψ c o s ϕ s i n ψ s i n θ c o s ϕ − c o s ψ s i n ϕ − s i n θ ; s i n ϕ c o s θ c o s ϕ c o s θ ] ∗ [ 0 0 − T ] \begin{bmatrix} \dot{v}_{x}\\ \dot{v}_{y}\\ \dot{v}_{z} \end{bmatrix}g\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}\frac{1}{m}\ast \begin{bmatrix} cos\theta cos\psi ; cos\psi sin\theta sin\phi-sin\psi cos\phi cos\psi sin\theta cos\phisin\psi sin\phi \\ cos\theta sin\psi ; sin\psi sin\theta sin\phicos\psi cos\phi sin\psi sin\theta cos\phi-cos\psi sin\phi\\ -sin\theta; sin\phi cos\theta cos\phi cos\theta \end{bmatrix}\ast\begin{bmatrix} 0\\ 0\\ -T \end{bmatrix} v˙xv˙yv˙z g 001 m1∗ cosθcosψ;cosψsinθsinϕ−sinψcosϕcosψsinθcosϕsinψsinϕcosθsinψ;sinψsinθsinϕcosψcosϕsinψsinθcosϕ−cosψsinϕ−sinθ;sinϕcosθcosϕcosθ ∗ 00−T 整理得 $\left{\begin{array}{l} \dot{v}{x}-\frac{T}{m}(\cos \psi \sin \theta \cos \phi\sin \psi \sin \phi) \ \dot{v}{y}-\frac{T}{m}(\sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi) \ \dot{v}_{z}g-\frac{T}{m} \cos \phi \cos \theta \end{array}\right. \$
姿态动力学模型
由欧拉方程可得 J ω ˙ b ω b × J ω b G a b τ b {J} \dot{{\omega}}^{b}{\omega}^{b} \times {J} {\omega}^{b}{G}_{a}^b{\tau}^b Jω˙bωb×JωbGabτb 式中 ω ˙ b \dot{{\omega}}^{b} ω˙b 表示在机体坐标系下的角速度 G a b {G}_{a}^b Gab表示陀螺力矩 τ b {\tau}^b τb 表示螺旋桨在机体轴上产生的力矩 。 角速度 ω b [ ω x b ω y b ω z b ] [ p q r ] \boldsymbol{\omega}^{b}\left[\begin{array}{l} \omega_{x}^b \\ \omega_{y}^b \\ \omega_{z}^b \end{array}\right]\left[\begin{array}{l} p \\ q \\ r \end{array}\right] ωb ωxbωybωzb pqr 陀螺力矩 Ga 当电机高速旋转的时候相当于一个陀螺。高速旋转的陀螺是非常稳定的个体具有保持自身轴向不变的能力 G a [ G a , ϕ G a , θ G a , ψ ] [ J 1 q ( ϖ 1 − ϖ 2 ϖ 3 − ϖ 4 ) J 1 p ( − ϖ 1 ϖ 2 − ϖ 3 ϖ 4 ) 0 ] {G}_{\boldsymbol{a}}\left[\begin{array}{c} G_{a, \phi} \\ G_{a, \theta} \\ G_{a, \psi} \end{array}\right]\left[\begin{array}{c} J_1 q\left(\varpi_{1}-\varpi_{2}\varpi_{3}-\varpi_{4}\right) \\ J_1 p\left(-\varpi_{1}\varpi_{2}-\varpi_{3}\varpi_{4}\right) \\ 0 \end{array}\right] Ga Ga,ϕGa,θGa,ψ J1q(ϖ1−ϖ2ϖ3−ϖ4)J1p(−ϖ1ϖ2−ϖ3ϖ4)0 基于假设1惯性矩阵 J可表示为 J [ I x x a m p ; − I x y a m p ; − I x z − I x y a m p ; I y y a m p ; − I y z − I x z a m p ; − I y z a m p ; I z z ] [ I x x a m p ; a m p ; a m p ; I y y a m p ; a m p ; a m p ; I z z ] \boldsymbol{J}\left[\begin{array}{rrr} I_{x x} amp; -I_{x y} amp; -I_{x z} \\ -I_{x y} amp; I_{y y} amp; -I_{y z} \\ -I_{x z} amp; -I_{y z} amp; I_{z z} \end{array}\right]\left[\begin{array}{lll} I_{x x} amp; amp; \\ amp; I_{y y} amp; \\ amp; amp; I_{z z} \end{array}\right] J Ixx−Ixy−Ixzamp;−Ixyamp;Iyyamp;−Iyzamp;−Ixzamp;−Iyzamp;Izz Ixxamp;amp;Iyyamp;amp;amp;amp;Izz 可得 [ I x x a m p ; a m p ; a m p ; I y y a m p ; a m p ; a m p ; I z z ] [ p ˙ q ˙ r ˙ ] [ p a m p ; q a m p ; r I x x p a m p ; I y y q a m p ; I z z r i a m p ; j a m p ; k ] [ J 1 q ( ϖ 1 − ϖ 2 ϖ 3 − ϖ 4 ) J 1 p ( − ϖ 1 ϖ 2 − ϖ 3 ϖ 4 ) 0 ] [ τ x τ y τ z ] \left[\begin{array}{lll} I_{x x} amp; amp; \\ amp; I_{y y} amp; \\ amp; amp; I_{z z} \end{array}\right]\left[\begin{array}{l} \dot{p} \\ \dot{q} \\ \dot{r} \end{array}\right]\begin{bmatrix} p amp; q amp; r \\ I_{xx}p amp; I_{yy}q amp; I_{zz}r\\ i amp; j amp; k \end{bmatrix}\left[\begin{array}{c} J_1 q\left(\varpi_{1}-\varpi_{2}\varpi_{3}-\varpi_{4}\right) \\ J_1 p\left(-\varpi_{1}\varpi_{2}-\varpi_{3}\varpi_{4}\right) \\ 0 \end{array}\right]\begin{bmatrix} \tau_x\\ \tau_y\\ \tau_z \end{bmatrix} Ixxamp;amp;Iyyamp;amp;amp;amp;Izz p˙q˙r˙ pIxxpiamp;qamp;Iyyqamp;jamp;ramp;Izzramp;k J1q(ϖ1−ϖ2ϖ3−ϖ4)J1p(−ϖ1ϖ2−ϖ3ϖ4)0 τxτyτz 整理得 { p ˙ 1 I x x [ τ x q r ( I y y − I z z ) − J 1 q Ω ] q ˙ 1 I y y [ τ y p r ( I z z − I x x ) J 1 p Ω ] r ˙ 1 I z z [ τ z p q ( I x x − I y y ) ] \left\{\begin{array}{l} \dot{p}\frac{1}{I_{x x}}\left[\tau_{x}q r\left(I_{y y}-I_{z z}\right)-J_1 q \Omega\right] \\ \dot{q}\frac{1}{I_{y y}}\left[\tau_{y}p r\left(I_{z z}-I_{x x}\right)J_1 p \Omega\right] \\ \dot{r}\frac{1}{I_{z z}}\left[\tau_{z}p q\left(I_{x x}-I_{y y}\right)\right] \end{array}\right. ⎩ ⎨ ⎧p˙Ixx1[τxqr(Iyy−Izz)−J1qΩ]q˙Iyy1[τypr(Izz−Ixx)J1pΩ]r˙Izz1[τzpq(Ixx−Iyy)]
四旋翼飞行器的运动学模型
运动学模型的输入为速度和角速度输出为位置和姿态 速度与位置的方程 { p ˙ e v e p e [ x a m p ; y a m p ; z ] T [ x ˙ a m p ; y ˙ a m p ; z ˙ ] T [ v x a m p ; v y a m p ; v z ] T \left\{\begin{array}{l} \dot{\boldsymbol{p}}^{e}\boldsymbol{v}^{e} \\ \boldsymbol{p}^{e}\left[\begin{array}{lll} x amp; y amp; z \end{array}\right]^{T} \\ \left[\begin{array}{lll} \dot{x} amp; \dot{y} amp; \dot{z} \end{array}\right]^{T}\left[\begin{array}{lll} v_{x} amp; v_{y} amp; v_{z} \end{array}\right]^{T} \end{array}\right. ⎩ ⎨ ⎧p˙evepe[xamp;yamp;z]T[x˙amp;y˙amp;z˙]T[vxamp;vyamp;vz]T 姿态角的变化率与机体的旋转角速度有如下关系: { Θ ˙ W ⋅ ω b Θ ˙ [ ϕ ˙ a m p ; θ ˙ a m p ; ψ ˙ ] T ω b [ p a m p ; q a m p ; r ] T W [ 1 a m p ; tan θ sin ϕ a m p ; tan θ cos ϕ 0 a m p ; cos ϕ a m p ; − sin ϕ 0 a m p ; sin ϕ / cos θ a m p ; cos ϕ / cos θ ] \left\{\begin{array}{l} \dot{\boldsymbol{\Theta}}\boldsymbol{W} \cdot \boldsymbol{\omega}^{b} \\ \dot{\boldsymbol{\Theta}}\left[\begin{array}{lll}\dot{\phi} amp; \dot{\theta} amp; \dot{\psi}\end{array}\right]^{T} \\ \omega^b \left[\begin{array}{lll}p amp; q amp; r\end{array}\right]^{T} \\ \boldsymbol{W}\left[\begin{array}{ccc} 1 amp; \tan \theta \sin \phi amp; \tan \theta \cos \phi \\ 0 amp; \cos \phi amp; -\sin \phi \\ 0 amp; \sin \phi / \cos \theta amp; \cos \phi / \cos \theta \end{array}\right] \end{array}\right. ⎩ ⎨ ⎧Θ˙W⋅ωbΘ˙[ϕ˙amp;θ˙amp;ψ˙]Tωb[pamp;qamp;r]TW 100amp;tanθsinϕamp;cosϕamp;sinϕ/cosθamp;tanθcosϕamp;−sinϕamp;cosϕ/cosθ 整理可得 [ ϕ ˙ θ ˙ ψ ˙ ] [ 1 a m p ; tan θ sin ϕ a m p ; tan θ cos ϕ 0 a m p ; cos ϕ a m p ; − sin ϕ 0 a m p ; sin ϕ / cos θ a m p ; cos ϕ / cos θ ] [ p q r ] \left[\begin{array}{l} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right]\left[\begin{array}{ccc} 1 amp; \tan \theta \sin \phi amp; \tan \theta \cos \phi \\ 0 amp; \cos \phi amp; -\sin \phi \\ 0 amp; \sin \phi / \cos \theta amp; \cos \phi / \cos \theta \end{array}\right]\left[\begin{array}{l} p \\ q \\ r \end{array}\right] ϕ˙θ˙ψ˙ 100amp;tanθsinϕamp;cosϕamp;sinϕ/cosθamp;tanθcosϕamp;−sinϕamp;cosϕ/cosθ pqr
汽车建模
车辆动力学建模是指通过数学模型描述车辆在不同运动和操控情况下的行为。这种建模是车辆动力学控制的基础能够帮助设计控制系统以优化车辆性能和稳定性。 车辆动力学模型及其控制的数学形式通常通过一系列微分方程来表示这些方程描述了车辆在运动过程中的各种物理量随时间的演变。 车辆动力学控制的目标是通过调整操纵输入使车辆达到期望的运动性能。典型的车辆动力学控制系统包括横向和纵向控制。 车辆动力学模型有很多种以下是一些常见的模型
单轨模型这是一种简化的模型它将车辆视为一个刚体只考虑车辆的横向运动。这种模型通常用于研究车辆的稳定性和操控性。双轨模型这种模型比单轨模型更复杂一些它将车辆视为两个并行的刚体左右两侧可以分别考虑车辆的横向和纵向运动。这种模型可以更准确地模拟车辆的动态行为。多体模型这种模型将车辆分解为多个相互连接的刚体如车身、悬挂系统、轮胎等每个刚体都有自己的运动方程。这种模型可以模拟车辆的复杂动态行为如悬挂系统的运动、轮胎的滑动等。非线性模型这种模型考虑了车辆动力学中的非线性因素如轮胎的非线性摩擦特性、悬挂系统的非线性刚度等。这种模型可以更准确地模拟车辆在极限条件下的行为。自行车模型Bicycle Model
首先要简化汽车运动其中自行车模型就是简单且有效的简化方式。自行车模型基于如下几个假设 车辆在垂直方向的运动被忽略掉了也就是说我们描述的车辆是一个二维平面上的运动物体可以等价与我们是站在天空中的俯视视角 车辆自行车模型是一种常用的车辆动力学模型它将车辆简化为一个自行车只考虑车辆的横向运动和转向运动。这种模型主要用于研究车辆的稳定性和操控性。 在自行车模型中车辆的运动状态可以由以下几个变量表示
位置车辆的位置通常由车辆的横向位置和纵向位置表示。速度车辆的速度通常由车辆的横向速度和纵向速度表示。航向角航向角是车辆的前进方向与水平方向的夹角。转向角转向角是车辆前轮与车辆的夹角。
车辆的动力学行为则可以通过以下的微分方程来描述
位置的变化率位置的变化率等于速度。速度的变化率速度的变化率等于加速度。航向角的变化率航向角的变化率等于车辆的转向速度。转向角的变化率转向角的变化率等于车辆的转向加速度。
其中 θ是其在 Yaw 方向的偏转角度它是相对于 x 轴的逆时针方向的角度v 是 θ 方向的速度L是车辆的轴距前后轮胎的距离, (x,y) 是车辆的坐标。 可以通过以下的微分方程来描述
位置的变化率位置的变化率等于速度。在二维平面上我们可以将其分解为x和y两个方向。假设车辆的速度为v航向角为θ那么位置的变化率可以表示为 d x d t v . c o s ( θ ) \frac{dx}{dt} v.cos(\theta) dtdxv.cos(θ) d y d t v . s i n ( θ ) \frac{dy}{dt} v.sin(\theta) dtdyv.sin(θ) 速度的变化率速度的变化率等于加速度。假设车辆的加速度为a那么速度的变化率可以表示为 d v d t a \frac{dv}{dt} a dtdva 航向角的变化率航向角的变化率等于车辆的转向速度。假设车辆的转向速度为ω那么航向角的变化率可以表示为 d t d θ ω \frac{dt}{d\theta} \omega dθdtω
这些方程描述了车辆在给定控制输入如油门、刹车和转向下的运动行为。
简化汽车模型 ( x . y . θ . ) ( v c o s θ v s i n θ r L t a n ϕ ) \begin{pmatrix} \stackrel{.}{x} \\ \stackrel{.}{y} \\ \stackrel{.}{\theta} \\ \end{pmatrix} \begin{pmatrix} v cos\theta\\ vsin\theta \\ \frac{r}{L}tan\phi\\ \end{pmatrix} x.y.θ. vcosθvsinθLrtanϕ
单轮模型和差速驱动模型” ( x . y . θ . ) ( c o s θ s i n θ 0 ) . v ( 0 0 1 ) . ω \begin{pmatrix} \stackrel{.}{x} \\ \stackrel{.}{y} \\ \stackrel{.}{\theta} \\ \end{pmatrix} \begin{pmatrix} cos\theta \\ sin\theta \\ 0 \\ \end{pmatrix}.v \begin{pmatrix} 0 \\ 0 \\ 1 \\ \end{pmatrix}.\omega x.y.θ. cosθsinθ0 .v 001 .ω ( x . y . θ . ) ( r 2 ( μ l μ r ) c o s θ r 2 ( μ l μ r ) s i n θ r L ( μ r − μ l ) ) \begin{pmatrix} \stackrel{.}{x} \\ \stackrel{.}{y} \\ \stackrel{.}{\theta} \\ \end{pmatrix} \begin{pmatrix} \frac{r}{2}(\mu_l\mu_r) cos\theta\\ \frac{r}{2}(\mu_l\mu_r)sin\theta \\ \frac{r}{L}(\mu_r-\mu_l) \\ \end{pmatrix} x.y.θ. 2r(μlμr)cosθ2r(μlμr)sinθLr(μr−μl)
经过动力学建模得到了一个控制方程前面几章也介绍了如何把移动机器人当成质点得到了一条质点轨迹路径。那么接下来的事情就是如何把动力学建模得到的控制方程融合到质点轨迹路径中。然后把各种约束融合进代价函数通过求解代价函数最小化得到符合约束的满足动力学控制要求的轨迹。 那么动力学控制方程融入质点轨迹主流有两种方法控制离散化、状态离散化。 1.控制离散化就是说我们已经知道动力学控制方程了那么我们能不能把质点轨迹上每个质点出了坐标信息还带上控制向量信息控制向量是一个连续的向量不好表示所以为了更方便求解减少求解空间我们就把控制向量离散化变成有限的可选项。这样可以更方便的写出代价函数也更容易求解得出代动力学约束的可行解。 2.状态离散意思就是我们把质点轨迹上每个质点带上移动机器人在这个质点的状态信息。然而状态也是一个连续的向量解空间大不利于求解。所以我们把状态离散成有限的状态表示这样在求解时候知道每个质点的状态求解问题就变成在如何求解状态间转移矩阵符合代价函数最小化问题。 下面章节就会具体介绍如何求解带状态约束的路径问题。
正文
State Lattice Planning
状态栅格顾名思义就在在普通的栅格化地图的基础上多加了机器人运动学模型的约束确保机器人能沿着相邻两点之间生成的路径运动并且能够具有状态连续性速度连续等。
如何构建状态栅格
状态栅格图中每个顶点的连线都是根据机器人的运动学模型生成的都是切实可行的路径而普通栅格 (grid)的方法并没有结合模型。在构建状态栅格图State Lattice Search之前需要对被控对象进行运动学分析即建立对应的运动学模型。运动学模型主要描述了位置、速度、加速度、转向角度等等状态量在时间上的状态转移以及对所关心的这些状态量的边界条件进行约束。 从控制空间和状态空间中进行采样对于机器人模型′ (, ) • 机器人是差动驱动的。 • 我们有机器人的初始状态 ₀。 • 对于State Lattice Search通常有两种采样方式
基于控制空间Control Space)的采样
在已知被控对象的情况下我们固定输入量u控制量与积分时 间T由此进行前向积分即可得到被控对象从任意初始状态s_{0}过度到的终止状态s_{f}。前向积分的方式能够简单粗暴地对被控对象在任意时间T之后的状态进行预测从而得到一系列状态集合这就是在控制空间中的采样。由于输入量u与积分时间T都是任意给定的因此基于控制空间的采样往往是没有明确的目的性的它的采样结果只能分布在某些状态附近但不能精确地采样到某一个给定状态。 选择一个固定时间间隔对系统进行前向模拟数值积分。 • 前向模拟 • 固定 , • 无任务引导 • 易于实现 • 规划效率较低 具体流程是: (1)为机器人选择一个初始状态s0。 (2)对控制输入u进行离散抽样,得到一组可行的控制集合{u1, u2,…}。 (3)对每一个控制输入ui,选择一个时间段T,进行前向仿真(numerical integration)获得一个终止状态si。 (4)重复步骤(2)-(3),直到构建完整的状态栅格图。
例子
设无人机机器人状态方程如下如下 State: s ( x y z x ˙ y ˙ z ˙ ) Input: u ( x ¨ y ¨ z ¨ ) s ˙ A ⋅ s B ⋅ u A [ 0 a m p ; I 3 a m p ; 0 a m p ; ⋯ a m p ; 0 0 a m p ; 0 a m p ; I 3 a m p ; ⋯ a m p ; 0 ⋮ a m p ; ⋱ a m p ; ⋱ a m p ; ⋱ a m p ; ⋮ 0 a m p ; ⋯ a m p ; ⋯ a m p ; 0 a m p ; I 3 0 a m p ; ⋯ a m p ; ⋯ a m p ; 0 a m p ; 0 ] B [ 0 0 ⋮ 0 I 1 ] \text{State: }\mathbf{s}{\left(\begin{array}{l}{x}\\ {y}\\ {z}\\ {\dot{x}}\\ {\dot{y}}\\ {\dot{z}}\end{array}\right)} \quad \text{Input: }u\left(\begin{matrix}{\ddot{x}}\\ {\ddot{y}}\\ {\ddot{z}}\\ \end{matrix}\right)\\ \dot{s}A\cdot sB\cdot u \\A\begin{bmatrix}0amp;I_3amp;0amp;\cdotsamp;0\\ 0amp;0amp;I_3amp;\cdotsamp;0\\ \vdotsamp;\ddotsamp;\ddotsamp;\ddotsamp;\vdots\\ 0amp;\cdotsamp;\cdotsamp;0amp;I_3\\ 0amp;\cdotsamp;\cdotsamp;0amp;0\end{bmatrix}\quad B\begin{bmatrix}0\\ 0\\ \vdots\\ 0\\ I_1\end{bmatrix} State: s xyzx˙y˙z˙ Input: u x¨y¨z¨ s˙A⋅sB⋅uA 00⋮00amp;I3amp;0amp;⋱amp;⋯amp;⋯amp;0amp;I3amp;⋱amp;⋯amp;⋯amp;⋯amp;⋯amp;⋱amp;0amp;0amp;0amp;0amp;⋮amp;I3amp;0 B 00⋮0I1 固定输入量 μ \mu μ和和时间 T然后进行前向积分就可以知道T后的机器人状态。尤其是当机器人是线性系统的时候。在知道T时刻机器人的末状态之后就可以通过状态转移方程 $s(t) e{At}s_0[\int_0te^{A(t-\sigma)}Bd\sigma]u_m\$ 来还原出过程之中的各个状态其中矩阵A是状态转移矩阵。假设 t τ t\tau tτ要求解 e A t e^{At} eAt的求解是一件很麻烦的事我们可以利用泰勒展开 $e{At}I\frac{At}{1!}\frac{(At)2}{2!}\frac{(At)3}{3!}\cdots\frac{(At)k}{k!}\cdots \$ A矩阵有一个性质就是nilpotent也就是说它在多次方之后会变为0那高次项就都被消去了这使得 e A t e^{At} eAt在(n-1)次矩阵多项式中都具有封闭的表达式更多的求解方式可参考现代控制理论。
上图展示了从初始状态x0开始的9个平面运动基元的示例。左图是一个受加速度控制n 2的系统右图是一个受冲量Jerk控制n 3的系统。黑箭头指示对应的控制输入。红色边界显示了由控制限制umax引导的终态的可行区域。初始速度和加速度分别为 v 0 v_0 v0 [1, 0, 0]^T和 a 0 a_0 a0 [0, 1, 0]^T仅适用于右图。 通过搜索获得的格子图注意事项
在搜索过程中可以在需要时再构建图形节省存储空间和计算时间。当发现新的节点状态时创建节点和边运动基元。节省计算时间/空间。 上图表示的是9个离散化示例和25个离散化示例
给一个更宏观的车的例子可以看到基于控制空间采样的应用 车状态方程 S t a t e : s ( x y θ ) I n p i t : μ ( v ϕ ) S y s t e m e q u a t i o n : ( x . y . θ . ) ( v c o s θ v s i n θ r L t a n ϕ ) State: s\begin{pmatrix} {x} \\ {y} \\ {\theta} \\ \end{pmatrix} \\ Inpit: \mu \begin{pmatrix} {v} \\ {\phi} \\ \end{pmatrix} \\ System equation: \begin{pmatrix} \stackrel{.}{x} \\ \stackrel{.}{y} \\ \stackrel{.}{\theta} \\ \end{pmatrix} \begin{pmatrix} v cos\theta\\ vsin\theta \\ \frac{r}{L}tan\phi\\ \end{pmatrix} State:s xyθ Inpit:μ(vϕ)Systemequation: x.y.θ. vcosθvsinθLrtanϕ 对于搜索树中的每一个 ∈
选择一个控制向量在短时间内积分方程将无碰撞的运动添加到搜索树中
具体步骤如下
选择一个 ∈ 选择, ∅ 和 从开始积分运动如果无碰撞则添加结果
基于状态空间State Space)的采样¹
这种采样方式则是在给定被控对象的初始以及目标状态后反向计算 中间过程。相对于前者该采样方式在工程上较难实施我们需要对其进行两点边界值问题的求解才能求得两个状态之间轨迹¹。两点边界值问题BVP经典的两点边界值问题其实就是多项式方程求系数的过程。然而求解BVP问题能够求解一条可通行轨迹却无法求得一条最优轨迹。或者当其边界条件部分确定时如位置确定速度、加速度状态不确定需要求解OBVP问题。OBVP问题可以认为是一个最优控制问题。对基于状态空间的采样方法而言求解OBVP问题即可求得任意两点之间符合运动学方程的最优轨迹。 选择一个 ₘ, 找到连接轨迹₀和ₘ之间的路径。 • 反向计算 • 需要计算 , • 良好的任务引导 • 难以实现 • 高规划效率 具体流程是: (1) 在状态空间中选择一个初始状态s0。 (2) 对s0的8个邻居状态进行边界值问题(boundary value problem, BVP)求解,找到从s0到邻居状态的可行轨迹。 (3) 以找到的邻居状态为中心,重复步骤(2),向外扩展构建状态栅格图。 总的来说状态栅格规划器State Lattice Planner是一种基于采样的满足动力学约束的路径规划算法³。它通过基于控制空间的采样方式和基于状态空间的采样方式构建出满足动力学约束的路径¹²。这两种方法各有优缺点基于控制空间的采样方式实现相对简单但效率不高而基于状态空间的采样方式虽然在工程上较难实施但它使得算法具有启发性和贪心性。
例子 图1Reeds-Shepp Car的可达性图。显示了可达性图构建的各个阶段。一个有三个动作的特殊离散化的Reeds-Shepp Car可以在工作空间中任意移动。
图2为Reeds-Shepp Car构造格子。在a)中我们定义了一个C空间的离散化这里选择了一个网格箭头表示允许的航向选择了一个原点b)对于原点周围的8个邻居节点找到了可行的路径c)同样的查询向外扩展到24个邻居只显示了一些直接的路径d)完整的格子。 构造状态格子 图1显示了一个Reeds-Shepp Car的可达性图它有三个动作适当地离散化了。随着汽车无限地行驶图是通过“编译”所有可行的运动而构造的。**然而状态格子是通过使用逆向路径生成器来找到从网格中的任何一个节点到原点图2中的黑色方块的路径而构造的。**在构造格子时选择原点是任意的因为从任何节点都有相同的运动集合可以将所有可行的路径复制到格子中的任何节点。在极限情况下当图1中的图通过Reeds-Shepp car无限长地扩展而图2中的格子通过包含从原点到任何节点的可行路径并将所得到的集合复制到所有节点而构建时由于表示所有可行的路径图和格子将是相同的。在不失一般性的情况下认为状态格子是系统可达性图的有效表示。 离散化 离散化将运动规划问题转化为一个顺序决策过程。采用典型的策略假设只在离散状态下做出决策。状态是格子中的节点连接状态的运动是边。格子中的每个节点代表一个4维姿态包括2D位置、航向和曲率。这种表示使得格子能够足够通用满足汽车的典型非完整约束和完整约束。假设位置被离散化为分隔的矩形网格任何有用的航向离散化都是可接受的。现在假设均匀地划分区间对于在区间中离散化曲率也是如此其中是最大曲率是最小转弯半径的倒数用与相同的长度单位表示。 离散化的规则性 如果离散化具有一定程度的规则性那么两个给定状态之间的空间关系会由于存在其他相同排列的状态对而经常重复。具有一定程度规则性的离散化导致了一组类似的运动选项。 状态格点的构造 假设状态空间离散化在至少平移坐标上是规则的。具体来说如果两个姿态之间的路径是可行的那么对于任意整数值另一个路径也是可行的。虽然这两种运动的起始和终止状态是不同的但运动本身可能以转向函数编码是相同的。事实上现在只考虑从原点发出的运动集合。只需要显式地表示这个规范集合。通过在任何其他节点重复这个表示就可以得到整个格点图2b。 车辆在多个可行离散状态空间采样两层示例图
两层格子图只有第一层不同因为不同的初始状态。不同的初始状态指的是系统从不同的位置、航向和曲率开始运动。这会影响格子图中的节点和边的分布和连接。 状态空间采样和控制空间采样是两种生成状态格子的方法它们的差异和优缺点如下
状态空间采样是一种从状态空间中采样配置然后用可行路径连接它们的方法。这种方法的优点是可以保证状态格子的完备性即任何可行的运动都可以在状态格子中找到。利用逆向路径生成器inverse path generator来找到从任意节点到目标节点的可行路径然后将这些路径作为状态格子的边。缺点是需要解决逆运动规划问题即给定起始和终止配置找到一条可行路径。这种方法的难点是如何高效地解决逆向路径生成问题即如何在有限的时间内找到一条满足运动约束、最小化某个代价函数、并且终止于离散状态的路径。通常需要使用数值方法或者近似方法可能会导致计算开销大或者解的质量低³控制空间采样是一种从控制空间中采样控制输入然后用它们生成一组基本运动motion primitives并将它们连接到状态格子中的方法。利用一组预先定义的基本运动motion primitives来构造状态格子的边。这些基本运动是由一些常量或者多项式控制输入产生的可以用解析的方式表示。这种方法的优点是可以避免逆运动规划问题而且可以根据不同的优化准则设计基本运动。缺点是需要保证基本运动的覆盖性即任何可行的运动都可以由基本运动的组合来近似。这种方法的难点是如何设计基本运动的集合使得它们能够覆盖所有可能的运动方向同时又能够满足一些优化标准例如最小化曲率变化、最大化直线运动等。这个问题通常需要精心地设计控制空间的离散化和基本运动的生成算法²。通常需要根据具体的运动模型和应用场景进行调整和优化。
求解
开始求解之前其实你应该问问自己为什么一个采样器需要去求解一个状态转移方程问题呢其实原因很简单如果有解就意味着状态采样点是可达的也就说这个点是有效的。 上面章节介绍了“基于状态空间采样”的采样点的生成流程里面有一个问题就是知道了起始、终止点的状态位置坐标、动力学模型参数向量如何求解状态转移方程。这个章节主要就是介绍如何求解状态转移方程会涉及到边界值求解知识。
边值问题BVPBVP是状态采样格子规划的基础。没有通用的解决方案BVP需要根据具体情况进行设计。通常涉及复杂的数值优化BVP需要设计一条轨迹 使得 (0) (T)
如何对求解问题建模 用5阶多项式来表示轨迹 _ _5^5 _4^4 _3^3 2_^2 _1 _0 边界条件 | | 位置 | 速度 | 加速度 | | — | — | — | — | | 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 T 2 1 1 0 0 0 0 2 0 0 20 T 3 12 T 2 6 T 1 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}000001\\T^5T^4T^3T^2T1\\ 000010\\ 5T^44T^33T^2T2^110\\000200\\20T^312T^26T^1200 \end{bmatrix} \begin{bmatrix}c_5\\c_4 \\ c_3\\ c_2\\c_1\\c_0 \end{bmatrix} ab0000 0T505T4020T30T404T3012T20T303T206T10T20T21220T1100110000 c5c4c3c2c1c0
针对约束条件建模最优化求解方程
BVP问题中加入一个优化目标确保生成成的状态曲线更加符合的工程实践比如定一个Objective要使得加速度的平方和最小状态转移的过程要尽可能的快如下所示 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^T g(x,u)dt\int_0^T(1u^TRu)dt\int_0^T(1a_x^2a_y^2a_z^2)dt J∫0Tg(x,u)dt∫0T(1uTRu)dt∫0T(1ax2ay2az2)dt 状态量输入量和系统方程表征如下 x ( p x p y p z v x v x v z ) , u ( a x a y a z ) , x ˙ f ( x , u ) ( v x v y v z a x a y a z ) x\begin{pmatrix}p_x\\ p_y\\ p_z \\ v_x\\ v_x\\ v_z\end{pmatrix},u\begin{pmatrix}a_x\\ a_y\\ a_z\end{pmatrix},\dot xf(x,u)\begin{pmatrix}v_x\\ v_y \\ v_z\\ a_x\\ a_y\\ a_z\end{pmatrix} x pxpypzvxvxvz ,u axayaz ,x˙f(x,u) vxvyvzaxayaz 边界条件表征如下 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 ) , e n d s t a t e : x ( T ) ( p x f p y f p z f ? ? ? ) i n i t i a l~s t a t e:x(0)\left(\begin{matrix}p_{x0}\\ p_{y0}\\ p_{z0}\\ v_{x0}\\ v_{y0}\\ v_{z0}\end{matrix}\right),end~state :x(T) \left(\begin{matrix}p_{xf}\\ p_{yf}\\ p_{zf}\\ ? \\ ?\\ ?\end{matrix}\right) initial state:x(0) px0py0pz0vx0vy0vz0 ,end state:x(T) pxfpyfpzf???
如何对问题求解
最优边值问题OBVP问题求解中的Pontryagin’s minimum principle是一种间接法可以用来求解有约束的最优控制问题。
Pontryagin’s minimum principle的基本思想是引入一个共轭变量或者说伴随变量构造一个哈密顿函数然后利用必要的最优性条件将原问题转化为一个边值问题再用数值方法求解。Pontryagin’s minimum principle的一般形式如下
假设有一个最优控制问题目标是最小化一个积分型的性能指标 min u J ( u ) ∫ t 0 t f f 0 ( x ( t ) , u ( t ) , t ) d t \min_u J(u) \int_{t_0}^{t_f} f_0(x(t),u(t),t) dt minuJ(u)∫t0tff0(x(t),u(t),t)dt 其中 x ( t ) x(t) x(t)是状态变量 u ( t ) u(t) u(t)是控制变量 f 0 f_0 f0是给定的函数。同时需要满足状态方程和边界条件 x ˙ ( t ) f ( x ( t ) , u ( t ) , t ) , x ( t 0 ) x 0 , x ( t f ) x f \dot{x}(t) f(x(t),u(t),t), \quad x(t_0) x_0, \quad x(t_f) x_f x˙(t)f(x(t),u(t),t),x(t0)x0,x(tf)xf 其中 f f f是给定的函数 x 0 x_0 x0和 x f x_f xf是给定的常数。另外还有控制约束 u ( t ) ∈ U , ∀ t ∈ [ t 0 , t f ] u(t) \in U, \quad \forall t \in [t_0, t_f] u(t)∈U,∀t∈[t0,tf] 其中 U U U是给定的控制集合通常是一个闭凸集。 Pontryagin’s minimum principle给出了这个问题的一个必要条件即存在一个共轭变量 λ ( t ) \lambda(t) λ(t)使得以下条件成立
共轭方程 λ ˙ ( t ) − ∂ H ∂ x ( x ∗ ( t ) , u ∗ ( t ) , λ ( t ) , t ) \dot{\lambda}(t) -\frac{\partial H}{\partial x}(x^*(t),u^*(t),\lambda(t),t) λ˙(t)−∂x∂H(x∗(t),u∗(t),λ(t),t)最小值原理 H ( x ∗ ( t ) , u ∗ ( t ) , λ ( t ) , t ) min u ∈ U H ( x ∗ ( t ) , u , λ ( t ) , t ) H(x^*(t),u^*(t),\lambda(t),t) \min_{u \in U} H(x^*(t),u,\lambda(t),t) H(x∗(t),u∗(t),λ(t),t)minu∈UH(x∗(t),u,λ(t),t)边界条件 λ ( t f ) ∂ f 0 ∂ x ( x ∗ ( t f ) , u ∗ ( t f ) , t f ) \lambda(t_f) \frac{\partial f_0}{\partial x}(x^*(t_f),u^*(t_f),t_f) λ(tf)∂x∂f0(x∗(tf),u∗(tf),tf)非平凡性 λ ( t ) ≠ 0 , ∀ t ∈ [ t 0 , t f ] \lambda(t) \neq 0, \quad \forall t \in [t_0, t_f] λ(t)0,∀t∈[t0,tf]
其中 H H H是哈密顿函数定义为 H ( x , u , λ , t ) f 0 ( x , u , t ) λ T f ( x , u , t ) H(x,u,\lambda,t) f_0(x,u,t) \lambda^T f(x,u,t) H(x,u,λ,t)f0(x,u,t)λTf(x,u,t) 其中 x ∗ ( t ) x^*(t) x∗(t)和 u ∗ ( t ) u^*(t) u∗(t)是最优状态和控制 λ T \lambda^T λT是 λ \lambda λ的转置。
Pontryagin’s minimum principle的应用步骤如下² 根据原问题构造哈密顿函数 H H H。根据共轭方程求解 λ ( t ) \lambda(t) λ(t)的微分方程。根据最小值原理求解 u ∗ ( t ) u^*(t) u∗(t)的表达式。将 u ∗ ( t ) u^*(t) u∗(t)代入状态方程求解 x ∗ ( t ) x^*(t) x∗(t)的微分方程。根据边界条件求解初始共轭变量 λ ( t 0 ) \lambda(t_0) λ(t0)的值。回代求解最优状态和控制 x ∗ ( t ) x^*(t) x∗(t)和 u ∗ ( t ) u^*(t) u∗(t)的值。 Pontryagin’s minimum principle的优点是可以处理一般形式的最优控制问题而不需要假设控制变量是可微的或者有界的。它也可以推广到多阶段、多目标、无穷维等复杂情况。缺点是它只给出了一个必要条件而不是充分条件所以可能存在多个候选解需要进一步检验。它也需要求解一个非线性的边值问题这可能涉及复杂的数值方法。 Generally中的 h ( s ( T ) ) h(s(T)) h(s(T))是一种软约束可以看成一种罚函数简单来说就是越趋近于期望状态的时候值就越小。但因为此处表示的是状态变量的软约束所以如若固定了终末时间机器人的所有状态则除了目标状态的 h ( s ( T ) ) 0 h(s(T))0 h(s(T))0之外其它情况下都为无穷会导致极小值原理中的 λ ( T ) − ∇ h ( s ∗ ( T ) ) \lambda(T)-\nabla h(s^*(T)) λ(T)−∇h(s∗(T))在T处不可导。如果只固定某一些状态量比如只固定位置那么边界条件则又恢复了可导性。 Pontryagin’s minimum principle的详细解释应用步骤
第一步根据原问题构造哈密顿函数 H H H。哈密顿函数是一个关于状态变量 x x x控制变量 u u u共轭变量 λ \lambda λ和时间 t t t的函数它的定义为 H ( x , u , λ , t ) f 0 ( x , u , t ) λ T f ( x , u , t ) H(x,u,\lambda,t) f_0(x,u,t) \lambda^T f(x,u,t) H(x,u,λ,t)f0(x,u,t)λTf(x,u,t) 其中 f 0 f_0 f0是原问题中的性能指标函数 f f f是原问题中的状态方程函数 λ T \lambda^T λT是 λ \lambda λ的转置。哈密顿函数的作用是将原问题中的积分型的目标函数转化为一个点型的函数便于求解。
第二步根据共轭方程求解 λ ( t ) \lambda(t) λ(t)的微分方程。共轭方程是一个关于 λ \lambda λ的一阶微分方程它的形式为 λ ˙ ( t ) − ∂ H ∂ x ( x ∗ ( t ) , u ∗ ( t ) , λ ( t ) , t ) \dot{\lambda}(t) -\frac{\partial H}{\partial x}(x^*(t),u^*(t),\lambda(t),t) λ˙(t)−∂x∂H(x∗(t),u∗(t),λ(t),t) 其中 λ ˙ \dot{\lambda} λ˙表示 λ \lambda λ对 t t t的导数 ∂ H ∂ x \frac{\partial H}{\partial x} ∂x∂H表示 H H H对 x x x的偏导数 x ∗ ( t ) x^*(t) x∗(t)和 u ∗ ( t ) u^*(t) u∗(t)是最优状态和控制。共轭方程的作用是将原问题中的状态变量和控制变量之间的关系转化为共轭变量和状态变量之间的关系便于求解。
第三步根据最小值原理求解 u ∗ ( t ) u^*(t) u∗(t)的表达式。最小值原理是一个关于 u u u的最优性条件它的形式为 H ( x ∗ ( t ) , u ∗ ( t ) , λ ( t ) , t ) min u ∈ U H ( x ∗ ( t ) , u , λ ( t ) , t ) H(x^*(t),u^*(t),\lambda(t),t) \min_{u \in U} H(x^*(t),u,\lambda(t),t) H(x∗(t),u∗(t),λ(t),t)minu∈UH(x∗(t),u,λ(t),t) 其中 U U U是原问题中的控制约束集合。最小值原理的作用是将原问题中的积分型的目标函数转化为一个极值问题便于求解。
第四步将 u ∗ ( t ) u^*(t) u∗(t)代入状态方程求解 x ∗ ( t ) x^*(t) x∗(t)的微分方程。状态方程是一个关于 x x x的一阶微分方程它的形式为 x ˙ ( t ) f ( x ( t ) , u ( t ) , t ) \dot{x}(t) f(x(t),u(t),t) x˙(t)f(x(t),u(t),t) 其中 x ˙ \dot{x} x˙表示 x x x对 t t t的导数。状态方程的作用是描述系统的动态行为便于求解。
第五步根据边界条件求解初始共轭变量 λ ( t 0 ) \lambda(t_0) λ(t0)的值。边界条件是一个关于 λ \lambda λ的代数方程它的形式为 λ ( t f ) ∂ f 0 ∂ x ( x ∗ ( t f ) , u ∗ ( t f ) , t f ) \lambda(t_f) \frac{\partial f_0}{\partial x}(x^*(t_f),u^*(t_f),t_f) λ(tf)∂x∂f0(x∗(tf),u∗(tf),tf) 其中 ∂ f 0 ∂ x \frac{\partial f_0}{\partial x} ∂x∂f0表示 f 0 f_0 f0对 x x x的偏导数 t f t_f tf是终止时间。边界条件的作用是确定共轭变量的初始值便于求解。
第六步回代求解最优状态和控制 x ∗ ( t ) x^*(t) x∗(t)和 u ∗ ( t ) u^*(t) u∗(t)的值。这一步是将前面求得的 λ ( t 0 ) \lambda(t_0) λ(t0) u ∗ ( t ) u^*(t) u∗(t)和 x ∗ ( t ) x^*(t) x∗(t)的表达式或数值代入得到最优解的具体形式或数值。这一步的作用是得到原问题的最优解便于分析和应用。
对于前面章节建模的最优化方程写出哈密顿函数 KaTeX parse error: {align} can be used only in display mode. 其中 λ ( λ 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为协态向量(costate)系统模型有多少个状态变量就有多少个协态。 当状态量和控制量都为最优时 λ ( t ) \boldsymbol{\lambda}(t) λ(t)是汉密顿函数的负梯度求解微分方程可以得到 λ ˙ \dot{\boldsymbol{\lambda}} λ˙以及 λ ( t ) \boldsymbol{\lambda}(t) λ(t)。 $\dot{\lambda}-\nabla H(x*,u,\lambda)(0\quad0\quad0\quad-\lambda_1\quad-\lambda_2\quad-\lambda_3)^T \$ λ ( t ) ( 2 α 1 2 α 2 2 α 3 − 2 α 1 t − 2 β 1 − 2 α 2 t − 2 β 2 − 2 α 3 t − 2 β 3 ) \lambda(t)\begin{pmatrix}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{pmatrix} λ(t) 2α12α22α3−2α1t−2β1−2α2t−2β2−2α3t−2β3 当状态量为最优 x ∗ x^* x∗时使得汉密顿函数取最小时候的控制量即是我们的最优控制输入 μ ∗ \mu^* μ∗。 $u*\arg\min\limits_{u(t)}H(x(t),u(t),\lambda(t))\begin{pmatrix}\alpha_1t \beta_1\ \alpha_2t \beta_2\ \alpha_3t \beta_3\end{pmatrix} \$ 通过两次前向积分, 得到最优的状态量 x ∗ ( t ) x^*(t) x∗(t)为 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 ) , x^{*}\left(\begin{array}{c}\frac{1}{6} \alpha_{1} t^{3}\frac{1}{2} \beta_{1} t^{2}v_{x 0} tp_{x 0} \\\frac{1}{6} \alpha_{2} t^{3}\frac{1}{2} \beta_{2} t^{2}v_{y 0} tp_{y 0} \\\frac{1}{6} \alpha_{3} t^{3}\frac{1}{2} \beta_{3} t^{2}v_{z 0} tp_{z 0} \\\frac{1}{2} \alpha_{1} t^{2}\beta_{1} tv_{x 0} \\\frac{1}{2} \alpha_{2} t^{2}\beta_{2} tv_{y 0} \\\frac{1}{2} \alpha_{3} t^{2}\beta_{3} tv_{z 0}\end{array}\right), x∗ 61α1t321β1t2vx0tpx061α2t321β2t2vy0tpy061α3t321β3t2vz0tpz021α1t2β1tvx021α2t2β2tvy021α3t2β3tvz0 , 在极小值原理中有边界条件如下 λ ( T ) − ∇ h ( x ∗ ( T ) ) \lambda(T)-\nabla h(x^*(T)) λ(T)−∇h(x∗(T)) 因为末端速度这个维度未制定,因此 h ( x ∗ ( T ) ) h(x^*(T)) h(x∗(T))显然和变量 v v v没什么关系因此求导为 0 ( λ 4 ( T ) λ 5 ( T ) λ 6 ( T ) ) ( − 2 α 1 T − 2 β 1 − 2 α 2 T − 2 β 2 − 2 α 3 T − 2 β 3 ) 0 \begin{pmatrix} \lambda_4(T)\\ \lambda_5(T)\\ \lambda_6(T)\end{pmatrix}\begin{pmatrix} -2\alpha_1T-2\beta_1\\ -2\alpha_2T-2\beta_2\\ -2\alpha_3T-2\beta_3\end{pmatrix}\boldsymbol{0} λ4(T)λ5(T)λ6(T) −2α1T−2β1−2α2T−2β2−2α3T−2β3 0 所以我们可以得到一个新的 λ ( t ) \lambda(t) λ(t): β − α T \boldsymbol{\beta}-\boldsymbol{\alpha}T β−αT 然后我们再利用终末条件 X ( T ) X(T) X(T)来求解得到 α \alpha α: α − 3 ( P f − P 0 − v 0 T ) T 3 − 3 Δ P T 3 , \boldsymbol{\alpha}-\frac{3(\boldsymbol{P_f}-\boldsymbol{P_0}-\boldsymbol{v_0}T)}{T^3}-\frac{3\Delta {\boldsymbol{P}}}{T^3}, α−T33(Pf−P0−v0T)−T33ΔP, 其中 P f P_f Pf是末端的位置状态, P 0 P_0 P0和 v 0 v_0 v0是起始的位置状态和速度状态. 所以我们得到最优的控制输入为 μ ∗ \mu^* μ∗: KaTeX parse error: {align} can be used only in display mode.
其中 ∥ Δ P ∥ 2 Δ P x 2 Δ P y 2 Δ P z 2 {\lVert \boldsymbol{\Delta P} \rVert}^2 \Delta P^2_x \Delta P^2_y \Delta P^2_z ∥ΔP∥2ΔPx2ΔPy2ΔPz2 J和 T 无关. 如果想要得到 J的最小值需要 J 的导数为0。 T 4 ( 2 V 0 x Δ P x 2 V 0 y Δ P y 2 V 0 z Δ P z ) ( 9 ∥ Δ P ∥ 2 ) 0 T^4(2V_{0x}\Delta P_x 2V_{0y}\Delta P_y 2V_{0z}\Delta P_z)(9{\lVert \boldsymbol{\Delta P} \rVert}^2) 0 T4(2V0xΔPx2V0yΔPy2V0zΔPz)(9∥ΔP∥2)0 到这边我们就已经可以求解出每个状态采样点的状态参数也就是我们可以得到每个采样点的具体状态参数了每个点都是确定了。换一句话说就是我们废了这么多力气到这个章节就是解决了State Lattice的建图过程下面章节要介绍的就是如何在图上做路径搜索。
状态栅格图上进行图搜索 图已经有了接下来的问题是我们要如何从图中得到想要的路径规划。前面几篇文章介绍的方法在这边是否还可以用呢答案是肯定的只是需要根据现在问题做个适配求解。 单层格子规划是一种常用的局部避障方案。不需要图搜索只需要轨迹选择。根据一个多项代价函数碰撞风险信息获取舒适度能量……对每条轨迹进行评分。
混合A*控制空间检索 如果要在线生成密集的动力学格点网格花费太多时间。如何剪枝一些节点呢局部点选择、根据下一节点走势做减枝、每个节点只算前2步…定义一个剪枝规则使用网格地图。
具体代码流程如下
• Maintain a priority queue to store all the nodes to be expanded
• The heuristic function h(n) for all nodes are pre-defined
• The priority queue is initialized with the start state XS
• Assign g(XS)0, and g(n)infinite for all other nodes in the graph
• Loop• If the queue is empty, return FALSE; break;#n(n)Choose a proper heuristic according to previous slides• Remove the node “n” with the lowest f(n)g(n)h(n) from the priority queue• Mark node “n” as expanded• If the node “n” is the goal state, return TRUE; break;#neighbors Find neighbors by forward integrating the state in the node.• For all unexpanded neighbors “m” of node “n”• If g(m) infinite• g(m) g(n) Cnm• Push node “m” into the queue#add Record the state inside node “m”• If g(m) g(n) Cnm• g(m) g(n) Cnm#add Update the state inside node “m”• end
• End LoopRRT*搜索路径
RRT算法和带动力学约束的路径生成RRT得差异
带动力学约束的路径生成如何采样
LTI线性时不变系统状态空间方程 x ( t ) A x ( t ) B u ( t ) c \mathbf{x}(t) \mathbf{Ax}(t) \mathbf{Bu}(t) \mathbf{c} x(t)Ax(t)Bu(t)c 例如对于双重积分器系统 x [ p v ] , A [ 0 I 0 0 ] , B [ 0 I ] \mathbf{x} \begin{bmatrix} p \\ v \end{bmatrix}, \quad \mathbf{A} \begin{bmatrix} 0 \mathbf{I} \\ 0 0 \end{bmatrix}, \quad \mathbf{B} \begin{bmatrix} 0 \\ \mathbf{I} \end{bmatrix} x[pv],A[00I0],B[0I] 与类似 RRT快速随机树在欧几里得空间中采样不同这要求在完整的状态空间中进行采样。
如何寻找近临点 如果没有运动约束可以使用欧几里得距离或曼哈顿距离。在具有运动约束的状态空间中引入最优控制。 如果引入最优控制我们可以定义从一个状态转移到另一个状态的成本函数。通常采用的是时间-能量最优的二次形式 c ( π ) ∫ 0 τ ( 1 u ( t ) T R u ( t ) ) d t c(\pi) \int_0^{\tau} \left(1 u(t)^T R u(t)\right) \, dt c(π)∫0τ(1u(t)TRu(t))dt 其中 c ( π ) c(\pi) c(π)是成本函数 π \pi π表示路径 u ( t ) u(t) u(t)表示在时间 t t t处的控制输入 τ \tau τ是路径的时间长度 R R R是权重矩阵。这个成本函数采用了时间和能量的二次形式对路径进行评估希望找到最小成本的路径。 如果从一个状态转移到另一个状态的成本较小那么这两个状态就是接近的。请注意如果反向转移成本可能会不同。 如果我们知道到达时间 τ \tau τ和状态转移的控制策略 u ( t ) u(t) u(t)我们就可以计算成本。值得庆幸的是这一切都包含在经典最优控制解决方案中OBVP。
无界最优控制解决方案
固定最终状态 x 1 x_1 x1固定最终时间 τ \tau τ u t ∗ R − 1 B T e A T ( τ − t ) G ( τ ) − 1 ( x 1 − e A τ x 0 ) u^*_t R^{-1}B^T e^{A^T (\tau - t)} G(\tau)^{-1} (x_1 - e^{A\tau}x_0) ut∗R−1BTeAT(τ−t)G(τ)−1(x1−eAτx0) 而 x t {x}_t xt描述如果没有施加控制输入状态 x x x在时间 t t t会是什么 x t e A t x 0 − ∫ 0 t e A ( t − t ′ ) θ x t ′ d t ′ x_t e^{At}x_0 - \int_0^t e^{A(t - t)} \theta x_{t} \, dt xteAtx0−∫0teA(t−t′)θxt′dt′ 这是微分方程的解 x ˙ t A θ x t c , θ x 0 x 0 \dot{x}_t A \theta x_t c, \quad \theta x_0 x_0 x˙tAθxtc,θx0x0 固定最终状态 x 1 x_1 x1自由最终时间 τ \tau τ 如果我们想要找到最优到达时间 τ \tau τ可以通过将最优控制策略 u t ∗ u^*_t ut∗带入成本函数 c π c_\pi cπ并计算积分来实现 c τ τ [ x 1 − θ x τ ] T G t − 1 [ x 1 − θ x τ ] c_\tau \tau [x_1 - \theta x_\tau]^T G_{t-1} [x_1 - \theta x_\tau] cττ[x1−θxτ]TGt−1[x1−θxτ] 通过对 c τ c_\tau cτ关于 τ \tau τ 求导可以找到最优的到达时间 d c τ d τ 1 − 2 ( A x 1 c ) T d τ − d τ T B R − 1 B T d τ \frac{dc_\tau}{d\tau} 1 - 2(Ax_1 c)^T d_\tau - d_\tau^T B R^{-1} B^T d_\tau dτdcτ1−2(Ax1c)Tdτ−dτTBR−1BTdτ 其中 d τ G t − 1 [ x 1 − θ x τ ] d_\tau G_{t-1}[x_1 - \theta x_\tau] dτGt−1[x1−θxτ]固定最终状态 x 1 x_1 x1自由最终时间 τ \tau τ 解方程 d c τ d τ 0 \frac{dc_\tau}{d\tau} 0 dτdcτ0得到 τ ∗ \tau^* τ∗。 需要注意的是函数 c τ c_\tau cτ可能具有多个局部最小值。对于一个双重积分器系统它是一个4次多项式。 给定上述定义的最优到达时间 τ ∗ \tau^* τ∗问题再次转变为一个固定最终状态固定最终时间的问题。
如何选择父节点
现在如果我们从随机状态进行采样我们可以计算控制策略和从树中的那些状态节点到采样状态的成本。选择一个具有最小成本的节点并检查 x ( t ) {x}(t) x(t)和 u ( t ) {u}(t) u(t)是否在边界内。如果找不到合格的父节点则重新对状态进行采样。
如何有效地找到附近的节点
每次我们从随机状态x_rand进行采样时都需要检查树中的每个节点以找到其父节点这相当于为每个节点解决一个 OBVP最优边值问题这样效率不高。 如果我们设置一个成本容忍度 ( ρ ) (\rho) (ρ)实际上可以计算状态的边界前向可达集可通过x_rand_到达以及状态的边界后向可达集可以以小于_ ( ρ ) (\rho) (ρ)_的成本到达x_rand。 如果我们以 kd-tree 的形式存储节点我们可以在树中进行范围查询。 c τ τ [ x 1 − θ x τ ] T G t − 1 [ x 1 − θ x τ ] c_\tau \tau [x_1 - \theta x_\tau]^T G_{t-1} [x_1 - \theta x_\tau] cττ[x1−θxτ]TGt−1[x1−θxτ] 这个公式描述了从状态 x 0 \mathbf{x}_0 x0转移到状态 x 1 \mathbf{x}_1 x1的成本随到达时间 τ \tau τ的变化。我们可以看到给定初始状态 x 0 \mathbf{x}_0 x0、成本容忍度 ρ \rho ρ和到达时间 τ \tau τ x 0 \mathbf{x}_0 x0的前向可达集为 x 1 ∣ τ x 1 − θ x τ ) T G t − 1 ( x 1 − θ x τ ) ρ x 1 ∣ ( x 1 − θ x τ ) T G t − 1 ( ρ − τ ) ( x 1 − θ x τ ) 1 E ( θ x τ , G t ( ρ − τ ) ) \mathbf{x}_1 \, | \, \tau \mathbf{x}_1 - \theta \mathbf{x}_\tau)^T G_{t-1} (\mathbf{x}_1 - \theta \mathbf{x}_\tau) \rho \\ \mathbf{x}_1 \, | \, (\mathbf{x}_1 - \theta \mathbf{x}_\tau)^T G_{t-1} (\rho - \tau) (\mathbf{x}_1 - \theta \mathbf{x}_\tau) 1 \\ \mathcal{E}(\theta \mathbf{x}_\tau, G_t(\rho - \tau)) x1∣τx1−θxτ)TGt−1(x1−θxτ)ρx1∣(x1−θxτ)TGt−1(ρ−τ)(x1−θxτ)1E(θxτ,Gt(ρ−τ)) 其中, E ( x , M ) \mathcal{E}(\mathbf{x}, \mathbf{M}) E(x,M)是以中心 x \mathbf{x} x和正定权重矩阵 M \mathbf{M} M定义的椭球形式上定义为 E ( x , M ) { x ′ ∣ x ′ T M − 1 ( x ′ − x ) 1 } \mathcal{E}(\mathbf{x}, \mathbf{M}) \{\mathbf{x} \, | \, \mathbf{x}^T \mathbf{M}^{-1} (\mathbf{x} - \mathbf{x}) 1\} E(x,M){x′∣x′TM−1(x′−x)1} 因此前向可达集是所有可能到达时间 τ \tau τ的高维椭球的并集。 为了简化我们对若干个 (\tau) 进行采样并计算每个 (\tau) 对应的椭球的轴对齐边界框并更新每个维度中的最大和最小值
类似地对于反向可达集的计算。 在进行“Near”查询和“ChooseParent”时可以从状态的反向可达集中找到 x near \mathbf{x}_{\text{near}} xnear。
如何进行“Rewire”
在进行“Rewire”时我们计算状态 x rand \mathbf{x}_{\text{rand}} xrand的前向可达集并解决 OBVPs最优边值问题。 绿色曲线不考虑障碍物 红色曲线是运动动力学轨迹规划器的结果 蓝色曲线是运动动力学轨迹规划器找到的第一条可行轨迹 黄色线是每个控制点处的控制输入。
小结
本文求解过程引入了动力学约束。从质点模型到引入动力学约束来求解可行路径是为了在后续求解中可以把不符合实体移动机器人动力学可行解过滤加速back end的求解。 文章从移动机器人建模开始介绍了四旋翼、车辆的几种简单建模方法。这部分资料是属于控制系统线性、非线性求解部分工作。 接着介绍了如何基于动力学约束来建图介绍了基于控制空间采样、基于状态空间采样两大类方法。详细的介绍了基于状态空间采样的建模和求解过程这部分求解过程涉及比较多的数学和物理模型有些难度可以多看几遍这边需要回答的一个问题是为什么要对状态转移矩阵求解一言概之就是如果有解那么代表有可达解符合采样要求可以留下。 基于采样得到图后接下来的问题就是如何来得到可行路径。这部分介绍的就是如何从构建图中找到可行路径包括图搜索方法的改造、RRT搜索树的改造和求解流程。
