拓冰网站定制 
简介面向自动驾驶与机器人方向开发者这是一份基于ROS的车辆横纵向仿真模型功能包将车辆模型封装为ROS节点通过Twist、Pose等消息及Topic接口输入油门、刹车、转向角并利用递推法更新运动状态便于验证与调试横纵向控制算法可快速搭建仿真环境测试横向路径跟踪与纵向速度控制策略。资源共24个文件压缩包仅39KB包含5个C头文件、3个源码文件、3个自定义msg消息定义、4个xml配置及catkin工作空间文件结构清晰适合有一定ROS与C基础的读者快速移植使用。包内demoKinematic示例可帮助理解车辆运动学模型的搭建与调用方式配套README与proto文件也提供了必要说明。已有1086人学习下载是学习ROS机器人仿真与控制算法验证的实用参考。1. 车辆横纵向耦合仿真是功能包的核心价值做过实车横向控制的人都遇到过一类问题纯跟踪算法在仿真里跟踪误差收敛得很漂亮一上车就在大曲率弯道外飘。原因往往不在控制算法本身而在仿真模型把横纵向解耦了。车辆转向时纵向速度的瞬态变化会通过轮胎力重新分配影响侧偏刚度这种耦合关系在离散化的单车模型里一旦被简化掉控制器的鲁棒性验证就失去了意义。这个 ROS 功能包解决的就是在仿真里复现横纵向动力学耦合这件事。它不依赖 CarSim 或 Carmaker而是把车辆的非线性动力学方程写进 ROS 节点通过标准话题接收加速度与前轮转角指令计算出横摆角速度、纵向速度与侧向速度再发布成车辆状态。适合没有商业仿真软件授权的团队也适合需要在 CI 环境里做规控算法回归测试的工程师。如果你的流是验证 MPC 或 LQR 这类模型预测控制器的表现这个包能把车辆模型的假设边界和真实差距摊开在你面前。2. ROS功能包的软件架构与工作空间组织2.1 功能包在 ROS 工作空间中的目录布局写这个包之前先把工作空间的结构理清楚。最常见的是用catkin_ws/src作为顶层目录功能包本身用ros2 pkg create或catkin_create_pkg生成但复杂的车辆模型我并不建议像教学例程那样把所有源文件堆在/src下。更实用的做法是分成三层autoware_vehicle_sim/ ├── CMakeLists.txt ├── package.xml ├── config/ │ ├── vehicle_params.yaml │ └── controller_gains.yaml ├── include/ │ └── vehicle_sim/ │ ├── vehicle_model.hpp │ ├── tire_model.hpp │ └── integrator.hpp ├── src/ │ ├── vehicle_sim_node.cpp │ ├── vehicle_model.cpp │ └── tire_model.cpp ├── launch/ │ └── sim.launch ├── rviz/ │ └── vehicle_display.rviz └── scripts/ └── plot_results.pyinclude目录放类声明src放实现config放 YAML 参数launch集中管理启动项。这里的关键是把车辆动力学方程和 ROS 通信层拆开这样模型可以在无 ROS 环境下用 gtest 单测后续如果要移植到别的中间件也不用大改。如果是从零搭环境热词里经常提到的“鱼香ROS一键安装”在 Ubuntu 22.04 ROS 2 Humble 下确实是最省事的路径。终端里执行wget http://fishros.com/install -O fishros . fishros会弹出选项菜单选一键安装 ROS 2 Humble 桌面版即可。装完后记得source /opt/ros/humble/setup.bash并把 source 语句写到~/.bashrc末尾避免每次手工激活。2.2 话题与服务接口设计车辆仿真节点要对外暴露两组话题订阅控制指令发布车辆状态。控制指令的标准格式在 Autoware 里是autoware_auto_control_msgs/msg/AckermannControlCommand但自己写通用功能包时用低耦合的话题更灵活。我一般这样定义订阅话题 /control/command std_msgs/Float32MultiArray [0] 纵向加速度 (m/s^2) [1] 前轮转角 (rad) 发布话题 /vehicle/status vehicle_sim_msgs/VehicleState 纵向速度vx (m/s) 横向速度vy (m/s) 横摆角速度yaw_rate (rad/s) 全局位置x, y (m) 航向角yaw (rad) 服务接口 /reset_simulation std_srvs/srv/Emptystd_msgs/Float32MultiArray的好处是发布端不用引入额外的消息依赖Python 的规控节点里一条pub.publish(Float32MultiArray(data[2.0, 0.15]))就能驱动仿真。缺点是数组下标含义脆弱所以要在 launch 文件里加documentation参数说明每个下标对应的物理量并在节点启动时打印一次话题说明。2.3 CMakeLists 的依赖与编译配置CMake 配置的核心点是开启 C17 支持并确保ament_cmake能正确找到消息生成的头文件。下面的配置片段是可以在 ROS 2 Humble 下直接用的cmake_minimum_required(VERSION 3.8) project(vehicle_sim) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang) add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) add_executable(vehicle_sim_node src/vehicle_sim_node.cpp src/vehicle_model.cpp src/tire_model.cpp ) target_include_directories(vehicle_sim_node PRIVATE include/vehicle_sim ) target_compile_features(vehicle_sim_node PUBLIC cxx_std_17) install(TARGETS vehicle_sim_node DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) ament_package()注意install(DIRECTORY launch config ...)这一行不能省launch 和 YAML 文件不安装的话ros2 launch 会报找不到文件。编译完成后用colcon build --packages-select vehicle_sim单独构建这个包别用colcon build全量构建否则依赖树里任何一个无关包的编译错误都会挡住你的验证。3. 车辆横纵向动力学模型的数学推导与代码实现3.1 从二自由度单车模型到横纵向耦合状态方程做横纵向耦合仿真二自由度单车模型是最常被低估的起点。所谓二自由度指的是车辆的横向运动与横摆运动这两个旋转/平移自由度纵向速度在模型里被当作时变参数而不是常值。顺带说一个热词里出现的“pso自动驾驶”“ros slam建图和自主导航”那些都是规控算法在整车模型上的下游应用本章的动力学实现是它们共同的模型底座。建模时做三个基本假设忽略车辆的垂向运动与俯仰、侧倾自由度两个前轮的转角相同合并为等效前轮转角轮胎工作在线性区侧偏力与侧偏角成正比在这个基础上纵向与横向的耦合通过纵向速度 ( v_x ) 的变化体现——纵向加速度改变 ( v_x )( v_x ) 进入横向动力学方程中的向心力项 ( v_x \cdot r ) 与轮胎侧偏角计算式横摆角速度又反过来影响纵向速度在全局坐标系下的积分。用状态空间写出来[ \begin{bmatrix} \dot{v}_x \ \dot{v}_y \ \dot{r} \end{bmatrix} \begin{bmatrix} a_x v_y \cdot r \ -\frac{C_f C_r}{m \cdot v_x} v_y - \left( v_x \frac{C_f \cdot l_f - C_r \cdot l_r}{m \cdot v_x} \right) r \frac{C_f}{m} \delta \ -\frac{C_f \cdot l_f - C_r \cdot l_r}{I_z \cdot v_x} v_y - \frac{C_f \cdot l_f^2 C_r \cdot l_r^2}{I_z \cdot v_x} r \frac{C_f \cdot l_f}{I_z} \delta \end{bmatrix} ]其中 ( C_f、C_r ) 是前后轴等效侧偏刚度( l_f、l_r ) 是质心到前后轴的距离( m ) 是整车质量( I_z ) 是横摆转动惯量( \delta ) 是前轮转角。注意 ( v_x ) 出现在分母上这意味着低速工况停车起步、蠕行会带来数值奇异一般会加一个下限保护例如 ( v_x \max(v_x, 0.1) )。3.2 离散化方法与积分器选择ROS 节点里做数值积分最常见的是定步长 Runge-Kutta 2 阶中点法比欧拉法精度高比 RK4 计算量小在 100 Hz 控制频率下足够。中点法的递推公式void VehicleModel::step(double accel_cmd, double steer_cmd, double dt) { // 中间时刻的纵向速度 double vx_mid state_.vx 0.5 * dt * accel_cmd; double vy_mid state_.vy 0.5 * dt * (compute_vy_dot(state_, accel_cmd, steer_cmd)); double r_mid state_.r 0.5 * dt * (compute_r_dot(state_, accel_cmd, steer_cmd)); // 用中间时刻状态更新 state_.vx dt * accel_cmd; state_.vy dt * compute_vy_dot(state_, accel_cmd, steer_cmd); state_.r dt * compute_r_dot(state_, accel_cmd, steer_cmd); // 横纵向位置积分 double yaw_new state_.yaw dt * r_mid; state_.x dt * (vx_mid * cos(yaw_new) - vy_mid * sin(yaw_new)); state_.y dt * (vx_mid * sin(yaw_new) vy_mid * cos(yaw_new)); state_.yaw yaw_new; }这个实现是 RK2 的变体横向速度与横摆角频率的更新先做一步中点预估然后把全局坐标的积分用预估的中间状态做。实际仿真里发现设置dt为 0.01 秒100 Hz时60 km/h 下连续跑发卡弯轨迹误差比欧拉法小一个数量级。积分器之外还有一个重要细节控制指令的生效延迟。真实车辆的执行器转向电机、驱动电机有滞后纯仿真如果忽略这个延迟控制器的相位裕度会被高估。功能包里可以额外加一个一阶惯性环节来模拟执行器动态代码如下// 模拟转向执行器的滞后 double steer_eff steer_filter_.update(steer_cmd, dt); // 一阶滤波: k / (tau * s 1) // tau 0.05 表示约 50ms 延迟3.3 车辆参数 YAML 的配置与标定车辆参数写在 YAML 里节点启动时用rclcpp::Node::declare_parameter加载。下面是一组典型的小型乘用车参数参数符号物理含义数值单位m整车质量1500kgIz横摆转动惯量2500kg·m²lf质心到前轴距离1.2mlr质心到后轴距离1.4mCf前轴等效侧偏刚度80000N/radCr后轴等效侧偏刚度90000N/radtau_steer转向执行器延迟0.05stau_accel纵向执行器延迟0.1s# config/vehicle_params.yaml /vehicle_sim_node: ros__parameters: mass: 1500.0 # 整车质量kg yaw_inertia: 2500.0 # 横摆转动惯量kg*m^2 dist_cg_front: 1.2 # 质心到前轴距离m dist_cg_rear: 1.4 # 质心到后轴距离m cornering_stiff_front: 80000.0 # 前轴侧偏刚度N/rad cornering_stiff_rear: 90000.0 # 后轴侧偏刚度N/rad steer_tau: 0.05 # 转向执行器时间常数s accel_tau: 0.1 # 纵向执行器时间常数s侧偏刚度在实车上很难直接测量常见的标定方法是做阶跃转向输入实验给一个固定的前轮转角记录稳态横摆角速度增益然后反推刚度比。如果你是做纯粹的算法验证不关心和具体车型的匹配直接用默认值即可。但要注意刷写cornering_stiff_front时保持和rear的差值不要超过一个数量级否则车辆模型会表现出过度转向或不足转向的病态特征控制器无论怎么调都稳不住。4. 用 Gazebo 与 RViz 搭建横纵向仿真的可视化闭环4.1 Gazebo 环境中的车辆外观与物理属性配置纯数值仿真输出的是坐标和速度看不见车辆姿态变化会带来两个问题一是很难判断横摆角与侧偏角的视觉对应关系二是控制器的调参过程缺乏直觉反馈。在 Ubuntu 22.04 下Gazebo 和 ROS 2 Humble 的搭配是默认的仿真组合安装时选择ros-humble-desktop就自带 Gazebo 11。为车辆创建 URDF 描述文件时底盘部分建议用简单的 box 几何体代替复杂的车壳网格。复杂网格会让碰撞检测的计算量暴涨而 box 在物理引擎里的碰撞反馈是稳定的。URDF 中摩擦系数的配置经常被忽略——轮胎的纵向滑移率与地面摩擦系数直接决定了能不能稳定加速默认值mu1和mu2最好都设为 0.8 到 1.0 之间模拟干燥沥青路面。!-- 前轮 link 定义 -- link namefront_left_wheel visual geometry cylinder radius0.32 length0.25/ /geometry /visual collision geometry cylinder radius0.32 length0.25/ /geometry /collision inertial mass value12.0/ inertia ixx0.1 ixy0.0 ixz0.0 iyy0.1 iyz0.0 izz0.2/ /inertial /link !-- 车轮与底盘的连接 -- joint namefront_left_joint typecontinuous parent linkbase_link/ child linkfront_left_wheel/ origin xyz1.2 0.8 0.0 rpy0 0 0/ axis xyz0 0 1/ !-- 注意continuous 关节绕轴旋转 -- /joint这个 URDF 只是让车辆在 Gazebo 里有形状和碰撞体积真正的动力学行为仍然由自己的vehicle_sim_node计算Gazebo 只负责显示和提供传感器数据。两种思路的区别在于一个是让 Gazebo 的物理引擎解算整车动力学另一个是把这个包作为运动学真值。实际工程里后者更可控因为物理引擎的轮胎模型参数一多就难收敛尤其是涉及ros2 bridge传输外部控制指令时。4.2 在 RViz 中叠加曲线轨迹与车辆模型启动 RViz 前写一个简单的 launch 文件把仿真节点、TF 广播、RViz 三件事一次做完。TF 广播在这里很关键RViz 里的车辆模型要跟随仿真状态运动靠的是base_link到odom的坐标变换# launch/sim.launch.py from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( packagevehicle_sim, executablevehicle_sim_node, namevehicle_sim_node, outputscreen, parameters[config/vehicle_params.yaml] ), Node( packagetf2_ros, executablestatic_transform_publisher, arguments[0, 0, 0, 0, 0, 0, odom, map] ), Node( packagerviz2, executablerviz2, namerviz2, arguments[-d, src/vehicle_sim/rviz/vehicle_display.rviz] ) ])static_transform_publisher把odom坐标系钉在map原点上车辆节点内部则发布base_link到odom的动态变换这样 RViz 里能看到小车在地图上移动。想验证横纵向耦合可以在 RViz 的 Path 显示里订阅一个记录位置的轨迹话题具体做法是给节点加一个nav_msgs/Path发布器每步仿真把当前位置追加到 Path 的队尾。4.3 用键盘节点驱动车速与转向指令没有外部规控算法时需要手动输入指令来验证模型行为。常见做法是写一个keyboard_control.py脚本读取终端按键事件映射成加速度和转角import rclpy from rclpy.node import Node from std_msgs.msg import Float32MultiArray import sys, tty, termios class KeyboardController(Node): def __init__(self): super().__init__(keyboard_controller) self.pub self.create_publisher(Float32MultiArray, /control/command, 10) self.timer self.create_timer(0.05, self.timer_callback) self.accel 0.0 self.steer 0.0 self.old_settings termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin.fileno()) def timer_callback(self): msg Float32MultiArray() msg.data [self.accel, self.steer] self.pub.publish(msg) def update(self, key): if key w: self.accel min(self.accel 0.2, 3.0) elif key s: self.accel max(self.accel - 0.2, -3.0) elif key a: self.steer max(self.steer - 0.05, -0.5) elif key d: self.steer min(self.steer 0.05, 0.5) elif key : self.accel 0.0 self.steer 0.0 print(f\r accel{self.accel:.1f} steer{self.steer:.2f} , end)这段脚本的核心逻辑是每 50ms 把按键映射后的值发布到控制话题松开键不会清零而是保持当前值方便做定速巡航的测试。注意最终运行前要恢复终端设置否则按CtrlC后终端输入会不回显def restore_terminal(): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) rclpy.init() node KeyboardController() try: while rclpy.ok(): key sys.stdin.read(1) node.update(key) except KeyboardInterrupt: pass finally: node.restore_terminal() node.destroy_node() rclpy.shutdown()5. 横纵向耦合的验证方法、误差评估与参数调优5.1 用正弦扫频识别模型的横向带宽仿真模型建好后第一个验证动作是给前轮转角一个线性增频的正弦扫频输入 ( \delta(t) A \cdot \sin(2\pi f(t) \cdot t) )其中频率 ( f(t) ) 从 0.1 Hz 线性增加到 2 Hz纵向加速度保持恒定。记录横摆角速度的幅值与相位变化就能算出模型的横向带宽也就是横摆响应开始衰减的频点。# 运行 30 秒扫频输入记录车辆状态到 ros2 bag ros2 bag record /vehicle/status -o sweep_test.bag # 用 Python 脚本做 FFT 分析输入输出 python3 scripts/frequency_response.py --bag sweep_test.bag一个健康的横纵向耦合模型横向带宽应该在 1 Hz 以上。如果带宽明显偏低先检查cornering_stiff_front和cornering_stiff_rear是否在合理范围再检查vx下限保护是否把纵向速度限制了。实际遇到过一个情况纵向速度下限设成了 0.5 m/s导致低速扫频时侧偏角计算严重失真带宽测出来不到 0.3 Hz调回 0.1 后恢复正常。5.2 横纵向误差的量化评估指标仿真模型验证不能只看轨迹重合度要分开评估纵向与横向分量。建议用以下三个指标纵向速度跟踪误差的均方根值 RMSE反映纵向模型的响应精度横向位置误差的最大绝对偏差 Max Abs Lateral Error横摆角速度的相关系数反映横摆动力学的一致性import numpy as np from scipy.stats import pearsonr def evaluate_tracking(log_file): data np.loadtxt(log_file, delimiter,, skiprows1) vx_ref, vx_act data[:, 0], data[:, 1] lat_err data[:, 2] yaw_rate_ref, yaw_rate_act data[:, 3], data[:, 4] vx_rmse np.sqrt(np.mean((vx_ref - vx_act) ** 2)) max_lat_err np.max(np.abs(lat_err)) corr, _ pearsonr(yaw_rate_ref, yaw_rate_act) print(fvx RMSE: {vx_rmse:.3f} m/s) print(fMax Lateral Error: {max_lat_err:.4f} m) print(fYaw Rate Correlation: {corr:.4f}) return vx_rmse, max_lat_err, corr注意横向位置误差要看的是控制目标与模型输出的误差而不是绝对坐标。如果绝对值漂移很大先检查积分器的数值稳定性再把时间步长调小到 0.005 试一次。5.3 参数敏感度与实车部署的衔接调优的最后一步是做参数敏感度分析。把整车质量的参数上下浮动 10%看模型的横向响应变化幅度。这一步能给你一个信心边界哪些参数必须标定准确哪些参数用经验值就行。如果你的下游算法是纵向 PID 加横向 LQR纵向加速度的accel_tau对控制效果影响最直接最好通过阶跃响应测试单独标定而yaw_inertia对控制器设计的稳健性影响较弱用估算值即可。实车部署时当整车模型输出与实车数据出现偏差把实车的 GPS/IMU 数据回放给仿真模型用最小二乘估计修正刚度和惯性参数修正完成后重新做本章的正弦扫频验证前后对比的带宽变化就是模型精度的提升量。这个闭环流程比单纯调控制器的 PID 增益更能帮团队积累车型参数库。本文还有配套的精品资源点击获取 ROS2横纵向耦合车辆动力学仿真功能包设计与实现
简介面向自动驾驶与机器人方向开发者这是一份基于ROS的车辆横纵向仿真模型功能包将车辆模型封装为ROS节点通过Twist、Pose等消息及Topic接口输入油门、刹车、转向角并利用递推法更新运动状态便于验证与调试横纵向控制算法可快速搭建仿真环境测试横向路径跟踪与纵向速度控制策略。资源共24个文件压缩包仅39KB包含5个C头文件、3个源码文件、3个自定义msg消息定义、4个xml配置及catkin工作空间文件结构清晰适合有一定ROS与C基础的读者快速移植使用。包内demoKinematic示例可帮助理解车辆运动学模型的搭建与调用方式配套README与proto文件也提供了必要说明。已有1086人学习下载是学习ROS机器人仿真与控制算法验证的实用参考。1. 车辆横纵向耦合仿真是功能包的核心价值做过实车横向控制的人都遇到过一类问题纯跟踪算法在仿真里跟踪误差收敛得很漂亮一上车就在大曲率弯道外飘。原因往往不在控制算法本身而在仿真模型把横纵向解耦了。车辆转向时纵向速度的瞬态变化会通过轮胎力重新分配影响侧偏刚度这种耦合关系在离散化的单车模型里一旦被简化掉控制器的鲁棒性验证就失去了意义。这个 ROS 功能包解决的就是在仿真里复现横纵向动力学耦合这件事。它不依赖 CarSim 或 Carmaker而是把车辆的非线性动力学方程写进 ROS 节点通过标准话题接收加速度与前轮转角指令计算出横摆角速度、纵向速度与侧向速度再发布成车辆状态。适合没有商业仿真软件授权的团队也适合需要在 CI 环境里做规控算法回归测试的工程师。如果你的流是验证 MPC 或 LQR 这类模型预测控制器的表现这个包能把车辆模型的假设边界和真实差距摊开在你面前。2. ROS功能包的软件架构与工作空间组织2.1 功能包在 ROS 工作空间中的目录布局写这个包之前先把工作空间的结构理清楚。最常见的是用catkin_ws/src作为顶层目录功能包本身用ros2 pkg create或catkin_create_pkg生成但复杂的车辆模型我并不建议像教学例程那样把所有源文件堆在/src下。更实用的做法是分成三层autoware_vehicle_sim/ ├── CMakeLists.txt ├── package.xml ├── config/ │ ├── vehicle_params.yaml │ └── controller_gains.yaml ├── include/ │ └── vehicle_sim/ │ ├── vehicle_model.hpp │ ├── tire_model.hpp │ └── integrator.hpp ├── src/ │ ├── vehicle_sim_node.cpp │ ├── vehicle_model.cpp │ └── tire_model.cpp ├── launch/ │ └── sim.launch ├── rviz/ │ └── vehicle_display.rviz └── scripts/ └── plot_results.pyinclude目录放类声明src放实现config放 YAML 参数launch集中管理启动项。这里的关键是把车辆动力学方程和 ROS 通信层拆开这样模型可以在无 ROS 环境下用 gtest 单测后续如果要移植到别的中间件也不用大改。如果是从零搭环境热词里经常提到的“鱼香ROS一键安装”在 Ubuntu 22.04 ROS 2 Humble 下确实是最省事的路径。终端里执行wget http://fishros.com/install -O fishros . fishros会弹出选项菜单选一键安装 ROS 2 Humble 桌面版即可。装完后记得source /opt/ros/humble/setup.bash并把 source 语句写到~/.bashrc末尾避免每次手工激活。2.2 话题与服务接口设计车辆仿真节点要对外暴露两组话题订阅控制指令发布车辆状态。控制指令的标准格式在 Autoware 里是autoware_auto_control_msgs/msg/AckermannControlCommand但自己写通用功能包时用低耦合的话题更灵活。我一般这样定义订阅话题 /control/command std_msgs/Float32MultiArray [0] 纵向加速度 (m/s^2) [1] 前轮转角 (rad) 发布话题 /vehicle/status vehicle_sim_msgs/VehicleState 纵向速度vx (m/s) 横向速度vy (m/s) 横摆角速度yaw_rate (rad/s) 全局位置x, y (m) 航向角yaw (rad) 服务接口 /reset_simulation std_srvs/srv/Emptystd_msgs/Float32MultiArray的好处是发布端不用引入额外的消息依赖Python 的规控节点里一条pub.publish(Float32MultiArray(data[2.0, 0.15]))就能驱动仿真。缺点是数组下标含义脆弱所以要在 launch 文件里加documentation参数说明每个下标对应的物理量并在节点启动时打印一次话题说明。2.3 CMakeLists 的依赖与编译配置CMake 配置的核心点是开启 C17 支持并确保ament_cmake能正确找到消息生成的头文件。下面的配置片段是可以在 ROS 2 Humble 下直接用的cmake_minimum_required(VERSION 3.8) project(vehicle_sim) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES Clang) add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) add_executable(vehicle_sim_node src/vehicle_sim_node.cpp src/vehicle_model.cpp src/tire_model.cpp ) target_include_directories(vehicle_sim_node PRIVATE include/vehicle_sim ) target_compile_features(vehicle_sim_node PUBLIC cxx_std_17) install(TARGETS vehicle_sim_node DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) ament_package()注意install(DIRECTORY launch config ...)这一行不能省launch 和 YAML 文件不安装的话ros2 launch 会报找不到文件。编译完成后用colcon build --packages-select vehicle_sim单独构建这个包别用colcon build全量构建否则依赖树里任何一个无关包的编译错误都会挡住你的验证。3. 车辆横纵向动力学模型的数学推导与代码实现3.1 从二自由度单车模型到横纵向耦合状态方程做横纵向耦合仿真二自由度单车模型是最常被低估的起点。所谓二自由度指的是车辆的横向运动与横摆运动这两个旋转/平移自由度纵向速度在模型里被当作时变参数而不是常值。顺带说一个热词里出现的“pso自动驾驶”“ros slam建图和自主导航”那些都是规控算法在整车模型上的下游应用本章的动力学实现是它们共同的模型底座。建模时做三个基本假设忽略车辆的垂向运动与俯仰、侧倾自由度两个前轮的转角相同合并为等效前轮转角轮胎工作在线性区侧偏力与侧偏角成正比在这个基础上纵向与横向的耦合通过纵向速度 ( v_x ) 的变化体现——纵向加速度改变 ( v_x )( v_x ) 进入横向动力学方程中的向心力项 ( v_x \cdot r ) 与轮胎侧偏角计算式横摆角速度又反过来影响纵向速度在全局坐标系下的积分。用状态空间写出来[ \begin{bmatrix} \dot{v}_x \ \dot{v}_y \ \dot{r} \end{bmatrix} \begin{bmatrix} a_x v_y \cdot r \ -\frac{C_f C_r}{m \cdot v_x} v_y - \left( v_x \frac{C_f \cdot l_f - C_r \cdot l_r}{m \cdot v_x} \right) r \frac{C_f}{m} \delta \ -\frac{C_f \cdot l_f - C_r \cdot l_r}{I_z \cdot v_x} v_y - \frac{C_f \cdot l_f^2 C_r \cdot l_r^2}{I_z \cdot v_x} r \frac{C_f \cdot l_f}{I_z} \delta \end{bmatrix} ]其中 ( C_f、C_r ) 是前后轴等效侧偏刚度( l_f、l_r ) 是质心到前后轴的距离( m ) 是整车质量( I_z ) 是横摆转动惯量( \delta ) 是前轮转角。注意 ( v_x ) 出现在分母上这意味着低速工况停车起步、蠕行会带来数值奇异一般会加一个下限保护例如 ( v_x \max(v_x, 0.1) )。3.2 离散化方法与积分器选择ROS 节点里做数值积分最常见的是定步长 Runge-Kutta 2 阶中点法比欧拉法精度高比 RK4 计算量小在 100 Hz 控制频率下足够。中点法的递推公式void VehicleModel::step(double accel_cmd, double steer_cmd, double dt) { // 中间时刻的纵向速度 double vx_mid state_.vx 0.5 * dt * accel_cmd; double vy_mid state_.vy 0.5 * dt * (compute_vy_dot(state_, accel_cmd, steer_cmd)); double r_mid state_.r 0.5 * dt * (compute_r_dot(state_, accel_cmd, steer_cmd)); // 用中间时刻状态更新 state_.vx dt * accel_cmd; state_.vy dt * compute_vy_dot(state_, accel_cmd, steer_cmd); state_.r dt * compute_r_dot(state_, accel_cmd, steer_cmd); // 横纵向位置积分 double yaw_new state_.yaw dt * r_mid; state_.x dt * (vx_mid * cos(yaw_new) - vy_mid * sin(yaw_new)); state_.y dt * (vx_mid * sin(yaw_new) vy_mid * cos(yaw_new)); state_.yaw yaw_new; }这个实现是 RK2 的变体横向速度与横摆角频率的更新先做一步中点预估然后把全局坐标的积分用预估的中间状态做。实际仿真里发现设置dt为 0.01 秒100 Hz时60 km/h 下连续跑发卡弯轨迹误差比欧拉法小一个数量级。积分器之外还有一个重要细节控制指令的生效延迟。真实车辆的执行器转向电机、驱动电机有滞后纯仿真如果忽略这个延迟控制器的相位裕度会被高估。功能包里可以额外加一个一阶惯性环节来模拟执行器动态代码如下// 模拟转向执行器的滞后 double steer_eff steer_filter_.update(steer_cmd, dt); // 一阶滤波: k / (tau * s 1) // tau 0.05 表示约 50ms 延迟3.3 车辆参数 YAML 的配置与标定车辆参数写在 YAML 里节点启动时用rclcpp::Node::declare_parameter加载。下面是一组典型的小型乘用车参数参数符号物理含义数值单位m整车质量1500kgIz横摆转动惯量2500kg·m²lf质心到前轴距离1.2mlr质心到后轴距离1.4mCf前轴等效侧偏刚度80000N/radCr后轴等效侧偏刚度90000N/radtau_steer转向执行器延迟0.05stau_accel纵向执行器延迟0.1s# config/vehicle_params.yaml /vehicle_sim_node: ros__parameters: mass: 1500.0 # 整车质量kg yaw_inertia: 2500.0 # 横摆转动惯量kg*m^2 dist_cg_front: 1.2 # 质心到前轴距离m dist_cg_rear: 1.4 # 质心到后轴距离m cornering_stiff_front: 80000.0 # 前轴侧偏刚度N/rad cornering_stiff_rear: 90000.0 # 后轴侧偏刚度N/rad steer_tau: 0.05 # 转向执行器时间常数s accel_tau: 0.1 # 纵向执行器时间常数s侧偏刚度在实车上很难直接测量常见的标定方法是做阶跃转向输入实验给一个固定的前轮转角记录稳态横摆角速度增益然后反推刚度比。如果你是做纯粹的算法验证不关心和具体车型的匹配直接用默认值即可。但要注意刷写cornering_stiff_front时保持和rear的差值不要超过一个数量级否则车辆模型会表现出过度转向或不足转向的病态特征控制器无论怎么调都稳不住。4. 用 Gazebo 与 RViz 搭建横纵向仿真的可视化闭环4.1 Gazebo 环境中的车辆外观与物理属性配置纯数值仿真输出的是坐标和速度看不见车辆姿态变化会带来两个问题一是很难判断横摆角与侧偏角的视觉对应关系二是控制器的调参过程缺乏直觉反馈。在 Ubuntu 22.04 下Gazebo 和 ROS 2 Humble 的搭配是默认的仿真组合安装时选择ros-humble-desktop就自带 Gazebo 11。为车辆创建 URDF 描述文件时底盘部分建议用简单的 box 几何体代替复杂的车壳网格。复杂网格会让碰撞检测的计算量暴涨而 box 在物理引擎里的碰撞反馈是稳定的。URDF 中摩擦系数的配置经常被忽略——轮胎的纵向滑移率与地面摩擦系数直接决定了能不能稳定加速默认值mu1和mu2最好都设为 0.8 到 1.0 之间模拟干燥沥青路面。!-- 前轮 link 定义 -- link namefront_left_wheel visual geometry cylinder radius0.32 length0.25/ /geometry /visual collision geometry cylinder radius0.32 length0.25/ /geometry /collision inertial mass value12.0/ inertia ixx0.1 ixy0.0 ixz0.0 iyy0.1 iyz0.0 izz0.2/ /inertial /link !-- 车轮与底盘的连接 -- joint namefront_left_joint typecontinuous parent linkbase_link/ child linkfront_left_wheel/ origin xyz1.2 0.8 0.0 rpy0 0 0/ axis xyz0 0 1/ !-- 注意continuous 关节绕轴旋转 -- /joint这个 URDF 只是让车辆在 Gazebo 里有形状和碰撞体积真正的动力学行为仍然由自己的vehicle_sim_node计算Gazebo 只负责显示和提供传感器数据。两种思路的区别在于一个是让 Gazebo 的物理引擎解算整车动力学另一个是把这个包作为运动学真值。实际工程里后者更可控因为物理引擎的轮胎模型参数一多就难收敛尤其是涉及ros2 bridge传输外部控制指令时。4.2 在 RViz 中叠加曲线轨迹与车辆模型启动 RViz 前写一个简单的 launch 文件把仿真节点、TF 广播、RViz 三件事一次做完。TF 广播在这里很关键RViz 里的车辆模型要跟随仿真状态运动靠的是base_link到odom的坐标变换# launch/sim.launch.py from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( packagevehicle_sim, executablevehicle_sim_node, namevehicle_sim_node, outputscreen, parameters[config/vehicle_params.yaml] ), Node( packagetf2_ros, executablestatic_transform_publisher, arguments[0, 0, 0, 0, 0, 0, odom, map] ), Node( packagerviz2, executablerviz2, namerviz2, arguments[-d, src/vehicle_sim/rviz/vehicle_display.rviz] ) ])static_transform_publisher把odom坐标系钉在map原点上车辆节点内部则发布base_link到odom的动态变换这样 RViz 里能看到小车在地图上移动。想验证横纵向耦合可以在 RViz 的 Path 显示里订阅一个记录位置的轨迹话题具体做法是给节点加一个nav_msgs/Path发布器每步仿真把当前位置追加到 Path 的队尾。4.3 用键盘节点驱动车速与转向指令没有外部规控算法时需要手动输入指令来验证模型行为。常见做法是写一个keyboard_control.py脚本读取终端按键事件映射成加速度和转角import rclpy from rclpy.node import Node from std_msgs.msg import Float32MultiArray import sys, tty, termios class KeyboardController(Node): def __init__(self): super().__init__(keyboard_controller) self.pub self.create_publisher(Float32MultiArray, /control/command, 10) self.timer self.create_timer(0.05, self.timer_callback) self.accel 0.0 self.steer 0.0 self.old_settings termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin.fileno()) def timer_callback(self): msg Float32MultiArray() msg.data [self.accel, self.steer] self.pub.publish(msg) def update(self, key): if key w: self.accel min(self.accel 0.2, 3.0) elif key s: self.accel max(self.accel - 0.2, -3.0) elif key a: self.steer max(self.steer - 0.05, -0.5) elif key d: self.steer min(self.steer 0.05, 0.5) elif key : self.accel 0.0 self.steer 0.0 print(f\r accel{self.accel:.1f} steer{self.steer:.2f} , end)这段脚本的核心逻辑是每 50ms 把按键映射后的值发布到控制话题松开键不会清零而是保持当前值方便做定速巡航的测试。注意最终运行前要恢复终端设置否则按CtrlC后终端输入会不回显def restore_terminal(): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) rclpy.init() node KeyboardController() try: while rclpy.ok(): key sys.stdin.read(1) node.update(key) except KeyboardInterrupt: pass finally: node.restore_terminal() node.destroy_node() rclpy.shutdown()5. 横纵向耦合的验证方法、误差评估与参数调优5.1 用正弦扫频识别模型的横向带宽仿真模型建好后第一个验证动作是给前轮转角一个线性增频的正弦扫频输入 ( \delta(t) A \cdot \sin(2\pi f(t) \cdot t) )其中频率 ( f(t) ) 从 0.1 Hz 线性增加到 2 Hz纵向加速度保持恒定。记录横摆角速度的幅值与相位变化就能算出模型的横向带宽也就是横摆响应开始衰减的频点。# 运行 30 秒扫频输入记录车辆状态到 ros2 bag ros2 bag record /vehicle/status -o sweep_test.bag # 用 Python 脚本做 FFT 分析输入输出 python3 scripts/frequency_response.py --bag sweep_test.bag一个健康的横纵向耦合模型横向带宽应该在 1 Hz 以上。如果带宽明显偏低先检查cornering_stiff_front和cornering_stiff_rear是否在合理范围再检查vx下限保护是否把纵向速度限制了。实际遇到过一个情况纵向速度下限设成了 0.5 m/s导致低速扫频时侧偏角计算严重失真带宽测出来不到 0.3 Hz调回 0.1 后恢复正常。5.2 横纵向误差的量化评估指标仿真模型验证不能只看轨迹重合度要分开评估纵向与横向分量。建议用以下三个指标纵向速度跟踪误差的均方根值 RMSE反映纵向模型的响应精度横向位置误差的最大绝对偏差 Max Abs Lateral Error横摆角速度的相关系数反映横摆动力学的一致性import numpy as np from scipy.stats import pearsonr def evaluate_tracking(log_file): data np.loadtxt(log_file, delimiter,, skiprows1) vx_ref, vx_act data[:, 0], data[:, 1] lat_err data[:, 2] yaw_rate_ref, yaw_rate_act data[:, 3], data[:, 4] vx_rmse np.sqrt(np.mean((vx_ref - vx_act) ** 2)) max_lat_err np.max(np.abs(lat_err)) corr, _ pearsonr(yaw_rate_ref, yaw_rate_act) print(fvx RMSE: {vx_rmse:.3f} m/s) print(fMax Lateral Error: {max_lat_err:.4f} m) print(fYaw Rate Correlation: {corr:.4f}) return vx_rmse, max_lat_err, corr注意横向位置误差要看的是控制目标与模型输出的误差而不是绝对坐标。如果绝对值漂移很大先检查积分器的数值稳定性再把时间步长调小到 0.005 试一次。5.3 参数敏感度与实车部署的衔接调优的最后一步是做参数敏感度分析。把整车质量的参数上下浮动 10%看模型的横向响应变化幅度。这一步能给你一个信心边界哪些参数必须标定准确哪些参数用经验值就行。如果你的下游算法是纵向 PID 加横向 LQR纵向加速度的accel_tau对控制效果影响最直接最好通过阶跃响应测试单独标定而yaw_inertia对控制器设计的稳健性影响较弱用估算值即可。实车部署时当整车模型输出与实车数据出现偏差把实车的 GPS/IMU 数据回放给仿真模型用最小二乘估计修正刚度和惯性参数修正完成后重新做本章的正弦扫频验证前后对比的带宽变化就是模型精度的提升量。这个闭环流程比单纯调控制器的 PID 增益更能帮团队积累车型参数库。本文还有配套的精品资源点击获取