精华内容
下载资源
问答
  • 在这项工作中,我们使用 PID 控制和滑模控制开发了自主车辆的横向控制策略,以最大限度地减少自主车辆的横向位移,以遵守参考路径。
  • 以类似的方式,在本研究中实施和研究了无人机横向控制系统,该系统已使用比例,积分和微分(PID)控制器,相位超前补偿器和信号约束控制器进行了优化。 这项研究的意义在于优化现有的无人机控制器工厂,以改善横向...
  • 无人驾驶车辆模型预测控制横向控制轨迹跟踪
  • 本人搭建的一些Carsim与Simulink的联合仿真模型,包括车道保持(LKA),自适应巡航(ACC),轨迹跟随,横向控制,预瞄跟随,单点预瞄,多点预瞄,滑模变结构控制,模糊控制等算法的介绍和实现,该下载资料不为CSDN...
  • 包含车道保持,横向控制,自适应巡航等多种carsim与simulink联合仿真模型,下载后请详细查看说明文档,提供给有需要的或者有相关设计需求的人,本人自己搭建,使用了PID,模糊控制,单点预瞄,多点预瞄,滑模变结构...
  • 智能车辆自主导航中 ,横向控制是指控制车辆 在不同的车速、载荷、路况以及风阻等条件下自动跟 踪行车路线 ,并保持一定的舒适性和平稳性要求 ,实 际上也就是车辆转向控制.智能车辆与一般的室外 轮式移动机器人相比 ,...
  • 基于视觉的智能车辆模糊滑模横向控制
  • 基于最优前轮侧偏力的智能汽车LQR横向控制_陈亮.pdf
  • 基于车辆模型的横向控制方法

    万次阅读 多人点赞 2020-03-24 14:37:05
    无人驾驶车辆基于模型横向控制、车辆运动学模型、纯跟踪、后轮反馈、前轮反馈控制算法;车辆动力学模型、车辆线性二自由度动力学模型、车辆路径跟踪偏差状态方程的构建、LQR 横向控制算法的求解


    1. 简介


    1.1. 无人驾驶车辆

    大到类似百度apollo无人车,无人配送车
    小到扫地机器人,其实都是无人驾驶车辆
    都是一个复杂的软硬件结合系统
    其软件算法模块主要包含:感知、定位、轨迹预测、行为决策、运动规划 及 反馈控制

    在这里介绍和补充的是 反馈控制部分
    部分软件实现可参考:个人开源项目 path-following-control


    1.2. 横向控制和纵向控制

    反馈控制是无人车 最底层 的软件算法模块,包括:

    • 横向控制,主要用于车辆方向盘的控制
    • 纵向控制,主要用于车辆油门、刹车的控制

    两者协同工作以 使无人车按照预定的参考轨迹行驶

    其中,横向控制根据上层运动规划输出的 路径曲率 等信息进行跟踪控制,以减少跟踪误差
    同时保证车辆行驶的 稳定性舒适性


    1.3. 横向控制

    根据横向控制使用车辆模型的不同,可以将其分为两种类型,包括:

    • 无模型 的横向控制方法
    • 基于模型 的横向控制方法

    1.3.1. 无模型横向控制

    无模型的横向控制即传统的 PID 控制算法
    将车辆当前的 路径跟踪偏差 作为输入量
    对跟踪偏差进行比例(Proportion)、积分(Integration)和微分(Differentiation)控制得到转向控制量
    具体原理可以参考:增量式PID控制的理解和应用

    但该算法由于没有考虑车辆本身的特性
    因此算法对外界干扰的鲁棒性较差,无法满足车辆在高速行驶过程中的有效控制


    1.3.2. 基于模型横向控制

    而其中基于模型的方法又可分为:

    • 基于车辆 运动学模型 的横向控制方法
    • 基于车辆 动力学模型 的横向控制方法

    因此,将主要介绍包括:

    • 基于车辆运动学模型的纯跟踪控制(Pure Pursuit)算法
    • 后轮反馈控制(Rear wheel feedback)
    • 前轮反馈控制(Front wheel feedback)算法
    • 基于车辆动力学模型的线性二次型调节器(Linear Quadratic Regulator,LQR)控制算法

    2. 基于车辆运动学模型的控制方法


    2.1. 车辆运动学模型

    基于车辆运动学模型的控制方法主要考虑了 车辆的运动学约束对车辆控制的影响

    首先了解一下常见的车辆运动学模型:
    在这里插入图片描述

    平时见到的汽车属于 阿克曼转向,通常可以简化为两轮的自行车模型

    在这里插入图片描述

    其假设车辆的前后轮均为刚体,只能沿着车轮滚动的方向前进,无侧向滑动

    由上述假设可得自行车模型的运动微分方程如下式:

    在这里插入图片描述

    角速度和角速度的关系:v=ωr,即 ω=v/r,因此这里 ψ ˙ \dot{\psi} ψ˙ = v * tan( δ δ δ) / L
    其他的比较简单,就不加以说明


    2.2. 纯跟踪控制算法

    纯跟踪控制算法(Pure Pursuit)是一种典型的横向控制方法
    最早由 R. Wallace 在1985年提出,该方法对外界的 鲁棒性 较好

    该算法的思想:
    基于当前车辆 后轮中心 位置,在参考路径上向 l l ld (自定义)的距离匹配一个预瞄点
    假设车辆后轮中心点可以按照一定的转弯半径𝑅 行驶抵达该预瞄点
    然后根据 预瞄距离 l l ld , 转弯半径𝑅,车辆坐标系下预瞄点的朝向角𝛼 之间的几何关系来 确定前轮转角

    在这里插入图片描述

    弧的弧度为2𝛼是因为这个三角形是等腰三角形,边长为𝑅,而其中一个角为 π π π/2-𝛼
    所以这个弧度为 π π π -( π π π / 2- 𝛼)* 2 = 2𝛼

    如图所示,弧线为经过车辆后轮中心以及预瞄点的圆弧
    圆弧的半径𝑅 与 朝向角𝛼和 预瞄距离 l l ld 有关,其实也就是与( x x xg y y yg)有关

    作一条辅助线d,垂直于𝑅,则:
    在这里插入图片描述
    根据图中的几何关系,𝑅, 𝛼, 与 l l ld 的关系为:

    在这里插入图片描述
    再作一条辅助线,垂直于 l l ld,则:
    在这里插入图片描述
    根据图中的几何关系,𝑅, 𝛼, 与 l l ld 的关系为:

    在这里插入图片描述
    根据运动学方程(2-1) 中 ψ ˙ \dot{\psi} ψ˙ = v ∗ t a n v * tan vtan( δ δ δ) / L / L /L
    t a n tan tan( δ δ δ) = ψ ˙ \dot{\psi} ψ˙ ∗ L / v * L / v L/v,而在这里 ψ ˙ \dot{\psi} ψ˙ / v / v /v = 𝑅,即:

    在这里插入图片描述
    再把式(2-4)带入(2-5)可得前轮转角为:
    在这里插入图片描述


    2.3. 后轮反馈控制算法

    后轮反馈式(Rear wheel feedback)是利用 后轮中心路径跟踪偏差量 来进行转向控制量计算的方法

    在这里插入图片描述

    根据运动学方程(2-1)及车辆后轮与参考路径的几何关系
    可推导出参考路径坐标系 (𝑠, 𝑙)

    Ferent 坐标系下 (𝑠, 𝑒, ψ \psi ψ𝑒) 的变化率为:

    在这里插入图片描述

    Frenet坐标系使用道路的中心线作为参考线,使用参考线的 切线向量法线向量 建立坐标系
    那么基于参考线的位置,就可以使用 纵向距离横向距离 来描述任意位置
    同时纵向和横向的速度、加速度、加加速度等信息也更便于计算

    对于二次连续可导的参考线,需要设计车身横摆角速度𝜔保证在李亚普洛夫方程下局部渐进收敛:
    在这里插入图片描述
    李亚普洛夫稳定(一个分析系统的稳定性方法)用数学的语言描述为:
    可控可微分的状态方程 x ˙ \dot{x} x˙ = f ( x , u ) f(x,u) f(x,u)

    在给定的参考轨 x x xref ( t ) (t) (t) 下,存在 ∀ \forall ε>0, t t t1< t t t2,𝛽>0,满足下列条件:
    :

    在这里插入图片描述
    对于李亚普洛夫稳定也分为渐进稳定和指数稳定

    李亚普洛夫稳定性理论是从能量的观点出发的
    系统的能量随着时间的推移不断变化(增加or减少),在有限时间内,系统趋于稳定,能量不再变化
    此时,系统的k+1时刻的能量减去k时刻的能量是<0的,即V(k+1)-V(k)<0,V代表能量函数

    渐进稳定指对于时变系统,𝛽在条件(1)下独立于时间 t t t1
    指数稳定指收敛率是以指数下降

    对于后轮反馈式算法,为了保证车辆的李亚普洛夫稳定性,车身横摆角速度𝜔可表示为式(2-10)

    在这里插入图片描述

    其中, 𝑘ψ 为横摆角偏差反馈控制增益, 𝑘e 为横向位置偏差反馈控制增益

    因此,根据运动学方程(2-1),可得到前轮转角𝛿为:
    在这里插入图片描述


    2.4. 前轮反馈控制算法

    前轮反馈控制(Front wheel feedback)又称 Stanley 控制
    其核心思想是利用 前轮中心路径跟踪偏差量 对方向盘转向控制量进行计算

    在这里插入图片描述

    前轮中心横向位置跟踪偏差𝑒 的变化率为:

    在这里插入图片描述

    式(2-12)中可以看出前轮横向位置偏差的变化率 e ˙ \dot{e} e˙ 可以直接通过 前轮转角𝛿 进行控制

    当定义 e ˙ \dot{e} e˙ = − k = -k =k𝑒 后,可以保证前轮横向位置偏差𝑒 满足 李亚普洛夫的指数化收敛,即:

    在这里插入图片描述

    李亚普洛夫指数:在相空间中相互靠近的两条轨线随着时间的推移,按指数分离或聚合的平均变化速率

    关于 a r c s i n arcsin arcsin 函数:
    在这里插入图片描述
    那么对于式(2-13)来说,当 ∣ − k e / v |-ke/v ke/vf ∣ > 1 |>1 >1 时,函数无解

    对于有限域内的指数收敛可以放宽到局部指数收敛,从而将前轮转角𝛿 定义为:

    在这里插入图片描述

    Stanley 前轮反馈控制算法 在变曲率路径且路径连续可导 的条件下,可以满足局部指数收敛特性
    但也可以看出该算法不适用于倒车的场景,导致对于自动泊车等工况不适用


    3. 基于车辆动力学模型的横向控制

    基于车辆运动学模型的方法忽略了车辆的动力学特性
    因此在 车速过快 或者 曲率变化率过大 的情况下该算法无法满足车辆的 稳定性 控制要求

    这里介绍一种典型的基于车辆动力学模型的横向控制方法,包括

    • 车辆线性二自由度动力学模型的构建
    • 车辆路径跟踪偏差状态方程的构建
    • LQR 横向控制算法的求解

    3.1. 车辆线性二自由度动力学模型

    对于基于车辆动力学模型的控制方法,首要的工作是对车辆动力学进行建模
    由于车辆动力学模型具有强 非线性时变 的特性,因此精确建模整个车辆动力学十分困难

    在横向控制中,主要关心的是车辆横向运动特性
    通常将车辆动力学模型简化为的二自由度横向动力学模型
    假设车身的纵向速度Vx 保持不变, 其横向动力学模型的两个自由度为:

    • 横向运动
    • 横摆运动

    由于精确的二自由度动力学模型是非线性的,为了便于进行实时的跟踪控制计算
    通常还需要在精确的二自由度动力学模型基础上进行一些简化近似,得到线性二自由度动力学模型

    这里介绍基于 转向小角度轮胎动力学 线性化假设下的车辆线性二自由度动力学模型
    下图为车辆为车辆横向动力学坐标系示意图
    在这里插入图片描述

    假设道路平整(不考虑道路横向的倾斜),且转向角度较小时
    则车身𝑦轴方向应用牛顿第二定律可得:

    在这里插入图片描述
    其中,𝑚是整车质量, 𝛼y 是车辆质心处的横向加速度, Fyf,Fyr 为地面给前轮胎和后轮施加的横向力
    而横向加速度𝛼y 由两部分组成:

    • 车辆沿车身𝑦轴横向运动产生的加速度 y ˙ \dot{y} y˙
    • 车身横摆运动产生的向心加速度 vx ψ ˙ \dot{\psi} ψ˙

    因此:
    在这里插入图片描述
    将(3-2)代入(3-1)可得:
    在这里插入图片描述
    然后,车辆绕𝑧轴的转矩平衡方程为:

    在这里插入图片描述
    其中, l l lf , l l lr 分别是前轮轴和后轮轴到质心的距离

    接下来对横向轮胎力 Fyf,Fyr进行计算
    实验结果表明,轮胎侧偏角较小时
    地面施加给轮胎的横向力(侧偏力)与轮胎侧偏角成线性关系(侧偏角是车轮速度方向与车轮纵轴的夹角)
    在这里插入图片描述
    前轮侧偏角为:
    在这里插入图片描述

    θ \theta θvf 是前轮胎速度方向与车身纵轴的夹角,𝛿为前轮转向角
    后轮的侧偏角为:
    在这里插入图片描述
    θ \theta θvr 是后轮胎速度方向与车身纵轴的夹角

    因此,前轮(2个)横向轮胎力为:
    在这里插入图片描述
    C𝛼f 是前轮侧偏刚度(cornering stiffness)

    同样,后轮(2个)横向轮胎力为:
    在这里插入图片描述
    C𝛼r 是后轮侧偏刚度

    计算 θ \theta θvf θ \theta θvr
    在这里插入图片描述

    θ \theta θvf θ \theta θvr 较小时可近似得到:
    在这里插入图片描述
    将(3-5),(3-6),(3-7),(3-8),(3-11),(3-12)代入(3-3)和(3-4)可得线性二自由度模型的状态方程:
    在这里插入图片描述


    3.2. 车辆路径跟踪偏差状态方程的构建

    有了式(3-13)的系统状态方程之后
    就可以分析出在给定的前轮转角输入下,车辆的横向位移、横向速度、横摆角以及横摆角速度的响应

    但是横向跟踪控制的目的是为了减小跟踪偏差
    需要的状态方程是能够分析在给定的前轮转角下车辆跟踪偏差的响应
    因此些偏差通常是:

    • 横向位置偏差𝑒1 及横向位置偏差变化率 e ˙ \dot{e} e˙1
    • 横摆角偏差𝑒2 及横摆角偏差变化率 e ˙ \dot{e} e˙2

    在这里插入图片描述
    下面介绍具体的转换过程
    车辆期望横摆角速度:
    在这里插入图片描述
    期望的横向加速度𝛼ydes 为:
    在这里插入图片描述
    则横向加速度偏差 e ˙ \dot{e} e˙1 为:
    在这里插入图片描述
    因此横向位置偏差变化率 e ˙ \dot{e} e˙1 (横向速度偏差)为:
    在这里插入图片描述
    此外,横摆角偏差 𝑒2 为:
    在这里插入图片描述
    将式(3-16),(3-17),(3-18)代入(3-3),(3-4)化简后可以得到新的状态方程:

    在这里插入图片描述
    即:
    在这里插入图片描述
    进一步忽略 ψ ˙ \dot{\psi} ψ˙des 项之后:
    在这里插入图片描述

    上式(3-20)就是横向控制所需的路径跟踪偏差状态方程


    3.3. LQR 横向控制算法的求解

    基于前面推导的路径跟踪偏差系统状态方程(3-20)
    就可以分析在前轮转角控制输入𝛿作用下,车辆路径跟踪偏差𝑥的响应特性

    期望的响应特性就是 跟踪偏差能够快速、稳定地趋近于零,并保持平衡
    同时前轮转角控制输入又尽可能小
    ,这就是一个典型的多目标优化最优控制问题

    且优化的目标函数可以表示为跟踪过程累计的跟踪偏差与累计的控制输入的加权和
    如式(3-21)所示:
    在这里插入图片描述
    其中, 𝑄为半正定的状态加权矩阵, 𝑅为正定的控制加权矩阵
    且 𝑄, 𝑅通常取为对角阵

    若只是 x x x δ \delta δ 做为代价,那么存在 x x x的正误差被 δ \delta δ的负误差消除
    相反,自身相乘,这样负值也会产生正平方,称这些为二次项

    𝑄矩阵元素变大意味着希望跟踪偏差能够快速趋近于零
    𝑅矩阵元素变大意味着希望控制输入能够尽可能小

    设M是n阶方阵,如果对任何非零向量z,都有zTMz> 0,其中zT 表示z的转置,就称M为正定矩阵

    则式(3-21)目标函数中
    第一项优化目标 ∫ 0 ∞ ( x T Q x )   d t \int_{0}^{∞} (x^TQx)\, dt 0(xTQx)dt 表示跟踪过程路径偏差的累积大小
    第二项优化目标 ∫ 0 ∞ ( δ T R δ )   d t \int_{0}^{∞} (\delta^TR\delta)\, dt 0(δTRδ)dt 表示跟踪过程控制能量的损耗

    这样就将横向控制问题转化为一个最优控制问题:
    求解最优的前轮转角控制输入 δ \delta δ* ,使得式(3-21)目标函数 𝐽 可以取极小值

    而式(3-21)是一个关于状态变量𝑥和控制输入𝛿的二次型目标函数
    对它的优化求解是一个典型的 LQR 最优控制问题

    根据 LQR 最优控制理论,对式(3-21)目标函数的优化求解
    解出的最优控制规律 δ \delta δ* 是关于状态变量𝑥的线性函数:

    在这里插入图片描述
    其中,𝐴d 和 Bd 代表状态方程(3-20)离散化后的𝐴, B1 矩阵, 𝑃是式(3-23)黎卡提方程的解:
    在这里插入图片描述
    因此,根据式(3-22)可以设计一个状态反馈调节器,通过状态反馈实现闭环最优控制,如图所示
    在这里插入图片描述
    根据求解的状态反馈调节器增益𝐾,则可以得到最优的状态反馈前轮转角 δ \delta δfb
    在这里插入图片描述
    由于在 LQR 状态反馈控制求解过程中没有考虑路径本身的动态变化特性
    因此只有式(3-25)的反馈控制输入时 LQR 控制在曲线行驶时会存在稳态误差

    稳态误差是系统从一个稳态过渡到新的稳态,或系统受扰动作用又重新平衡后,系统出现的偏差

    为了消除该稳态误差, 还需要加上一个前馈控制输入量 δ \delta δff
    前馈控制考虑了路径的曲率及车辆的转向不足特性
    在这里插入图片描述
    其中, KV为不足转向梯度,表征了车辆的转向特性
    e2ss 为稳态时的角度跟踪偏差
    k3 为𝐾矩阵中的角度偏差反馈控制增益部分
    在这里插入图片描述
    因此,总的 LQR 前轮转角输入为:
    在这里插入图片描述


    4. 总结

    本文主要介绍了基于车辆运动学和动力学模型的横向控制算法
    从上文对两类方法的分析可以看出,两类方法在应用场景上都存在一定的局限性,并具有较强的互补性
    因此在实际使用中,综合使用这些方法会更有效
    在这里插入图片描述

    无模型的 PID 横向控制算法参数少,简单易用
    但是由于没有考虑车辆系统动力学特性及路径本身的动态变化特性,对外界干扰的鲁棒性较差
    在高速或曲率较大的弯道场景时,会出现较大的跟踪误差和“画龙”现象
    因此,该方法比较适合应用于低速曲率较小的路径跟踪控制场景中

    基于运动学模型的横向控制算法中
    Pure Pursuit 和 Stanley 前轮反馈算法在中低速场景下,他们的路径跟踪的性能较好

    Pure Pursuit 在大的跟踪误差和非连续的路径场景下鲁棒性较好
    其控制的关键在于对最佳前向预瞄距离的确定
    其中,增大前向预瞄距离将提高车辆控制的稳定性
    但随之会带来路径跟踪性能降低及稳态误差增大的后果,表现出转弯内切现象

    相比于 Pure Pursuit 算法,Stanley 前轮反馈算法还额外考虑了横摆角偏差
    因此在大多数场景下,跟踪性能更佳
    然而,由于没有设置前向预瞄,Stanley 算法会出现转向过度的情况
    与 Pure Pursuit 和 Stanley 算法相比,后轮反馈控制算法计算更加复杂, 对路径的平滑性要求更高
    在中等速度下的跟踪性能及鲁棒性与 Stanley 方法近似
    然而在速度较大时,稳态误差也会变大,从而导致控制效果不佳

    LQR 算法使用二自由度动力学模型来设计横向控制器
    与前述基于运动学模型的几种算法相比,LQR 参数调节更加复杂
    其不仅需要获取车辆自身的模型参数
    还需要调节LQR 目标函数的 Q,R 矩阵以获得较优的跟踪性能

    LQR 算法的优点在于,通过与转向前馈进行有效结合,LQR 能够很好的解决曲线行驶时的稳态跟踪误差
    中等速度曲线行驶时其稳态误差趋近于零,从而极大提升跟踪性能
    LQR 非常适用于路径平滑的高速公路及城市驾驶场景,具有较好的车辆高速控制性能

    但是,由于模型的固有缺陷,LQR 与前馈控制的结合也无法解决所有跟踪控制问题
    由于该方法采用基于简化后的二自由度动力学模型
    因此当车辆运动不满足二自由度动力学模型转向小角度,或轮胎动力学线性化的假设条件时
    LQR 算法的跟踪性能会大幅降低,从而导致控制失效


    参考: 走进自动驾驶横向控制,你需要这篇超长综述


    谢谢

    展开全文
  • 横向控制算法分析

    千次阅读 热门讨论 2020-03-13 15:50:51
    路径跟踪算法分析 概述 ...初始速度0.5m/s 跟踪圆弧曲率-0.2 LQR 基于运动模型建立LQR横向控制算法,具体仿真条件如下: 曲率:0.1 速度:10 km/h 基于LQR的横向控制仿真 通过上述视频可以看出,控制误差大概1cm左右。

    概述

    本文介绍常见几种路径跟踪算法,并分析各自的优势。目前主要分为基于几何关系控制设计和基于模型的控制设计。其中几何关系可以分为,纯追踪算法、前轴反馈和后轴反馈算法。基于模型可以分为运动模型和动力学模型,控制算法可以使用LQR优化或者使用MPC。

    基于几何关系设计

    纯追踪算法

    对于几何路径跟踪一般简化车辆模型为单车模型,如下图所示:

    详细内容参考:控制算法-纯追踪法

    前轴反馈

    前轴反馈控制也就是常说的Stanley方法,几何关系如下:

    详细内容参考:控制算法-Stanley法

    三角函数曲线跟踪

    在这里插入图片描述
    在这里插入图片描述

    圆弧跟踪

    在这里插入图片描述
    在这里插入图片描述

    基于后轴反馈

    后轴反馈
    详细内容参考:控制算法-后轮位置反馈

    仿真系统框图

    基于下图框图,搭建仿真环境。
    仿真系统框图
    仿真条件:

    • 仿真周期20ms
    • 目标曲线采样间隔 5
    • 初始位置(0,0)
    • 初始偏航角0度
    余弦曲线跟踪

    在这里插入图片描述
    由上图可知,对于曲率连续变化的跟踪效果较好。
    在这里插入图片描述
    输出控制抖动也较小。

    圆弧跟踪

    在这里插入图片描述
    由上图可知,对于曲率恒定的圆弧曲线,控制效果较差,存在较大的位置偏差。
    在这里插入图片描述
    由上图可知,控制率震荡较大。

    C++仿真效果

    仿真条件:

    • 初始位置为 ( 0 , 0 ) (0,0) (0,0)
    • 初始偏航角为 0 0 0
    • 仿真周期20ms
    • 目标曲线采样间隔 1

    车辆刚起步时,由于输出转向角为0,而此时的圆弧曲率为-0.2,所以存在一段转向角按照最大角速度转到目标转向角的过程。跟踪过程中,从图中转向角的振荡幅度存在小范围的变化。在两个圆弧的交界处,由于曲率由-0.2突变到0.2,方向盘转向角需要按照最大角速度转到目标角度。在该过程中存在较大误差,但是在角度达到目标角度后,误差逐渐缩小,最后完全跟踪上目标曲线。

    圆弧前进

    在这里插入图片描述

    圆弧后退

    在这里插入图片描述

    动力学仿真

    在动力学仿真环境中测试如下:

    基于后轴反馈的横向控制

    实车数据

    下图是实际车辆测试结果,测试条件如下:

    • 初始位置(0,0),初始偏航角0°
    • 目标位置(5,-5),目标偏航角-90°
    • 初始转向角0°
    • 初始速度0.5m/s
    • 跟踪圆弧曲率-0.2
      在这里插入图片描述
      在这里插入图片描述

    LQR

    基于运动模型建立LQR横向控制算法,具体仿真条件如下:

    • 曲率:0.1
    • 速度:10 km/h

      基于LQR的横向控制仿真


      通过上述视频可以看出,控制误差大概1cm左右。
    展开全文
  • 横向控制 | Stanley算法

    2021-05-15 22:10:21
    路径跟踪——Stanley算法1.算法简介1.1算法推断2.源码解析3.效果演示 1.算法简介 参考链接: ...论文链接: ...1.1算法推断 ...Stanley方法是一种基于横向跟踪误差(cross-track error:eee为前轴中心到最近路径点(p

    横向控制 | Stanley算法


    参考链接:
    https://blog.csdn.net/renyushuai900/article/details/98460758
    论文链接:
    http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf
    Github链接:
    https://github.com/chanchanchan97/ROS

    1. 算法简介

    在这里插入图片描述

    Stanley方法是一种基于横向跟踪误差(cross-track error: e e e为前轴中心到最近路径点( p x p_x px​, p y p_y py​)的距离)的非线性反馈函数,并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量,其中包含横向误差e和航向误差 θ e θ_e θe​。
    δ ( t ) = δ e ( t ) + δ θ e ( t ) δ(t)=δ_e(t)+δ_{\theta_e}(t) δ(t)=δe(t)+δθe(t)在不考虑横向跟踪误差 e e e的情况下,前轮偏角和给定路径切线方向一致,如图所示。其中 θ e θ_e θe​表示车辆航向与最近路径点切线方向之间的夹角,在没有任何横向误差的情况下,前轮方向与所在路径点的方向相同:
    δ θ e ( t ) = θ e ( t ) δ_{\theta_e}(t)=\theta_e(t) δθe(t)=θe(t)在不考虑航向跟踪误差 θ e θ_e θe​的情况下,横向跟踪误差越大,前轮转向角越大,假设车辆预期轨迹在距离前轮 d ( t ) d(t) d(t)处与给定路径上最近点切线相交,根据几何关系得出如下非线性比例函数:
    δ e ( t ) = a r c t a n e ( t ) d ( t ) = a r c t a n k e ( t ) v ( t ) δ_e(t)=arctan\frac{e(t)}{d(t)}=arctan\frac{ke(t)}{v(t)} δe(t)=arctand(t)e(t)=arctanv(t)ke(t)
    其中 d ( t ) d(t) d(t)与车速相关,最后用车速 v ( t ) v(t) v(t),增益参数k表示。随着横向误差的增加,arctan函数产生一个直接指向期望路径的前轮偏角,并且收敛受车速 v ( t ) v(t) v(t)限制。
    综合两方面控制因素,基本转向角控制率如下:
    δ ( t ) = θ e ( t ) + a r c t a n k e ( t ) v ( t ) δ(t)=\theta_e(t)+arctan\frac{ke(t)}{v(t)} δ(t)=θe(t)+arctanv(t)ke(t)使用线性自行车运动模型,可以得到横向误差的变化率:
    e ˙ ( t ) = − v ( t ) s i n δ e ( t ) \dot{e}(t)=-v(t)sinδ_e(t) e˙(t)=v(t)sinδe(t)其中 s i n δ e ​ ( t ) sinδ_e​(t) sinδe(t)根据几何关系可知:
    s i n δ e ( t ) = e ( t ) d ( t ) 2 + ( e ( t ) ) 2 = k e ( t ) v ( t ) 2 + ( k e ( t ) ) 2 sinδ_e(t)=\frac{e(t)}{\sqrt{d(t)^2+(e(t))^2}}=\frac{ke(t)}{\sqrt{v(t)^2+(ke(t))^2}} sinδe(t)=d(t)2+(e(t))2 e(t)=v(t)2+(ke(t))2 ke(t)
    e ˙ ( t ) = − v ( t ) k e ( t ) v ( t ) 2 + ( k e ( t ) ) 2 = − k e ( t ) 1 + ( k e ( t ) v ( t ) ) 2 \dot{e}(t)=\frac{-v(t)ke(t)}{\sqrt{v(t)^2+(ke(t))^2}}=\frac{-ke(t)}{\sqrt{1+(\frac{ke(t)}{v(t)})^2}} e˙(t)=v(t)2+(ke(t))2 v(t)ke(t)=1+(v(t)ke(t))2 ke(t)当横向跟踪误差 e ( t ) e(t) e(t)很小时, ( k e ( t ) v ( t ) ) 2 → 0 (\frac{ke(t)}{v(t)})^{2}→0 (v(t)ke(t))20:
    e ˙ ( t ) ≈ − k e ( t ) \dot{e}(t)\approx-ke(t) e˙(t)ke(t)通过积分上式:
    e ( t ) = e ( 0 ) × e − k t e(t)=e(0)×e^{-kt} e(t)=e(0)×ekt因此横向误差指数收敛于e(t)=0,参数k决定了收敛速度。对于任意横向误差,微分方程都单调的收敛到0。

    2. 源码解析

    Note:由于该部分代码与Pure_Persuit算法的代码有较大相似之处,因此这里只讲解核心部分calculateTwistCommand()函数及其不同之处。
    (1) calculateTwistCommand()函数part.1

    lad = 0.0 #look ahead distance accumulator
    targetIndex = len(self.currentWaypoints.waypoints) - 1
    for i in range(len(self.currentWaypoints.waypoints)):
    	if((i+1) < len(self.currentWaypoints.waypoints)):
    		this_x = self.currentWaypoints.waypoints[i].pose.pose.position.x
    		this_y = self.currentWaypoints.waypoints[i].pose.pose.position.y
    		next_x = self.currentWaypoints.waypoints[i+1].pose.pose.position.x
    		next_y = self.currentWaypoints.waypoints[i+1].pose.pose.position.y
    		lad = lad + math.hypot(next_x - this_x, next_y - this_y)
    		if(lad > HORIZON):
    			targetIndex = i+1
    			break
    
    targetWaypoint = self.currentWaypoints.waypoints[targetIndex]
    
    targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x
    
    targetX = targetWaypoint.pose.pose.position.x
    targetY = targetWaypoint.pose.pose.position.y		
    currentX = self.currentPose.pose.position.x
    currentY = self.currentPose.pose.position.y
    #print '[',targetX, targetY, ']'
    #print targetWaypoint.pose.pose.orientation
    print self.currentVelocity
    
    #get waypoint yaw angle
    waypoint_quanternion = (targetWaypoint.pose.pose.orientation.x, targetWaypoint.pose.pose.orientation.y, targetWaypoint.pose.pose.orientation.z, targetWaypoint.pose.pose.orientation.w)
    waypoint_euler = tf.transformations.euler_from_quaternion(waypoint_quanternion)
    TargetYaw = waypoint_euler[2]
    print 'TargetYaw = ', TargetYaw
    		
    #get vehicle yaw angle
    quanternion = (self.currentPose.pose.orientation.x, self.currentPose.pose.orientation.y, self.currentPose.pose.orientation.z, self.currentPose.pose.orientation.w)
    euler = tf.transformations.euler_from_quaternion(quanternion)
    CurrentYaw = euler[2]
    print 'CurrentYaw = ', CurrentYaw
    

    说明:

    • 这部分与Pure_Persuit算法基本相同,大致分为三个小部分,因此只简述其中每一小部分的作用。
    • 第一部分的主要作用是找到距离当前位置最近的局部路径点的横、纵坐标。
    • 第二部分的主要作用是获取该局部路径点对应的目标航向角。
    • 第三部分的主要作用是获取小车当前的航向角。

    (2) calculateTwistCommand()函数part.2

    #get the difference between yaw angle of vehicle and tangent line of waypoint
    if TargetYaw >= 0:
    	alpha = TargetYaw - CurrentYaw
    	flag_alpha = 1
    else:
    	if TargetYaw * CurrentYaw < 0:
    		if (TargetYaw < 0) and (CurrentYaw > 0):
    			alpha = (math.pi - CurrentYaw) + (math.pi + TargetYaw)
    			flag_alpha = 2
    		else:
    			alpha = -((math.pi + CurrentYaw) + (math.pi - TargetYaw))
    			flag_alpha = 3
    	else:
    		alpha = TargetYaw - CurrentYaw
    		flag_alpha = 4
    
    print 'flag_alpha = ', flag_alpha
    print '(', currentX, currentY, ')'
    print '(', targetX, targetY, ')'
    print 'alpha = ', alpha
    print 'x = ', targetX - currentX
    print 'y = ', targetY - current
    

    说明:

    • 该部分的主要作用是计算得到目标航向角与当前实际航向角之间的航向误差alpha。

    (3) calculateTwistCommand()函数part.3

    #get the error between target position and current position
    if alpha >= 0:
    	error = abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
    else:
    	error = -abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
    print 'error = ', error
    

    说明:

    • 该部分的主要作用是计算得到小车前轮中心轴位置与目标路径点之间的横向误差error。

    (4) calculateTwistCommand()函数part.4

    #get the velocity of vehicle
    vel = math.sqrt(math.pow(self.currentVelocity.twist.linear.x, 2) + math.pow(self.currentVelocity.twist.linear.y, 2))
    print 'vel = ', vel
    

    说明:

    • 该部分的主要作用是获取小车在当前航向角方向上的速度值。

    (5) calculateTwistCommand()函数part.5

    #get the nonlinear proportional function from geometry of Vehicular mobility model
    if vel < 0.001:
    	delta = 0
    else:
    	delta = math.atan(k * error / vel)
    print 'delta = ', delta
    

    说明:

    • 该部分的主要作用是根据几何关系得出一个非线性比例函数,从而产生一个直接指向期望路径的前轮偏角delta,并且收敛受车速v(t)限制。

    (6) calculateTwistCommand()函数part.6

    #get the distance between final position and current position
    l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
    l2 = math.sqrt(math.pow(currentX - final_targetX, 2) + math.pow(currentY - final_targetY, 2))
    		
    if l > 0.001:
    	if l2 > 0.1:
    		theta = alpha + delta
    		print 'theta = ', theta
    		#self.ackermann_steering_control(targetSpeed, theta)
    		self.ackermann_steering_control(15, -theta)
    		
    		#get twist command
    		left_steer = Float64()
    		right_steer = Float64()
    		left_vel = Float64()
    		right_vel = Float64()
    		left_steer.data = left_angle
    		right_steer.data = right_angle
    		left_vel.data = left_speed
    		right_vel.data = right_speed
    		flag = 1
    	else:
    		left_steer = Float64()
    		right_steer = Float64()
    		left_vel = Float64()
    		right_vel = Float64()
    		left_steer.data = 0
    		right_steer.data = 0
    		left_vel.data = 0
    		right_vel.data = 0
    		flag = 2
    else:
    	left_steer = Float64()
    	right_steer = Float64()
    	left_vel = Float64()
    	right_vel = Float64()
    	left_steer.data = 0
    	right_steer.data = 0
    	left_vel.data = 0
    	right_vel.data = 0
    	flag = 3
    
    print 'flag = ', flag
    		
    return left_vel, right_vel, left_steer, right_steer
    

    说明:

    • 该部分的主要作用是获取小车当前位置与目标位置之间的距离,若小车当前位置与终点位置(0,4)的距离小于0.1m,则小车到达终点,此时停止运动。

    3. 效果演示

    ROS|平平无奇路径跟踪仿真(Stanley)

    展开全文
  • 无人驾驶汽车横向控制模型

    千次阅读 2018-12-04 21:31:06
    无人驾驶横向控制模型 1.车辆单车二自由度模式 车辆二自由度模型如下: 如对具体推导感兴趣,可以查看余志生的汽车理论教材,当然后期也会对其更新理论推导方面的知识。 2. 误差控制模型 3.总结 ...

    版权声明:本文为博主原创文章,未经博主允许不得转载。

    无人驾驶横向控制模型

    1.车辆单车二自由度模式


    车辆二自由度模型如下:
    在这里插入图片描述
    在这里插入图片描述
    如对具体推导感兴趣,可以查看余志生的汽车理论教材,当然后期也会对其更新理论推导方面的知识。

    2. 误差控制模型

    在这里插入图片描述在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    3.总结

    在这里插入图片描述

    4.参考文献

    在这里插入图片描述

    展开全文
  • 横向控制改,横向控制和纵向控制,matlab源码.zip
  • 车辆前轮转角(横向)、车辆油门开度,刹车开度(纵向) pure pursuit 控制核心思想 纯跟踪控制算法是基于车辆运动学模型(将车辆简单描述为两轮自行车模型)的一种算法。 该算法的步骤是: 找到path(输入的局部...
  • 在这里直接附上知乎链接:走进自动驾驶横向控制,你需要这篇超长综述 - 美团无人配送的文章 - 知乎 https://zhuanlan.zhihu.com/p/46377932
  • 车辆横向运动学模型 如下图所示为自行车模型。 存在假设:前轮和后轮的运动方向都是沿着轮胎旋转方向,也就是轮胎的slip angle是0。在低速(<5m/s)时,这个假设是合理的。 从几何关系进行推导: 在三角形OCA中,...
  • matlab学习自动驾驶(1)横向控制例程-Lareral Control Tutorial Lareral Control Tutorialmatlab学习自动驾驶(1)横向控制例程-Lareral Control Tutorial前言一、概述二、Lateral controller3.场景创建4.运行模型...
  • 智能驾驶车辆横向控制算法

    千次阅读 2021-05-06 17:58:03
    基于车辆模型的横向控制方法 1. 简介 1.1. 无人驾驶车辆 1.2. 横向控制和纵向控制 1.3. 横向控制 1.3.1. 无模型横向控制 1.3.2. 基于模型横向控制 2. 基于车辆运动学模型的控制方法 2.1. 车辆运动学模型 2.2. 纯...
  • 自动驾驶横向控制之Pure Pursuit 最近从网上找到一套Coursera自动驾驶课程的资料,讲解十分系统详细,我个人比较关注Control和Planning部分的学习,暂时打算随着学习进度进行整理。 纵向控制是控制车辆的速度,本...
  • 无人机横向控制论文

    2012-04-14 21:57:00
    无人机横向控制论文 鲁棒 PID混合控制 希望对大家有用
  • 如上图所示,第一个部分用于减小航向角误差,第二个部分用于减小横向误差,所以最终的控制律就是将这些结合起来,如下图 然后,横向误差容易证明是指数收敛的: 最后,一些调整改进,加入一个ksk_sks​,防止低速...
  • 基于RBF神经网络滑模控制的车辆横向控制研究.pdf
  • 为提高车辆自动驾驶系统的运动性能, 基于模糊逻辑和滑模控制理论设计了一种车辆纵向和横向运动综合控制系统。该控制系统通过对前轮转向角度、发动机节气门开度、制动液压及主动横摆力矩进行协调控制, 使车辆能够以...
  • 自动驾驶汽车横向控制综述.pdf
  • 行业分类-作业装置- 智能驾驶车辆横向控制方法及控制系统.zip
  • 基于神经网络滑模的智能车辆横向控制.pdf
  • 自动驾驶横向控制模型及方法研究综述.pdf
  • 基于模型预测的自动驾驶横向控制算法研究.pdf

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 117,433
精华内容 46,973
关键字:

横向控制