【自动驾驶】控制算法(四)坐标变换与横向误差微分方程

写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。

👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花

📚 系列专栏:【运动控制】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!

🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~


文章目录

  • 引言
  • 一、微分方程与规划轨迹
    • 1、误差定义
    • 2、代价函数定义
  • 二、误差微分方程的推导
    • 1、误差微分方程的建立
    • 2、横向误差与速度误差的关系
      • 简单证明
    • 3、投影点速度的计算
    • 4、航向角误差与速度误差的联系
    • 5、误差微分方程的线性化
  • 三、线性误差微分方程组
    • 1、航向误差问题
    • 2、投影点航向角速度计算问题
    • 3、线性误差微分方程组的形式问题
  • 参考资料


引言

  本篇博客是 自动驾驶控制算法 系列的第四节,这一节是本系列课程的核心内容。内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。

  在上一节得到了这样的微分方程:
在这里插入图片描述
  反映了前轮转角对侧向速度和横摆角的影响,可以写成如下形式:
x ˙ = A x + B u \dot{x}=Ax+Bu x˙=Ax+Bu  此方程是做控制的核心,因为控制的目的就是让车按照规划的轨迹形式。


一、微分方程与规划轨迹

  比如,在绝对坐标系上规划一条轨迹:
在这里插入图片描述
  在这条轨迹上取一些离散点作为参考点,运动控制的目标是通过控制方向盘,让真实车辆的位置、速度以及航向角尽可能接近规划的位置、速度、航向角。

  假设在 t = 5 s t= 5s t=5s 时,规划点包含了一系列信息 ( X r , Y r , v r , θ r , a r ) (X_r,Y_r,v_r,\theta_r,a_r) (Xr,Yr,vr,θr,ar) ,意味着按照规划点,在第五秒时,车辆位置应该是 ( x r , y r ) (x_r,y_r) (xr,yr),速度应该是 v r v_r vr,航向角应该是 θ r \theta_r θr,加速度应该是 a r a_r ar,这些 ( X r , Y r , v r , θ r , a r ) (X_r,Y_r,v_r,\theta_r,a_r) (Xr,Yr,vr,θr,ar) 数据是在规划模块上已经规划好的已知数据。

1、误差定义

  但是如果车辆真实运动状态和规划点之间存在误差:

在这里插入图片描述

  图中绿色线为误差,在真实轨迹和规划轨迹之间,有纵向误差、横向误差、航向角误差、速度误差、加速度误差。一般定义误差为真实值减去规划值,即
e r r ⃗ = x ⃗ − x ⃗ r \vec{e_{rr}}=\vec{x}-\vec{x}_{r} err =x x r其中,规划值 x ⃗ r \vec{x}_{r} x r已知,真实值 x ⃗ \vec{x} x 满足上面所求的物理规律。

  将真实值 x x x 用已知的规划值 x r x_r xr 以及误差 e r r e_{rr} err 代替,代入微分方程,得到误差微分方程:
x ˙ = A x + B u ⇒ e r r = A ˉ e r r + B ˉ u \dot{x}=Ax+Bu\Rightarrow e_{rr}=\bar{A}e_{rr}+\bar{B}u x˙=Ax+Buerr=Aˉerr+Bˉu误差微分方程是我们真正要的东西,它反映了误差 e r r e_{rr} err 随控制量 u u u 变化的物理规律。

2、代价函数定义

  控制目标是选择合适的 u u u,让 x x x 尽可能接近 x r x_r xr ,等价于让误差的绝对值尽可能小。

  定义代价函数为误差 e r r e_{rr} err 的平方加控制量 u u u 的平方最小。
min ⁡ J = e r r 2 + u 2 \min J=e_{rr}^{2}+u^{2} minJ=err2+u2  除了保证误差最小,还希望花费最小。误差 e r r e_{rr} err 的平方和 u u u 的平方可以加权,不一定是 1 : 1 1: 1 1:1 关系,可能是 a a a b b b 的关系:
J = a e r r 2 + b u 2 J=ae_{rr}^{2}+bu^{2} J=aerr2+bu2由于误差 e r r e_{rr} err 和控制量 u u u 一般为列向量,所以加权平方可改写成这样的形式:
J = e r r T Q e r r + u T R u J=e_{rr}^{T}Qe_{rr}+u^{T}Ru J=errTQerr+uTRu 其中, Q Q Q R R R 为对角矩阵。

  这样变成 J = e r r T Q e r r + u T R u J=e_{rr}^{T}Qe_{rr}+u^{T}Ru J=errTQerr+uTRu 在约束条件 x ˙ = A ˉ e r r + B ˉ u \dot{x}=\bar{A}e_{rr}+\bar{B}u x˙=Aˉerr+Bˉu 下取最小值的数学问题。其实就是控制里的LQRL 就是 x ˙ = A ˉ e r r + B ˉ u \dot{x}=\bar{A}e_{rr}+\bar{B}u x˙=Aˉerr+Bˉu,即线性约束, Q Q Q R R R 为权重矩阵。


二、误差微分方程的推导

  下面介绍怎样实现 x ˙ = A x + B u ⇒ e r r = A ˉ e r r + B ˉ u \dot{x}=Ax+Bu\Rightarrow e_{rr}=\bar{A}e_{rr}+\bar{B}u x˙=Ax+Buerr=Aˉerr+Bˉu 的转化过程。

关于误差的线性微分方程非常重要,因为只要把它解出来,后面就是在约束条件下求小值的问题。

  注意:坐标系选择自然坐标系而不是直角坐标系,要求自然坐标系下 e r r = A ˉ e r r + B ˉ u e_{rr}=\bar{A}e_{rr}+\bar{B}u err=Aˉerr+Bˉu 的表达式。

1、误差微分方程的建立

  假设在绝对坐标系下,以已有的规划轨迹为坐标轴建立自然坐标系:
在这里插入图片描述

  车辆速度为 v v v,在自然坐标系下投影点的切线方向为 τ r \tau_r τr,投影点的法线方向为 n r n_r nr,投影点速度为 s ˙ \dot s s˙,车辆到投影点的距离为 d d d v v v s ˙ \dot s s˙ x x x 轴夹角分为 θ \theta θ θ r \theta_r θr

定义误差如下:

  • 横向误差为 d d d
  • 航向误差为 θ − θ r \theta - \theta_r θθr
  • 速度误差 v − s ˙ v-\dot s vs˙

其中,速度误差属于纵向公式,本篇博客介绍横向控制,所以速度误差暂时先不管。

  注意: v v v s ˙ \dot s s˙ 是速度大小,不是速度。因为速度方向由航向误差决定,航向误差是控制速度的方向,速度误差仅仅是速度大小的误差,而不是速度矢量的误差。

2、横向误差与速度误差的关系

  在车辆真实速度处建立标架, τ \tau τ 为速度方向, n n n 为速度法线方向。

  注意: τ \tau τ n n n 不是车身坐标系的 x x x 轴和 y y y 轴方向,因为 v v v 是质心速度,质心和绝对坐标系 X X X 轴的夹角为航向角 θ \theta θ,而车身坐标系是以横摆角 φ \varphi φ 为方向,即 x x x 轴沿着横摆角方向, y y y 轴垂直于横摆角方向,所以 τ \tau τ n n n 并不是车身坐标系的 x x x 轴和 y y y 轴方向

  过坐标原点 O O O 做两个向量,一头指向投影点 x ⃗ r \vec x_r x r,一头指向真实的车辆位置 x ⃗ \vec x x x ⃗ \vec x x x ⃗ r \vec x_r x r d d d 构成三角形,就有
x r ⃗ + d n r ⃗ = x ⃗ \vec{x_r}+d\vec{n_r}=\vec{x} xr +dnr =x 其中, d d d 是数,不是向量,所以要作为向量的计算法,用线段大小 d d d 乘以的单位方向向量,即 d = ( x ⃗ − x ⃗ r ) ⋅ n ⃗ r d=(\vec{x}-\vec{x}_{r})\cdot\vec{n}_{r} d=(x x r)n r这样得到 d d d 的具体表达式。

  但算出来 d 还不够,因为 x ˙ = A x + B u \dot{x}=Ax+Bu x˙=Ax+Bu,是以 v y 、 φ ˙ v_y、\dot \varphi vyφ˙ 为基本的位置量。为了和 x ˙ = A x + B u \dot{x}=Ax+Bu x˙=Ax+Bu 联系起来,需要对 d d d 进行求导运算。

  注意: n ⃗ r \vec n_r n r 是向量不是常矢量,会随曲线投影点的变化而变化,大小是单位数量,但是方向会不断变化,它的方向变化和曲线的曲率有关,所以 n ⃗ r \vec n_r n r 不是常矢量。

  对 d d d 求导得到:
d ˙ = ( x ˙ ⃗ − x ˙ ⃗ r ) ⋅ n ⃗ r + ( x ⃗ − x ⃗ r ) ⋅ n ˙ ⃗ r \dot{d}=(\vec{\dot{x}}-\vec{\dot{x}}_{r})\cdot\vec{n}_{r}+(\vec{x}-\vec{x}_{r})\cdot\vec{\dot{n}}_{r} d˙=(x˙ x˙ r)n r+(x x r)n˙ r

  • 车辆质心的真实位矢 x x x,其导数是 x ˙ ⃗ = ∣ v ⃗ ∣ τ ⃗ \vec{\dot{x}}=|\vec{v}|\vec{\tau} x˙ =v τ
  • x ˙ ⃗ r \vec{\dot{x}}_r x˙ r 是投影点的位矢,其导数为 x ˙ ⃗ r = s ˙ τ ⃗ r \vec{\dot{x}}_{r}=\dot{s}\vec{\tau}_{r} x˙ r=s˙τ r

简单证明

  有人可能对向量问题不熟悉,下面简单证明。

  比如在直角坐标系下:
在这里插入图片描述

  有一点沿轨迹运动,其位矢为 r ⃗ \vec r r ,经过 d t dt dt 的时间,运动到下一点,它的位矢变成 r + d r r +dr r+dr,根据向量的三角形法则,蓝色线就是 d r dr dr,根据复合求导
d r ⃗ d t = d r ⃗ d s ⋅ d s d t \frac{d\vec{r}}{dt}=\frac{d\vec{r}}{ds}\cdot\frac{ds}{dt} dtdr =dsdr dtds其中, d s ds ds 就是图中红色弧线。

  当 d t → 0 dt\rightarrow 0 dt0时, d r d r dr d s ds ds 大小趋于相等, d r dr dr的方向趋向于 τ ⃗ \vec \tau τ 的切线方向。所以 d r ⃗ d t = 1 ⋅ τ ⃗ ⋅ d s d t \frac{d\vec{r}}{dt}=1\cdot\vec{\tau}\cdot\frac{ds}{dt} dtdr =1τ dtds其中, τ ⃗ r \vec \tau_r τ r r r r 的切线方向,所以 x ˙ ⃗ r = s ˙ τ ⃗ r \vec{\dot{x}}_{r}=\dot{s}\vec{\tau}_{r} x˙ r=s˙τ r


  回到刚才所讲的内容。 d d d 的导数:
d ˙ = ( ∣ v ⃗ ∣ τ ⃗ − s ˙ τ ⃗ r ) ⋅ n ⃗ r + ( x ⃗ − x ⃗ r ) ⋅ d n ⃗ r d t \dot d=(|\vec{v}|\vec{\tau}-\dot{s}\vec{\tau}_{r})\cdot\vec{n}_{r}+(\vec{x}-\vec{x}_{r})\cdot\frac{d\vec{n}_{r}}{dt} d˙=(v τ s˙τ r)n r+(x x r)dtdn r  同样 d n r ⃗ d t \frac{d\vec{n_{r}}}{dt} dtdnr 可以用复合求导:
d n r ⃗ d t = d n r ⃗ d s ⋅ d s d t \frac{d\vec{n_{r}}}{dt}=\frac{d\vec{n_{r}}}{ds}\cdot\frac{ds}{dt} dtdnr =dsdnr dtds其中, d s d t = s ˙ \frac{ds}{dt}=\dot{s} dtds=s˙,但 d n r ⃗ d s \frac{d\vec{n_{r}}}{ds} dsdnr 等于什么呢?这里要用到数学上的向量微积分中的 Frenet 公式。

  二维曲线的 Frenet 公式:
d τ ⃗ d s = κ n ⃗ d n ⃗ d s = − κ τ ⃗ \frac{d\vec{\tau}}{ds}=\kappa \vec{n}\quad\quad\frac{d\vec{n}}{ds}=-\kappa \vec{\tau} dsdτ =κn dsdn =κτ 其中, κ \kappa κ 是曲线的曲率。

  把 Frenet 公式带进去,得到 d n r ⃗ d t = s ˙ ( − k τ ⃗ r ) \frac{d\vec{n_{r}}}{dt}=\dot{s}(-k\vec{\tau}_{r}) dtdnr =s˙(kτ r),根据向量的加减法, d = ( x ⃗ − x ⃗ r ) ⋅ n ⃗ r d=(\vec{x}-\vec{x}_{r})\cdot\vec{n}_{r} d=(x x r)n r,再带到 d ˙ \dot d d˙ 里:
d ˙ = ( ∣ v ⃗ ∣ z ⃗ − n ⃗ r ) + d n ⃗ r ⋅ ( − k s ˙ τ ⃗ r ) \dot d=(|\vec{v}|\vec{z}-\vec{n}_{r})+d\vec{n}_{r}\cdot(-k\dot{s}\vec{\tau}_{r}) d˙=(v z n r)+dn r(ks˙τ r)  又因为 τ ⃗ r \vec \tau_r τ r n ⃗ r \vec{n}_r n r 垂直,所以后面一项为 0 0 0,即
d ˙ = ∣ v ⃗ ∣ ∣ τ ⃗ ∣ ∣ n r ⃗ ∣ cos ⁡ < τ ⃗ , n r ⃗ > \dot d=|\vec{v}||\vec{\tau}||\vec{n_{r}}|\cos<\vec{\tau},\vec{n_{r}}> d˙=v ∣∣τ ∣∣nr cos<τ ,nr >   τ ⃗ \vec \tau τ n ⃗ r \vec{n}_r n r 间的几何关系如下:
在这里插入图片描述

   τ ⃗ \vec \tau τ n ⃗ r \vec{n}_r n r之间的角度等于 π 2 − ( θ − θ r ) \frac\pi2-(\theta-\theta r) 2π(θθr),所以:
d ˙ = ∣ v ⃗ ∣ cos ⁡ ( π 2 − ( θ − θ r ) ) = ∣ v ⃗ ∣ sin ⁡ ( θ − θ r ) \dot d=|\vec{v}|\cos(\frac{\pi}{2}-(\theta-\theta r))=|\vec{v}|\sin(\theta-\theta r) d˙=v cos(2π(θθr))=v sin(θθr)  因为 τ ⃗ \vec \tau τ n ⃗ r \vec{n}_r n r 都是单位矢量,所以它们的模都是1。

  这样就建立了 d ˙ \dot d d˙ v v v θ \theta θ θ r \theta_r θr 之间的关系。

3、投影点速度的计算

  根据几何关系 x r ⃗ + d n r ⃗ = x ⃗ \vec{x_r}+d\vec{n_r}=\vec{x} xr +dnr =x 计算。

  两边求导 x r ⃗ \vec{x_r} xr 的导数:
x ˙ ⃗ r + d ˙ n ⃗ r + d n ˙ ⃗ r = x ˙ ⃗ \vec{\dot x}_{r}+\dot d\vec{n}_{r}+d\vec{\dot n}_{r}=\vec{\dot x} x˙ r+d˙n r+dn˙ r=x˙   把之前求得的结果带进去:
s ˙ τ ⃗ r + ∣ v ⃗ ∣ sin ⁡ ( θ − θ r ) n ⃗ r + d ( − k s ˙ τ ⃗ r ) = ∣ v ⃗ ∣ τ ⃗ \dot{s}\vec{\tau}_{r}+|\vec{v}|\sin(\theta-\theta_r)\vec{n}_{r}+d(-k\dot{s}\vec{\tau}_{r})=|\vec{v}|\vec{\tau} s˙τ r+v sin(θθr)n r+d(ks˙τ r)=v τ   等式两边同时点成 τ r \tau_r τr
s ˙ + ( − k d s ˙ ) = ∣ v ⃗ ∣ τ ⃗ ⋅ τ ⃗ r = ∣ v ⃗ ∣ cos ⁡ ( θ − θ r ) \dot{s} + (-kd\dot{s})=|\vec{v}| \vec{\tau}\cdot\vec{\tau}_{r}=|\vec{v}|\cos(\theta-\theta_{r}) s˙+(kds˙)=v τ τ r=v cos(θθr)
则:
s ˙ = ∣ v ⃗ ∣ cos ⁡ ( θ − θ r ) 1 − κ d \dot{s}=\frac{|\vec{v}|\cos(\theta-\theta r)}{1-\kappa d} s˙=1κdv cos(θθr)  得到两个非常重要的公式:
d ˙ = ∣ v ⃗ ∣ sin ⁡ ( θ − θ r ) \dot d=|\vec{v}|\sin(\theta-\theta r) d˙=v sin(θθr) s ˙ = ∣ v ⃗ ∣ cos ⁡ ( θ − θ r ) 1 − κ d \dot{s}=\frac{|\vec{v}|\cos(\theta-\theta r)}{1-\kappa d} s˙=1κdv cos(θθr)  这两个公式可以说是一切工作的起点,后续讲无人驾驶控制算法,推导的起点就是这两个公式。

4、航向角误差与速度误差的联系

  但得到这两个公式还不够,因为还没有办法和 v y v_y vy φ \varphi φ 联系在一起。

  将航向角 θ = φ + β \theta=\varphi+\beta θ=φ+β 代进去:
d ˙ = ∣ v ⃗ ∣ sin ⁡ ( β + φ − θ r ) = ∣ v ⃗ ∣ sin ⁡ β cos ⁡ ( φ − θ r ) + ∣ v ⃗ ∣ cos ⁡ β sin ⁡ ( φ − θ r ) = v y cos ⁡ ( φ − θ r ) + v x sin ⁡ ( φ − θ r ) \begin{align*} \dot{d} &= |\vec{v}|\sin \left( \beta +\varphi -\theta _r \right) \\ &= |\vec{v}|\sin \beta \cos \left( \varphi -\theta _r \right) +|\vec{v}|\cos \beta \sin \left( \varphi -\theta _r \right) \\ &= v_y\cos \left( \varphi -\theta _r \right) +v_x\sin \left( \varphi -\theta _r \right) \end{align*} d˙=v sin(β+φθr)=v sinβcos(φθr)+v cosβsin(φθr)=vycos(φθr)+vxsin(φθr)
  一般认为 φ − θ r \varphi -\theta _r φθr是小量,所以又可进一步化简为:
d ˙ ≈ v y + v x ( φ − θ r ) \dot d \approx v_y+v_x(\varphi-\theta_r) d˙vy+vx(φθr)  同样可以将 s ˙ \dot s s˙写成 v x v_x vx v y v_y vy 的形式,不过到纵向控制才用得到,所以关于 s ˙ \dot s s˙ 的话题先不讲。

  求出来纵向误差 d d d,在自然坐标系里,令 e d = d , e φ = φ − θ r e_{d}=d,e_{\varphi}=\varphi-\theta_r ed=d,eφ=φθr,注意 e φ e_{\varphi} eφ 并不是航向误差。航向误差应该是航向角减去参考点的夹角 θ − θ r \theta-\theta_r θθr,即 φ + β − θ r \varphi+\beta-\theta_r φ+βθr

  当然在不严格的理论中,可以认为航向误差是 φ − θ r \varphi-\theta_r φθr,因为质心侧偏角 β \beta β和横摆角 φ \varphi φ 相比较小,可以近似认为是航向误差,但要清楚它并不是航向误差,不能直接把它忽略掉,认为 β = 0 \beta =0 β=0

  在本节可近似认为 e φ e_{\varphi} eφ 就是航向误差,但仅限本节,所以要有潜意识, e φ e_{\varphi} eφ 其实并不是航向误差,具体以后再讲,目前还涉及不到。

  有了 e d e_d ed e φ e_\varphi eφ,可将 d ˙ \dot d d˙ 的方程改写为:
e ˙ d = v x e φ + v y v y = e ˙ d − v x e φ v ˙ y = e ¨ d − v x e ˙ φ \begin{aligned} \dot{e}_d&=v_xe_{\varphi}+v_y\\ v_y&=\dot{e}_d-v_xe_{\varphi}\\ \dot{v}_y&=\ddot{e}_d-v_x\dot{e}_{\varphi}\\ \end{aligned} e˙dvyv˙y=vxeφ+vy=e˙dvxeφ=e¨dvxe˙φ  假设 v x v_x vx 是常数,那么 v ˙ x \dot v_x v˙x就被忽略掉了。
e φ = φ − θ r e ˙ φ = φ ˙ − θ ˙ r e ¨ φ = φ ¨ − θ ¨ r ≈ φ ¨ e_{\varphi}=\varphi -\theta _r\quad \dot{e}_{\varphi}=\dot{\varphi}-\dot{\theta}_r\quad \ddot{e}_{\varphi}=\ddot{\varphi}-\ddot{\theta}_r\approx \ddot{\varphi} eφ=φθre˙φ=φ˙θ˙re¨φ=φ¨θ¨rφ¨  忽略掉 θ r {\theta}_r θr 二阶以上的导数。因为道路一般曲率变化比较平缓,所以 θ r {\theta}_r θr 考虑到一阶导数为止。
  引入 e d e_d ed e φ e_\varphi eφ,可得到:
{ v y = e ˙ d − v x e φ v ˙ y = e ¨ d − v x e ˙ φ φ ˙ = e ˙ φ + θ ˙ r φ ¨ = e ¨ φ \left\{ \begin{array}{l} v_y=\dot{e}_d-v_xe_{\varphi}\\ \dot{v}_y=\ddot{e}_d-v_x\dot{e}_{\varphi}\\ \dot{\varphi}=\dot{e}_{\varphi}+\dot{\theta}_r\\ \ddot{\varphi}=\ddot{e}_{\varphi}\\ \end{array} \right. vy=e˙dvxeφv˙y=e¨dvxe˙φφ˙=e˙φ+θ˙rφ¨=e¨φ  代入二自由度动力学微分方程:
在这里插入图片描述
得到:
e ¨ d = ( C α f + C α r m v x ) e ˙ d + ( − C α f + C α r m ) e φ + ( a C α f − b C α r m v x ) e ˙ φ + ( a C α f − b C α r m v x − v x ) θ ˙ r + ( − C α f m ) δ \begin{aligned} \ddot{e}_d&=\left( \frac{C_{\alpha f}+C_{\alpha r}}{mv_x} \right) \dot{e}_d+\left( -\frac{C_{\alpha f}+C_{\alpha r}}{m} \right) e_{\varphi}\\ &+\left( \frac{aC_{\alpha f}-bC_{\alpha r}}{mv_x} \right) \dot{e}_{\varphi}+\left( \frac{aC_{\alpha f}-bC_{\alpha r}}{mv_x}-v_x \right) \dot{\theta}_r\\ &+\left( -\frac{C_{\alpha f}}{m} \right) \delta\\ \end{aligned} e¨d=(mvxCαf+Cαr)e˙d+(mCαf+Cαr)eφ+(mvxaCαfbCαr)e˙φ+(mvxaCαfbCαrvx)θ˙r+(mCαf)δ
e ¨ φ = ( a C α f − b C α r I v x ) e ˙ d + ( − a C α f − b C α r I ) e φ + ( a 2 C α f + b 2 C α r I v x ) e ˙ φ + ( a 2 C α f + b 2 C α r I v x ) θ ˙ r + ( − a C α f I ) δ \begin{aligned} \ddot{e}_{\varphi}&=\left( \frac{aC_{\alpha f}-bC_{\alpha r}}{Iv_x} \right) \dot{e}_d+\left( -\frac{aC_{\alpha f}-bC_{\alpha r}}{I} \right) e_{\varphi}\\ &+\left( \frac{a^2C_{\alpha f}+b^2C_{\alpha r}}{Iv_x} \right) \dot{e}_{\varphi}+\left( \frac{a^2C_{\alpha f}+b^2C_{\alpha r}}{Iv_x} \right) \dot{\theta}_r\\ &+\left( -\frac{aC_{\alpha f}}{I} \right) \delta \end{aligned} e¨φ=(IvxaCαfbCαr)e˙d+(IaCαfbCαr)eφ+(Ivxa2Cαf+b2Cαr)e˙φ+(Ivxa2Cαf+b2Cαr)θ˙r+(IaCαf)δ

5、误差微分方程的线性化

  上面两个方程太长了,看起来不方便,把它简化一下:
e ¨ d = a 1 e ˙ d + a 2 e φ + a 3 e ˙ φ + b 1 θ ˙ r + c 1 δ e ¨ φ = a 4 e ˙ d + a 5 e φ + a 6 e ˙ φ + b 2 θ ˙ r + c 2 δ \begin{matrix} \ddot{e}_d=& a_1\dot{e}_d+a_2e_{\varphi}+a_3\dot{e}_{\varphi}+b_1\dot{\theta}_r+c_1\delta\\ \ddot{e}_{\varphi}=& a_4\dot{e}_d+a_5e_{\varphi}+a_6\dot{e}_{\varphi}+b_2\dot{\theta}_r+c_2\delta\\ \end{matrix} e¨d=e¨φ=a1e˙d+a2eφ+a3e˙φ+b1θ˙r+c1δa4e˙d+a5eφ+a6e˙φ+b2θ˙r+c2δ  这有点线性微分方程 X ˙ = A X + B u \dot X =AX+Bu X˙=AX+Bu 意思,但很明显现在还不是线性微分方程组,将它改造:
e ¨ d = 0 ⋅ e d + a 1 e ˙ d + a 2 e φ + a 3 e ˙ φ + b 1 θ ˙ r + c 1 δ e ¨ φ = 0 ⋅ e d + a 4 e ˙ d + a 5 e φ + a 6 e ˙ φ + b 2 θ ˙ r + c 2 δ e ˙ d = 0 ⋅ e d + e ˙ d + 0 ⋅ e φ + 0 ⋅ e ˙ φ + 0 ⋅ θ ˙ r + 0 ⋅ δ e ˙ φ = 0 ⋅ e d + 0 ⋅ e ˙ d + 0 ⋅ e φ + e ˙ φ + 0 ⋅ θ r + 0 ⋅ δ \begin{aligned} \ddot{e}_d&=0\cdot e_d+a_1\dot{e}_d+a_2e_{\varphi}+a_3\dot{e}_{\varphi}+b_1\dot{\theta}_r+c_1\delta\\ \ddot{e}_{\varphi}&=0\cdot e_d+a_4\dot{e}_d+a_5e_{\varphi}+a_6\dot{e}_{\varphi}+b_2\dot{\theta}_r+c_2\delta\\ \dot{e}_d&=0\cdot e_d+\dot{e}_d+0\cdot e_{\varphi}+0\cdot \dot{e}_{\varphi}+0\cdot \dot{\theta}_r+0\cdot \delta\\ \dot{e}_{\varphi}&=0\cdot e_d+0\cdot \dot{e}_d+0\cdot e_{\varphi}+\dot{e}_{\varphi}+0\cdot \theta _r+0\cdot \delta\\ \end{aligned} e¨de¨φe˙de˙φ=0ed+a1e˙d+a2eφ+a3e˙φ+b1θ˙r+c1δ=0ed+a4e˙d+a5eφ+a6e˙φ+b2θ˙r+c2δ=0ed+e˙d+0eφ+0e˙φ+0θ˙r+0δ=0ed+0e˙d+0eφ+e˙φ+0θr+0δ


三、线性误差微分方程组

  将上式变成线性微分方程组,写成矩阵形式:
在这里插入图片描述

  得到基于自然坐标系下的线性误差微分方程组:
e ˙ r r = A e r r + B u + C θ ˙ r \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_{r} e˙rr=Aerr+Bu+Cθ˙r  这样的方程才是我们想要的控制方程,才能将控制问题转化为求代价函数极小值的问题。

  注意:这里的A B和第三节的A B不是同一个符号

  第四节大部分任务已经完成,讲到了关于误差的线性微分方程该怎么推导。

不过本节遗留了三个问题:

1、航向误差问题

  定义 e φ = φ − θ r e_{\varphi}=\varphi-\theta_{r} eφ=φθr,但实际上航向误差应该是 θ − θ r \theta-\theta_r θθr,将 θ − θ r \theta-\theta_r θθr 近似认为 φ − θ r \varphi-\theta_{r} φθr 有没有问题?

2、投影点航向角速度计算问题

  误差的线性微分方程有 θ ˙ r \dot\theta_r θ˙r,该怎么计算呢?

3、线性误差微分方程组的形式问题

  如果是 e ˙ r r = A e r r + B u \dot{e}_{rr}=Ae_{rr}+Bu e˙rr=Aerr+Bu 的形式,可以用 LQR 计算。但现在形式是 e ˙ r r = A e r r + B u + C θ ˙ r \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_{r} e˙rr=Aerr+Bu+Cθ˙r,后面还有个小尾巴 C θ ˙ r C\dot{\theta}_{r} Cθ˙r,和 LQR 问题比较像,但不完全一致。

  其实我们能控制的只有方向盘转角 u u u,而 θ ˙ r \dot\theta_r θ˙r是代表道路的几何信息,是没有办法控制的,能控制的只有方向盘转角,对于 θ ˙ r \dot\theta_r θ˙r 没有办法控制问题,该怎么办呢?

  在下一节博客会具体解释,欢迎关注!


参考资料

  【基础】自动驾驶控制算法第四讲 坐标变换与横向误差微分方程


后记:

🌟 感谢您耐心阅读这篇关于 坐标变换与横向误差微分方程 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡

🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.rhkb.cn/news/408631.html

如若内容造成侵权/违法违规/事实不符,请联系长河编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

从法律风险的角度来看,项目经理遇到不清楚或不明确问题时的处理

大家好&#xff0c;我是不会魔法的兔子&#xff0c;在北京从事律师工作&#xff0c;日常分享项目管理风险预防方面的内容。 序言 在项目开展过程中&#xff0c;有时候会遇到一些不清楚或不明确的状况&#xff0c;但碍于项目进度的紧迫性&#xff0c;不得不硬着头皮做决策&…

Golang | Leetcode Golang题解之第368题最大整除子集

题目&#xff1a; 题解&#xff1a; func largestDivisibleSubset(nums []int) (res []int) {sort.Ints(nums)// 第 1 步&#xff1a;动态规划找出最大子集的个数、最大子集中的最大整数n : len(nums)dp : make([]int, n)for i : range dp {dp[i] 1}maxSize, maxVal : 1, 1fo…

CMake构建学习笔记4-libjpeg库的构建

libjpeg是一个广泛使用的开源库&#xff0c;用于处理JPEG&#xff08;Joint Photographic Experts Group&#xff09;图像格式的编码、解码、压缩和解压缩功能&#xff0c;是许多图像处理软件和库的基础。 libjpeg本身的构建没什么特别的&#xff0c;不过值得说道的是libjpeg存…

[Other]-安装ruby、ascli、ascp

最近新接到这样一个需求&#xff0c;将生物原始数据上传到某中心&#xff0c;其中用到ascp命令&#xff0c;阴差阳错的装了ruby、ascli&#xff0c;这里就都一并介绍下安装方式&#xff0c;由于服务器老旧默认安装时ruby2.0&#xff0c;又 升级到2.7等引发的一系列问题&#xf…

力扣(动态规划)-343整数拆分;96不同的二叉搜索树

整数拆分 题目&#xff1a; 给定⼀个正整数 n&#xff0c;将其拆分为⾄少两个正整数的和&#xff0c;并使这些整数的乘积最⼤化。 返回你可以获得的最⼤乘积。 示例 1: 输⼊: 2 输出: 1 解释: 2 1 1, 1 1 1。 示例 2: 输⼊: 10 输出: 36 解释: 10 3 3 4, 3 3…

Python 递归(recursion) 和 迭代(iteration)

递归 (recursion) 是指在函数的定义中使用函数自身的方法&#xff0c;直观上来看&#xff0c;就是某个函数自己调用自己。递归的基本思想就是把规模大的问题转化为规模小的相同的子问题来解决。 在函数实现时&#xff0c;因为大问题和小问题是一样的问题&#xff0c;因此大问题…

一款人性化的终端用户界面工具

A collection of human friendly terminal user interface. Screenshot • Installation • Usage • Configuration • Thanks 截图 历史文件预览 注意: find file 依赖 fzf. file browser依赖 ranger / lf / … 安装 git clone https://github.com/StubbornVegeta/Sta…

3秒内搞定服务器端口扫描!用RustScan快速查看开放端口

文章目录 3秒内搞定服务器端口扫描&#xff01;用RustScan快速查看开放端口1. RustScan简介2. RustScan特点3. RustScan的基本使用3.1 创建alias别名3.2 基本用法3.3 常用参数说明3.4 示例4. 注意事项 最近开始公众号文章也开始同步更新了&#xff0c;对Java、大数据、人工智能…

字节微前端框架Garfish

Garfish 是字节跳动开源的微前端框架&#xff0c;旨在应对现代 Web 应用在前端生态繁荣与应用日益复杂化背景下的挑战。本文将介绍如何使用 Garfish&#xff0c;提供代码示例&#xff0c;并与另一流行的微前端框架 Qiankun 进行对比分析。 安装 Garfish 首先&#xff0c;安装…

大模型对齐:DPO vs PPO

现在这些大型语言模型&#xff08;LLMs&#xff09;&#xff0c;可真是火得不行&#xff0c;各行各业都离不开它们了。它们能处理和写出跟我们差不多的文本&#xff0c;这让自然语言处理、写东西、还有客服这些领域都焕然一新。不过呢&#xff0c;这技术进步的同时也带来了一个…

Unity+Addressable

前期准备 下载一个hfs本地服务器&#xff0c;打开即可 HFS ~ HTTP 文件服务器 (rejetto.com) 1.安装Addressable插件 创建组 2.使用图片创建预制体 放入Addressable Groups内 3.右键 新建组 创建预制体t拖拽放入新建组里 新组命名为Gameobject 简化名称 4.创建一个测试脚本 …

Array List 练习(添加手机对象并返回要求的数据)

package ArrayListDemo;import java.util.ArrayList;public class ArrayListDemo7 {public static void main(String[] args) {//1.创建集合对象ArrayList<Phone> list new ArrayList<Phone>();//2.创建手机对象Phone ph1 new Phone("小米",1000);Pho…

使用 setResponseStatus 函数设置响应状态码

title: 使用 setResponseStatus 函数设置响应状态码 date: 2024/8/25 updated: 2024/8/25 author: cmdragon excerpt: 通过 setResponseStatus 函数,你可以轻松地在 Nuxt.js 中设置响应的状态码。这不仅能帮助用户更好地理解发生了什么,还能在需要时显示自定义的错误页面。…

rust api接口开发(以登陆和中间件鉴权为例)

rust rest api接口开发 所需依赖 axumtokioredis cargo add axum redis cargo add tokio --featuresfull路由服务创建和运行 //子路由 let v1router axum::Router::new(); //主路由,并将子路由绑定到主路由 let routeraxum::Router::new().nest("/v1",v1router)…

Pytorch实现CIFAR10训练模型

文章目录 简述模型结构模型参数、优化器、损失函数参数初始化优化器损失函数 模型训练、测试集预测、模型保存、日志记录训练测试集测试模型保存模型训练完整代码 tensorboard训练可视化结果train_loss测试准确率测试集loss 模型应用模型独立应用代码api.py预测结果 简述 使用…

Axure设计之三级菜单导航教程(中继器)

中继器作为复杂的元件&#xff0c;通常被用来制作“高保真”的动态原型&#xff0c;以达到良好的视觉效果和交互效果。本文将教大家通过AxureRP9工具如何使用中继器设计三级菜单导航。 一、案例效果 原型预览&#xff1a;https://1zvcwx.axshare.com 主要效果&#xff1a; 1…

数据结构(Java实现):链表与LinkedList

文章目录 1. 单向链表1.1 链表的概念及结构1.2 链表的实现1.2.1 单向链表类和节点1.2.2 打印每个节点的值1.2.3 计算链表长度1.2.4 头插节点1.2.5 尾插节点1.2.6 在指定下标插入新节点1.2.7 判断是否存在某个节点1.2.8 移除某个节点1.2.9 移除所有指定节点1.2.10 清空链表1.2.1…

redis | 认识非关系型数据库Redis的哈希数据类型

Redis 非关 kv型 哈希通用命令python 操作hash应用场景 数据类型 数据类型丰富&#xff0c;字符串strings,散列hashes,列表lists&#xff0c;集合sets,有序集合sorted sets等等 哈希 定义 1、由field和关联的value组成的键值对 类似于python的键值对 2、field和value.是字符…

一文学会Shell中case语句和函数

大家好呀&#xff01;今天简单聊一聊Shell中的case语句与函数。在多选择情况下使用case语句将非常方便&#xff0c;同时&#xff0c;函数的学习和使用对于学好一门编程语言也是非常重要的。 一、case语句 case语句为多选择语句。可以用case语句匹配一个值与一个模式&#xff0c…

OpenCV绘图函数详解及其用法示例

MFC类库中的CDC类有划线,画矩形,画椭圆,画多边形,文字等绘图函数,OpenCV也有类似的绘图函数。二者的区别在于MFC画图是在一定的区域内绘制图形,而OpenCV则是在图像上绘制,主要用于图像标注。 OpenCV的常用绘图函数有arrowedLine,circle ,drawContours, drawMarker, dra…