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

本文目录

一、差速轮式机器人
二、差速驱动机器人运动学模型
三、对外接口
- [3.1 输入接口](#3.1 输入接口)
- [3.2 输出接口](#3.2 输出接口)
四、控制器参数
五、配置控制器参数
六、编写硬件抽象接口
七、控制机器人移动
八、源码

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

一、差速轮式机器人

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

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

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

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

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

其中，
V − 机器人线速度 ω − 机器人角速度 X O Y − 世界坐标系 X B Y B − 机器人坐标系 φ − 机器人在世界坐标系的角度 r − 车轮半径 b − 轮距 I C R − 瞬时旋转中心 R − 瞬心到机器人中心的距离 v L , v R − 左右轮接地点切向线速度 ω L , ω R − 左右轮角速度 V - 机器人线速度\\ \omega - 机器人角速度\\ XOY - 世界坐标系\\ X_BY_B - 机器人坐标系\\ \varphi - 机器人在世界坐标系的角度\\ r - 车轮半径\\ b - 轮距\\ ICR - 瞬时旋转中心\\ R - 瞬心到机器人中心的距离\\ v_L,v_R - 左右轮接地点切向线速度\\ \omega_L,\omega_R - 左右轮角速度 V−机器人线速度ω−机器人角速度XOY−世界坐标系XBYB−机器人坐标系φ−机器人在世界坐标系的角度r−车轮半径b−轮距ICR−瞬时旋转中心R−瞬心到机器人中心的距离vL,vR−左右轮接地点切向线速度ωL,ωR−左右轮角速度

有如下关系：
ω ⋅ ( R + b / 2 ) = v R ω ⋅ ( R − b / 2 ) = v L \omega \cdot (R + b/2) = v_R\\ \omega \cdot (R - b/2) = v_L ω⋅(R+b/2)=vRω⋅(R−b/2)=vL

解这两个方程可得 ω \omega ω 和 R R R ：
ω = ( v R − v L ) / b R = b / 2 ⋅ ( v R + v L ) / ( v R − v L ) \omega = (v_R-v_L)/b\\ R = b/2 \cdot(v_R+v_L)/(v_R-v_L) ω=(vR−vL)/bR=b/2⋅(vR+vL)/(vR−vL)

利用角速度方程可得机器人瞬时速度 V V V ：
V = ω ⋅ R = ( v R + v L ) / 2 V = \omega \cdot R = (v_R+v_L)/2 V=ω⋅R=(vR+vL)/2

车轮切向速度也可以写成：
v R = r ⋅ ω R v L = r ⋅ ω L v_R = r \cdot \omega_R\\ v_L = r \cdot \omega_L vR=r⋅ωRvL=r⋅ωL

则机器人在本体坐标系中的运动学模型为：
[ x ˙ B y ˙ B φ ˙ ] = [ v ⋅ x B v ⋅ y B ω ] = ⏞ v = r ω [ r 2 r 2 0 0 − r b r b ] [ ω L ω R ] \begin{bmatrix} \dot{x}_B \\ \dot{y}_B \\ \dot{\varphi} \end{bmatrix} = \begin{bmatrix} v \cdot x_B \\ v \cdot y_B \\ \omega \end{bmatrix} \overbrace{=}^{v=r\omega} \begin{bmatrix} \frac r2 & \frac r2 \\ 0 & 0 \\ -\frac rb & \frac rb \end{bmatrix} \begin{bmatrix} \omega_L \\ \omega_R \end{bmatrix} x˙By˙Bφ˙ = v⋅xBv⋅yBω = v=rω 2r0−br2r0br [ωLωR]

再通过坐标变换，最终可以得到机器人在世界坐标中的运动学模型：
[ x ˙ y ˙ φ ˙ ] = [ cos ⁡ φ 0 sin ⁡ φ 0 0 1 ] [ V ω ] \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\varphi} \end{bmatrix} = \begin{bmatrix} \cos\varphi & 0 \\ \sin\varphi & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} V \\ \omega \end{bmatrix} x˙y˙φ˙ = cosφsinφ0001 [Vω]

其中， V V V 和 ω \omega ω 为控制变量。

通常我们需要通过机器人的速度和结构参数逆解出左右轮的转速，用于控制电机。在这种情况下，可以很容易地重新表述前面提到的方程。使用如下方程：
R = V / ω ω R = v R / r ω L = v L / r R = V/\omega\\ \omega_R = v_R/r\\ \omega_L = v_L/r R=V/ωωR=vR/rωL=vL/r

可得左右轮角速度方程：
ω R = V + ω ⋅ b / 2 r ω L = V − ω ⋅ b / 2 r \omega_R = \frac{V+\omega \cdot b/2}{r} \\ \omega_L = \frac{V-\omega \cdot b/2}{r} ωR=rV+ω⋅b/2ωL=rV−ω⋅b/2

三、对外接口

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）

五、配置控制器参数

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

yaml 复制代码

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]

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

yaml 复制代码

# 用于控制器硬件接口配置
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: 50
  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]
  cmd_vel_timeout: 100
  velocity_rolling_window_size: 1

  publish_cmd: true
  base_frame_id: base_link
  enable_odom_tf: true
  odom_frame_id: odom

  # 轮间距和轮半径
  wheel_separation: 0.38
  wheel_radius: 0.06
  wheel_separation_multiplier: 1.0
  wheel_radius_multiplier: 1.0

  # 速度和加速度限制
  linear:
    x:
      has_velocity_limits: true
      max_velocity: 1.0 # m/s
      has_acceleration_limits: true
      max_acceleration: 3.0 # m/s^2
  angular:
    z:
      has_velocity_limits: true
      max_velocity: 2.0 # rad/s
      has_acceleration_limits: true
      max_acceleration: 6.0 # rad/s^2

六、编写硬件抽象接口

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

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

cpp 复制代码

#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

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 interface
        hardware_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

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;
}

七、控制机器人移动

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

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

cpp 复制代码

#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中的可视化结果如下：

八、源码

项目源码