禅城区做网站策划,app编辑软件,宁波网站建设公司制作网站,天津网站建设市场ROS 系列学习教程(总目录) 本文目录 一、差速轮式机器人二、差速驱动机器人运动学模型三、对外接口3.1 输入接口3.2 输出接口 四、控制器参数五、配置控制器参数六、编写硬件抽象接口七、控制机器人移动八、源码 ros_control 提供了多种控制器#xff0c;其中 diff_drive_cont…
ROS 系列学习教程(总目录) 本文目录 一、差速轮式机器人二、差速驱动机器人运动学模型三、对外接口3.1 输入接口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 ω⋅(Rb/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_Rv_L)/(v_R-v_L) ω(vR−vL)/bRb/2⋅(vRvL)/(vR−vL) 利用角速度方程可得机器人瞬时速度 V V V V ω ⋅ R ( v R v L ) / 2 V \omega \cdot R (v_Rv_L)/2 Vω⋅R(vRvL)/2 车轮切向速度也可以写成 v R r ⋅ ω R v L r ⋅ ω L v_R r \cdot \omega_R\\ v_L r \cdot \omega_L vRr⋅ωRvLr⋅ω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{}^{vr\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ω vrω 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 RV/ωωRvR/rωLvL/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} ωRrVω⋅b/2ωLrV−ω⋅b/2
三、对外接口
diff_drive_controller 主要通过订阅速度命令作为模块的输入然后解析运动学模型控制电机达到控制机器人的目的。
3.1 输入接口 cmd_velgeometry_msgs/Twist 位于控制器的命名空间下给机器人发布速度
3.2 输出接口 odomnav_msgs/Odometry 位于控制器的命名空间下根据硬件反馈计算的里程计信息 /tftf/tfMessage 从 odom 转换为 base_link cmd_vel_outgeometry_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.0wheel_separationdouble轮距左轮和右轮之间的距离。如果未指定此参数diff_drive_controller 将尝试从 URDF 读取值wheel_separation_multiplierdouble轮距参数的系数。用于解释机器人模型和真实机器人之间的差异。默认值1.0wheel_radiusdouble车轮半径。默认两侧车轮都具有相同的尺寸。如果未指定此参数diff_drive_controller 将尝试从 URDF 读取值。wheel_radius_multiplierdouble车轮半径参数的系数。用于解释机器人模型和真实机器人之间的差异。默认值1.0cmd_vel_timeoutdouble两个连续速度命令之间允许的时间间隔。此延迟后将向车轮发送零速命令。单位s默认值0.5base_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^3angularobject角速度配置参数 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^3enable_odom_tfbool是否直接发布到 TF默认值 true odom_frame_idstring里程计的frame_id默认值“/odom”publish_cmdbool发布要执行的速度命令。用于监控限制器对控制器输入的影响。默认值 Falseallow_multiple_cmd_vel_publishersbool将其设置为 true 将允许输入接口 ~/cmd_vel 有多个发布者。如果将其设置为 false则如果 ~/cmd_vel 有多个发布者则会导致控制器停止运行。默认值 Falsevelocity_rolling_window_sizeint用于计算里程计 twist.linear.x 和 twist.angular.z 速度的平均速度样本数量默认值 10
五、配置控制器参数
最小配置示例即必要配置项
diff_drive_controller:type: diff_drive_controller/DiffDriveControllerleft_wheel: left_wheel_jointright_wheel: right_wheel_jointpose_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/JointStateControllerpublish_rate: 50# diff_drive_controller 控制器
diff_drive_controller:type: diff_drive_controller/DiffDriveControllerleft_wheel: left_wheel_jointright_wheel: right_wheel_jointpublish_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.hclass 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::vectorJointInfo m_joints;
};#endif // DIFF_DRIVE_HARDWARE_INTERFACE_H源文件diff_drive_hardware_interface.cpp
#include diff_drive_control/diff_drive_hardware_interface.hDiffDriveHWInterface::DiffDriveHWInterface(ros::NodeHandle nh) : m_nh(nh)
{
}/*** brief 初始化关节信息* 注册抽象硬件接口* */
void DiffDriveHWInterface::init()
{std::vectorstd::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;}
}控制节点diff_drive_control_node.cpp
#include ros/ros.h
#include controller_manager/controller_manager.h
#include diff_drive_control/diff_drive_hardware_interface.hint 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.hgeometry_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.advertisegeometry_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中的可视化结果如下 八、源码
项目源码
