PythonRobotics 转向运动学模型全解：从转弯半径、曲率到横摆角速度与目标速度推导

PythonRobotics 转向运动学模型全解从转弯半径、曲率到横摆角速度与目标速度推导【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics导读本文基于 PythonRobotics 仓库附录文档 steering_motion_model_main.rst系统讲解轮式机器人/车辆的转向运动学模型Steering Motion Model从轴距与转向角的几何关系出发完整推导转弯半径、曲率、横摆角速度的计算公式并进一步给出由轨迹上连续两个位置反算转弯半径/曲率以及由最大转向速度约束推导目标最低速度的方法。这些公式是仓库中 Pure Pursuit、LQR 转向控制、MPC、CGMRes-NMPC 等路径跟踪算法的共同数学基础读完本文你将掌握这套运动学模型的完整推导链并能将公式与仓库源码中的离散化实现一一对应。图 1自行车模型bicycle model几何示意。车辆以轴距 WB前后轮中心距离绕圆心转弯转向角 δ 与转弯半径 R 之间满足三角关系 tan δ WB / R。图 2机器人从时刻 t 的位置运动到 t1 的位置两位置间距为 d两位置处的航向角差为 θ由等腰三角形与垂线可导出 R d / (2·sin(θ/2))。一、转向运动学模型的基本设定转向运动学模型通常称为自行车模型 / bicycle model是车辆路径规划与路径跟踪中最常用的简化模型。其核心假设是车辆由前轮转向前后轴之间通过刚性车架连接定义**轴距wheelbaseWB**为后轮中心到前轮中心的距离转向时前轮相对车辆纵轴偏转一个转向角 δ当转向角固定时车辆沿一个以某圆心为圆心的圆弧行驶该圆弧的半径即转弯半径 R且转弯圆经过后轮中心。在仓库中轴距以WB或L命名出现在多个路径跟踪算法的参数区。例如pure_pursuit/pure_pursuit.py 中WB 2.9 # [m] wheel base of vehiclemodel_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py 中WB 2.5 # [m]cgmres_nmpc/cgmres_nmpc.py 中WB 0.25 # [m] wheel base。从这些示例可以看出轴距在不同仿真场景下取值不同0.25 m2.9 m而所有公式都以WB为比例尺度这正体现了轴距在运动学模型中的核心地位。二、由转向角计算转弯半径与曲率2.1 转弯半径公式如 图 1 所示当转向角倾斜 δ 时根据轴距 WB与转弯圆通过后轮中心这一假设之间的几何关系可以建立直角三角形直角边分别为 WB 与 R对角为 δ从而得到[ R \frac{WB}{\tan\delta} ]式中R转弯半径mWB轴距mδ前轮转向角rad。2.2 曲率公式曲率 κ定义为转弯半径的倒数描述路径的弯曲程度单位为1/m[ \kappa \frac{1}{R} \frac{\tan\delta}{WB} ]当 δ 0 时R → ∞、κ 0对应直线行驶转向角越大曲率越大转弯越急。该公式在仓库中的典型离散化体现为横摆角更新lqr_steer_control/lqr_steer_control.pystate.yaw state.yaw state.v / L * math.tan(delta) * dt其中state.v / L * math.tan(delta)正是v · κ v · tan(δ)/L的离散形式对应下文横摆角速度公式pure_pursuit/pure_pursuit.py 加入倒车方向因子self.yaw self.direction * self.v / WB * math.tan(delta) * dtdirection为1前进或-1倒车说明同一套运动学模型通过方向符号即可支持正反向行驶model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py 将同一关系线性化进状态转移矩阵A[3, 2] DT * math.tan(delta) / WB表明 MPC 的状态预测同样以tan(δ)/WB作为航向对转向角的敏感度。2.3 弧长、角度差与横摆角速度在上图中转弯圆上两点之间的航向角差 Δθ等于从转弯圆心看这两点所张的圆心角。由弧长公式弧长 半径 × 圆心角[ \Delta\theta \frac{s}{R} ]其中s为转弯圆上两点之间的弧长路径距离。进一步横摆角速度yaw rateω表示车辆航向角随时间的变化率。将上式两边对时间求导弧长变化率即车速 v ds/dt可得[ \omega \frac{v}{R} ]结合曲率定义又可写作[ \omega v \cdot \kappa ]其中v为车辆速度m/s。这是路径跟踪领域最常用的运动学关系之一给定速度与曲率即可得到航向角速率。仓库中的源码印证cgmres_nmpc/cgmres_nmpc.py 的车辆运动模型d_yaw v / WB * sin(u_2)在 CGMRes-NMPC 中控制量u_2即前轮转向角v / WB * sin(u_2)是横摆角速度的另一种近似形式当 δ 较小时sin δ ≈ tan δpure_pursuit/pure_pursuit.py 由曲率关系反解转向角指令delta state.direction * math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)这是 Pure Pursuit 通过前视几何求出曲率后、再逆用tan δ WB · κ求转向角的典型用法。三、由轨迹上连续两个位置反算转弯半径在实际系统中我们往往观测到的是离散轨迹点位置 航向而不是直接知道转弯半径。因此需要从两个连续的位置反算 R 与 κ。如 图 2 所示机器人从时刻 t 的点运动到时刻 t1 的点。每个点用二维位置(x_t, y_t)与航向角θ_t表示。3.1 两点间距离[ d \sqrt{(x_{t1} - x_t)^2 (y_{t1} - y_t)^2} ]3.2 航向角差与几何推导两个位置从转弯圆心引出的两条半径之间的夹角为[ \theta \theta_{t1} - \theta_t ]从转弯圆心向连接两点的直线长度 d作垂线可将等腰三角形一分为二得到直角三角形其中斜边为 R、对边为 d/2、夹角为 θ/2于是[ \sin\frac{\theta}{2} \frac{d}{2R} ]3.3 转弯半径与曲率公式由上式解出转弯半径[ R \frac{d}{2\sin\frac{\theta}{2}} ]曲率 κ 为转弯半径的倒数[ \kappa \frac{2\sin\frac{\theta}{2}}{d} ]注意事项与适用边界当 θ 0直线行驶时sin(θ/2) 0公式出现除零说明直线段上无法用该方法定义有限转弯半径此时 κ 0 为极限值该方法仅依赖两个相邻位姿点适合作为局部曲率估计器例如在轨迹跟踪误差分析或曲率前馈控制中在线计算参考路径曲率若轨迹是连续参数化曲线如三次样条仓库采用解析微分方式求曲率cubic_spline_planner.py 中calc_curvature使用k (ddy * dx - ddx * dy) / ((dx ** 2 dy ** 2) ** (3 / 2))两者互为补充离散观测用本节公式参数化路径用微分公式。四、由最大转向速度推导目标最低速度本节解决一个工程实际问题转向执行机构舵机/转向电机的转向角变化速率有限当车辆沿曲率剧烈变化的路径高速行驶时转向机构可能跟不上路径所需的曲率变化从而产生跟踪误差。因此需要根据最大转向速度反推出允许的最低车速。4.1 最大曲率变化率若最大转向速度为δ̇_maxrad/s对曲率公式κ tan δ / WB关于时间求导可得到最大曲率变化率[ \dot{\kappa}{max} \frac{\tan\dot{\delta}{max}}{WB} ]注意这里原文采用tan(δ̇_max)/WB的符号形式工程上也可写作κ̇_max δ̇_max / (WB · cos²δ)对 κ 关于 δ 求导的精确形式在小转向角假设下二者近似一致。4.2 由离散轨迹估计曲率变化率另一方面利用第三节由连续两个位置计算曲率的方法轨迹上的曲率变化率可以离散估计为[ \dot{\kappa}{max} \frac{\kappa{t1} - \kappa_t}{\Delta t} ]即相邻两个轨迹点曲率之差除以时间步长。4.3 目标最低速度公式假设车辆不应超过最大曲率变化率即[ \frac{\kappa_{t1} - \kappa_t}{\Delta t} \le \frac{\tan\dot{\delta}_{max}}{WB} ]将Δt解出并代入速度定义v ≈ d/Δt其中d_{t1}、d_t分别为相邻点间距可得目标最低速度[ v_{min} \frac{d_{t1} d_t}{\Delta t} \frac{d_{t1} d_t}{\kappa_{t1} - \kappa_t} \cdot \frac{\tan\dot{\delta}_{max}}{WB} ]公式解读分子d_{t1} d_t是相邻两个采样间隔的路径长度之和近似对应两个采样周期的行驶距离分母κ_{t1} - κ_t是曲率增量曲率变化越剧烈要求车速越低因子tan(δ̇_max)/WB是执行机构能力上限转向速度越快、轴距越小允许的车速越高。工程含义当参考路径的曲率变化率超过转向机构能力时控制器必须将目标速度降到v_min以下否则路径跟踪将因转向滞后而发散。这正是速度规划speed planning与路径跟踪联合设计的典型约束之一常见于 Frenet 坐标系下的速度决策与 MPC 的输入约束设计。五、公式在仓库中的整体应用脉络本附录的公式并非孤立的理论而是贯穿整个 PythonRobotics 路径跟踪模块的运动学基础。按使用层次归纳如下公式仓库应用位置用途ω v·tan(δ)/WBpure_pursuit.py、lqr_steer_control.py、rear_wheel_feedback_control.py、stanley_control.py航向角状态更新车辆仿真δ atan2(WB·κ, 1)的逆用pure_pursuit.py由期望曲率反算转向角指令d_yaw v/WB·sin(u_2)cgmres_nmpc.pyNMPC 车辆运动学约束A[3,2] DT·tan(δ)/WBmodel_predictive_speed_and_steer_control.pyMPC 线性化状态转移矩阵曲率解析计算k (ddy·dx − ddx·dy)/(dx²dy²)^{3/2}cubic_spline_planner.py样条参考路径的曲率求值曲率作为路径规划输入dubins_path_planner.pyDubins 路径以curvature为关键参数生成圆弧段可以看出附录给出的是运动学层面的硬关系而各算法文件给出的是这些关系在离散时间下的具体实现。阅读本文后再回头查看上述任意一个跟踪算法的状态更新代码都能直接读出每一步的物理含义。六、小结与进一步阅读本附录从三个角度完整给出了转向运动学模型的推导正向已知转向角 δ 与轴距 WB → 转弯半径 R、曲率 κ、横摆角速度 ω反向已知相邻轨迹位姿 → 反算转弯半径 R、曲率 κ约束已知最大转向速度 → 推导允许的目标最低速度 v_min。这些公式构成车辆运动学分析的最小完备集是理解 Pure Pursuit、LQR 转向控制、Stanley、后轮反馈控制以及 MPC/NMPC 系列算法的前提。可结合仓库继续深入阅读附录其余内容Kalmanfilter_basics_main.rst、Kalmanfilter_basics_2_main.rst、steering_motion_model_main.rst路径跟踪实现PathTracking 模块下的各算法源码与对应测试如 test_pure_pursuit.py、test_lqr_steer_control.py路径规划中的曲率使用cubic_spline_planner.py、dubins_path_planner.py。说明本文所有公式与参数均依据当前仓库中的文档、源码与测试整理运行环境与参数默认值请以仓库实际代码为准。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考