企业官网建设流程全解析

PythonRobotics 后轮反馈转向控制：基于单车模型的路径跟踪仿真与 PID 速度控制实现解析

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics

导读：本文深入讲解 PythonRobotics 项目中PathTracking/rear_wheel_feedback_control模块——一套基于后轮反馈（Rear Wheel Feedback）转向控制与PID 速度控制的路径跟踪仿真实现。文章将从车辆运动模型、参考路径生成、跟踪误差计算、转向控制律推导、速度控制与仿真主循环六个层面逐层拆解，并结合源码与测试用例给出可复现的运行方式。读完本文，你将掌握该控制算法在自车模型（自行车模型）上的完整落地思路，以及如何把 "A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles"（arXiv:1604.07446）中提出的经典控制律转化为可运行代码。

一、算法概述：用后轮反馈 + PID 完成路径跟踪

根据 官方文档 的定义，该模块实现的是：

Path tracking simulation with rear wheel feedback steering control and PID speed control.

即：后轮反馈转向控制 + PID 速度控制的路径跟踪仿真。车辆以设定的目标速度沿一条三次样条（Cubic Spline）参考路径行驶，转向角由"后轮反馈控制律"计算，加速度由比例（P）速度控制器计算，最终在仿真时间内到达终点。在 路径跟踪总览文档 中，它是与 pure pursuit、Stanley、LQR、MPC 等并列的反馈式路径跟踪方案之一，属于该系列中实现最精简、最适合入门阅读的算法之一。

整个仿真在一个 Python 文件中完成，核心实现位于 rear_wheel_feedback_control.py，主要组件如下：

二、车辆运动模型：经典自行车模型（Kinematic Bicycle Model）

后轮反馈控制律基于运动学自行车模型，在代码中由State类实现（rear_wheel_feedback_control.py#L29-L41）：

class State: def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direction=1): self.x = x self.y = y self.yaw = yaw self.v = v self.direction = direction def update(self, a, delta, dt): self.x = self.x + self.v * math.cos(self.yaw) * dt self.y = self.y + self.v * math.sin(self.yaw) * dt self.yaw = self.yaw + self.v / L * math.tan(delta) * dt self.v = self.v + a * dt

其对应微分方程为：

• ẋ = v·cos(ψ)
• ẏ = v·sin(ψ)
• ψ̇ = v/L·tan(δ)
• v̇ = a

其中 ψ 为航向角（yaw），δ 为前轮转角，L = 2.9 [m] 为轴距（代码第 25 行全局常量）。注意这里的状态 x、y 描述的是车辆后轴中心的位置——这正是"后轮反馈"命名由来：控制律以车辆后轴为参考点，测量后轴中心到参考路径的横向偏差作为误差信号。

模型的离散化采用一阶欧拉积分，仿真步长dt = 0.1 [s]（代码第 24 行）。direction字段记录当前行驶方向（1 前进、-1 倒车），供目标速度规划使用。

三、参考路径构建：三次样条插值（Cubic Spline）

参考路径由用户给定的离散路点生成。在main()中（rear_wheel_feedback_control.py#L191-L198）：

ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0] ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0] goal = [ax[-1], ay[-1]] reference_path = CubicSplinePath(ax, ay)

CubicSplinePath类（rear_wheel_feedback_control.py#L43-L96）利用scipy.interpolate.CubicSpline把路径参数化为弧长 s 的一元函数：以累计折线长度 s 为自变量，分别对 x、y 做三次样条拟合，并预先计算一阶导数（dX、dY）与二阶导数（ddX、ddY）。在此基础上提供两个关键接口：

• calc_yaw(s)：由一阶导数求路径切线角atan2(dy, dx)，即参考航向；
• calc_curvature(s)：由一阶、二阶导数求曲率(ddy·dx − ddx·dy) / (dx² + dy²)^(3/2)。

弧长参数化的好处是：后续的"最近点投影"可以直接在一维自变量 s 上做最优化搜索，效率与稳定性都远高于在二维平面上逐点遍历。仿真时也以s = np.arange(0, path_ref.length, 0.1)的步长对样条曲线采样用于绘图。

四、跟踪误差计算：最近点投影与误差符号修正

后轮反馈控制律需要的误差量——横向偏差 e、路径曲率 k、参考航向 yaw_ref——由calc_track_error(x, y, s0)计算（rear_wheel_feedback_control.py#L81-L96）：

1. 最近点搜索：从上一时刻的弧长位置s0出发，用scipy.optimize.fmin_cg（共轭梯度法）最小化车辆位置到路径点(X(s), Y(s))的平方距离，同时传入解析雅可比2·dx·(x−X(s)) + 2·dy·(y−Y(s))加速收敛；
2. 误差符号修正：计算路径切线方向与"车辆指向最近点方向"的夹角angle = pi_2_pi(yaw − atan2(dyl, dxl))，若夹角为负则令e *= -1，使横向偏差带有左右符号；
3. 返回(e, k, yaw_ref, s)，其中s作为下一时刻的搜索初值s0，形成"跟踪式"最近点搜索，避免了每帧全路径扫描。

pi_2_pi将角度归一化到 (−π, π]，其底层调用仓库通用工具 utils/angle.py 中的angle_mod（默认模范围为 [−π, π)），保证角度差计算不会因跨越 ±π 边界而跳变。

五、核心转向控制律：后轮反馈（Rear Wheel Feedback）

转向控制律在rear_wheel_feedback_control(state, e, k, yaw_ref)中实现（rear_wheel_feedback_control.py#L105-L117）：

def rear_wheel_feedback_control(state, e, k, yaw_ref): v = state.v th_e = pi_2_pi(state.yaw - yaw_ref) omega = v * k * math.cos(th_e) / (1.0 - k * e) - \ KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e if th_e == 0.0 or omega == 0.0: return 0.0 delta = math.atan2(L * omega / v, 1.0) return delta

该控制律源自Paden 等人《A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles》（arXiv:1604.07446），即文档与 README.md 中引用的参考文献。其数学本质是：先构造车辆角速度的期望值 ω，再通过运动学反解出前轮转角 δ：

• 第一项v·k·cos(θe) / (1 − k·e)：前馈项，让车辆角速度跟随路径曲率 k 引起的期望转弯率，其中1 − k·e修正了横向偏差导致的转弯半径变化；
• 第二项−KTH·|v|·θe：航向误差反馈项，按航向误差 θe 的比例把车辆"拉回"与路径切线平行，|v|使增益随速度增大（速度越高转向越灵敏），KTH = 1.0；
• 第三项−KE·v·sin(θe)·e/θe：横向位置反馈项，按横向偏差 e 修正转向，KE = 0.5，且当 θe→0 时sin(θe)/θe → 1保持数值稳定。

最终δ = atan2(L·ω/v, 1.0)由单车模型 yaw 动力学ψ̇ = v/L·tan(δ)反解得来，因此车速 v 不能为 0（代码中当 θe 或 ω 为 0 时直接返回 0 转角，避免除零）。三个关键增益常量定义在文件头部（rear_wheel_feedback_control.py#L19-L25）：

Kp = 1.0 # speed proportional gain KTH = 1.0 # steering control parameter (yaw error gain) KE = 0.5 # steering control parameter (lateral error gain) dt = 0.1 # [s] L = 2.9 # [m]

六、PID 速度控制与目标速度规划（含倒车切换）

速度控制采用比例控制器（代码注释称 PID，实际实现仅含 P 项），pid_control（rear_wheel_feedback_control.py#L98-L100）：

def pid_control(target, current): a = Kp * (target - current) return a

即加速度a = Kp·(v_ref − v)，Kp = 1.0 为速度比例增益。

目标速度由calc_target_speed(state, yaw_ref)动态规划（rear_wheel_feedback_control.py#L176-L189）：

def calc_target_speed(state, yaw_ref): target_speed = 10.0 / 3.6 # 10 km/h 换算为 m/s dyaw = yaw_ref - state.yaw switch = math.pi / 4.0 <= dyaw < math.pi / 2.0 if switch: state.direction *= -1 # 航向差过大时切换方向（倒车） return 0.0 if state.direction != 1: return -target_speed # 倒车时目标速度为负 return target_speed

这里有一个实用的小技巧：当车辆航向与参考路径切线方向的夹角落在45°~90°区间时，认为路径出现急转弯，此时把direction置反并让车速先归零，随后以负速度（倒车）继续跟踪——这正是示例路径末端"回头弯"能顺利通过的原因，也体现了反馈控制结合简单状态切换处理大角度机动的方式。

七、仿真主循环：误差迭代、控制、更新与到达判定

simulate(path_ref, goal)（rear_wheel_feedback_control.py#L120-L174）以dt = 0.1s为步长循环执行：

1. 计算跟踪误差：e, k, yaw_ref, s0 = path_ref.calc_track_error(state.x, state.y, s0)（s0 逐帧递推，即上一节所述的跟踪式最近点搜索）；
2. 计算转向角：di = rear_wheel_feedback_control(state, e, k, yaw_ref)；
3. 计算加速度：先由calc_target_speed求目标速度，再ai = pid_control(speed_ref, state.v)；
4. 更新状态：state.update(ai, di, dt)，时间累加；
5. 到达判定：当与终点goal的欧氏距离 ≤goal_dis = 0.3 [m]时打印"Goal"并跳出循环；最大仿真时间T = 500.0 [s]作为兜底上限。

循环中还有一个细节：assert goal_flag, "Cannot goal"（rear_wheel_feedback_control.py#L203）——仿真结束后若未到达终点直接抛断言，这既是模块自检，也为下面的自动化测试提供了可判定结果。

可视化方面（show_animation = True时）：每帧实时绘制参考路径（红色 course）、历史轨迹（蓝色点）、当前目标点（绿色 x），标题实时显示speed[km/h]与当前弧长参数s0，并支持按ESC 键随时退出仿真（rear_wheel_feedback_control.py#L161-L172）。仿真结束后还会绘制三张静态图：路径跟踪结果、样条航向角沿弧长分布、曲率沿弧长分布。

八、运行与测试

直接运行仿真（需安装 matplotlib、numpy、scipy，见 requirements/requirements.txt）：

python PathTracking/rear_wheel_feedback_control/rear_wheel_feedback_control.py

控制台输出rear wheel feedback tracking start!!，随后弹出实时动画窗口；仿真结束到达终点时输出Goal。

关闭动画以批处理方式运行：将模块级变量show_animation置为False即可，这正是自动化测试采用的方式——tests/test_rear_wheel_feedback.py 中：

import conftest # Add root path to sys.path from PathTracking.rear_wheel_feedback_control import rear_wheel_feedback_control as m def test1(): m.show_animation = False m.main()

测试通过 tests/conftest.py 把仓库根目录加入sys.path后直接调用m.main()，若车辆未能到达终点，assert goal_flag会令测试失败——因此该测试用例同时验证了"路径可达性"这一核心功能。

参数速查表（修改文件头部全局常量即可调整仿真行为）：

九、与其他路径跟踪算法的关系

在 路径跟踪总览 中，PythonRobotics 将路径跟踪分为反馈控制类与优化控制类两大类，后轮反馈控制属于前者，与同目录下的 pure_pursuit（纯跟踪）、Stanley 控制 形成对照：Pure Pursuit 以前轮/参考点几何为基准，Stanley 以车头前轴为基准，而本模块的后轮反馈控制以车辆后轴为基准，直接对横向偏差与航向偏差构造李雅普诺夫式反馈。三者对比阅读，可以直观理解同一类几何跟踪问题的不同误差定义与控制律设计取舍。

参考

• Paden et al.,A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles, arXiv:1604.07446 —— 后轮反馈控制律的原始出处（本模块的 官方文档 与 README.md 均引用此文）。

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics