自动驾驶控制算法-横向误差微分方程LQR前馈控制

news/2024/12/28 12:16:19/

本文是学习自动驾驶控制算法第六讲 前馈控制与航向误差以及前两节的学习笔记。

1 横向误差微分方程

以规划的轨迹作为自然坐标系,计算自车在轨迹上的投影点,进而计算误差:
在这里插入图片描述
如图所示,横向误差为 d d d,航向误差为 θ − θ r \theta-\theta_r θθr,投影点的速度大小为 s ˙ \dot{s} s˙,注意这里的 θ \theta θ是航向角,与横摆角 φ \varphi φ相差一个侧偏角 β \beta β
θ = φ + β \begin{equation} \theta=\varphi+\beta \end{equation} θ=φ+β
根据之前所介绍的笛卡尔坐标系与自然坐标系的转换关系可知:
d ˙ = v sin ⁡ ( θ − θ r ) \begin{equation} \dot{d}=v\sin(\theta-\theta_r) \end{equation} d˙=vsin(θθr)
s ˙ = v cos ⁡ ( θ − θ r ) 1 − k r d \begin{equation} \dot{s}=\frac{v\cos(\theta-\theta_r)}{1-k_rd} \end{equation} s˙=1krdvcos(θθr)
这里 k r k_r kr是投影点处的曲率。
结合式1和2
d ˙ = v sin ⁡ ( φ + β − θ r ) = v sin ⁡ β cos ⁡ ( φ − θ ) + v cos ⁡ β sin ⁡ ( φ − θ ) \begin{equation} \dot{d}=v\sin(\varphi+\beta-\theta_r)=v\sin{\beta}\cos{(\varphi-\theta)}+v\cos{\beta}\sin{(\varphi-\theta)} \end{equation} d˙=vsin(φ+βθr)=vsinβcos(φθ)+vcosβsin(φθ)
φ − θ r \varphi-\theta_r φθr为小量,所以上式进一步简化为
d ˙ = v y + v x ( φ − θ r ) \begin{equation} \dot{d}=v_y+v_x(\varphi-\theta_r) \end{equation} d˙=vy+vx(φθr)

e d = d \begin{equation} e_d=d \end{equation} ed=d
e φ = φ − θ r \begin{equation} e_{\varphi}=\varphi-\theta_{r} \end{equation} eφ=φθr
求一阶二阶导数则有
e d ˙ = v x e φ + v y \begin{equation} \dot{e_d}=v_xe_{\varphi}+v_y \end{equation} ed˙=vxeφ+vy
假设 v x v_x vx是常数
v y ˙ = e d ¨ − v x e φ ˙ \begin{equation} \dot{v_y}=\ddot{e_d}-v_x\dot{e_{\varphi}} \end{equation} vy˙=ed¨vxeφ˙
e ¨ φ = φ ¨ − θ ¨ r ≈ φ ¨ \begin{equation} \ddot{e}_{\varphi}=\ddot{\varphi}-\ddot{\theta}_{r}≈\ddot{\varphi} \end{equation} e¨φ=φ¨θ¨rφ¨
这里 θ ¨ r \ddot{\theta}_r θ¨r约等于0,是因为轨迹通常比较平滑。
综合可得
{ v y = e ˙ d − v x e φ v ˙ y = e ¨ d − v x e ˙ φ φ ˙ = e ˙ φ + θ ˙ r φ ¨ = e ¨ φ \begin{equation} \begin{cases} 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{cases} \end{equation} vy=e˙dvxeφv˙y=e¨dvxe˙φφ˙=e˙φ+θ˙rφ¨=e¨φ
由上节的公式:
[ v y ˙ φ ¨ ] = [ C α f + C α r m v x a C α f − b C α r m v x − v x a C α f − b C α r I v x a 2 C α f + b 2 C α r I v x ] [ v y φ ˙ ] + [ − C α f m − a C α f I ] δ \begin{equation} \begin{bmatrix} \dot{v_y} \\ \ddot{\varphi} \end{bmatrix}= \begin{bmatrix} \frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} & \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x \\ \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x} & \frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix} \begin{bmatrix} v_y \\ \dot{\varphi} \end{bmatrix}+ \begin{bmatrix} -\frac{C_{\alpha{f}}}{m} \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix}\delta \end{equation} [vy˙φ¨]=[mvxCαf+CαrIvxaCαfbCαrmvxaCαfbCαrvxIvxa2Cαf+b2Cαr][vyφ˙]+[mCαfIaCαf]δ
结合式11和式12可得
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{equation} \ddot{e}_d=\frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} \dot{e}_d+(-\frac{C_{\alpha{f}}+C_{\alpha{r}}}{m})e_{\varphi}+\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}\dot{e}_{\varphi}+(\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x)\dot{\theta}_r+(-\frac{C_{\alpha{f}}}{m})\delta \end{equation} e¨d=mvxCαf+Cαre˙d+(mCαf+Cαr)eφ+mvxaCαfbCαre˙φ+(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{equation} \ddot{e}_{\varphi}=\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x} \dot{e}_d+(-\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{I})e_{\varphi}+\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x}\dot{e}_{\varphi}+(\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x})\dot{\theta}_r+(-\frac{aC_{\alpha{f}}}{I})\delta \end{equation} e¨φ=IvxaCαfbCαre˙d+(IaCαfbCαr)eφ+Ivxa2Cαf+b2Cαre˙φ+(Ivxa2Cαf+b2Cαr)θ˙r+(IaCαf)δ
进而有
[ e ˙ d e ¨ d e ˙ φ e ¨ φ ] = [ 0 1 0 0 0 C α f + C α r m v x − C α f + C α r m a C α f − b C α r m v x 0 0 0 1 0 a C α f − b C α r I v x − a C α f − b C α r I a 2 C α f + b 2 C α r I v x ] [ e d e ˙ d e φ e ˙ φ ] + [ 0 − C α f m 0 − a C α f I ] δ + [ 0 a C α f − b C α r m v x − v x 0 a 2 C α f + b 2 C α r I v x ] θ ˙ r \begin{equation} \begin{bmatrix} \dot{e}_d \\ \ddot{e}_{d} \\ \dot{e}_{\varphi} \\ \ddot{e}_{\varphi} \end{bmatrix}= \begin{bmatrix} 0&1&0&0 \\ 0&\frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} &-\frac{C_{\alpha{f}}+C_{\alpha{r}}}{m}&\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x} \\ 0&0&0&1 \\ 0&\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x}&-\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{I}&\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix} \begin{bmatrix} e_d \\ \dot{e}_d \\ e_{\varphi} \\ \dot{e}_{\varphi} \end{bmatrix}+ \begin{bmatrix} 0\\ -\frac{C_{\alpha{f}}}{m} \\ 0 \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix}\delta+ \begin{bmatrix} 0 \\ \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x \\ 0 \\ \frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix}\dot{\theta}_r \end{equation} e˙de¨de˙φe¨φ = 00001mvxCαf+Cαr0IvxaCαfbCαr0mCαf+Cαr0IaCαfbCαr0mvxaCαfbCαr1Ivxa2Cαf+b2Cαr ede˙deφe˙φ + 0mCαf0IaCαf δ+ 0mvxaCαfbCαrvx0Ivxa2Cαf+b2Cαr θ˙r
e ˙ r r = A e r r + B u + C θ ˙ r \begin{equation} \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_r \end{equation} e˙rr=Aerr+Bu+Cθ˙r

2 LQR原理

对于式16,先暂时不考虑最后一项,那么有
e ˙ r r = A e r r + B u \begin{equation} \dot{e}_{rr}=Ae_{rr}+Bu \end{equation} e˙rr=Aerr+Bu
目的是选择合适的 u u u使得 ∣ e ˉ r r ∣ |\boldsymbol{\bar{e}}_{\boldsymbol{rr}}| eˉrr尽可能小,也即式
J = w a e r r 2 + w b u 2 \begin{equation} J=w_a{\boldsymbol{e}}^2_{\boldsymbol{rr}}+w_bu^2 \end{equation} J=waerr2+wbu2
尽可能小,进一步也即
J = e r r T Q e r r + u T R u \begin{equation} J={\boldsymbol{e}}^T_{\boldsymbol{rr}}Q{\boldsymbol{e}}_{\boldsymbol{rr}}+u^TRu \end{equation} J=errTQerr+uTRu
尽可能小,其中 Q Q Q R R R是对角矩阵,问题就变成了在式17的约束下使 J J J取最小值。

2.1 连续方程离散化

式17写成一般形式
x ˙ = A x + B u \begin{equation} \dot{x}=Ax+Bu \end{equation} x˙=Ax+Bu
上式两边积分
∫ t t + d t x ˙ ( τ ) d τ = ∫ t t + d t A x ( τ ) d τ + ∫ t t + d t B u ( τ ) d τ \begin{equation} \int_t^{t+dt}\dot{x}(\tau)d\tau=\int_t^{t+dt}Ax(\tau)d\tau+\int_t^{t+dt}Bu(\tau)d\tau \end{equation} tt+dtx˙(τ)dτ=tt+dtAx(τ)dτ+tt+dtBu(τ)dτ
得到
x ( t + d t ) − x ( t ) = A x ( ξ ) d t + B u ( ξ ) d t \begin{equation} x(t+dt)-x(t)=Ax(\xi)dt+Bu(\xi)dt \end{equation} x(t+dt)x(t)=Ax(ξ)dt+Bu(ξ)dt
A ( ξ ) A(\xi) A(ξ)采用中值欧拉法,对 u ( ξ ) u(\xi) u(ξ)采用向前欧拉法(因为 u ( t + d t ) u(t+dt) u(t+dt)未知)得到:
x ( t + d t ) = x ( t ) + A d t ( x ( t + d t ) + x ( t ) 2 ) + B u ( t ) d t \begin{equation} x(t+dt)=x(t)+Adt(\frac{x(t+dt)+x(t)}{2})+Bu(t)dt \end{equation} x(t+dt)=x(t)+Adt(2x(t+dt)+x(t))+Bu(t)dt
( I − A d t 2 ) x ( t + d t ) = ( I + A d t 2 ) x ( t ) + B u ( t ) d t \begin{equation} (I-\frac{Adt}{2})x(t+dt)=(I+\frac{Adt}{2})x(t)+Bu(t)dt \end{equation} (I2Adt)x(t+dt)=(I+2Adt)x(t)+Bu(t)dt
x ( t + d t ) = ( I − A d t 2 ) − 1 ( I + A d t 2 ) x ( t ) + ( I − A d t 2 ) − 1 B d t u ( t ) ≈ ( I − A d t 2 ) − 1 ( I + A d t 2 ) x ( t ) + B d t u ( t ) \begin{equation} \begin{split} x(t+dt) &= (I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})x(t)+(I-\frac{Adt}{2})^{-1}Bdtu(t) \\ &≈(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})x(t)+Bdtu(t) \end{split} \end{equation} x(t+dt)=(I2Adt)1(I+2Adt)x(t)+(I2Adt)1Bdtu(t)(I2Adt)1(I+2Adt)x(t)+Bdtu(t)
x k + 1 = A ˉ x k + B ˉ u k \begin{equation} x_{k+1}=\bar{A}x_k+\bar{B}{u_k} \end{equation} xk+1=Aˉxk+Bˉuk

2.2 LQR

问题就是在式26的约束下,求 u u u使式
J = ∑ k = 1 ∞ x k T Q x k + u k T R u k \begin{equation} J=\sum_{k=1}^\infty{x^T_{k}Qx_k}+u^T_kRu_k \end{equation} J=k=1xkTQxk+ukTRuk
取得最小值。
u u u的形式为
u = − K x \begin{equation} u=-Kx \end{equation} u=Kx
K = ( R + B ˉ T P B ˉ ) − 1 B ˉ T P A ˉ \begin{equation} K=(R+\bar{B}^TP\bar{B})^{-1}\bar{B}^TP\bar{A} \end{equation} K=(R+BˉTPBˉ)1BˉTPAˉ
其中其 P P P就是离散时间 R i c c a t i Riccati Riccati方程
P = Q + A ˉ T P A ˉ − A ˉ T P B ˉ ( R + B ˉ T P B ˉ ) − 1 B ˉ T P A ˉ \begin{equation} P=Q+\bar{A}^TP\bar{A}-\bar{A}^TP\bar{B}(R+\bar{B}^TP\bar{B})^{-1}\bar{B}^TP\bar{A} \end{equation} P=Q+AˉTPAˉAˉTPBˉ(R+BˉTPBˉ)1BˉTPAˉ
的解。

3 前馈控制与航向误差

对于式16,如果使用上一节的LQR结果(式28、29),
e ˙ r r = ( A − B K ) e r r + C θ ˙ r \begin{equation} \dot{e}_{rr}=(A-BK)e_{rr}+C\dot{\theta}_r \end{equation} e˙rr=(ABK)err+Cθ˙r
无论 K K K取何值, e ˙ r r \dot{e}_{rr} e˙rr e r r {e}_{rr} err不可能同时为0,那么 e r r {e}_{rr} err也就不会为0,系统存在稳态误差
引入前馈控制消除稳态误差
u = − K x + δ f \begin{equation} u=-Kx+\delta_f \end{equation} u=Kx+δf
在这里插入图片描述
e ˙ r r = ( A − B K ) e r r + B δ f + C θ ˙ r \begin{equation} \dot{e}_{rr}=(A-BK)e_{rr}+B\delta_f+C\dot{\theta}_r \end{equation} e˙rr=(ABK)err+Bδf+Cθ˙r
系统稳定后, e ˙ r r = 0 \dot{e}_{rr}=0 e˙rr=0
e r r = − ( A − B K ) − 1 ( B δ f + C θ ˙ r ) \begin{equation} e_{rr}=-(A-BK)^{-1}(B\delta_f+C\dot{\theta}_r) \end{equation} err=(ABK)1(Bδf+Cθ˙r)
选取合适的 δ f \delta_f δf,使 e r r {e}_{rr} err尽可能接近0。
式34展开后得:
e r r = [ 1 k 1 { δ f − θ ˙ r v x [ a + b − b k 3 − m v x 2 a + b ( b c f + a c r k 3 − a c r ) ] } 0 − θ ˙ r v x ( b + a a + b m v x 2 c α f ) 0 ] \begin{equation} e_{rr}= \begin{bmatrix} \frac{1}{k_1}\{\delta_f-\frac{\dot{\theta}_r}{v_x}[a+b-bk_3-\frac{mv^2_x}{a+b}(\frac{b}{c_f}+\frac{a}{c_r}k_3-\frac{a}{c_r})]\} \\ 0\\ -\frac{\dot{\theta}_r}{v_x}(b+\frac{a}{a+b}\frac{mv^2_x}{c_{\alpha{f}}})\\ 0 \end{bmatrix} \end{equation} err= k11{δfvxθ˙r[a+bbk3a+bmvx2(cfb+crak3cra)]}0vxθ˙r(b+a+bacαfmvx2)0
可知当
δ f = θ ˙ r v x [ a + b − b k 3 − m v x 2 a + b ( b c f + a c r k 3 − a c r ) ] \begin{equation} \delta_f=\frac{\dot{\theta}_r}{v_x}[a+b-bk_3-\frac{mv^2_x}{a+b}(\frac{b}{c_f}+\frac{a}{c_r}k_3-\frac{a}{c_r})] \end{equation} δf=vxθ˙r[a+bbk3a+bmvx2(cfb+crak3cra)]
时, e d {e}_{d} ed等于0,其中 k 3 k_3 k3是反馈 K K K中的第三个元素。
通过一系列化简,式35的第三个元素可近似等于 − β -\beta β,即
e φ = − β \begin{equation} e_{\varphi}=-\beta \end{equation} eφ=β
因为目的是 θ − θ r = 0 \theta-\theta_r=0 θθr=0,那么 e φ {e}_{\varphi} eφ的稳态误差刚好就是 − β -\beta β


http://www.ppmy.cn/news/1558784.html

相关文章

Fast adaptively balanced min-cut clustering

#0.论文信息 标题:Fast adaptively balanced min-cut clustering期刊:Pattern Recognition作者: Feiping Nie , Fangyuan Xie , Jingyu Wang ,Xuelong Li机构: China Telecom, Northwestern Polytechnic al University.代码链接: #1.摘要 …

低空经济的地理信息支撑:构建安全、高效的飞行管理体系

随着无人机等低空飞行器的广泛应用,低空空域管理的重要性日益凸显。地理信息技术作为低空空域管理的重要支撑,对于保障低空经济的健康发展具有不可替代的作用。 地理信息技术在低空空域管理中的作用 地理信息技术在低空空域管理中扮演着关键角色&#x…

科研内训:助力科研成果转化--定制化服务、以需求为导向,深刻理解科研团队在不同领域中的特定需求,从内容设计到实施全流程,确保真正契合的实际需求

科研内训:助力科研成果转化--定制化服务 以需求为导向,深刻理解科研团队在不同领域中的特定需求,从内容设计到实施全流程,确保真正契合的实际需求。 优势:1定制化方案 根据科研方向、团队需求及项目背景量身定制&#…

【hackmyvm】deba靶机wp

tags: HMVnodejs反序列化CVE-2017-5941wine命令定时任务 1. 基本信息^toc 文章目录 1. 基本信息^toc2. 信息收集2.1. 端口扫描2.2. 目录扫描 3. nodejs反序列化 (CVE-2017-5941)4. www-data提权low用户5. 定时任务提权6. wine命令 提权root6.1. 利用CS获取root 靶机链接 http…

Jenkins入门使用

Jenkins入门使用 1先安装jdk才能运行jenkins yum install -y java-1.8.0-openjdk.x86_64 2 安装jenkins,运行,进行端口绑定,启动jenkins docker search jenkins docker pull jenkins/jenkins docker run -d -u root -p 8080:8080 -p 50000:50…

租赁小程序的优势与应用场景分析

内容概要 在现代经济浪潮中,租赁小程序逐渐成为不少企业和用户的心头好。想象一下,你需要一把专业相机,但这玩意儿放在家里好像没有什么机会被用到。这个时候,租赁小程序就派上用场了!通过线上平台,各种高…

Docker中的MYSQL导入本地SQL语句

在本地mysql安装的bin目录下打开cmd窗口并执行以下命令导出sql文件 mysqldump -uroot -p mysql >schema.sql mysql -数据库 schema.sql -导出的SQL语句文件名 使用xftp上传文件到centos7中的某个文件夹中 使用docker cp schema.sql mysql:.(有一个点)上传到mys…

XL系列433芯片、2.4G收发芯片 通讯对码说明

XL系列433芯片对码说明: 发射芯片 XL4456 通过数据脚接收高低电平然后经过调制将波形发出,而接收芯片 XL520 通过接收波形后进行解调,数据脚输出高低电平。至于具体的通信协议,需要用户自定义,一般而言,使…