何为响应式网站,建设网站的规则,wordpress 博客 主题,在家里怎样做网站本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记#xff0c;本系列文章共包含四篇文章#xff0c;依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容 本系列文章链接如下#xff1a; 最优轨迹生… 本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记本系列文章共包含四篇文章依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容 本系列文章链接如下 最优轨迹生成一—— 微分平坦 最优轨迹生成二—— 无约束BVP轨迹优化 最优轨迹生成三—— 无约束BIVP轨迹优化 最优轨迹生成四—— 带约束轨迹优化 一、微分平坦 1、全局路径规划与局部路径规划 全局路径规划算法一般具有具备全局最优性、可以处理复杂的环境、很多时候只需要低阶信息但全规划局算法在高维空间中效率较慢全局规划算法在高维空间很难进行采样很难满足动力学约束、高维空间收敛速度慢。 局部路径规划算法一般具有局部最优性可以处理复杂的动力学约束局部规划算法常采用优化的方法在高维空间中也可以快速的逼近局部最优解收敛较快但需要对动力学约束的显式建模、进行参数辨识、需要使用高阶信息 基于上面对全局规划算法和局部规划算法的优缺点考虑在进行轨迹规划时常分为前端和后端前端往往是在低维空间搜索一个低维的可行路径或可行域然后再通过后端做优化 2、轨迹 “轨迹是足够光滑的路径”这种认知是不准确的轨迹是需要包含时间信息的它是一种时间参数化的路径轨迹也不一定是光滑的轨迹包含时间和空间两种特性。 如果对一个二维的位置进行时间参数化就是一个二维的位置轨迹我们也可以对二维的位置、速度、姿态角 进行时间参数化这样得到的轨迹就是对位置速度姿态的轨迹。同理可拓展到三维情况 轨迹中常说的光滑一般不仅仅指视觉上看起来是光滑而是指该轨迹要满足动力学约束常使用微分方程表示 x ˙ f ( x , u ) \dot{x}f(x,u) x˙f(x,u)还要去最小化一个能量泛函 min ∫ 0 T L ( x , u ) d t \min\int_0^T\mathcal{L}(x,u)\mathrm{d}t min∫0TL(x,u)dt 3、为什么需要轨迹优化 向前面介绍的Hybrid A * 算法等满足动力学的规划算法已经可以得到满足动力学约束的路径为什么还需要进行轨迹优化呢因为我们做轨迹优化不仅仅要考虑动力学约束还要考虑时间最优、能量最优、硬件限制、其他任务等。 4、微分平坦特性 Differential Flatness 微分平坦是一种常微分方程描述了无人系统的一种特性。这个常微分方程描述了状态和输入之间的关系如果一个系统称为微分平坦系统那么我们可以找到一个变量 z ∈ R m z\in\mathbb{R}^{m} z∈Rm称为平坦输出它的有限导数可以唯一地确定所有状态和输入 x Ψ x ( z , z ˙ , ⋯ , z ( s − 1 ) ) , u Ψ u ( z , z ˙ , ⋯ , z ( s ) ) . \begin{aligned}x\Psi_x(z,\dot{z},\cdots,z^{(s-1)}),\\u\Psi_u(z,\dot{z},\cdots,z^{(s)}).\end{aligned} xuΨx(z,z˙,⋯,z(s−1)),Ψu(z,z˙,⋯,z(s)). 所以我们定义一个映射使得z轨迹的0阶导、1阶导、2阶导…有限阶导数来唯一的确定系统的所有状态和输入如上所示。 将上面的x和u代入到下式中,可以得到一个恒等式拥有这个特性的系统称为微分平坦系统。 x ˙ f ( x ) g ( x ) u , f : R n ↦ R n , g : R n ↦ R n , x ∈ R n , u ∈ R m , rank ( g ) m . \begin{array}{c}\dot{x}f(x)g(x)u,\\f{:}~\mathbb{R}^n\mapsto\mathbb{R}^n,~g{:}~\mathbb{R}^n\mapsto\mathbb{R}^n,~x\in\mathbb{R}^n,~u\in\mathbb{R}^m,~\operatorname{rank}(g)m.\end{array} x˙f(x)g(x)u,f: Rn↦Rn, g: Rn↦Rn, x∈Rn, u∈Rm, rank(g)m. 微分平坦度消除掉微分约束所以我们可以在规划的时候不考虑动力学约束规划z的轨迹当得到z的轨迹后只要它具有零阶到s-1阶的导且连续那么由其算出的x和u必然满足动力学约束 x ˙ f ( x ) g ( x ) u \dot{x}f(x)g(x)u x˙f(x)g(x)u 像常见的四旋翼无人机多旋翼的无人机空气阻力和速度成比例关系、电机朝上或者角度满足一定关系时都是满足微分平坦特性的。 我们利用微分平坦特性来避免处理无人机的微分等式约束。 假设无人机的状态是由三维的位置r、速度v、角速率w、姿态矩阵R组成 x { r , v , R , ω } ∈ R 3 × R 3 × S O ( 3 ) × R 3 x\{r,v,R,\omega\}\in\mathbb{R}^3\times\mathbb{R}^3\times\mathrm{SO}(3)\times\mathbb{R}^3 x{r,v,R,ω}∈R3×R3×SO(3)×R3 控制输入选为总推力f和三轴扭矩 τ \tau τ u { f , τ } ∈ R ≥ 0 × R 3 u\{f,\tau\}\in\mathbb{R}_{\geq0}\times\mathbb{R}^3 u{f,τ}∈R≥0×R3 动力学方程常微分 x ˙ f ( x ) g ( x ) u \dot xf(x)g(x)u x˙f(x)g(x)u描述了f函数与u的关系其具体表达式如下第一个公式表示位置的微分是速度第二个公式是无人机的合力方程 F m v 2 Fmv^2 Fmv2,第三个公式是姿态旋转矩阵的微分与角速度的关系第四个公式是欧拉公式加一个增量 { r ˙ v , m v ˙ − m g e 3 − R D R T σ ( ∥ v ∥ ) v R f e 3 , R ˙ R ω ^ , M ω ˙ τ − ω × M ω − A ( ω ) − B ( R T v ) . \left\{\begin{aligned}\dot{r}v,\\m\dot{v}-mge_3-RDR^\mathrm{T}\sigma(\|v\|)vRfe_3,\\\dot{R}R\hat{\omega},\\M\dot{\omega}\tau-\omega\times M\omega-A(\omega)-B(R^\mathrm{T}v).\end{aligned}\right. ⎩ ⎨ ⎧r˙mv˙R˙Mω˙v,−mge3−RDRTσ(∥v∥)vRfe3,Rω^,τ−ω×Mω−A(ω)−B(RTv). 微分平坦输出选择为无人机三轴位置r和偏航角ψ z { r , ψ } ∈ R 3 × S O ( 2 ) z\{r,\psi\}\in\mathbb{R}^3\times\mathrm{SO}(2) z{r,ψ}∈R3×SO(2) 下面来看如何用微分平坦输出z及其有限阶导数来表示x和u即如何用r、w及它们的有限阶导数表示r、v、R、w、f 、 τ \tau τ 状态x中的r和v与微分平坦输出z的关系可以很容易得到如下所示 r r v r ˙ rr\quad\quad v\dot{r} rrvr˙ 把前面的牛顿方程乘以机体坐标系下的x轴和y轴后可以得到下式 x b R e 1 , y b R e 2 ( R e i ) T m v ˙ ( R e i ) T ( − m g e 3 − R D R T σ ( ∥ v ∥ ) v R f e 3 ) , ∀ i ∈ { 1 , 2 } \begin{gathered}x_bRe_1,y_bRe_2\\(Re_i)^\mathrm{T}m\dot{v}(Re_i)^\mathrm{T}\left(-mge_3-RDR^\mathrm{T}\sigma(\|v\|)vRfe_3\right),\mathrm{~}\forall i\in\{1,2\}\end{gathered} xbRe1,ybRe2(Rei)Tmv˙(Rei)T(−mge3−RDRTσ(∥v∥)vRfe3), ∀i∈{1,2} 化简后得到下式 ( R e i ) T ( v ˙ d h m σ ( ∥ v ∥ ) v g e 3 ) 0 , ∀ i ∈ { 1 , 2 } . (Re_i)^{\mathrm{T}}(\dot{v}\frac{d_h}m\sigma(\|v\|)vge_3)0,\mathrm{~}\forall i\in\{1,2\}. (Rei)T(v˙mdhσ(∥v∥)vge3)0, ∀i∈{1,2}. 这个公式的几何意义是机体坐标系下的x轴和y轴都与 d h m σ ( ∥ v ∥ ) v g e 3 \frac{d_h}m\sigma(\|v\|)vge_3 mdhσ(∥v∥)vge3垂直所以 d h m σ ( ∥ v ∥ ) v g e 3 \frac{d_h}m\sigma(\|v\|)vge_3 mdhσ(∥v∥)vge3必然垂直于机体坐标系下的x轴和y轴构成的平面即平行于机体坐标系下的z轴 x b ⊥ ( v ˙ d h m σ ( ∥ v ∥ ) v g e 3 ) y b ⊥ ( v ˙ d h m σ ( ∥ v ∥ ) v g e 3 ) \begin{aligned}x_b\perp(\dot{v}\frac{d_h}m\sigma(\|v\|)vge_3)\\y_b\perp(\dot{v}\frac{d_h}m\sigma(\|v\|)vge_3)\end{aligned} xbyb⊥(v˙mdhσ(∥v∥)vge3)⊥(v˙mdhσ(∥v∥)vge3) 由于推力和机体坐标系下的z轴方向相同所以可以将其进行归一化 z b N ( v ˙ d h m σ ( ∥ v ∥ ) v g e 3 ) , N ( x ) : x / ∥ x ∥ 2 \color{red}{z_b\mathcal{N}(\dot{v}\frac{d_h}m\sigma(\|v\|)vge_3),\mathcal{N}(x):x/\|x\|_2} zbN(v˙mdhσ(∥v∥)vge3),N(x):x/∥x∥2 至此我们成功通过r、r的一阶导、r的二阶导获得了机体坐标系下的z轴方向向量 z b z_b zb 我们可以利用机体朝上来获得推力 ( R e 3 ) T m v ˙ ( R e 3 ) T ( − m g e 3 − R D R T σ ( ∥ v ∥ ) v R f e 3 ) (Re_3)^\text{T}m\dot{v}(Re_3)^\text{T}{ \left ( - m g e _ 3 - R D R ^\text{T}\sigma(\|v\|)vRfe_3\right)} (Re3)Tmv˙(Re3)T(−mge3−RDRTσ(∥v∥)vRfe3) 由上式可得 f z b T ( m v ˙ d v σ ( ∥ v ∥ ) v m g e 3 ) \color{red}fz_b^\mathrm{T}(m\dot{v}d_v\sigma(\|v\|)vmge_3) fzbT(mv˙dvσ(∥v∥)vmge3) 至此我们成功用微分平坦输出z表示了推力f 现在偏航航向和机体坐标下的z轴方向都已知偏航旋转四元数为 q ψ ( cos ( ψ / 2 ) , 0 , 0 , sin ( ψ / 2 ) ) T q_\psi\left(\cos(\psi/2),0,0,\sin(\psi/2)\right)^\mathrm{T} qψ(cos(ψ/2),0,0,sin(ψ/2))T 倾斜旋转四元数为 q z 1 2 ( 1 z b ( 3 ) ) ( 1 z b ( 3 ) , − z b ( 2 ) , z b ( 1 ) , 0 ) T q_z\frac1{\sqrt{2(1z_b(3))}}(1z_b(3),-z_b(2),z_b(1),0)^\mathrm{T} qz2(1zb(3)) 1(1zb(3),−zb(2),zb(1),0)T 又因为 q q z ⊗ q ψ qq_z\otimes q_\psi qqz⊗qψ 所以可得 q 1 2 ( 1 z b ( 3 ) ) ( ( 1 z b ( 3 ) ) cos ( ψ / 2 ) − z b ( 2 ) cos ( ψ / 2 ) z b ( 1 ) sin ( ψ / 2 ) z b ( 1 ) cos ( ψ / 2 ) z b ( 2 ) sin ( ψ / 2 ) ( 1 z b ( 3 ) ) sin ( ψ / 2 ) , ) \color{red}{q\frac1{\sqrt{2(1z_b(3))}}\begin{pmatrix}(1z_b(3))\cos(\psi/2)\\-z_b(2)\cos(\psi/2)z_b(1)\sin(\psi/2)\\z_b(1)\cos(\psi/2)z_b(2)\sin(\psi/2)\\(1z_b(3))\sin(\psi/2),\end{pmatrix}} q2(1zb(3)) 1 (1zb(3))cos(ψ/2)−zb(2)cos(ψ/2)zb(1)sin(ψ/2)zb(1)cos(ψ/2)zb(2)sin(ψ/2)(1zb(3))sin(ψ/2), 至此我们获得了用微分平坦输出z表示的姿态 现在旋转是已知的 R ˙ R ω ^ \dot{R}R\hat{\omega} R˙Rω^ 这意味着 ω ( R T R ˙ ) ∨ \omega(R^\mathrm{T}\dot{R})^\vee ω(RTR˙)∨ 上式等效于 ω 2 ( q z ⊗ q ψ ) − 1 ⊗ ( q ˙ z ⊗ q ψ q z ⊗ q ˙ ψ ) \omega2(q_z\otimes q_\psi)^{-1}\otimes(\dot{q}_z\otimes q_\psiq_z\otimes\dot{q}_\psi) ω2(qz⊗qψ)−1⊗(q˙z⊗qψqz⊗q˙ψ) 代入倾斜旋转四元数和偏航旋转四元数给出下式 ω ( z ˙ b ( 1 ) sin ( ψ ) − z ˙ b ( 2 ) cos ( ψ ) − z ˙ b ( 3 ) ( z b ( 1 ) sin ( ψ ) − z b ( 2 ) cos ( ψ ) ) / ( 1 z b ( 3 ) ) z ˙ b ( 1 ) cos ( ψ ) z ˙ b ( 2 ) sin ( ψ ) − z ˙ b ( 3 ) ( z b ( 1 ) cos ( ψ ) z b ( 2 ) sin ( ψ ) ) / ( 1 z b ( 3 ) ) ( z b ( 2 ) z ˙ b ( 1 ) − z b ( 1 ) z ˙ b ( 2 ) ) / ( 1 z b ( 3 ) ) ψ ˙ , ) \color{red}{\omega\begin{pmatrix}\dot{z}_b(1)\sin(\psi)-\dot{z}_b(2)\cos(\psi)-\dot{z}_b(3)(z_b(1)\sin(\psi)-z_b(2)\cos(\psi))/(1z_b(3))\\\dot{z}_b(1)\cos(\psi)\dot{z}_b(2)\sin(\psi)-\dot{z}_b(3)(z_b(1)\cos(\psi)z_b(2)\sin(\psi))/(1z_b(3))\\(z_b(2)\dot{z}_b(1)-z_b(1)\dot{z}_b(2))/(1z_b(3))\dot{\psi},\end{pmatrix}} ω z˙b(1)sin(ψ)−z˙b(2)cos(ψ)−z˙b(3)(zb(1)sin(ψ)−zb(2)cos(ψ))/(1zb(3))z˙b(1)cos(ψ)z˙b(2)sin(ψ)−z˙b(3)(zb(1)cos(ψ)zb(2)sin(ψ))/(1zb(3))(zb(2)z˙b(1)−zb(1)z˙b(2))/(1zb(3))ψ˙, 其中 z ˙ b d h m D N ( v ˙ d h m σ ( ∥ v ∥ ) v g e 3 ) T ( m d h v ¨ σ ( ∥ v ∥ ) v ˙ σ ˙ ( ∥ v ∥ ) v T v ˙ ∥ v ∥ v ) , D N ( x ) : 1 ∥ x ∥ ( I − x x T x T x ) . \begin{gathered}\color{red}{\dot{z}_b}{\frac{d_h}m\mathcal{DN}(\dot{v}\frac{d_h}m\sigma(\|v\|)vge_3)^T(\frac m{d_h}\ddot{v}\sigma(\|v\|)\dot{v}\dot{\sigma}(\|v\|)\frac{v^\mathrm{T}\dot{v}}{\|v\|}v)},\\\color{red}\mathcal{DN}(x):\color{red}{\frac1{\|x\|}\left(\mathrm{I}-\frac{xx^\mathrm{T}}{x^\mathrm{T}x}\right)}.\end{gathered} z˙bmdhDN(v˙mdhσ(∥v∥)vge3)T(dhmv¨σ(∥v∥)v˙σ˙(∥v∥)∥v∥vTv˙v),DN(x):∥x∥1(I−xTxxxT). 到这里几乎所有的量除了力矩都被微分平台输出z及其有限阶导数表示出来了 上面给出了w的表达式对其求微分利用平坦输出及其有限阶导数可得 τ M ω ˙ ω × M ω A ( ω ) B ( R T v ) \color{red}{\tauM\dot{\omega}\omega\times M\omegaA(\omega)B(R^\mathrm{T}v)} τMω˙ω×MωA(ω)B(RTv) 至此 解析完成在微分等式所确定的平面上进行优化算法是很困难的通过微分平坦我们得到一个关于微分平坦输出z的平直的空间他没有任何的等式和曲面约束只保留了不等式约束在该空间中做优化是容易的且得到解一定是满足微分等式的。 我们得到微分平坦输出z的轨迹后我们可以利用z在任何时刻的一阶导、二阶导、三阶导… 来直接调用上面推导出的对应的x和u的表达式红色公式获得最终的x和u的轨迹给控制器进行跟踪以上也是基于微分平坦的规划和控制的基本思路。 平坦输出轨迹需要满足一定的光滑性我们可以利用样条或者其他方法进行拼接构成其需要进行参数化因此平坦输 z { r , ψ } ∈ R 3 × S O ( 2 ) z\{r,\psi\}\in\mathbb{R}^3\times\mathrm{SO}(2) z{r,ψ}∈R3×SO(2)在出空间中的轨迹为 z ( t ) : [ 0 , T ] ↦ R 3 × S O ( 2 ) z(t){:}[0,T]\mapsto\mathbb{R}^3\times\mathrm{SO}(2) z(t):[0,T]↦R3×SO(2) 通过样条参数化平坦输出轨迹的优点如下 •容易确定平滑准则 •容易和封闭形式的导数计算 •解耦三维轨迹生成 •高数值稳定性 参考资料 1、深蓝学院-移动机器人运动规划
