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

在这里插入图片描述

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

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

一、差速轮式机器人

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

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

image-20240623192426694

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

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

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

image-20240623192426694

其中,
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ω(Rb/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) ω=(vRvL)/bR=b/2(vR+vL)/(vRvL)
利用角速度方程可得机器人瞬时速度 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φ˙ = vxBvyBω = v=rω 2r0br2r0br [ω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_wheelstring /string[…]左轮关节名称或关节名称列表
right_wheelstring /string[…]右轮关节名称或关节名称列表
pose_covariance_diagonaldouble[6]用于里程计位姿发布的协方差矩阵的对角线
twist_covariance_diagonaldouble[6]用于里程计 twist 发布的协方差矩阵的对角线
publish_ratedouble发布里程计的频率,用于 tf 和 odom(单位:Hz,默认值: 50.0)
wheel_separationdouble轮距,左轮和右轮之间的距离。如果未指定此参数,diff_drive_controller 将尝试从 URDF 读取值
wheel_separation_multiplierdouble轮距参数的系数。用于解释机器人模型和真实机器人之间的差异。(默认值:1.0)
wheel_radiusdouble车轮半径。默认两侧车轮都具有相同的尺寸。如果未指定此参数,diff_drive_controller 将尝试从 URDF 读取值。
wheel_radius_multiplierdouble车轮半径参数的系数。用于解释机器人模型和真实机器人之间的差异。(默认值:1.0)
cmd_vel_timeoutdouble两个连续速度命令之间允许的时间间隔。此延迟后,将向车轮发送零速命令。(单位:s,默认值:0.5)
base_frame_idstring用于填充Odometry消息和TF的child_frame_id(默认值:“base_link”)
linearobject线性速度配置参数
+ xobjectx轴,两轮差速机器人线速度只有x轴
++ has_velocity_limitsbool控制器是否限制线速度。(默认值: false)
++ max_velocitydouble最大线速度(单位:m/s)
++ min_velocitydouble最小线速度(单位:m/s)。未指定时,使用max_velocity
++ has_acceleration_limitsbool控制器是否限制线加速度。(默认值: false)
++ max_accelerationdouble最大线加速度(单位:m/s^2)
++ min_accelerationdouble最小线加速度(单位:m/s^2)。未指定时,使用max_acceleration
++ has_jerk_limitsbool控制器是否限制线加速度的变化快慢(默认值: false)
++ max_jerkdouble最大 jerk(单位:m/s^3)
angularobject角速度配置参数
+ zobjectz轴,两轮差速机器人角速度只有z轴
++ has_velocity_limitsbool控制器是否应该限制角速度(默认值: false)
++ max_velocitydouble最大角速度(单位:rad/s)
++ min_velocitydouble最小角速度(单位:rad/s)。将其设置为 0.0 将禁用逆时针旋转。未指定时,将使用max_velocity
++ has_acceleration_limitsbool控制器是否应该限制角加速度(默认值: false)
++ max_accelerationdouble最大角加速度(单位:rad/s^2)
++ min_accelerationdouble最小角加速度(单位为 rad/s^2)。未指定时,使用max_acceleration。
++ has_jerk_limitsbool控制器是否限制角加速度的变化快慢(默认值: false)
++ max_jerkdouble最大 jerk(单位:rad/s^3)
enable_odom_tfbool是否直接发布到 TF(默认值: true )
odom_frame_idstring里程计的frame_id(默认值:“/odom”)
publish_cmdbool发布要执行的速度命令。用于监控限制器对控制器输入的影响。(默认值: False)
allow_multiple_cmd_vel_publishersbool将其设置为 true 将允许输入接口 ~/cmd_vel 有多个发布者。如果将其设置为 false,则如果 ~/cmd_vel 有多个发布者,则会导致控制器停止运行。(默认值: False)
velocity_rolling_window_sizeint用于计算里程计 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: 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 <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 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 <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中的可视化结果如下:

在这里插入图片描述

八、源码

项目源码

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

万俟淋曦

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值