目录
摘要
一、为什么底盘控制代码要做算法封装？
1.1 底盘控制不是简单发布 /cmd _vel
1.2 为什么不能所有代码都写在一个回调函数里？
1.3 封装的核心作用
二、class、struct、enum class 在底盘控制中的作用
2.1 class：适合封装有行为的算法模块
2.2 struct：适合保存状态和参数
2.3 enum class：适合表示底盘控制模式
三、std::vector、std::array、std::clamp 常见写法
3.1 std::vector：适合保存数量不固定的数据
3.2 std::array：适合保存数量固定的数据
3.3 std::clamp：速度限幅常用写法
四、std::sin、std::cos、std::atan2 与机器人位姿计算
4.1 为什么底盘算法需要三角函数？
4.2 std::sin 和 std::cos 的使用
4.3 std::atan2：计算目标方向角
4.4 使用 atan2 做角度归一化
五、rclcpp::Time 与 dt 控制周期计算
5.1 rclcpp::Time 是什么？
5.2 dt 是什么？
5.3 ROS2 中 dt 的计算方式
5.4 为什么 dt 要做保护？
六、PID 控制器封装
6.1 PID 在底盘控制中的作用
6.2 用 struct 保存 PID 参数
6.3 用 class 封装 PID 控制器
6.4 PID 在底盘控制中的使用
6.5 完整 ROS2 C++ PID 写法
七、运动学模型封装
7.1 什么是底盘运动学模型？
7.2 差速底盘运动学模型
7.3 结合 ChassisParam 封装运动学模型
7.4 里程计位姿更新封装
7.5 一个完整的底盘控制算法关系
八、总结
摘要
前面几篇文章已经分别整理了 ROS2 C++ 中比较核心的内容。
第一部分主要讲的是 ROS2 C++ 常见 rclcpp 写法，链接如下：
ROS2 C++ 常见 rclcpp 写法保姆级教程-CSDN博客
https://blog.csdn.net/m0_58954356/article/details/162202727?spm=1001.2014.3001.5502例如：
rclcpp::init(argc, argv);
rclcpp::spin(node);
rclcpp::shutdown();
rclcpp::Node
rclcpp::Publisher
rclcpp::Subscription
rclcpp::TimerBase
这些内容主要解决的是：
第二部分又补充了 ROS2 C++ 基础语法，链接如下：
ROS2 C++ 基础语法保姆级教程-CSDN博客https://blog.csdn.net/m0_58954356/article/details/162235498?sharetype=blogdetail&sharerId=162235498&sharerefer=PC&sharesource=m0_58954356&spm=1011.2480.3001.8118例如：
auto
const
this
->
.
class
public
private
SharedPtr
std::make_shared
std::bind
lambda
这些内容主要解决的是：
第三部分又进一步整理了 ROS2 C++ 进阶写法，链接如下：
ROS2 C++ 进阶语法保姆级教程-CSDN博客https://blog.csdn.net/m0_58954356/article/details/162213658?spm=1001.2014.3001.5502例如：
rclcpp::Service
rclcpp::Client
rclcpp_action::Server
rclcpp_action::Client
rclcpp::Parameter
rclcpp::QoS
rclcpp::executors::MultiThreadedExecutor
rclcpp::CallbackGroup
tf2_ros::TransformBroadcaster
message_filters::Synchronizer
这些内容主要解决的是：
但是，如果继续深入到 AGV 底盘运动控制算法，只会写 ROS2 节点、发布者、订阅者、定时器还不够。
因为底盘控制代码中经常会出现：
运控算法相关 C++ 写法
├── class
├── struct
├── enum class
├── std::vector
├── std::array
├── std::clamp
├── std::sin / std::cos / std::atan2
├── rclcpp::Time
├── dt 计算
└── PID / 运动学模型封装
这些写法看起来还是 C++ 语法，但它们在底盘控制算法中都有非常明确的工程作用。
比如：
RobotState
├── 用来保存机器人当前状态
ChassisParam
├── 用来保存底盘参数
PidController
├── 用来封装 PID 控制算法
DiffDriveKinematics
├── 用来封装差速底盘运动学模型
rclcpp::Time + dt
├── 用来计算控制周期
std::clamp
├── 用来限制速度和控制输出
std::sin / std::cos / std::atan2
└── 用来更新位姿和处理角度
所以，本篇文章作为第四部分，重点讲解 ROS2 C++ 底盘运动控制算法中最常见的 C++ 写法。
本篇要解决的核心问题是：
理解完这些内容之后，再去看底盘控制、差速模型、麦克纳姆模型、里程计积分、PID 速度闭环控制时，就会清楚很多。
一、为什么底盘控制代码要做算法封装？
1.1 底盘控制不是简单发布 /cmd _vel
在前面的 Topic 通信中，我们已经知道小车速度控制常见话题是：
/cmd_vel
消息类型通常是：
geometry_msgs::msg::Twist
最简单的控制方式就是发布一个速度指令：
geometry_msgs::msg::Twist cmd;
cmd.linear.x = 0.2;
cmd.angular.z = 0.0;
pub_->publish(cmd);
这段代码可以让小车前进。
但是，真正的 AGV 底盘控制远远不只是发布 /cmd_vel。
完整一点的底盘控制流程通常是：
接收目标速度 /cmd_vel
↓
限制最大速度
↓
计算控制周期 dt
↓
运动学解算
↓
得到左右轮目标速度
↓
读取当前轮速反馈
↓
PID 速度闭环控制
↓
输出电机控制量
↓
更新里程计 odom
↓
发布 tf 坐标变换
也就是说，底盘控制里面既有 ROS2 通信，也有控制算法。
1.2 为什么不能所有代码都写在一个回调函数里？
有些初学者刚开始写代码时，可能会把所有逻辑都写在一个函数里：
void cmd_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
double v = msg->linear.x;
double w = msg->angular.z;
// 速度限幅
// 运动学计算
// PID 控制
// 电机输出
// 里程计更新
}
这样写虽然短期能跑，但是后面会越来越乱。
因为底盘控制里会有很多数据：
如果这些内容全部散落在一个函数里，代码会很难维护。
所以实际工程中更推荐封装成：
RobotState
├── 保存机器人状态
ChassisParam
├── 保存底盘参数
PidController
├── 封装 PID 控制算法
DiffDriveKinematics
├── 封装差速底盘运动学模型
ChassisController
└── 封装 ROS2 底盘控制节点
1.3 封装的核心作用
可以这样理解：
封装之前
├── 所有变量和算法混在一起
├── 函数越来越长
├── 很难复用
├── 很难调试
└── 很难扩展其他底盘模型
封装之后
├── 状态归状态
├── 参数归参数
├── PID 归 PID
├── 运动学归运动学
├── ROS2 节点归 ROS2 节点
└── 代码结构更清晰
所以，底盘控制算法中经常会看到 class、struct、std::array、std::clamp、rclcpp::Time，不是为了把代码写复杂，而是为了让控制系统更清晰、更稳定、更容易扩展。
二、class、struct、enum class 在底盘控制中的作用
2.1 class：适合封装有行为的算法模块
class 可以理解成：
class = 数据 + 函数
在底盘控制中，很多模块不只是保存数据，还要执行计算逻辑。
例如：
PID 控制器
├── 有 kp、ki、kd 参数
├── 有积分误差
├── 有上一次误差
└── 还要计算控制输出
运动学模型
├── 有轮子半径
├── 有轮距
└── 还要根据 v、w 计算轮速
这类模块就很适合用 class 封装。
例如：
class PidController
{
public:
double update(double target, double current, double dt)
{
double error = target – current;
return error;
}
};
这里的 PidController 就是一个 PID 控制器类。
它不是单纯保存数据，而是有 update() 这个计算函数。
所以可以简单记住：
class
├── 适合封装算法
├── 适合封装控制器
├── 适合封装运动学模型
└── 适合封装 ROS2 节点
2.2 struct：适合保存状态和参数
struct 更适合保存一组相关数据。
例如机器人状态可以写成：
struct RobotState
{
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
double linear_velocity = 0.0;
double angular_velocity = 0.0;
};
它表示：
RobotState
├── x：机器人当前位置 x
├── y：机器人当前位置 y
├── yaw：机器人当前航向角
├── linear_velocity：当前线速度
└── angular_velocity：当前角速度
再比如底盘参数：
struct ChassisParam
{
double wheel_radius = 0.1;
double wheel_base = 0.5;
double max_linear_speed = 1.0;
double max_angular_speed = 1.0;
};
它表示：
ChassisParam
├── wheel_radius：轮子半径
├── wheel_base：左右轮之间的距离
├── max_linear_speed：最大线速度
└── max_angular_speed：最大角速度
为什么不直接写很多变量？
比如：
double x;
double y;
double yaw;
double wheel_radius;
double wheel_base;
这样当然也能写，但是变量一多就容易混乱。
封装成 struct 后，代码含义更清楚：
RobotState state;
ChassisParam param;
state.x = 1.0;
param.wheel_radius = 0.1;
一眼就能看出：
state.x
├── 是机器人状态里的 x
param.wheel_radius
└── 是底盘参数里的轮子半径
2.3 enum class：适合表示底盘控制模式
底盘控制中经常需要表示机器人当前处于什么模式。
例如：
如果用普通数字表示：
int mode = 2;
就不够清晰。
因为看到 2 时，不知道它到底表示自动模式、手动模式，还是故障模式。
所以可以用 enum class：枚举类
给一组固定状态起名字。
enum class MotionMode
{
STOP,
MANUAL,
AUTO,
EMERGENCY,
ERROR
};
小车运动模式 MotionMode
├── STOP 停止
├── MANUAL 手动
├── AUTO 自动
├── EMERGENCY 急停
└── ERROR 故障
使用时：
MotionMode mode = MotionMode::AUTO;
if (mode == MotionMode::AUTO)
{
// 执行自动控制逻辑
}
这样代码可读性更好。
可以理解成：
enum class
├── 用来表示有限的几种状态
├── 比普通 int 更清晰
├── 比普通 enum 更安全
└── 很适合表示控制模式、任务状态、故障状态
在 AGV 底盘控制中，常见用法包括：
MotionMode
├── 运动模式
ControlMode
├── 控制模式
ErrorState
├── 故障状态
TaskState
└── 任务状态
三、std::vector、std::array、std::clamp 常见写法
3.1 std::vector：适合保存数量不固定的数据
std::vector 是 C++ 中的动态数组。
需要引入头文件：
#include <vector>
它的特点是：
例如路径点数量通常是不固定的。
可以定义一个路径点结构体：
struct PathPoint
{
double x = 0.0;
double y = 0.0;
};
然后用 std::vector 保存路径：
std::vector<PathPoint> path_points;
添加路径点：
path_points.push_back({0.0, 0.0});
path_points.push_back({1.0, 0.0});
path_points.push_back({2.0, 0.5});
在 AGV 中，std::vector 常用于：
比如路径跟踪算法中，机器人可能需要沿着多个点行驶：
P0 → P1 → P2 → P3 → P4
这些点的数量不固定，所以适合用 std::vector 保存。
3.2 std::array：适合保存数量固定的数据
std::array 是固定长度数组。
需要引入头文件：
#include <array>
它的特点是：
比如差速底盘有两个轮子：
左轮
右轮
可以写成：
std::array<double, 2> wheel_speed = {0.0, 0.0};
其中：
wheel_speed[0]
表示左轮速度。
wheel_speed[1]
表示右轮速度。
四轮底盘可以写成：
std::array<double, 4> wheel_speed = {0.0, 0.0, 0.0, 0.0};
在 AGV 底盘控制中，std::array 常用于：
简单区分：
std::vector
├── 数量不固定
└── 例如路径点、轨迹点
std::array
├── 数量固定
└── 例如左右轮、四个轮子、四个电机
3.3 std::clamp：速度限幅常用写法
std::clamp 用来把一个数限制在指定范围内。
需要引入头文件：
#include <algorithm>
基本写法：
value = std::clamp(value, min_value, max_value);
例如限制线速度：
double linear_x = 1.5;
linear_x = std::clamp(linear_x, -1.0, 1.0);
在底盘控制中，速度限幅非常重要：
cmd.linear.x = std::clamp(
cmd.linear.x,
-param_.max_linear_speed,
param_.max_linear_speed
);
cmd.angular.z = std::clamp(
cmd.angular.z,
-param_.max_angular_speed,
param_.max_angular_speed
);
这样可以防止速度指令过大。
它的作用是：
PID 输出也经常需要限幅：
output = std::clamp(output, output_min_, output_max_);
因为控制器输出过大，可能会导致电机突然加速，影响稳定性。
四、std::sin、std::cos、std::atan2 与机器人位姿计算
4.1 为什么底盘算法需要三角函数？
机器人在平面中运动时，通常用三个量表示位姿：
x
├── 机器人在世界坐标系下的 x 坐标
y
├── 机器人在世界坐标系下的 y 坐标
yaw
└── 机器人当前朝向角
如果机器人当前线速度是 v，角速度是 w，控制周期是 dt，那么可以用下面方式更新位姿：
state.x += v * std::cos(state.yaw) * dt;
state.y += v * std::sin(state.yaw) * dt;
state.yaw += w * dt;
可以理解成：
cos(yaw)
├── 表示机器人朝向在 x 方向上的分量
sin(yaw)
├── 表示机器人朝向在 y 方向上的分量
dt
└── 表示这次运动持续了多久
也就是说：
机器人朝哪个方向
↓
就沿着哪个方向更新 x、y 坐标
4.2 std::sin 和 std::cos 的使用
std::sin() 和 std::cos() 来自头文件：
#include <cmath>
注意：
std::sin()
std::cos()
里面传入的是弧度，不是角度。
例如：
0 度
├── 0 rad
90 度
├── 1.57 rad
180 度
├── 3.14 rad
360 度
└── 6.28 rad
在 ROS2 机器人代码中，yaw 一般也是弧度。
所以常见写法是：
double dx = v * std::cos(yaw) * dt;
double dy = v * std::sin(yaw) * dt;
然后更新状态：
state.x += dx;
state.y += dy;
4.3 std::atan2：计算目标方向角
std::atan2(y, x) 常用于根据两个方向差值计算角度。
例如机器人当前位置是：
state.x = 0.0
state.y = 0.0
目标点是：
target.x = 2.0
target.y = 1.0
那么目标方向角可以这样计算：
double dx = target.x – state.x;
double dy = target.y – state.y;
double target_yaw = std::atan2(dy, dx);
这段代码的意思是：
根据当前位置和目标点位置，计算机器人应该朝向哪个方向
在 AGV 中，std::atan2 常用于：
4.4 使用 atan2 做角度归一化
机器人控制中，角度可能会不断累加。
比如：
state.yaw += w * dt;
如果一直转圈，yaw 可能会变得很大。
但是通常我们希望角度保持在：
-π 到 π
常见写法是：
double normalize_angle(double angle)
{
return std::atan2(std::sin(angle), std::cos(angle));
}
使用时：
state.yaw = normalize_angle(state.yaw);
航向角误差也可以这样处理：
double yaw_error = normalize_angle(target_yaw – state.yaw);
这样可以避免角度跳变。
例如：
目标角度接近 +π
当前角度接近 -π
如果不做归一化，计算出来的误差可能会非常大，导致机器人突然大幅转向。
五、rclcpp::Time 与 dt 控制周期计算
5.1 rclcpp::Time 是什么？
rclcpp::Time 是 ROS2 C++ 中表示时间的类型。
在 ROS2 节点中，经常写：
rclcpp::Time now = this->now();
意思是：
获取当前 ROS2 时间
在底盘控制中，时间非常关键。
因为下面这些计算都离不开时间：
5.2 dt 是什么？
dt 是 delta time 的缩写。
可以理解成：
两次控制循环之间的时间间隔
例如：
控制器 100Hz 运行
├── dt 大约是 0.01 秒
控制器 50Hz 运行
├── dt 大约是 0.02 秒
控制器 20Hz 运行
└── dt 大约是 0.05 秒
在底盘控制中，dt 非常重要。
因为机器人位姿更新通常是：
state.x += v * std::cos(state.yaw) * dt;
state.y += v * std::sin(state.yaw) * dt;
state.yaw += w * dt;
PID 控制中也需要：
// P：比例项 当前误差有多大
// I：积分项 过去误差累计有多少
// D：微分项 误差变化得有多快
double error = target – current;
// P：比例项
double p_out = kp_ * error;
// I：积分项
integral_ += error * dt;
double i_out = ki_ * integral_;
// D：微分项
double derivative = (error – last_error_) / dt;
double d_out = kd_ * derivative;
// PID 总输出
double output = p_out + i_out + d_out;
// 保存当前误差，给下一次使用
last_error_ = error;
所以没有 dt，就很难正确计算运动过程。
5.3 ROS2 中 dt 的计算方式
常见写法如下：
rclcpp::Time now = this->now();
double dt = (now – last_time_).seconds();
last_time_ = now;
完整示例：
class ChassisController : public rclcpp::Node
{
public:
ChassisController() : Node("chassis_controller")
{
last_time_ = this->now(); // 表示上一次进入 control_loop() 时记录的时间
}
// 刚创建节点时，还没有进入过 control_loop()，所以必须先给它一个初始值。
// 节点刚启动时，先把当前时间作为初始的 last_time_
// 过了一小会儿，第一次进入 control_loop()： 时间做差
private:
void control_loop()
{
rclcpp::Time now = this->now();
double dt = (now – last_time_).seconds();
last_time_ = now;
if (dt <= 0.0)
{
return;
}
// 后面执行 PID、运动学、里程计更新
}
rclcpp::Time last_time_;
};
其中：
(now – last_time_).seconds()
表示：
当前时间 – 上一次时间 = 两次循环之间的时间间隔
.seconds() 表示把时间间隔转换成秒。
5.4 为什么 dt 要做保护？
如果 dt <= 0.0，PID 微分项会出问题：
double derivative = (error – last_error_) / dt;
（1）如果 dt 为 0，就会出现除以 0 的问题。
所以通常会写：
if (dt <= 0.0)
{
return;
}
（2）有时还会限制 dt 的范围：
dt = std::clamp(dt, 0.001, 0.1);
意思是：
这样可以防止程序卡顿、时间跳变导致控制输出异常。
六、PID 控制器封装
6.1 PID 在底盘控制中的作用
PID 是底盘控制中非常常见的控制算法。
它通常用于：
可以简单理解成：
例如：
目标轮速：1.0 m/s
当前轮速：0.7 m/s
误差：0.3 m/s
PID 输出：增加电机控制量
PID 包含三部分：
P：比例项
├── 当前误差越大，输出越大
I：积分项
├── 累计过去误差，用来消除长期偏差
D：微分项
└── 关注误差变化速度，用来抑制震荡
6.2 用 struct 保存 PID 参数
PID 参数可以用 struct 保存：
struct PidParam
{
double kp = 0.0;
double ki = 0.0;
double kd = 0.0;
double output_min = -1.0;
double output_max = 1.0;
};
这里：
kp
├── 比例系数
ki
├── 积分系数
kd
├── 微分系数
output_min
├── 最小输出
output_max
└── 最大输出
这样写的好处是：
例如：
PidParam left_pid_param;
left_pid_param.kp = 1.0;
left_pid_param.ki = 0.1;
left_pid_param.kd = 0.01;
6.3 用 class 封装 PID 控制器
PID 控制器不仅有参数，还需要保存积分误差和上一次误差。
所以更适合用 class 封装：
#include <algorithm>
// std::clamp() 在 <algorithm> 头文件里。限幅作用
class PidController
{
public:
// explicit 禁止隐式转换
explicit PidController(const PidParam & param) : param_(param)
// 创建 PidController 对象时，必须传入一个 PidParam 参数对象，并把它保存到类成员 param_ 里。
// 传入 PID 参数，但不修改它，并且用引用避免拷贝
// 成员初始化列表，用传进来的 param 初始化成员变量 param_ 等价于：param_ = param;
{
}
double update(double target, double current, double dt)
{
if (dt <= 0.0)
{
return 0.0;
}
double error = target – current;
integral_ += error * dt;
double derivative = (error – last_error_) / dt;
double output =
param_.kp * error +
param_.ki * integral_ +
param_.kd * derivative;
output = std::clamp(output, param_.output_min, param_.output_max);
last_error_ = error;
return output;
}
void reset()
// 清空 PID 的历史状态。
{
integral_ = 0.0;
last_error_ = 0.0;
}
private:
// 三个成员变量
PidParam param_;
double integral_ = 0.0;
double last_error_ = 0.0;
};
这段代码可以拆开理解：
param_
├── 保存 PID 参数
integral_
├── 保存积分误差
last_error_
├── 保存上一次误差
update()
├── 根据目标值、当前值和 dt 计算控制输出
reset()
└── 重置 PID 状态
6.4 PID 在底盘控制中的使用
假设目标轮速是：
double target_left_speed = 1.0;
当前轮速是：
double current_left_speed = 0.7;
那么 PID 输出可以这样计算：
double left_output = left_pid_.update(
target_left_speed,
current_left_speed,
dt
);
如果是差速底盘，通常左右轮各有一个 PID：
PidController left_pid_;
PidController right_pid_;
控制流程大概是：
目标线速度 v、角速度 w
↓
运动学模型计算左右轮目标速度
↓
读取左右轮当前速度
↓
左右轮 PID 分别计算控制输出
↓
输出给电机驱动
所以 PID 封装的意义是：
6.5 完整 ROS2 C++ PID 写法
- PidParam 参数结构体
- PidController PID 控制器类
- ChassisController ROS2 底盘控制节点
- /odom 订阅当前速度
- /cmd_vel 发布控制速度
#include <algorithm>
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
using namespace std::chrono_literals;
// 一、PID 参数结构体
struct PidParam
{
double kp = 0.0; // 比例系数
double ki = 0.0; // 积分系数
double kd = 0.0; // 微分系数
double output_min = -1.0; // 输出最小值
double output_max = 1.0; // 输出最大值
double integral_min = -1.0; // 积分最小值，防止积分过大
double integral_max = 1.0; // 积分最大值，防止积分过大
};
// 二、PID 控制器类
class PidController
{
public:
explicit PidController(const PidParam & param)
: param_(param)
{
}
double update(double target, double current, double dt)
{
if (dt <= 0.0)
{
return 0.0;
}
// 1. 计算误差
double error = target – current;
// 2. 比例项 P
double p_out = param_.kp * error;
// 3. 积分项 I
integral_ += error * dt;
// 积分限幅，防止积分无限累加
integral_ = std::clamp(
integral_,
param_.integral_min,
param_.integral_max
);
double i_out = param_.ki * integral_;
// 4. 微分项 D
double derivative = 0.0;
if (!first_update_)
{
derivative = (error – last_error_) / dt;
}
else
{
first_update_ = false;
}
double d_out = param_.kd * derivative;
// 5. PID 总输出
double output = p_out + i_out + d_out;
// 6. 输出限幅
output = std::clamp(
output,
param_.output_min,
param_.output_max
);
// 7. 保存当前误差，给下一次计算微分用
last_error_ = error;
return output;
}
void reset()
{
integral_ = 0.0;
last_error_ = 0.0;
first_update_ = true;
}
private:
PidParam param_;
double integral_ = 0.0;
double last_error_ = 0.0;
bool first_update_ = true;
};
// 三、底盘控制节点
class ChassisController : public rclcpp::Node
{
public:
ChassisController()
: Node("chassis_controller"),
speed_pid_(PidParam{
1.0, // kp
0.1, // ki
0.01, // kd
-0.5, // output_min
0.5, // output_max
-1.0, // integral_min
1.0 // integral_max
})
{
// 初始化时间
last_time_ = this->now();
// 创建速度发布者，向 /cmd_vel 发布速度指令
cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/cmd_vel",
10
);
// 订阅 /odom，获取当前小车速度
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom",
10,
std::bind(
&ChassisController::odom_callback,
this,
std::placeholders::_1
)
);
// 创建定时器，每 20ms 执行一次控制循环
timer_ = this->create_wall_timer(
20ms,
std::bind(&ChassisController::control_loop, this)
);
RCLCPP_INFO(this->get_logger(), "chassis_controller node has started.");
}
private:
// 里程计回调函数，获取当前速度
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
current_speed_ = msg->twist.twist.linear.x;
}
// 控制循环
void control_loop()
{
// 1. 计算 dt
rclcpp::Time now = this->now();
double dt = (now – last_time_).seconds();
last_time_ = now;
if (dt <= 0.0)
{
return;
}
// 2. 设置目标速度
double target_speed = 0.3; // 目标速度 0.3 m/s
// 3. PID 计算输出
double output_speed = speed_pid_.update(
target_speed,
current_speed_,
dt
);
// 4. 生成 Twist 速度消息
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = output_speed;
cmd_vel.linear.y = 0.0;
cmd_vel.linear.z = 0.0;
cmd_vel.angular.x = 0.0;
cmd_vel.angular.y = 0.0;
cmd_vel.angular.z = 0.0;
// 5. 发布速度指令
cmd_pub_->publish(cmd_vel);
RCLCPP_INFO(
this->get_logger(),
"target: %.2f, current: %.2f, output: %.2f, dt: %.3f",
target_speed,
current_speed_,
output_speed,
dt
);
}
private:
// 发布 /cmd_vel
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
// 订阅 /odom
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// 定时器
rclcpp::TimerBase::SharedPtr timer_;
// PID 控制器
PidController speed_pid_;
// 时间记录
rclcpp::Time last_time_;
// 当前速度，由 /odom 回调更新
double current_speed_ = 0.0;
};
// 四、main 函数
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ChassisController>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
七、运动学模型封装
7.1 什么是底盘运动学模型？
运动学模型可以理解成：
机器人整体速度 和 轮子速度 之间的转换关系
在 ROS2 中，上层通常发送的是：
线速度 v
角速度 w
也就是 /cmd_vel 里的：
cmd.linear.x
cmd.angular.z
但是底盘电机真正需要控制的是：
左轮速度
右轮速度
所以需要一个运动学模型，把机器人整体速度转换成轮子速度。
这个过程叫：
逆运动学解算
可以理解成：
机器人速度 v、w
↓
运动学模型
↓
左右轮目标速度
7.2 差速底盘运动学模型
差速底盘最常见的参数是：
wheel_radius
├── 轮子半径
wheel_base
└── 左右轮之间的距离
输入：
v
├── 机器人线速度
w
└── 机器人角速度
输出：
left_wheel_speed
├── 左轮角速度
right_wheel_speed
└── 右轮角速度
常见公式是：
- 左轮角速度 = (v – w * wheel_base / 2) / wheel_radius
- 右轮角速度 = (v + w * wheel_base / 2) / wheel_radius
对应 C++ 写法：
#include <array>
class DiffDriveKinematics
{
public:
DiffDriveKinematics(double wheel_radius, double wheel_base)
: wheel_radius_(wheel_radius),
wheel_base_(wheel_base)
{
}
std::array<double, 2> inverse(double v, double w)
{
double left_wheel_speed =
(v – w * wheel_base_ / 2.0) / wheel_radius_;
double right_wheel_speed =
(v + w * wheel_base_ / 2.0) / wheel_radius_;
return {left_wheel_speed, right_wheel_speed};
}
private:
double wheel_radius_;
double wheel_base_;
};
这里：
std::array<double, 2>
表示返回两个轮子的速度。
wheel_speed[0]
├── 左轮速度
wheel_speed[1]
└── 右轮速度
7.3 结合 ChassisParam 封装运动学模型
为了让参数更清晰，可以直接使用前面定义的 ChassisParam：
class ChassisModel
{
public:
explicit ChassisModel(const ChassisParam & param)
: param_(param)
{
}
std::array<double, 2> calculate_wheel_speed(double v, double w)
{
v = std::clamp(
v,
-param_.max_linear_speed,
param_.max_linear_speed
);
w = std::clamp(
w,
-param_.max_angular_speed,
param_.max_angular_speed
);
double left =
(v – w * param_.wheel_base / 2.0) / param_.wheel_radius;
double right =
(v + w * param_.wheel_base / 2.0) / param_.wheel_radius;
return {left, right};
}
private:
ChassisParam param_;
};
这段代码里面包含了：
ChassisParam
├── 保存底盘参数
std::clamp
├── 限制线速度和角速度
std::array
├── 返回左右轮速度
calculate_wheel_speed()
└── 完成差速底盘逆运动学解算
这样封装后，ROS2 节点里只需要调用：
auto wheel_speed = chassis_model_.calculate_wheel_speed(v, w);
double left_target = wheel_speed[0];
double right_target = wheel_speed[1];
代码会非常清晰。
7.4 里程计位姿更新封装
底盘控制中除了把速度转换成轮速，还经常需要根据速度估算机器人位姿。
可以封装一个里程计更新函数：
#include <cmath>
double normalize_angle(double angle)
{
return std::atan2(std::sin(angle), std::cos(angle));
}
void update_odometry(RobotState & state, double v, double w, double dt)
{
if (dt <= 0.0)
{
return;
}
state.x += v * std::cos(state.yaw) * dt;
state.y += v * std::sin(state.yaw) * dt;
state.yaw += w * dt;
state.yaw = normalize_angle(state.yaw);
state.linear_velocity = v;
state.angular_velocity = w;
}
这里：
RobotState
├── 保存机器人 x、y、yaw
v
├── 当前线速度
w
├── 当前角速度
dt
├── 控制周期
std::cos / std::sin
├── 根据 yaw 更新 x、y
std::atan2
└── 对 yaw 做角度归一化
7.5 一个完整的底盘控制算法关系
到这里，可以把前面的内容串起来：
/cmd_vel
↓
获取目标 v、w
↓
std::clamp 速度限幅
↓
ChassisModel 运动学解算
↓
得到左右轮目标速度
↓
PidController 计算左右轮控制输出
↓
输出给电机
↓
rclcpp::Time 计算 dt
↓
update_odometry 更新 RobotState
↓
发布 /odom 和 tf
对应的代码关系大概是：
struct RobotState
{
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
double linear_velocity = 0.0;
double angular_velocity = 0.0;
};
struct ChassisParam
{
double wheel_radius = 0.1;
double wheel_base = 0.5;
double max_linear_speed = 1.0;
double max_angular_speed = 1.0;
};
enum class MotionMode
{
STOP,
MANUAL,
AUTO,
EMERGENCY,
ERROR
};
然后：
RobotState
├── 保存机器人状态
ChassisParam
├── 保存底盘参数
MotionMode
├── 保存当前运动模式
PidController
├── 封装 PID 控制算法
ChassisModel
└── 封装底盘运动学模型
这就是底盘控制代码里面为什么要封装这些内容的原因。
八、总结
本篇文章作为 ROS2 C++ 系列的第四部分，主要从 AGV 底盘运动控制算法的角度，整理了常见 C++ 写法在实际机器人控制代码中的作用。
前面几篇主要解决的是：
第一部分
├── 看懂 rclcpp 基础写法
第二部分
├── ROS2 C++ 的基础语法
第三部分
├── 补齐 ROS2 C++ 进阶模块框架
而本篇开始进一步进入：
AGV 底盘运动控制算法常见写法
其中：
class
├── 适合封装 PID、运动学模型、控制节点等算法模块
struct
├── 适合保存机器人状态、底盘参数、速度指令等数据
enum class
├── 适合表示运动模式、控制模式、故障状态
std::vector
├── 适合保存路径点、轨迹点、历史数据等数量不固定的数据
std::array
├── 适合保存左右轮速度、四轮速度等数量固定的数据
std::clamp
├── 常用于速度限幅和控制输出限幅
std::sin / std::cos
├── 常用于根据 yaw 更新 x、y 位置
std::atan2
├── 常用于计算目标方向角和角度归一化
rclcpp::Time
├── 用于获取 ROS2 当前时间
dt
├── 表示两次控制循环之间的时间间隔
PID
├── 用于速度闭环、角速度闭环、轮速控制
运动学模型
└── 用于把机器人整体速度转换成轮子速度
所以，底盘控制代码里面之所以经常封装：
RobotState
ChassisParam
PidController
DiffDriveKinematics
ChassisModel
不是为了把代码写复杂，而是因为底盘控制本身就包含状态、参数、控制器、运动学和时间计算。
把这些内容合理封装之后，代码会更加清晰：
这样后面继续扩展：
都会更加自然。
也就是说，本篇文章是从 ROS2 C++ 基础代码进入 AGV 底盘运动控制算法的过渡篇。
理解了本篇内容，后面再去写完整的底盘控制节点，就不只是知道代码怎么写，而是能真正理解：
这才是从 ROS2 C++ 入门，真正走向 AGV 底盘运动控制算法开发的关键一步。
