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=−R−1BTP(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=0∑N−1[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;

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;
}

自动驾驶控制算法——基础控制算法原理相关推荐

Apollo星火计划学习笔记——第六讲上自动驾驶感知基础（I）
文章目录 1. 感知的作用 2. 常见传感器 2.1 相机 2.1.1 相机的种类 2.1.2 相机的特点 2.2 激光雷达 2.2.1 激光雷达系统构成与方案 2.2.2 激光雷达光源 2.2.3 ...
Apollo星火计划学习笔记——第六讲下（Apollo自动驾驶-感知基础II）
文章目录前言 1. Apollo感知框架介绍 1.1 Apollo感知模块概述 1.2 飞桨PaddlePaddle 1.3 跟踪 1.4 Apollo感知框架 2. Lidar障碍物检测 2.1 ...
Udacity_Simulator自动驾驶仿真基础知识！
自动驾驶基础知识真正的完全自动驾驶汽车必须解决三个独立的任务:感知(了解周围世界发生了什么).预测(确定下一步会发生什么)和驾驶策略(采取适当的行动) 自动驾驶级别: Level 1级别的自动驾驶包 ...
自动驾驶毫米波雷达的原理分析和应用案例
作者 | 智车科技编辑 | 智车科技点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[多传感器融合]技术交流群 / 导读 / 近两 ...
面向2022届毕业生-自动驾驶/SLAM/DL/C++ 岗位收集整理
收集整理 Created by leijobs876@gmail.com 目标方向: C/C++ 开发 Andorid 开发深度学习部署自动驾驶感知与定位 ROS机器人开发文章目录收集整理 * ...
如何通俗易懂地理解自动驾驶？
从自动驾驶的架构出发往往最能够理解自动驾驶的原理,大众对自动驾驶最浅显易懂的理解就是感知,决策,执行.所有机器人都是这样的架构. 感知回答周围有什么的问题,类似人的眼睛,耳朵.通过摄像头,雷达,地图等 ...
全球自动驾驶汽车发展指数哪家强？美德领衔，中国第七
来源:新智驾近日,罗兰贝格与德国著名汽车研究机构亚琛汽车工程技术有限公司共同发布<2017年第四季度全球自动驾驶汽车发展指数>报告,基于行业和市场两个维度,对全球主要汽车大国在自动驾驶领 ...
自动驾驶汽车的安全技术特点
"安全第一"是自动驾驶的核心理念和价值观.自动驾驶车辆的整体系统安全设计是一项复杂的系统工程, 涉及车载自动驾驶系统的核心算法策略设计. 硬件和软件冗余安全设计.远程云代驾技术. ...
“狂飙”的自动驾驶车，用了哪些规控算法？
视频中这辆"狂飙"的自动驾驶车,正通过MPC轨迹跟踪控制器使其沿着规划好的轨迹行驶.作者用C++实现了MPC算法,并利用ROS与自动驾驶仿真器CARLA进行联合仿真,测试了算法的有 ...

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

Pure Pursuit 纯几何跟踪算法

Stanley算法（前轮反馈控制）

LQR控制算法（全状态反馈控制-最优控制）

自动驾驶控制算法——基础控制算法原理相关推荐

最新文章

热门文章