PythonRobotics 中的 LQR 转向控制基于线性二次型调节器的路径跟踪仿真详解【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics导读本文基于 PythonRobotics 仓库中的 LQR 转向控制Linear–quadratic regulator steering control仿真模块系统讲解如何利用 LQR 控制器计算最优转向角、配合 PID 速度控制器使车辆平滑准确地跟踪由三次样条生成的期望轨迹。读完本文你将掌握该模块的车辆运动学建模、状态空间推导、离散 Riccati 方程求解原理以及源码中每个关键函数与参数的工程含义并能够直接运行仿真与测试复现结果。一、模块概述本文对应文档为 lqr_steering_control_main.rst核心实现位于 lqr_steer_control.py。该模块属于 PathTracking 目录下的路径跟踪方法之一在 path_tracking_main.rst 中与 pure pursuit、Stanley、rear wheel feedback、MPC 等控制方法并列介绍。模块的核心思路是转向控制基于当前车辆状态与期望轨迹之间的反馈通过 LQR 控制器计算最优转向角 δ速度控制使用 PID 控制器本实现中为比例 P 控制调节加速度以跟踪目标速度轨迹生成参考路径由三次样条插值cubic_spline_planner.py 中的calc_spline_course生成输出位置、航向角与曲率序列。整个仿真在closed_loop_prediction中以固定时间步长闭环推进直到车辆到达目标点或达到最大仿真时间。图仿真使用的车辆几何模型。e 为相对期望轨迹的横向误差θ 为方向航向误差V 为车辆当前速度L 为轴距δ 为转向角。二、车辆运动模型2.1 误差状态的定义如下图所示e表示车辆相对期望轨迹的横向误差θ表示方向误差车辆航向与期望轨迹切线方向的夹角。相应地ė和θ̇表示这两个误差的变化率。以时间步t递推横向误差与方向误差的更新方程运动学方程为e_t e_{t-1} ė_{t-1} · dt θ_t θ_{t-1} θ̇_{t-1} · dt其中dt为仿真时间步长源码中dt 0.1 [s]。2.2 横向误差变化率横向误差e的变化率由车速V与方向误差θ决定ė_t V · sin(θ_{t-1})当方向误差较小时θ ≈ 0利用小角度近似sin(θ) θ可将其线性化为ė_t V · θ_{t-1}2.3 方向误差变化率方向误差θ的变化率由车速V、轴距L与转向角δ决定自行车模型几何关系θ̇_t (V / L) · tan(δ)当转向角较小时δ ≈ 0利用小角度近似tan(δ) δ可将其线性化为θ̇_t (V / L) · δ这两组线性化方程是后续构造状态空间线性模型的基础。上述运动学更新在源码中由update(state, a, delta)函数实现lqr_steer_control.pystate.x state.x state.v * math.cos(state.yaw) * dt state.y state.y state.v * math.sin(state.yaw) * dt state.yaw state.yaw state.v / L * math.tan(delta) * dt state.v state.v a * dt注意仿真器中的车辆运动学更新使用的是未线性化的完整模型cos/tan原函数线性化只用于 LQR 控制器的模型设计这正是反馈控制在模型失配下依然能稳定工作的体现。三、控制模型状态空间表示为了把车辆动力学写成线性模型定义状态向量x与控制输入向量ux_t [e_t, ė_t, θ_t, θ̇_t]ᵀ u_t δ_t状态向量包含横向误差、横向误差变化率、方向误差、方向误差变化率四个分量控制输入为转向角 δ。线性状态转移方程为x_{t1} A·x_t B·u_t其中系统矩阵A与输入矩阵B为┌ 1 dt 0 0 ┐ │ 0 0 v 0 │ A │ 0 0 1 dt │ │ 0 0 0 0 ┘ ┌ 0 ┐ │ 0 │ B │ 0 │ └ v / L ┘A矩阵刻画了误差状态自身的演化规律横向误差随ė与dt累积、方向误差随θ̇与dt累积等B矩阵表明转向角 δ 通过v/L的增益直接影响方向误差变化率θ̇。在源码 lqr_steer_control.py 中该矩阵按如下方式构造A np.zeros((4, 4)) A[0, 0] 1.0 A[0, 1] dt A[1, 2] v A[2, 2] 1.0 A[2, 3] dt B np.zeros((4, 1)) B[3, 0] v / L其中v为当前车速、L 0.5 [m]为车辆轴距。从源码结构看A矩阵中的v项随当前车速实时更新因此系统矩阵本质上是时变的控制器在每个控制周期都会基于当前车速重新求解增益矩阵K。四、LQR 控制器设计4.1 代价函数与最优反馈律LQR 的目标是求解使如下二次代价函数最小的最优控制输入uJ Σ_{t0}^{N} (x_tᵀ·Q·x_t u_tᵀ·R·u_t)其中Q与R分别为状态权重矩阵与控制输入权重矩阵Q越大越强调尽快消除状态误差R越大越抑制控制量的幅值即转向角变化越平缓。对线性系统x_{t1} A·x_t B·u_t最优控制律为状态反馈形式u_t -K·x_t其中反馈增益矩阵K通过求解Riccati 方程获得。4.2 源码中的权重参数源码 lqr_steer_control.py 定义Kp 1.0 # speed proportional gain Q np.eye(4) # 4×4 单位阵四个状态分量权重均为 1 R np.eye(1) # 1×1控制输入权重为 1 dt 0.1 # 时间步长 [s] L 0.5 # 车辆轴距 [m] max_steer np.deg2rad(45.0) # 最大转向角 [rad]约 ±45°即默认状态下四个误差分量的权重相等转向代价为 1。实际工程中可通过调大Q中对应对角元来强化对横向误差或方向误差的惩罚或通过调大R来限制转向动作的激进程度。4.3 离散 Riccati 方程的迭代求解由于采用离散时间模型需要求解离散代数 Riccati 方程DARE。源码中的solve_DARE函数lqr_steer_control.py采用不动点迭代法def solve_DARE(A, B, Q, R): X Q Xn Q max_iter 150 eps 0.01 for i in range(max_iter): Xn A.T X A - A.T X B \ la.inv(R B.T X B) B.T X A Q if (abs(Xn - X)).max() eps: break X Xn return Xn迭代在矩阵变化量的最大绝对值小于阈值eps 0.01时提前收敛最多迭代 150 次返回 Riccati 方程的解矩阵X。4.4 增益计算与闭环稳定性验证dlqr函数lqr_steer_control.py在求得X后计算反馈增益矩阵并利用闭环矩阵A - B·K的特征值验证稳定性def dlqr(A, B, Q, R): X solve_DARE(A, B, Q, R) K la.inv(B.T X B R) (B.T X A) eigVals, eigVecs la.eig(A - B K) return K, X, eigVals增益公式K (BᵀXB R)⁻¹·BᵀXA为离散 LQR 的标准解闭环矩阵A - B·K的特征值若全部位于单位圆内则说明闭环系统渐近稳定该实现保留特征值返回值便于离线校验。五、转向控制律前馈 反馈lqr_steering_control是模块的核心函数lqr_steer_control.py每个控制周期执行以下步骤5.1 求取最近参考点ind, e calc_nearest_index(state, cx, cy, cyaw)calc_nearest_indexlqr_steer_control.py通过穷举所有参考点找出与当前车辆位置欧氏距离最小的索引ind并根据车辆相对参考点的方位判断横向误差e的符号车辆位于轨迹左侧或右侧。5.2 组装误差状态向量k ck[ind] # 参考点曲率 v state.v # 当前车速 th_e pi_2_pi(state.yaw - cyaw[ind]) # 方向误差归一化到 [-π, π) x np.zeros((4, 1)) x[0, 0] e # 横向误差 x[1, 0] (e - pe) / dt # 横向误差变化率数值微分 x[2, 0] th_e # 方向误差 x[3, 0] (th_e - pth_e) / dt # 方向误差变化率数值微分方向误差通过 angle.py 中的angle_mod归一化到[-π, π)区间避免角度环绕造成跳变。5.3 计算转向角前馈 反馈ff math.atan2(L * k, 1) # 前馈项跟踪参考曲率所需的稳态转向角 fb pi_2_pi((-K x)[0, 0]) # 反馈项LQR 状态反馈 delta ff fb最终转向角由两部分叠加前馈项ff atan2(L·k, 1)由参考点的曲率k与轴距L计算对应车辆在稳态下沿曲线行驶所需的基础转向角反馈项fb -K·x由 LQR 状态反馈修正误差其幅值受权重矩阵Q、R与当前车速共同影响。随后转向角经过max_steer±45°限幅后作用于车辆模型见update函数开头。5.4 速度控制PID速度控制采用比例控制器lqr_steer_control.pydef pid_control(target, current): a Kp * (target - current) return a加速度a Kp·(v_target - v)与速度误差成正比比例增益Kp 1.0。目标速度来自calc_speed_profile生成的速度剖面。5.5 速度剖面与倒车逻辑calc_speed_profilelqr_steer_control.py根据相邻参考点航向角差dyaw构造带方向切换的速度序列当π/4 ≤ dyaw π/2时判定为需要掉头的大弯速度方向反转倒车并在切换点速度置 0终点速度置 0实现停车。这使得仿真支持“前进—倒车”的往复机动对转向控制的鲁棒性要求更高。六、闭环仿真流程与可视化closed_loop_prediction函数lqr_steer_control.py组织完整的闭环仿真初始化车辆状态State(x0, y0, yaw0, v0)每个时间步调用lqr_steering_control求转向角dl调用pid_control求加速度ai再用update推进车辆状态车速低于stop_speed 0.05 [m/s]时推进目标索引处理停车点与目标点距离小于goal_dis 0.3 [m]时打印Goal并结束最大仿真时间T 500.0 [s]作为兜底终止条件开启show_animation时实时绘制轨迹、参考路径与目标点按Esc键可提前退出。main函数lqr_steer_control.py演示了完整流程ax [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0] ay [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0] goal [ax[-1], ay[-1]] cx, cy, cyaw, ck, s cubic_spline_planner.calc_spline_course(ax, ay, ds0.1) target_speed 10.0 / 3.6 # km/h - m/s sp calc_speed_profile(cx, cy, cyaw, target_speed) t, x, y, yaw, v closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)7 个途经点ax/ay先经三次样条以ds0.1 [m]的间距插值成密集参考轨迹目标速度 10 km/h 换算为 2.78 m/s仿真结束后绘制 3 幅图跟踪轨迹对比输入点/样条/实际轨迹、航向角随路程的变化、曲率随路程的变化。七、运行与测试验证7.1 直接运行仿真在仓库根目录执行python PathTracking/lqr_steer_control/lqr_steer_control.py控制台输出LQR steering control tracking start!!车辆到达终点后输出Goal。若想关闭动画以无界面方式运行可将源码第 30-31 行的show_animation改为False。7.2 单元测试仓库提供了对应的测试用例 test_lqr_steer_control.pydef test1(): m.show_animation False m.main()测试关闭动画后执行完整main()闭环仿真验证从样条路径生成、速度剖面构造到 LQR 转向跟踪的整条链路可以无异常跑通。可通过 pytest 运行python -m pytest tests/test_lqr_steer_control.py -v7.3 依赖说明模块依赖 NumPy、SciPyscipy.linalg用于矩阵求逆与特征值计算与 Matplotlib可视化。仓库的 requirements.txt 与 environment.yml 提供了完整依赖清单。八、参数速查表参数默认值含义备注Kp1.0速度比例增益PID 速度控制中的 P 项Qnp.eye(4)状态权重矩阵对应[e, ė, θ, θ̇]四个分量的权重Rnp.eye(1)控制权重矩阵惩罚转向角幅值dt0.1 s仿真时间步长状态递推与控制周期L0.5 m车辆轴距运动学模型关键几何参数max_steer±45°最大转向角施加在转向指令上的硬限幅T500.0 s最大仿真时间兜底终止条件goal_dis0.3 m到达判定距离与目标点距离阈值stop_speed0.05 m/s停车速度阈值低于此速度推进目标索引target_speed10 km/h期望巡航速度main中换算为 m/s九、小结与延伸本模块展示了 LQR 在路径跟踪中的典型应用范式建模用自行车模型 小角度近似把非线性车辆运动学线性化为x_{t1} A·x_t B·u_t设计通过离散 Riccati 方程求最优反馈增益K形成u -K·x的最优状态反馈实现转向指令由“前馈曲率补偿 反馈LQR”叠加构成速度由 PID 单独控制两者协同完成轨迹跟踪验证仓库附带 test_lqr_steer_control.py 与可直接运行的main()可在几分钟内复现仿真。本文对应的英文原文文档位于 lqr_steering_control_main.rst。若需要进一步了解在 LQR 框架中同时控制速度与转向状态向量扩为 5 维、控制输入扩为[δ, a]的扩展实现可对比阅读同目录下的 lqr_speed_and_steering_control_main.rst 及其实现 lqr_speed_steer_control.py。此外仓库 PathTracking 目录还提供了 pure pursuit、Stanley、rear wheel feedback、MPC 等多种路径跟踪方法便于横向对比不同控制器的跟踪特性。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
PythonRobotics 中的 LQR 转向控制：基于线性二次型调节器的路径跟踪仿真详解
PythonRobotics 中的 LQR 转向控制基于线性二次型调节器的路径跟踪仿真详解【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics导读本文基于 PythonRobotics 仓库中的 LQR 转向控制Linear–quadratic regulator steering control仿真模块系统讲解如何利用 LQR 控制器计算最优转向角、配合 PID 速度控制器使车辆平滑准确地跟踪由三次样条生成的期望轨迹。读完本文你将掌握该模块的车辆运动学建模、状态空间推导、离散 Riccati 方程求解原理以及源码中每个关键函数与参数的工程含义并能够直接运行仿真与测试复现结果。一、模块概述本文对应文档为 lqr_steering_control_main.rst核心实现位于 lqr_steer_control.py。该模块属于 PathTracking 目录下的路径跟踪方法之一在 path_tracking_main.rst 中与 pure pursuit、Stanley、rear wheel feedback、MPC 等控制方法并列介绍。模块的核心思路是转向控制基于当前车辆状态与期望轨迹之间的反馈通过 LQR 控制器计算最优转向角 δ速度控制使用 PID 控制器本实现中为比例 P 控制调节加速度以跟踪目标速度轨迹生成参考路径由三次样条插值cubic_spline_planner.py 中的calc_spline_course生成输出位置、航向角与曲率序列。整个仿真在closed_loop_prediction中以固定时间步长闭环推进直到车辆到达目标点或达到最大仿真时间。图仿真使用的车辆几何模型。e 为相对期望轨迹的横向误差θ 为方向航向误差V 为车辆当前速度L 为轴距δ 为转向角。二、车辆运动模型2.1 误差状态的定义如下图所示e表示车辆相对期望轨迹的横向误差θ表示方向误差车辆航向与期望轨迹切线方向的夹角。相应地ė和θ̇表示这两个误差的变化率。以时间步t递推横向误差与方向误差的更新方程运动学方程为e_t e_{t-1} ė_{t-1} · dt θ_t θ_{t-1} θ̇_{t-1} · dt其中dt为仿真时间步长源码中dt 0.1 [s]。2.2 横向误差变化率横向误差e的变化率由车速V与方向误差θ决定ė_t V · sin(θ_{t-1})当方向误差较小时θ ≈ 0利用小角度近似sin(θ) θ可将其线性化为ė_t V · θ_{t-1}2.3 方向误差变化率方向误差θ的变化率由车速V、轴距L与转向角δ决定自行车模型几何关系θ̇_t (V / L) · tan(δ)当转向角较小时δ ≈ 0利用小角度近似tan(δ) δ可将其线性化为θ̇_t (V / L) · δ这两组线性化方程是后续构造状态空间线性模型的基础。上述运动学更新在源码中由update(state, a, delta)函数实现lqr_steer_control.pystate.x state.x state.v * math.cos(state.yaw) * dt state.y state.y state.v * math.sin(state.yaw) * dt state.yaw state.yaw state.v / L * math.tan(delta) * dt state.v state.v a * dt注意仿真器中的车辆运动学更新使用的是未线性化的完整模型cos/tan原函数线性化只用于 LQR 控制器的模型设计这正是反馈控制在模型失配下依然能稳定工作的体现。三、控制模型状态空间表示为了把车辆动力学写成线性模型定义状态向量x与控制输入向量ux_t [e_t, ė_t, θ_t, θ̇_t]ᵀ u_t δ_t状态向量包含横向误差、横向误差变化率、方向误差、方向误差变化率四个分量控制输入为转向角 δ。线性状态转移方程为x_{t1} A·x_t B·u_t其中系统矩阵A与输入矩阵B为┌ 1 dt 0 0 ┐ │ 0 0 v 0 │ A │ 0 0 1 dt │ │ 0 0 0 0 ┘ ┌ 0 ┐ │ 0 │ B │ 0 │ └ v / L ┘A矩阵刻画了误差状态自身的演化规律横向误差随ė与dt累积、方向误差随θ̇与dt累积等B矩阵表明转向角 δ 通过v/L的增益直接影响方向误差变化率θ̇。在源码 lqr_steer_control.py 中该矩阵按如下方式构造A np.zeros((4, 4)) A[0, 0] 1.0 A[0, 1] dt A[1, 2] v A[2, 2] 1.0 A[2, 3] dt B np.zeros((4, 1)) B[3, 0] v / L其中v为当前车速、L 0.5 [m]为车辆轴距。从源码结构看A矩阵中的v项随当前车速实时更新因此系统矩阵本质上是时变的控制器在每个控制周期都会基于当前车速重新求解增益矩阵K。四、LQR 控制器设计4.1 代价函数与最优反馈律LQR 的目标是求解使如下二次代价函数最小的最优控制输入uJ Σ_{t0}^{N} (x_tᵀ·Q·x_t u_tᵀ·R·u_t)其中Q与R分别为状态权重矩阵与控制输入权重矩阵Q越大越强调尽快消除状态误差R越大越抑制控制量的幅值即转向角变化越平缓。对线性系统x_{t1} A·x_t B·u_t最优控制律为状态反馈形式u_t -K·x_t其中反馈增益矩阵K通过求解Riccati 方程获得。4.2 源码中的权重参数源码 lqr_steer_control.py 定义Kp 1.0 # speed proportional gain Q np.eye(4) # 4×4 单位阵四个状态分量权重均为 1 R np.eye(1) # 1×1控制输入权重为 1 dt 0.1 # 时间步长 [s] L 0.5 # 车辆轴距 [m] max_steer np.deg2rad(45.0) # 最大转向角 [rad]约 ±45°即默认状态下四个误差分量的权重相等转向代价为 1。实际工程中可通过调大Q中对应对角元来强化对横向误差或方向误差的惩罚或通过调大R来限制转向动作的激进程度。4.3 离散 Riccati 方程的迭代求解由于采用离散时间模型需要求解离散代数 Riccati 方程DARE。源码中的solve_DARE函数lqr_steer_control.py采用不动点迭代法def solve_DARE(A, B, Q, R): X Q Xn Q max_iter 150 eps 0.01 for i in range(max_iter): Xn A.T X A - A.T X B \ la.inv(R B.T X B) B.T X A Q if (abs(Xn - X)).max() eps: break X Xn return Xn迭代在矩阵变化量的最大绝对值小于阈值eps 0.01时提前收敛最多迭代 150 次返回 Riccati 方程的解矩阵X。4.4 增益计算与闭环稳定性验证dlqr函数lqr_steer_control.py在求得X后计算反馈增益矩阵并利用闭环矩阵A - B·K的特征值验证稳定性def dlqr(A, B, Q, R): X solve_DARE(A, B, Q, R) K la.inv(B.T X B R) (B.T X A) eigVals, eigVecs la.eig(A - B K) return K, X, eigVals增益公式K (BᵀXB R)⁻¹·BᵀXA为离散 LQR 的标准解闭环矩阵A - B·K的特征值若全部位于单位圆内则说明闭环系统渐近稳定该实现保留特征值返回值便于离线校验。五、转向控制律前馈 反馈lqr_steering_control是模块的核心函数lqr_steer_control.py每个控制周期执行以下步骤5.1 求取最近参考点ind, e calc_nearest_index(state, cx, cy, cyaw)calc_nearest_indexlqr_steer_control.py通过穷举所有参考点找出与当前车辆位置欧氏距离最小的索引ind并根据车辆相对参考点的方位判断横向误差e的符号车辆位于轨迹左侧或右侧。5.2 组装误差状态向量k ck[ind] # 参考点曲率 v state.v # 当前车速 th_e pi_2_pi(state.yaw - cyaw[ind]) # 方向误差归一化到 [-π, π) x np.zeros((4, 1)) x[0, 0] e # 横向误差 x[1, 0] (e - pe) / dt # 横向误差变化率数值微分 x[2, 0] th_e # 方向误差 x[3, 0] (th_e - pth_e) / dt # 方向误差变化率数值微分方向误差通过 angle.py 中的angle_mod归一化到[-π, π)区间避免角度环绕造成跳变。5.3 计算转向角前馈 反馈ff math.atan2(L * k, 1) # 前馈项跟踪参考曲率所需的稳态转向角 fb pi_2_pi((-K x)[0, 0]) # 反馈项LQR 状态反馈 delta ff fb最终转向角由两部分叠加前馈项ff atan2(L·k, 1)由参考点的曲率k与轴距L计算对应车辆在稳态下沿曲线行驶所需的基础转向角反馈项fb -K·x由 LQR 状态反馈修正误差其幅值受权重矩阵Q、R与当前车速共同影响。随后转向角经过max_steer±45°限幅后作用于车辆模型见update函数开头。5.4 速度控制PID速度控制采用比例控制器lqr_steer_control.pydef pid_control(target, current): a Kp * (target - current) return a加速度a Kp·(v_target - v)与速度误差成正比比例增益Kp 1.0。目标速度来自calc_speed_profile生成的速度剖面。5.5 速度剖面与倒车逻辑calc_speed_profilelqr_steer_control.py根据相邻参考点航向角差dyaw构造带方向切换的速度序列当π/4 ≤ dyaw π/2时判定为需要掉头的大弯速度方向反转倒车并在切换点速度置 0终点速度置 0实现停车。这使得仿真支持“前进—倒车”的往复机动对转向控制的鲁棒性要求更高。六、闭环仿真流程与可视化closed_loop_prediction函数lqr_steer_control.py组织完整的闭环仿真初始化车辆状态State(x0, y0, yaw0, v0)每个时间步调用lqr_steering_control求转向角dl调用pid_control求加速度ai再用update推进车辆状态车速低于stop_speed 0.05 [m/s]时推进目标索引处理停车点与目标点距离小于goal_dis 0.3 [m]时打印Goal并结束最大仿真时间T 500.0 [s]作为兜底终止条件开启show_animation时实时绘制轨迹、参考路径与目标点按Esc键可提前退出。main函数lqr_steer_control.py演示了完整流程ax [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0] ay [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0] goal [ax[-1], ay[-1]] cx, cy, cyaw, ck, s cubic_spline_planner.calc_spline_course(ax, ay, ds0.1) target_speed 10.0 / 3.6 # km/h - m/s sp calc_speed_profile(cx, cy, cyaw, target_speed) t, x, y, yaw, v closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)7 个途经点ax/ay先经三次样条以ds0.1 [m]的间距插值成密集参考轨迹目标速度 10 km/h 换算为 2.78 m/s仿真结束后绘制 3 幅图跟踪轨迹对比输入点/样条/实际轨迹、航向角随路程的变化、曲率随路程的变化。七、运行与测试验证7.1 直接运行仿真在仓库根目录执行python PathTracking/lqr_steer_control/lqr_steer_control.py控制台输出LQR steering control tracking start!!车辆到达终点后输出Goal。若想关闭动画以无界面方式运行可将源码第 30-31 行的show_animation改为False。7.2 单元测试仓库提供了对应的测试用例 test_lqr_steer_control.pydef test1(): m.show_animation False m.main()测试关闭动画后执行完整main()闭环仿真验证从样条路径生成、速度剖面构造到 LQR 转向跟踪的整条链路可以无异常跑通。可通过 pytest 运行python -m pytest tests/test_lqr_steer_control.py -v7.3 依赖说明模块依赖 NumPy、SciPyscipy.linalg用于矩阵求逆与特征值计算与 Matplotlib可视化。仓库的 requirements.txt 与 environment.yml 提供了完整依赖清单。八、参数速查表参数默认值含义备注Kp1.0速度比例增益PID 速度控制中的 P 项Qnp.eye(4)状态权重矩阵对应[e, ė, θ, θ̇]四个分量的权重Rnp.eye(1)控制权重矩阵惩罚转向角幅值dt0.1 s仿真时间步长状态递推与控制周期L0.5 m车辆轴距运动学模型关键几何参数max_steer±45°最大转向角施加在转向指令上的硬限幅T500.0 s最大仿真时间兜底终止条件goal_dis0.3 m到达判定距离与目标点距离阈值stop_speed0.05 m/s停车速度阈值低于此速度推进目标索引target_speed10 km/h期望巡航速度main中换算为 m/s九、小结与延伸本模块展示了 LQR 在路径跟踪中的典型应用范式建模用自行车模型 小角度近似把非线性车辆运动学线性化为x_{t1} A·x_t B·u_t设计通过离散 Riccati 方程求最优反馈增益K形成u -K·x的最优状态反馈实现转向指令由“前馈曲率补偿 反馈LQR”叠加构成速度由 PID 单独控制两者协同完成轨迹跟踪验证仓库附带 test_lqr_steer_control.py 与可直接运行的main()可在几分钟内复现仿真。本文对应的英文原文文档位于 lqr_steering_control_main.rst。若需要进一步了解在 LQR 框架中同时控制速度与转向状态向量扩为 5 维、控制输入扩为[δ, a]的扩展实现可对比阅读同目录下的 lqr_speed_and_steering_control_main.rst 及其实现 lqr_speed_steer_control.py。此外仓库 PathTracking 目录还提供了 pure pursuit、Stanley、rear wheel feedback、MPC 等多种路径跟踪方法便于横向对比不同控制器的跟踪特性。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考