自动驾驶控制算法——基础控制算法原理

这篇具有很好参考价值的文章主要介绍了自动驾驶控制算法——基础控制算法原理。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

Pure Pursuit 纯几何跟踪算法

Pure Pursuit算法是一种通过车辆运动几何学(阿克曼转向几何)对前轮转角进行调整来消除横向偏差的横向控制算法,其参考点为后轴中心点。

该算法的思想是:基于当前车辆后轴中心位置,在参考路径上向前 l d l_d ld的距离匹配一个预瞄点,将此预瞄点作为该周期的goal point。假设车辆后轴中心点可以按照一定的转弯半径R形式抵达该预瞄点,然后根据预瞄距离 l d l_d ld、转弯半径R和车辆坐标系下预瞄点的朝向角 α \alpha α之间的几何关系来确定前轮转角,其原理图如下:
自动驾驶控制算法,自动驾驶控制算法,算法,人工智能,自动驾驶

为了使车辆后轴中心点跟踪圆弧虚线路径到达C点,在三角形OAC中,需要满足的正弦定理关系:

l d s i n 2 α = R s i n ( π 2 − α ) (1) \frac{l_d}{sin2\alpha}=\frac{R}{sin(\frac{\pi}{2}-\alpha)} \tag{1} sin2αld=sin(2πα)R(1)

又因为: s i n 2 α = 2 s i n α c o s α , s i n ( π 2 − α ) = c o s α sin2\alpha=2sin\alpha cos\alpha,sin(\frac{\pi}{2}-\alpha)=cos\alpha sin2α=2sinαcosα,sin(2πα)=cosα,所以化简上述公式可得:

R = l d 2 s i n α (2) R=\frac{l_d}{2sin\alpha} \tag{2} R=2sinαld(2)

为了达到这种几何关系,作为控制量的前轮转角需要满足什么关系呢?在阿克曼转向(三角形OAB)中,有:

t a n δ ≈ L R (3) tan\delta \approx \frac{L}{R} \tag{3} tanδRL(3)

因此,联立上面的公式(2)(3),得到:

δ ( t ) = a r c t a n ( 2 L s i n α ( t ) l d ) (4) \delta(t) = arctan(\frac{2Lsin\alpha(t)}{l_d}) \tag{4} δ(t)=arctan(ld2Lsinα(t))(4)

其中,车辆坐标系下预瞄点的朝向角 α ( t ) \alpha(t) α(t)为变量。

另外,定义横向位置误差为车辆当前姿态和预瞄点在横向上的误差:

e y ( t ) = l d s i n α ( t ) (5) e_y(t) = l_d sin\alpha(t) \tag{5} ey(t)=ldsinα(t)(5)

联立公式(4)(5),可得到前轮转角计算公式:
δ ( t ) = a r c t a n ( 2 L l d 2 e y ( t ) ) (6) \delta(t) =arctan(\frac{2L}{l_d^2}e_y(t)) \tag{6} δ(t)=arctan(ld22Ley(t))(6)
一般而言,考虑小角度假设,公式(6)可变成:
δ ( t ) = a r c t a n ( 2 L l d 2 e y ( t ) ) ≈ 2 L l d 2 e y ( t ) (7) \delta(t) = arctan(\frac{2L}{l_d^2}e_y(t)) \approx \frac{2L}{l_d^2}e_y(t) \tag{7} δ(t)=arctan(ld22Ley(t))ld22Ley(t)(7)

因此,纯跟踪法本质上是一个P控制器,输入为横向位置误差,输出为前轮转角。跟踪的控制效果将由预瞄距离 l d l_d ld决定,通常 l d l_d ld定义为关于速度的函数表达式:
l d = l d 0 + k v (8) l_d =l_{d0}+k v \tag{8} ld=ld0+kv(8)

算法特点:

  • 短的预瞄距离提供更精确的跟踪,而较长预瞄距离提供更平滑的跟踪。
  • 参数k值太小将导致不稳定,而k值太大将导致跟踪精度下降。
  • 高水平的鲁棒性:例如,对路径中的不连续性的良好处理。
  • 弯道跟踪中,随着速度的增加,稳态误差也增大。

Stanley算法(前轮反馈控制)

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

Stanley方法是一种基于横向跟踪误差为前轴中心到最近路径点的距离的非线性反馈函数,并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量。

Stanley法计算得到的前轮转角控制量由两部分构成:

  • 一部分是航向误差引起的转角,即当前车身方向与参考轨迹最近点的切线方向的夹角;
  • 另一部分是横向误差引起的转角,即前轴中心到参考轨迹最近点的横向距离。

Stanley法的原理示意图如下所示:

自动驾驶控制算法,自动驾驶控制算法,算法,人工智能,自动驾驶

在不考虑横向跟踪误差的情况下,前轮转角应当与给定路径参考点的切线方向一致。其中, θ φ \theta_\varphi θφ表示车辆航向与最近路径点切线方向之间的夹角,在没有任何横向误差的情况下,前轮方向应与所在路径点的方向相同,故消除航向误差的前轮转角为;
δ φ ( t ) = θ φ ( t ) (9) \delta_\varphi(t) = \theta_\varphi(t) \tag{9} δφ(t)=θφ(t)(9)
在不考虑航向跟踪偏差的情况下,横向跟踪误差越大,前轮转向角越大,假设车辆预期轨迹在距离前轮 d ( t ) d(t) d(t)处与参考路径上最近点切线相交,根据几何关系得出如下非线性比例函数:
δ y ( t ) = θ y ( t ) = a r c t a n ( e y ( t ) d ( t ) ) = a r c t a n ( k e y ( t ) v ( t ) ) (10) \delta_y(t)=\theta_y(t)=arctan(\frac{e_y(t)}{d(t)})=arctan(\frac{k e_y(t)}{v(t)}) \tag{10} δy(t)=θy(t)=arctan(d(t)ey(t))=arctan(v(t)key(t))(10)
其中 d ( t ) d(t) d(t)与车速相关,用车速 v ( t ) v(t) v(t)和增益参数k表示.

因此,前轮转角为:
δ ( t ) = δ φ ( t ) + δ y ( t ) = δ φ ( t ) + a r c t a n ( k e y ( t ) v ( t ) ) , δ ( t ) ∈ [ δ m i n , δ m a x ] (11) \delta(t)=\delta_\varphi(t)+\delta_y(t)=\delta_\varphi(t)+arctan(\frac{k e_y(t)}{v(t)}),\delta(t) \in [\delta_{min},\delta_{max}] \tag{11} δ(t)=δφ(t)+δy(t)=δφ(t)+arctan(v(t)key(t)),δ(t)[δmin,δmax](11)

算法特点:

  • 如果速度反向可能导致数值不稳定,可以向控制器添加正的松弛常数 k s k_s ks;
  • 在实车中,以更高的速度行驶时成为一个问题;
  • 可以通过在航向上添加前馈项来改进曲线的跟踪;
  • 与Pure Pursuit相比,Stanley方法更直观,但它在调优时也会遇到类似的问题。
  • 与Pure Pursuit相比,经过精心调整的Stanley跟踪器不会“偷工减料”,但相当于过冲转弯,这种影响可以归因于没有预瞄距离。
  • 与Pure Pursuit方法类似,中高速情况下,曲线跟踪的的稳态误差变得显著。

LQR控制算法(全状态反馈控制-最优控制)

LQR控制算法是一种广泛应用于工业控制和机器人控制中的线性控制算法,它利用状态反馈控制来实现系统的稳定性和优化性能。

LQR控制算法的基本思想:通过对系统的状态进行反馈控制,使系统的输出响应达到最优,同时保持系统的稳定性。具体来说,LQR控制算法通过将系统的状态反馈控制矩阵K计算出来,使得系统的状态能够以最小的代价函数进行控制,控制示意图如下:

自动驾驶控制算法,自动驾驶控制算法,算法,人工智能,自动驾驶

在LQR控制算法中,代价函数用来描述系统的响应性能和能耗。通常情况下,代价函数可以表示为系统状态与控制输入的加权和,即:
J = ∫ [ X ( t ) T Q X ( t ) + U ( t ) T R U ( t ) ] d t (12) J = ∫[X(t)^T Q X(t) + U(t)^T R U(t)]dt \tag{12} J=[X(t)TQX(t)+U(t)TRU(t)]dt(12)
其中,Q和R分别是状态和控制输入的权重矩阵,它们的大小和系统状态和控制输入的维度相同。通过对代价函数进行求导并令其等于0,可以得到连续的状态反馈矩阵K的表达式:
K = − R − 1 B T P (13) K = -R^{-1} B^T P \tag{13} K=R1BTP(13)
其中, P P P是Riccati方程的解,它可以通过求解Riccati方程得到,具体的求解方法可以使用数值方法,例如MATLAB中的lqr函数。

一旦得到了状态反馈矩阵K,就可以将其用于控制系统中。具体来说,控制输入(连续的)可以表示为:
U ( t ) = − K X ( t ) (14) U(t) = -KX(t) \tag{14} U(t)=KX(t)(14)
通过这种方式,系统的状态可以被反馈到控制输入中,从而实现系统的控制。

以车辆的横向控制为例:

步骤1:建立道路-车辆误差状态空间方程
X ( t ) ˙ = A X ( t ) + B U ( t ) Y ( t ) = C X ( t ) (15) \dot{X(t)}=A X(t) + B U(t) \\ Y(t)=C X(t) \tag{15} X(t)˙=AX(t)+BU(t)Y(t)=CX(t)(15)
步骤2:对连续的误差状态方程进行离散化,一般A矩阵采用中点欧拉离散,B及C矩阵采用前向欧拉离散,得到离散的误差状态方程如下:
X ( k + T ) = A d X ( k ) + B d U ( k ) Y ( k ) = C d X ( k ) (16) X(k+T)=A_d X(k) + B_d U(k) \\ Y(k)=C_d X(k) \tag{16} X(k+T)=AdX(k)+BdU(k)Y(k)=CdX(k)(16)

步骤3:构建代价函数 J J J,建立离散的LQR问题
J = 1 2 X T ( N ) F X ( N ) + 1 2 ∑ k = 0 N − 1 [ X T ( k ) Q X ( k ) + U T ( k ) R U ( k ) ] (17) J = \frac{1}{2}X^T(N)FX(N) + \frac{1}{2} \sum_{k = 0}^{N-1} [X^T(k) Q X(k) + U^T(k) R U(k)] \tag{17} J=21XT(N)FX(N)+21k=0N1[XT(k)QX(k)+UT(k)RU(k)](17)

步骤4:求解离散的LQR问题(动态规划法)

结论:
U ( k ) = − K ( k ) X ( k ) (18) U(k) = -K(k) X(k) \tag{18} U(k)=K(k)X(k)(18)
其中:
K ( k ) = [ B T ( k ) P ( k + 1 ) B ( k ) + R ( k ) ] − 1 B T ( k ) P ( k + 1 ) A ( k ) (19) K(k) = [B^T(k)P(k+1)B(k) + R(k)]^{-1} B^T(k)P(k+1)A(k) \tag{19} K(k)=[BT(k)P(k+1)B(k)+R(k)]1BT(k)P(k+1)A(k)(19)
而且离散的Riccati方程为:
P ( k ) = A T ( k ) P ( k + 1 ) A ( k ) − A T ( k ) P ( k + 1 ) B ( k ) [ B T ( k ) P ( k + 1 ) B ( k ) + R ( k ) ] − 1 B T ( k ) P ( k + 1 ) A ( k ) (20) P(k) = A^T(k)P(k+1)A(k) - A^T(k)P(k+1)B(k) [B^T(k)P(k+1)B(k) + R(k)]^{-1} B^T(k)P(k+1)A(k) \tag{20} P(k)=AT(k)P(k+1)A(k)AT(k)P(k+1)B(k)[BT(k)P(k+1)B(k)+R(k)]1BT(k)P(k+1)A(k)(20)
因此,求解出矩阵 P P P就可以得到控制律。

求解矩阵 P P P

1、令 P P P等于最终状态权重矩阵;

2、迭代黎卡提方程求出新的 P n e x t P_{next} Pnext;

3、当两次P的差值足够小时,计算反馈矩阵K;

4、根据反馈矩阵K获取最优控制量U;文章来源地址https://www.toymoban.com/news/detail-798015.html

matrix lqrSolver(matrix A,matrix B,matrix Q, matrix R,double esp,double iterate_max)
{
    double gap = 999999; 
    double iterate = 0; 
    matrix P = Q; 
    matrix P_next = zero(4, 4); 
    while (iterate < iterate_max && gap > esp) 
    { 
        P_next = A.transpose() * P * A - A.transpose() * P * B * (B.transpose() * P * B + R).inv() * B.transpose() * P * A; 
        gap = (P_next - P).norm(); 
        P = P_next; 
        iterate++; 
    } 
    return P_next;
}

到了这里,关于自动驾驶控制算法——基础控制算法原理的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 自动驾驶软件和人工智能

    自动驾驶汽车的核心在于其软件系统,而其中的机器学习和深度学习技术是使车辆能够感知、理解、决策和行动的关键。本文将深入探讨这些技术在自动驾驶中的应用,包括感知、定位、路径规划以及道路标志和交通信号的识别。 机器学习和深度学习在自动驾驶中的应用是实

    2024年02月07日
    浏览(61)
  • 如何使用RPA自动化人工智能和自动驾驶汽车

    人工智能和自动驾驶汽车是当今科技领域的热门话题。在这篇文章中,我们将探讨如何使用RPA(Robotic Process Automation)自动化人工智能和自动驾驶汽车。 RPA是一种自动化软件技术,它可以自动完成人类工作,提高工作效率。在人工智能和自动驾驶汽车领域,RPA可以帮助我们自动

    2024年02月20日
    浏览(73)
  • 人工智能与自动驾驶:智能出行时代的未来之路

           首先,我们先来说下什么是人工智能, 人工智能 (Artificial Intelligence,简称AI)是一门研究如何使计算机系统能够模拟、仿真人类智能的技术和科学领域。它涉及构建智能代理,使其能够感知环境、理解和学习知识,以及通过推理、决策和问题解决等方式与环境进行

    2024年02月03日
    浏览(73)
  • 走进人工智能|自动驾驶 开启智能出行新时代

    自动驾驶,也被称为无人驾驶或自动驾驶汽车,是指能够在没有人类干预的情况下自主地感知环境、决策和控制车辆行驶的技术和系统。 自动驾驶汽车是一种通过电脑系统实现无人驾驶的智能汽车。这种车辆依靠人工智能、视觉计算、雷达、监控装置和全球定位系统协同合作

    2024年02月14日
    浏览(53)
  • 【AI赋能】人工智能在自动驾驶时代的应用

    引言 人工智能引领现代,智能AI赋能未来。 它在当今社会和科技领域中具有重要性。 本文将着重探讨人工智能对自动驾驶技术的深度赋能和应用场景等。 有时我们乘坐网约车的时候,能打到无人驾驶汽车,全程均为AI语音播报: 自动驾驶是指通过使用 各种传感器 、 计算机

    2024年02月17日
    浏览(67)
  • 深度学习之路:自动驾驶沙盘与人工智能专业的完美融合

    引言: 在数字化时代,深度学习如一颗耀眼的明星,将人工智能推向新的高峰。本文将深度剖析自动驾驶沙盘与人工智能专业的紧密结合,旨在揭示这一融合对于中职和高职类人工智能专业的学子们的巨大意义。通过以图像识别技术为入口,我们将探讨自动驾驶沙盘在培养学

    2024年02月04日
    浏览(68)
  • 【探索AI未来】自动驾驶时代下的人工智能技术与挑战

    自动驾驶时代是指人工智能和相关技术在汽车行业中广泛应用,使得 汽车能够在不需要人类干预的情况下自主进行驾驶操作 的车辆新时代。在自动驾驶时代,车辆配备了感知、决策和控制系统,利用传感器、摄像头、雷达、激光等设备来获取周围环境信息,并通过人工智能

    2024年02月11日
    浏览(74)
  • 【深入探讨人工智能】AI大模型在自动驾驶中的应用

    当今, AI大模型 是一个火热的。随着人工智能的迅猛发展,AI大模型在各个领域展现出了巨大的潜力和应用价值。在自动驾驶领域,AI大模型的应用驱动自动驾驶算法具备更强的泛化能力。 那么 AI大模型 为自动驾驶赋能了什么?它的未来发展前景又是怎样? 本文将以

    2024年02月08日
    浏览(58)
  • 自动驾驶国家新一代人工智能开放创新平台产业化应用

    【摘要】:当前,全球新一轮科技革命和产业变革正孕育兴起,自动驾驶作为人工智能最重要的应用载体之一,对于加快交通强国、智能汽车强国建设,具有十分突出的战略意义。我国自动驾驶研发应用,面临技术、资金、应用等诸多挑战,为此,需要打造一套符合我国国情

    2024年02月14日
    浏览(69)
  • 特斯拉Dojo超算:AI训练平台的自动驾驶与通用人工智能之关键

    特斯拉公开Dojo超算架构细节,AI训练算力平台成为其自动驾驶与通用人工智能布局的关键一环 在近日举行的Hot Chips 34会议上,特斯拉披露了其自主研发的AI超算Dojo的详细信息。Dojo是一个可定制的超级计算机,从芯片到系统全部由特斯拉自主设计,主要目标是高效运行各种机

    2024年02月07日
    浏览(67)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包