[ROS 系列学习教程] 建模与仿真 - 使用 ros_control 控制差速轮式机器人

ROS 系列学习教程(总目录)

ros_control 提供了多种控制器，其中 diff_drive_controller 用于控制差速驱动轮式机器人。

一、差速轮式机器人

差速轮式机器人是一种移动机器人，其运动基于机器人身体两侧的两个独立驱动轮。因此，它可以通过改变轮子的相对旋转速度来改变方向，不需要额外的转向运动。具有这种驱动器的机器人通常有一个或多个脚轮，以防止车辆倾斜。

如果两个轮子以相同的方向和速度驱动，机器人将沿直线行驶。如果两个轮子以相同的速度朝相反的方向转动，如所示图所示，机器人将绕轴的中心点旋转。否则，根据旋转速度和方向，旋转中心可能落在由两个轮胎接触点定义的线上的任何位置。当机器人沿直线行驶时，旋转中心距离机器人无限远。由于机器人的方向取决于两个驱动轮的旋转速度和方向，因此应精确感测和控制这些量。

差速转向机器人与汽车中使用的差速 齿轮类似，两个车轮可以有不同的转速，但与差速齿轮系统不同，差速转向系统将为两个车轮提供动力。差速轮式机器人在机器人技术中得到广泛应用，因为它们的运动易于编程并且可以很好地控制。当今市场上几乎所有的消费机器人都使用差速转向，主要是因为它成本低且简单。

二、差速驱动机器人运动学模型

如下图为轮式机器人的差速驱动运动学模型示意图：

其中，
$$\begin{gathered} V-机器人线速度 \\ \omega -机器人角速度 \\ XOY-世界坐标系 \\ {X}_B{Y}_B-机器人坐标系 \\ \phi -机器人在世界坐标系的角度 \\ r-车轮半径 \\ b-轮距 \\ ICR-瞬时旋转中心 \\ R-瞬心到机器人中心的距离 \\ {v}_L,v_R-左右轮接地点切向线速度 \\ {\omega }_L,{\omega }_R-左右轮角速度 \end{gathered}$$

有如下关系：
$$\begin{gathered} \omega \cdot (R+b/2)=v_R \\ \omega \cdot (R-b/2)=v_L \end{gathered}$$
解这两个方程可得 $\omega$ 和 $R$ ：
$$\begin{gathered} \omega =(v_R-v_L)/b \\ R=b/2\cdot (v_R+v_L)/(v_R-v_L) \end{gathered}$$
利用角速度方程可得机器人瞬时速度 $V$ ：
$$V=\omega \cdot R=(v_R+v_L)/2$$
车轮切向速度也可以写成：
$$\begin{gathered} v_R=r\cdot {\omega }_R \\ {v}_L=r\cdot {\omega }_L \end{gathered}$$
则机器人在本体坐标系中的运动学模型为：
$$\left[\begin{array}{c}{\dot{x}}_B\\ {\dot{y}}_B\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}v\cdot x_B\\ v\cdot y_B\\ \omega \end{array}\right]\stackrel{v=r\omega }{\overbrace{=}}\left[\begin{array}{cc}\frac{r}{2}& \frac{r}{2}\\ 0& 0\\ -\frac{r}{b}& \frac{r}{b}\end{array}\right]\left[\begin{array}{c}{\omega }_L\\ {\omega }_R\end{array}\right]$$
再通过坐标变换，最终可以得到机器人在世界坐标中的运动学模型：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{cc}\cos \phi & 0\\ \sin \phi & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}V\\ \omega \end{array}\right]$$
其中，$V$ 和 $\omega$ 为控制变量。

通常我们需要通过机器人的速度和结构参数逆解出左右轮的转速，用于控制电机。在这种情况下，可以很容易地重新表述前面提到的方程。使用如下方程：
$$\begin{gathered} R=V/\omega \\ {\omega }_R=v_R/r \\ {\omega }_L=v_L/r \end{gathered}$$
可得左右轮角速度方程：
$$\begin{gathered} {\omega }_R=\frac{V+\omega \cdot b/2}{r} \\ {\omega }_L=\frac{V-\omega \cdot b/2}{r} \end{gathered}$$

三、对外接口

diff_drive_controller 主要通过订阅速度命令作为模块的输入，然后解析运动学模型控制电机，达到控制机器人的目的。

3.1 输入接口

• cmd_vel（geometry_msgs/Twist）

  位于控制器的命名空间下，给机器人发布速度

3.2 输出接口

• odom（nav_msgs/Odometry）

  位于控制器的命名空间下，根据硬件反馈计算的里程计信息

• /tf（tf/tfMessage）

  从 odom 转换为 base_link

• cmd_vel_out（geometry_msgs/TwistStamped）

  当 publish_cmd 参数设置为 True 时可用。在控制器的输入上应用限制器后的 Twist。

四、控制器参数

diff_drive_controller 提供了一些参数，用于配置机器人控制。

参数	数据类型	说明
left_wheel	string /string[…]	左轮关节名称或关节名称列表
right_wheel	string /string[…]	右轮关节名称或关节名称列表
pose_covariance_diagonal	double[6]	用于里程计位姿发布的协方差矩阵的对角线
twist_covariance_diagonal	double[6]	用于里程计 twist 发布的协方差矩阵的对角线
publish_rate	double	发布里程计的频率，用于 tf 和 odom（单位：Hz，默认值： 50.0）
wheel_separation	double	轮距，左轮和右轮之间的距离。如果未指定此参数，diff_drive_controller 将尝试从 URDF 读取值
wheel_separation_multiplier	double	轮距参数的系数。用于解释机器人模型和真实机器人之间的差异。（默认值：1.0）
wheel_radius	double	车轮半径。默认两侧车轮都具有相同的尺寸。如果未指定此参数，diff_drive_controller 将尝试从 URDF 读取值。
wheel_radius_multiplier	double	车轮半径参数的系数。用于解释机器人模型和真实机器人之间的差异。（默认值：1.0）
cmd_vel_timeout	double	两个连续速度命令之间允许的时间间隔。此延迟后，将向车轮发送零速命令。（单位：s，默认值：0.5）
base_frame_id	string	用于填充Odometry消息和TF的child_frame_id（默认值：“base_link”）
linear	object	线性速度配置参数
+ x	object	x轴，两轮差速机器人线速度只有x轴
++ has_velocity_limits	bool	控制器是否限制线速度。（默认值： false）
++ max_velocity	double	最大线速度（单位：m/s）
++ min_velocity	double	最小线速度（单位：m/s）。未指定时，使用max_velocity
++ has_acceleration_limits	bool	控制器是否限制线加速度。（默认值： false）
++ max_acceleration	double	最大线加速度（单位：m/s^2）
++ min_acceleration	double	最小线加速度（单位：m/s^2）。未指定时，使用max_acceleration
++ has_jerk_limits	bool	控制器是否限制线加速度的变化快慢（默认值： false）
++ max_jerk	double	最大 jerk（单位：m/s^3）
angular	object	角速度配置参数
+ z	object	z轴，两轮差速机器人角速度只有z轴
++ has_velocity_limits	bool	控制器是否应该限制角速度（默认值： false）
++ max_velocity	double	最大角速度（单位：rad/s）
++ min_velocity	double	最小角速度（单位：rad/s）。将其设置为 0.0 将禁用逆时针旋转。未指定时，将使用max_velocity
++ has_acceleration_limits	bool	控制器是否应该限制角加速度（默认值： false）
++ max_acceleration	double	最大角加速度（单位：rad/s^2）
++ min_acceleration	double	最小角加速度（单位为 rad/s^2）。未指定时，使用max_acceleration。
++ has_jerk_limits	bool	控制器是否限制角加速度的变化快慢（默认值： false）
++ max_jerk	double	最大 jerk（单位：rad/s^3）
enable_odom_tf	bool	是否直接发布到 TF（默认值： true ）
odom_frame_id	string	里程计的frame_id（默认值：“/odom”）
publish_cmd	bool	发布要执行的速度命令。用于监控限制器对控制器输入的影响。（默认值： False）
allow_multiple_cmd_vel_publishers	bool	将其设置为 true 将允许输入接口 ~/cmd_vel 有多个发布者。如果将其设置为 false，则如果 ~/cmd_vel 有多个发布者，则会导致控制器停止运行。（默认值： False）
velocity_rolling_window_size	int	用于计算里程计 twist.linear.x 和 twist.angular.z 速度的平均速度样本数量（默认值： 10）

五、配置控制器参数

最小配置示例（即必要配置项）：

diff_drive_controller:type: "diff_drive_controller/DiffDriveController"left_wheel: "left_wheel_joint"right_wheel: "right_wheel_joint"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]

该差速轮式机器人完整配置：

# 用于控制器硬件接口配置
hardware_interface:joints:- left_wheel_joint- right_wheel_joint- front_caster_joint- back_caster_joint# joint_state_controller 控制器，用于发布各关节状态
joint_state_controller:type: "joint_state_controller/JointStateController"publish_rate: 50# diff_drive_controller 控制器
diff_drive_controller:type: "diff_drive_controller/DiffDriveController"left_wheel: "left_wheel_joint"right_wheel: "right_wheel_joint"publish_rate: 50pose_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]cmd_vel_timeout: 100velocity_rolling_window_size: 1publish_cmd: truebase_frame_id: base_linkenable_odom_tf: trueodom_frame_id: odom# 轮间距和轮半径wheel_separation: 0.38wheel_radius: 0.06wheel_separation_multiplier: 1.0wheel_radius_multiplier: 1.0# 速度和加速度限制linear:x:has_velocity_limits: truemax_velocity: 1.0 # m/shas_acceleration_limits: truemax_acceleration: 3.0 # m/s^2angular:z:has_velocity_limits: truemax_velocity: 2.0 # rad/shas_acceleration_limits: truemax_acceleration: 6.0 # rad/s^2

六、编写硬件抽象接口

下面写一个两轮差速硬件接口，使用速度控制接口 VelocityJointInterface 控制 joint 的速度，使用 JointStateInterface 获取 joint 的位置、速度、力等信息。

硬件抽象接口头文件：diff_drive_hardware_interface.h

#ifndef DIFF_DRIVE_HARDWARE_INTERFACE_H
#define DIFF_DRIVE_HARDWARE_INTERFACE_H#include <ros/ros.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <controller_manager/controller_manager.h>class DiffDriveHWInterface : public hardware_interface::RobotHW
{
public:struct JointInfo{std::string name;double cmd;double pos;double vel;double eff;JointInfo() : name(""), cmd(0.0), pos(0.0), vel(0.0), eff(0.0){}JointInfo(std::string name_) : name(name_), cmd(0.0), pos(0.0), vel(0.0), eff(0.0){}JointInfo(std::string name_, double cmd_, double pos_, double vel_, double dff_) : name(name_), cmd(cmd_), pos(pos_), vel(vel_), eff(dff_){}};public:DiffDriveHWInterface(ros::NodeHandle &nh);void init();void read(const ros::Duration &period);void write(const ros::Duration &period);private:ros::NodeHandle m_nh;hardware_interface::JointStateInterface m_jnt_state_interface;hardware_interface::VelocityJointInterface m_jnt_vel_interface;std::vector<JointInfo> m_joints;
};#endif // DIFF_DRIVE_HARDWARE_INTERFACE_H

源文件：diff_drive_hardware_interface.cpp

#include "diff_drive_control/diff_drive_hardware_interface.h"DiffDriveHWInterface::DiffDriveHWInterface(ros::NodeHandle &nh) : m_nh(nh)
{
}/*** @brief 初始化关节信息*        注册抽象硬件接口* */
void DiffDriveHWInterface::init()
{std::vector<std::string> joint_names;m_nh.getParam("/hardware_interface/joints", joint_names);for (std::string name : joint_names){m_joints.push_back(JointInfo(name));}for (auto &joint : m_joints){ROS_INFO("get joint: %s", joint.name.c_str());// Initialize hardware interfacehardware_interface::JointStateHandle state_handle(joint.name, &joint.pos, &joint.vel, &joint.eff);m_jnt_state_interface.registerHandle(state_handle);hardware_interface::JointHandle vel_handle(m_jnt_state_interface.getHandle(joint.name), &joint.cmd);m_jnt_vel_interface.registerHandle(vel_handle);}registerInterface(&m_jnt_state_interface);registerInterface(&m_jnt_vel_interface);
}void DiffDriveHWInterface::read(const ros::Duration &period)
{// Read the state of the hardware (e.g., from sensors)
}void DiffDriveHWInterface::write(const ros::Duration &period)
{// Send the command to the hardware (e.g., to actuators)for (auto &joint : m_joints){joint.pos += joint.vel * period.toSec();// if (joint.vel != joint.cmd)// {//     ROS_INFO("write, joint: %s, cmd: %lf", joint.name.c_str(), joint.cmd);// }joint.vel = joint.cmd;}
}

控制节点：diff_drive_control_node.cpp

#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include "diff_drive_control/diff_drive_hardware_interface.h"int main(int argc, char **argv)
{ros::init(argc, argv, "diff_drive_control_node");ros::NodeHandle nh;DiffDriveHWInterface diff_drive(nh);diff_drive.init();controller_manager::ControllerManager cm(&diff_drive, nh);ros::Rate rate(50.0);ros::AsyncSpinner spinner(1);spinner.start();while (ros::ok()){ros::Duration period = rate.expectedCycleTime();diff_drive.write(period);cm.update(ros::Time::now(), period);rate.sleep();}return 0;
}

七、控制机器人移动

机器人模型与硬件接口都准备好了，接下来开始编写业务代码控制机器人。

我们仅给机器人发送速度指令，模拟机器人移动任务，如下：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>geometry_msgs::Twist moveRobot(const double& linear, const double& angular)
{geometry_msgs::Twist msg;msg.linear.x = linear; // 线速度msg.linear.y = 0.0;msg.linear.z = 0.0;msg.angular.x = 0.0;msg.angular.y = 0.0;msg.angular.z = angular; // 角速度ROS_INFO("moveRobot, linear: %.3lf, angular: %.1lf", linear, angular*180/M_PI);return msg;
}int main(int argc, char** argv)
{ros::init(argc, argv, "diff_drive_business");ros::NodeHandle nh;ros::Publisher velPub = nh.advertise<geometry_msgs::Twist>("/diff_drive_controller/cmd_vel", 10);ros::Rate rate(10);while (ros::ok()){velPub.publish(moveRobot(0.5, 0));ros::Duration(3.0).sleep();velPub.publish(moveRobot(0, 1.57));ros::Duration(1.0).sleep();rate.sleep();}return 0;
}

编译执行该节点，在rviz中的可视化结果如下：

八、源码

项目源码