基于ROS的智能车PID轨迹跟踪仿真与设计

简介这份资源面向学习ROS与智能车控制的学生和开发者提供一套基于PID算法的轨迹跟踪仿真与设计完整方案可用于课程设计、毕业设计或算法入门实践。压缩包共61个文件约154KB包含7个cpp源文件与3个py脚本承载核心跟踪逻辑16个yaml与7个launch文件负责参数配置和节点启动另有world、rviz、urdf、xacro等仿真与模型描述文件以及pgm地图、dae模型和pdf文档说明结构完整便于按模块查阅。已有213人学习下载源码经本地编译可运行评审分达95分以上难度适中且经助教老师审定。读者可据此理解PID在轨迹跟踪中的参数整定与误差调节思路掌握ROS节点通信、仿真环境搭建与可视化调试流程并借助文档快速复现实验适合作为算法验证与二次开发的参考模板。1. 从零搭一套 ROS 智能车轨迹跟踪仿真PID 到底控的是什么很多人第一次接触智能车轨迹跟踪脑子里想的是「车沿着线走」但真正上手 ROS 仿真才发现核心问题其实是给定一条期望轨迹如何让车实际位姿在有限时间内收敛到这条轨迹上并且横向误差不发散。这个标题里的「基于 ROS 实现」意味着整套东西跑在 ROS 的节点通信框架下「PID 跟踪算法」意味着控制器用的是比例-积分-微分三环「仿真与设计」意味着你不需要真车在 Gazebo 或类似仿真环境里就能把闭环跑通。适合谁适合已经会写基本 ROS 节点、想搞明白轨迹跟踪控制器到底怎么调、怎么验证的在校学生和初级工程师。我见过太多人把 PID 三个参数拍脑袋填进去车要么画龙要么直接冲出赛道这篇就把这套东西从建模到调参讲透。2. 轨迹跟踪的误差模型与 PID 控制律设计2.1 为什么不能直接对位置误差做 PID智能车轨迹跟踪最常见的翻车现场是把「车当前坐标减去目标点坐标」直接丢给 PID输出当成速度指令。这样做的后果是车在直线段勉强能走一进弯道就内外摇摆因为位置误差和航向误差是耦合的。正确的做法是先建立车辆的运动学误差模型。假设车辆采用差速驱动模型状态量为 ([x, y, \theta])期望轨迹上对应最近点的位姿为 ([x_r, y_r, \theta_r])。定义纵向误差 (e_x)、横向误差 (e_y)、航向误差 (e_\theta)在车体坐标系下[ \begin{bmatrix} e_x \ e_y \ e_\theta \end{bmatrix}\begin{bmatrix} \cos\theta \sin\theta 0 \ -\sin\theta \cos\theta 0 \ 0 0 1 \end{bmatrix} \begin{bmatrix} x_r - x \ y_r - y \ \theta_r - \theta \end{bmatrix} ]这个变换的意义是把全局坐标下的偏差转到车体视角下横向误差 (e_y) 才是真正决定车会不会跑偏的量。PID 控制器一般设计为线速度 (v v_r \cos e_\theta k_{px} e_x)角速度 (\omega \omega_r k_{py} v_r e_y k_{p\theta} e_\theta k_i \int e_\theta dt k_d \frac{de_\theta}{dt})前馈项 (v_r, \omega_r) 来自参考轨迹反馈项才是 PID 的活。很多教程只讲反馈不讲前馈导致车在弯道永远滞后这是选型上的关键区别。2.2 在 ROS 里搭出误差计算节点下面这段代码是误差计算节点的核心放在tracking_error.cpp里订阅/odom和/reference_path发布/tracking_error。// tracking_error.cpp #include ros/ros.h #include nav_msgs/Odometry.h #include nav_msgs/Path.h #include geometry_msgs/Vector3.h #include tf/tf.h #include cmath class ErrorCalculator { public: ErrorCalculator() { odom_sub_ nh_.subscribe(/odom, 10, ErrorCalculator::odomCb, this); path_sub_ nh_.subscribe(/reference_path, 1, ErrorCalculator::pathCb, this); err_pub_ nh_.advertisegeometry_msgs::Vector3(/tracking_error, 10); } private: ros::NodeHandle nh_; ros::Subscriber odom_sub_, path_sub_; ros::Publisher err_pub_; nav_msgs::Odometry cur_odom_; nav_msgs::Path ref_path_; bool odom_ready_ false, path_ready_ false; void odomCb(const nav_msgs::Odometry::ConstPtr msg) { cur_odom_ *msg; odom_ready_ true; tryPublish(); } void pathCb(const nav_msgs::Path::ConstPtr msg) { ref_path_ *msg; path_ready_ true; tryPublish(); } // 在参考轨迹上找离当前车体最近的投影点 int findNearestIndex(double x, double y) { int best 0; double best_d 1e9; for (size_t i 0; i ref_path_.poses.size(); i) { double dx ref_path_.poses[i].pose.position.x - x; double dy ref_path_.poses[i].pose.position.y - y; double d dx * dx dy * dy; if (d best_d) { best_d d; best i; } } return best; } void tryPublish() { if (!odom_ready_ || !path_ready_ || ref_path_.poses.empty()) return; double x cur_odom_.pose.pose.position.x; double y cur_odom_.pose.pose.position.y; double theta tf::getYaw(cur_odom_.pose.pose.orientation); int idx findNearestIndex(x, y); // 用相邻两点差分估计参考航向避免直接取四元数抖动 int idx_next std::min(idx 1, (int)ref_path_.poses.size() - 1); double xr ref_path_.poses[idx].pose.position.x; double yr ref_path_.poses[idx].pose.position.y; double xr_n ref_path_.poses[idx_next].pose.position.x; double yr_n ref_path_.poses[idx_next].pose.position.y; double theta_r std::atan2(yr_n - yr, xr_n - xr); double dx xr - x; double dy yr - y; double ex std::cos(theta) * dx std::sin(theta) * dy; double ey -std::sin(theta) * dx std::cos(theta) * dy; double et theta_r - theta; // 角度归一化到 [-pi, pi] while (et M_PI) et - 2 * M_PI; while (et -M_PI) et 2 * M_PI; geometry_msgs::Vector3 err; err.x ex; err.y ey; err.z et; err_pub_.publish(err); } }; int main(int argc, char** argv) { ros::init(argc, argv, tracking_error_node); ErrorCalculator node; ros::spin(); return 0; }逻辑说明findNearestIndex做的是最近点搜索这是轨迹跟踪里最容易被忽略的一步。如果参考轨迹点稀疏最近点跳变会导致误差突变PID 输出跟着抖。参数上/odom的队列长度给 10 是为了容忍仿真里的短时延迟/reference_path给 1 是因为路径一旦更新就应立刻生效。角度归一化那两行是血泪经验少了它车在 (\pi) 附近会突然反向打角。2.3 PID 控制节点的实现与参数含义控制节点订阅/tracking_error输出/cmd_vel。下面是最小实现// pid_controller.cpp #include ros/ros.h #include geometry_msgs/Vector3.h #include geometry_msgs/Twist.h class PidController { public: PidController() : nh_(~) { nh_.param(kp_y, kp_y_, 1.2); nh_.param(kp_theta, kp_theta_, 2.0); nh_.param(ki_theta, ki_theta_, 0.05); nh_.param(kd_theta, kd_theta_, 0.1); nh_.param(v_base, v_base_, 0.5); nh_.param(v_max, v_max_, 1.0); nh_.param(w_max, w_max_, 1.5); err_sub_ nh_.subscribe(/tracking_error, 10, PidController::errCb, this); cmd_pub_ nh_.advertisegeometry_msgs::Twist(/cmd_vel, 10); last_time_ ros::Time::now(); } private: ros::NodeHandle nh_; ros::Subscriber err_sub_; ros::Publisher cmd_pub_; double kp_y_, kp_theta_, ki_theta_, kd_theta_; double v_base_, v_max_, w_max_; double integral_et_ 0.0, last_et_ 0.0; ros::Time last_time_; void errCb(const geometry_msgs::Vector3::ConstPtr err) { ros::Time now ros::Time::now(); double dt (now - last_time_).toSec(); if (dt 0.0 || dt 0.5) { last_time_ now; return; } // 防时间跳变 last_time_ now; double ey err-y; double et err-z; integral_et_ et * dt; // 积分限幅防止弯道长时间偏差导致积分饱和 if (integral_et_ 0.5) integral_et_ 0.5; if (integral_et_ -0.5) integral_et_ -0.5; double det (et - last_et_) / dt; last_et_ et; double v v_base_; double w kp_y_ * v * ey kp_theta_ * et ki_theta_ * integral_et_ kd_theta_ * det; if (v v_max_) v v_max_; if (v -v_max_) v -v_max_; if (w w_max_) w w_max_; if (w -w_max_) w -w_max_; geometry_msgs::Twist cmd; cmd.linear.x v; cmd.angular.z w; cmd_pub_.publish(cmd); } }; int main(int argc, char** argv) { ros::init(argc, argv, pid_controller_node); PidController node; ros::spin(); return 0; }参数说明kp_y是横向误差增益乘以当前线速度 (v) 是为了让高速时转向更敏感这是轨迹跟踪里常见的增益调度手法。kp_theta管航向回正太小车会切内弯太大直接震荡。ki_theta用来消除稳态航向偏差但必须限幅否则弯道积分饱和后出弯会甩尾。kd_theta抑制超调仿真里给 0.1 左右就够真车上因为噪声大通常要加低通滤波。v_base是巡航速度v_max和w_max是执行器保护仿真里可以放宽真车上必须按底盘能力设。3. 在 Gazebo 里把闭环跑起来URDF、插件与 launch 编排3.1 差速车 URDF 的关键 link 与 joint仿真能不能收敛一半取决于模型对不对。差速车最少需要这些 linkbase_footprint、base_link、left_wheel_link、right_wheel_link、caster_link以及一个用于激光或视觉的sensor_link。joint 上左右轮是continuous类型caster 是fixed或continuous都行。!-- diff_car.urdf.xacro 片段 -- link namebase_link visual geometrybox size0.4 0.3 0.15//geometry /visual collision geometrybox size0.4 0.3 0.15//geometry /collision inertial mass value5.0/ inertia ixx0.05 iyy0.08 izz0.1 ixy0 ixz0 iyz0/ /inertial /link joint nameleft_wheel_joint typecontinuous parent linkbase_link/ child linkleft_wheel_link/ origin xyz0 0.18 -0.05 rpy0 0 0/ axis xyz0 1 0/ /joint注意inertial不能省Gazebo 里没有惯量的 link 会直接飞出去。轮子半径和间距要和后面diff_drive_controller的配置一致否则里程计和实际运动对不上误差计算全错。3.2 用 diff_drive_controller 接管轮速ROS 生态里最省事的做法是用ros_control的diff_drive_controller它订阅/cmd_vel发布/odom和 tf。# config/diff_drive.yaml diff_drive_controller: type: diff_drive_controller/DiffDriveController left_wheel: [left_wheel_joint] right_wheel: [right_wheel_joint] publish_rate: 50.0 pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] wheel_separation: 0.36 wheel_radius: 0.05 base_frame_id: base_footprint odom_frame_id: odom enable_odom_tf: true cmd_vel_timeout: 0.5wheel_separation和wheel_radius必须和 URDF 完全一致差 1 厘米都会让轨迹跟踪在长距离上累积偏差。cmd_vel_timeout设 0.5 秒意味着控制节点挂掉后 0.5 秒车会停这是安全兜底。3.3 launch 文件编排与参考轨迹发布一个能跑的 launch 至少包含Gazebo 空世界、机器人模型、控制器 spawn、误差节点、PID 节点、参考轨迹发布节点。!-- tracking_sim.launch -- launch include file$(find gazebo_ros)/launch/empty_world.launch arg namepaused valuefalse/ /include param namerobot_description command$(find xacro)/xacro $(find my_car)/urdf/diff_car.urdf.xacro/ node namespawn_model pkggazebo_ros typespawn_model args-urdf -model diff_car -param robot_description -x 0 -y 0 -z 0.1/ rosparam file$(find my_car)/config/diff_drive.yaml commandload/ node namecontroller_spawner pkgcontroller_manager typespawner argsdiff_drive_controller/ node nametracking_error_node pkgmy_car typetracking_error_node outputscreen/ node namepid_controller_node pkgmy_car typepid_controller_node outputscreen param namekp_y value1.2/ param namekp_theta value2.0/ param nameki_theta value0.05/ param namekd_theta value0.1/ param namev_base value0.5/ /node node namepath_publisher pkgmy_car typepath_publisher.py outputscreen/ /launch参考轨迹发布节点可以用 Python 快速写一个正弦或圆形轨迹#!/usr/bin/env python import rospy from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped import math rospy.init_node(path_publisher) pub rospy.Publisher(/reference_path, Path, queue_size1, latchTrue) path Path() path.header.frame_id odom # 生成一个半径 3 米的圆200 个点 for i in range(200): theta 2 * math.pi * i / 200.0 pose PoseStamped() pose.header.frame_id odom pose.pose.position.x 3.0 * math.cos(theta) pose.pose.position.y 3.0 * math.sin(theta) path.poses.append(pose) path.header.stamp rospy.Time.now() pub.publish(path) rospy.spin()latchTrue保证误差节点晚启动也能收到路径。圆形轨迹是验证跟踪效果最直接的场景因为曲率恒定PID 参数好不好一眼就能看出来。4. 调参与排错横向误差不收敛时先查这五处4.1 现象车在直线段画龙左右摆原因通常是kp_theta过大或kd_theta缺失。航向环增益太高时车一有偏差就猛打角冲过目标航向再反向修正形成极限环。解决先把kp_y降到 0.5kp_theta从 1.0 开始加每次加 0.2直到直线段不摆但弯道能跟上。kd_theta给 0.05 到 0.15 之间能明显压住超调。4.2 现象弯道持续切内弯出弯后偏一大截这是前馈缺失或ki_theta饱和。如果控制律里没有 (v_r \cos e_\theta) 这一项车在弯道永远慢半拍。解决在 PID 节点里增加参考速度前馈或者把ki_theta限幅从 0.5 降到 0.2并加积分分离——当 (|e_\theta| 0.3) rad 时暂停积分。4.3 现象里程计和实际位置对不上误差越跑越大先查wheel_separation和wheel_radius是否和 URDF 一致再查 Gazebo 的实时因子。仿真跑得太快时/odom的积分会漂。解决在 launch 里限制real_time_update_rate或者把publish_rate从 50 降到 30给控制器留出计算时间。4.4 现象车直接原地打转或冲出去多半是误差符号搞反了。车体坐标系下横向误差 (e_y) 的正负定义不同教材不一样。验证方法手动把车放在参考轨迹左侧看/tracking_error的y分量符号如果和控制器里用的符号相反角速度输出就会正反馈。解决在误差节点里加一个符号参数调一次就固定下来。4.5 现象仿真里能跑换条轨迹就崩参考轨迹点太稀疏或曲率突变。最近点搜索在稀疏路径上会跳变导致误差不连续。解决对参考路径做插值点间距控制在 0.05 到 0.1 米之间或者在误差节点里加一个低通滤波对 (e_y) 和 (e_\theta) 做一阶平滑截止频率 5 到 10 Hz。5. 从仿真到可复现一套参数扫描与验证的实操习惯仿真跑通只是第一步真正让这套东西有价值的是可复现的验证流程。我一般会写一个参数扫描脚本把kp_y、kp_theta、v_base做成网格每组跑 30 秒圆形轨迹记录横向误差的均方根和最大值。# sweep_params.py import rospy, subprocess, itertools, time from nav_msgs.msg import Odometry from geometry_msgs.msg import Vector3 import math def run_one(kp_y, kp_theta, v_base): # 用 roslaunch 启动参数通过命令行覆盖 cmd [roslaunch, my_car, tracking_sim.launch, pid_controller_node/kp_y:{}.format(kp_y), pid_controller_node/kp_theta:{}.format(kp_theta), pid_controller_node/v_base:{}.format(v_base)] proc subprocess.Popen(cmd) time.sleep(8) # 等仿真稳定 errors [] def cb(msg): errors.append(abs(msg.y)) sub rospy.Subscriber(/tracking_error, Vector3, cb) time.sleep(20) sub.unregister() proc.terminate() if not errors: return None rms math.sqrt(sum(e*e for e in errors) / len(errors)) return rms, max(errors) rospy.init_node(sweep, anonymousTrue) best None for kp_y, kp_theta, v_base in itertools.product( [0.6, 0.9, 1.2, 1.5], [1.0, 1.5, 2.0, 2.5], [0.3, 0.5, 0.7]): res run_one(kp_y, kp_theta, v_base) if res is None: continue rms, mx res print(kp_y{} kp_theta{} v{} rms{:.4f} max{:.4f}.format( kp_y, kp_theta, v_base, rms, mx)) if best is None or rms best[0]: best (rms, kp_y, kp_theta, v_base) print(BEST:, best)这个脚本的价值在于把「感觉调得差不多」变成「有数字支撑」。参数扫描时注意每次启动仿真要留足稳定时间Gazebo 刚起来那几秒物理引擎还在预热误差数据不可信。另外/tracking_error的采样频率要和控制器一致否则统计出来的 RMS 会偏乐观。验证阶段我还会做两件事一是把参考轨迹换成 S 形和直角弯看参数是否仍然稳定二是故意把初始位姿偏 0.5 米、偏 30 度看收敛时间。如果 S 形能过但直角弯切内弯说明前馈不够如果初始偏差大就震荡说明积分限幅太松。这些边界测试比单纯跑圆形有用得多。最后说个习惯每次改完参数把 launch 文件、yaml 和误差曲线截图一起存档命名带上日期和参数组合。我吃过亏调出一组好参数没记第二天复现不出来只能从头再来。这套仿真方案值不值得投入如果你要验证轨迹跟踪算法、对比 PID 和 MPC 的差异、或者给真车调参做预演它省下的试错成本远超搭建时间。希望帮到你。本文还有配套的精品资源点击获取