写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_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−xr其中,规划值 x ⃗ r \vec{x}_{r} xr已知,真实值 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+Bu⇒err=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 下取最小值的数学问题。其实就是控制里的LQR,L 就是 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+Bu⇒err=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 v−s˙
其中,速度误差属于纵向公式,本篇博客介绍横向控制,所以速度误差暂时先不管。
注意: 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 xr,一头指向真实的车辆位置 x ⃗ \vec x x。 x ⃗ \vec x x 、 x ⃗ r \vec x_r xr 和 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−xr)⋅nr这样得到 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 nr 是向量不是常矢量,会随曲线投影点的变化而变化,大小是单位数量,但是方向会不断变化,它的方向变化和曲线的曲率有关,所以 n ⃗ r \vec n_r nr 不是常矢量。
对 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)⋅nr+(x−xr)⋅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 dt→0时, 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)⋅nr+(x−xr)⋅dtdnr 同样 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τ=κndsdn=−κτ其中, κ \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−xr)⋅nr,再带到 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−nr)+dnr⋅(−ks˙τr) 又因为 τ ⃗ r \vec \tau_r τr 和 n ⃗ r \vec{n}_r nr 垂直,所以后面一项为 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 nr 间的几何关系如下:
τ ⃗ \vec \tau τ和 n ⃗ r \vec{n}_r nr之间的角度等于 π 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 nr 都是单位矢量,所以它们的模都是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˙nr+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)nr+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−κd∣v∣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−κd∣v∣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˙d−vxeφ=e¨d−vxe˙φ 假设 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˙d−vxeφv˙y=e¨d−vxe˙φφ˙=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αf−bCαr)e˙φ+(mvxaCαf−bCαr−vx)θ˙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αf−bCαr)e˙d+(−IaCαf−bCα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˙φ=0⋅ed+a1e˙d+a2eφ+a3e˙φ+b1θ˙r+c1δ=0⋅ed+a4e˙d+a5eφ+a6e˙φ+b2θ˙r+c2δ=0⋅ed+e˙d+0⋅eφ+0⋅e˙φ+0⋅θ˙r+0⋅δ=0⋅ed+0⋅e˙d+0⋅eφ+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 没有办法控制问题,该怎么办呢?
在下一节博客会具体解释,欢迎关注!
参考资料
【基础】自动驾驶控制算法第四讲 坐标变换与横向误差微分方程
后记:
🌟 感谢您耐心阅读这篇关于 坐标变换与横向误差微分方程 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀