西安做網站朋朋抖音關鍵詞推廣
ROS 系列學習教程(總目錄)
本文目錄
- 一、差速輪式機器人
- 二、差速驅動機器人運動學模型
- 三、對外接口
- 3.1 輸入接口
- 3.2 輸出接口
- 四、控制器參數(shù)
- 五、配置控制器參數(shù)
- 六、編寫硬件抽象接口
- 七、控制機器人移動
- 八、源碼
ros_control 提供了多種控制器,其中 diff_drive_controller
用于控制差速驅動輪式機器人。
一、差速輪式機器人
差速輪式機器人是一種移動機器人,其運動基于機器人身體兩側的兩個獨立驅動輪。因此,它可以通過改變輪子的相對旋轉速度來改變方向,不需要額外的轉向運動。具有這種驅動器的機器人通常有一個或多個腳輪,以防止車輛傾斜。
如果兩個輪子以相同的方向和速度驅動,機器人將沿直線行駛。如果兩個輪子以相同的速度朝相反的方向轉動,如所示圖所示,機器人將繞軸的中心點旋轉。否則,根據(jù)旋轉速度和方向,旋轉中心可能落在由兩個輪胎接觸點定義的線上的任何位置。當機器人沿直線行駛時,旋轉中心距離機器人無限遠。由于機器人的方向取決于兩個驅動輪的旋轉速度和方向,因此應精確感測和控制這些量。

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

其中,
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?世界坐標系XB?YB??機器人坐標系φ?機器人在世界坐標系的角度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?ωR?vL?=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˙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 ω ] \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φ0?001? ?[Vω?]
其中, V V V 和 ω \omega ω 為控制變量。
通常我們需要通過機器人的速度和結構參數(shù)逆解出左右輪的轉速,用于控制電機。在這種情況下,可以很容易地重新表述前面提到的方程。使用如下方程:
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)
位于控制器的命名空間下,給機器人發(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中的可視化結果如下:
八、源碼
項目源碼