[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}$$
则机器人在本体坐标系中的运动学模型为：
[ x ˙ B y ˙ B φ ˙ ] = [ v ⋅ x B v ⋅ y B ω ] = ⏞ v = r ω [ r 2 r 2 0 0 − r b r b ] [ ω L ω R ]

= \overbrace{=}^{v=r\omega} ​x˙B​y˙​B​φ˙​​ ​= ​v⋅xB​v⋅yB​ω​ ​= v=rω ​2r​0−br​​2r​0br​​ ​[ωL​ωR​​]
再通过坐标变换，最终可以得到机器人在世界坐标中的运动学模型：
[ x ˙ y ˙ φ ˙ ] = [ cos ⁡ φ 0 sin ⁡ φ 0 0 1 ] [ V ω ]

[x˙y˙φ˙]" role="presentation">⎡⎣⎢x˙y˙φ˙⎤⎦⎥$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]$$

=

[cos⁡φ0sin⁡φ001]" role="presentation">⎡⎣⎢cosφsinφ0001⎤⎦⎥$$\left[\begin{array}{cc}\cos \phi & 0\\ \sin \phi & 0\\ 0& 1\end{array}\right]$$

[Vω]" role="presentation">[Vω]$$\left[\begin{array}{c}V\\ \omega \end{array}\right]$$

​x˙y˙​φ˙​​ ​= ​cosφsinφ0​001​ ​[Vω​]
其中，$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 提供了一些参数，用于配置机器人控制。

五、配置控制器参数

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

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: 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

#ifndef DIFF_DRIVE_HARDWARE_INTERFACE_H
#define DIFF_DRIVE_HARDWARE_INTERFACE_H

#include 
#include 
#include 
#include 
#include 

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

#include 
#include 
#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 
#include 

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

八、源码

项目源码