ROS 系列學習教程(總目錄)

ros_control 提供了多種控制器，其中 diff_drive_controller 用于控制差速驅動輪式機器人。

一、差速輪式機器人

差速輪式機器人是一種移動機器人，其運動基于機器人身體兩側的兩個獨立驅動輪。因此，它可以通過改變輪子的相對旋轉速度來改變方向，不需要額外的轉向運動。具有這種驅動器的機器人通常有一個或多個腳輪，以防止車輛傾斜。

如果兩個輪子以相同的方向和速度驅動，機器人將沿直線行駛。如果兩個輪子以相同的速度朝相反的方向轉動，如所示圖所示，機器人將繞軸的中心點旋轉。否則，根據(jù)旋轉速度和方向，旋轉中心可能落在由兩個輪胎接觸點定義的線上的任何位置。當機器人沿直線行駛時，旋轉中心距離機器人無限遠。由于機器人的方向取決于兩個驅動輪的旋轉速度和方向，因此應精確感測和控制這些量。

差速轉向機器人與汽車中使用的差速 齒輪類似，兩個車輪可以有不同的轉速，但與差速齒輪系統(tǒng)不同，差速轉向系統(tǒng)將為兩個車輪提供動力。差速輪式機器人在機器人技術中得到廣泛應用，因為它們的運動易于編程并且可以很好地控制。當今市場上幾乎所有的消費機器人都使用差速轉向，主要是因為它成本低且簡單。

二、差速驅動機器人運動學模型

如下圖為輪式機器人的差速驅動運動學模型示意圖：

其中，
$$\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 ?(R+b/2)=v_R \\ \omega ?(R?b/2)=v_L \end{gathered}$$
解這兩個方程可得 $\omega$ 和 $R$ ：
$$\begin{gathered} \omega =(v_R?v_L)/b \\ R=b/2?(v_R+v_L)/(v_R?v_L) \end{gathered}$$
利用角速度方程可得機器人瞬時速度 $V$ ：
$$V=\omega ?R=(v_R+v_L)/2$$
車輪切向速度也可以寫成：
$$\begin{gathered} v_R=r?{\omega }_R \\ {v}_L=r?{\omega }_L \end{gathered}$$
則機器人在本體坐標系中的運動學模型為：
$$\left[\begin{array}{c}{\dot{x}}_B\\ {\dot{y}}_B\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}v?x_B\\ v?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$ 為控制變量。

通常我們需要通過機器人的速度和結構參數(shù)逆解出左右輪的轉速，用于控制電機。在這種情況下，可以很容易地重新表述前面提到的方程。使用如下方程：
$$\begin{gathered} R=V/\omega \\ {\omega }_R=v_R/r \\ {\omega }_L=v_L/r \end{gathered}$$
可得左右輪角速度方程：
$$\begin{gathered} {\omega }_R=\frac{V+\omega ?b/2}{r} \\ {\omega }_L=\frac{V?\omega ?b/2}{r} \end{gathered}$$

三、對外接口

diff_drive_controller 主要通過訂閱速度命令作為模塊的輸入，然后解析運動學模型控制電機，達到控制機器人的目的。

3.1 輸入接口

• cmd_vel（geometry_msgs/Twist）

  位于控制器的命名空間下，給機器人發(fā)布速度

3.2 輸出接口

• odom（nav_msgs/Odometry）

  位于控制器的命名空間下，根據(jù)硬件反饋計算的里程計信息

• /tf（tf/tfMessage）

  從 odom 轉換為 base_link

• cmd_vel_out（geometry_msgs/TwistStamped）

  當 publish_cmd 參數(shù)設置為 True 時可用。在控制器的輸入上應用限制器后的 Twist。

四、控制器參數(shù)

diff_drive_controller 提供了一些參數(shù)，用于配置機器人控制。

參數(shù)	數(shù)據(jù)類型	說明
left_wheel	string /string[…]	左輪關節(jié)名稱或關節(jié)名稱列表
right_wheel	string /string[…]	右輪關節(jié)名稱或關節(jié)名稱列表
pose_covariance_diagonal	double[6]	用于里程計位姿發(fā)布的協(xié)方差矩陣的對角線
twist_covariance_diagonal	double[6]	用于里程計 twist 發(fā)布的協(xié)方差矩陣的對角線
publish_rate	double	發(fā)布里程計的頻率，用于 tf 和 odom（單位：Hz，默認值： 50.0）
wheel_separation	double	輪距，左輪和右輪之間的距離。如果未指定此參數(shù)，diff_drive_controller 將嘗試從 URDF 讀取值
wheel_separation_multiplier	double	輪距參數(shù)的系數(shù)。用于解釋機器人模型和真實機器人之間的差異。（默認值：1.0）
wheel_radius	double	車輪半徑。默認兩側車輪都具有相同的尺寸。如果未指定此參數(shù)，diff_drive_controller 將嘗試從 URDF 讀取值。
wheel_radius_multiplier	double	車輪半徑參數(shù)的系數(shù)。用于解釋機器人模型和真實機器人之間的差異。（默認值：1.0）
cmd_vel_timeout	double	兩個連續(xù)速度命令之間允許的時間間隔。此延遲后，將向車輪發(fā)送零速命令。（單位：s，默認值：0.5）
base_frame_id	string	用于填充Odometry消息和TF的child_frame_id（默認值：“base_link”）
linear	object	線性速度配置參數(shù)
+ 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	角速度配置參數(shù)
+ 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	是否直接發(fā)布到 TF（默認值： true ）
odom_frame_id	string	里程計的frame_id（默認值：“/odom”）
publish_cmd	bool	發(fā)布要執(zhí)行的速度命令。用于監(jiān)控限制器對控制器輸入的影響。（默認值： False）
allow_multiple_cmd_vel_publishers	bool	將其設置為 true 將允許輸入接口 ~/cmd_vel 有多個發(fā)布者。如果將其設置為 false，則如果 ~/cmd_vel 有多個發(fā)布者，則會導致控制器停止運行。（默認值： False）
velocity_rolling_window_size	int	用于計算里程計 twist.linear.x 和 twist.angular.z 速度的平均速度樣本數(shù)量（默認值： 10）

五、配置控制器參數(shù)

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

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 控制器，用于發(fā)布各關節(jié)狀態(tài)
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 初始化關節(jié)信息*        注冊抽象硬件接口* */
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;}
}

控制節(jié)點：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;
}

七、控制機器人移動

機器人模型與硬件接口都準備好了，接下來開始編寫業(yè)務代碼控制機器人。

我們僅給機器人發(fā)送速度指令，模擬機器人移動任務，如下：

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

編譯執(zhí)行該節(jié)點，在rviz中的可視化結果如下：

八、源碼

項目源碼