差速轮式机器人仿真升级：Gazebo与Rviz集成及传感器仿真详解

1、前言

本篇文章我们介绍的内容是差速轮式机器人进行升级 ，我们添加相关的物理属性，使用gazebo+rviz进行仿真。 通过编写机器人控制的程序，遥控机器人在gazeob仿真环境中移动，并通过rviz实时察看 camera，kinect和lidar三种传感器的仿真效果。

2、编写环节

2.1 绘制 hometown_room 空间模型

（1）创建 mbot_gazebo 软件包，以及相关文件

cd ~/catkin_ws/src
catkin_create_pkg mbot_gazebo urdf xacro gazebo_plugins gazebo_ros gazebo_ros_control geometry_msgs roscpp rospy

cd mbot_gazebo 
mkdir -p config launch meshes scripts urdf/sensor worlds

（2）启动gazebo，绘制hometown_room 空间模型，并保存 hometown_room.world 文件。

roslaunch gazebo_ros hometown_room.launch

2.2 为机器人模型添加物理属性和差速控制器插件

（1）在 mbot_gazebo 的 urdf 目录内，创建机器人模型文件

cd ~/catkin_ws/src/mbot_gazebo 
touch mbot_base.xacro
touch sensor/camera.xacro sensor/kinect.xacro sensor/laser.xacor

（2）转动惯量和惯性矩阵：
转动惯量：Moment of Inertia，在物理学中，用于描述物体绕特定轴线旋转时的惯性大小，也就是物体抵抗旋转运动的能力。具体而言，对于某个轴线，离该轴线越远的物体质量对转动惯量的贡献越大。
惯性矩阵：描述物体关于某一点的转动惯量的3x3的对称矩阵。

在ROS中，这个矩阵的形式如下：

    | ixx  ixy  ixz |
    | ixy  iyy  iyz |
    | ixz  iyz  izz |

对角线上的元素（ixx, iyy, izz）分别表示物体绕x轴、y轴和z轴的主转动惯量。非对角元素（ixy, ixz, iyz）表示物体关于不同轴之间的耦合作用，也就是说他们表示当物体同时绕多个轴旋转时存在的影响，这些被称为非对角转动惯量。
对于均匀的几何体（如长方体、圆柱体、球体等），它们的主轴通常与物体的对称轴相沿。在这种情况下，如果我们选择这些对称轴作为参考系（坐标系），那么非对角元素（也就是耦合项）将会消失，所以设为0。如果，物体旋转轴并不完全沿着这些主轴，那么就需要计算和考虑非对角元素了。
（3）圆柱体的惯性矩阵：m是物体质量，r是圆柱半径，h是圆柱高度

ixx：圆柱体绕x轴的转动惯量，I = m * (3 * r^2 + h^2) / 12
iyy：圆柱体绕y轴的转动惯量，与绕x轴相同
izz：圆柱体绕z轴的转动惯量，I = m * r^2 / 2
ixy, ixz, iyz均为0,表示非对角转动惯量为0。

（4）球体的惯性矩阵：m是物体质量，r是圆柱半径
ixx/iyy/izz：球体绕x,y,z轴的转动惯量公式：I = 2 * m * r^2 / 5
ixy, ixz, iyz均为0,表示非对角转动惯量为0。

（5） 一个刚体 link，有外观和物理两大属性，其中物理属性又包括：惯性矩阵（inertial matrix）和碰撞参数（collision properties）。

  <xacro:macro name="cylinder_inertial_matrix" params="m r h">
      <inertial>
          <mass value="${m}" />
          <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
              iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
              izz="${m*r*r/2}" /> 
      </inertial>
  </xacro:macro>

以差速轮式机器人万向轮为例，其惯性矩阵为：

  <xacro:macro name="sphere_inertial_matrix" params="m r">
      <inertial>
          <mass value="${m}" />
          <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
              iyy="${2*m*r*r/5}" iyz="0" 
              izz="${2*m*r*r/5}" />
      </inertial>
  </xacro:macro>

至于刚体的碰撞参数，一般与外观一致，以底盘base_link为例，其外观，惯性矩阵和碰撞参数为：

    <link name="base_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder length="${base_length}" radius="${base_radius}"/>
        </geometry>
        <material name="red" />
      </visual>
      <collision>
          <origin xyz=" 0 0 0" rpy="0 0 0" />
          <geometry>
              <cylinder length="${base_length}" radius="${base_radius}"/>
          </geometry>
      </collision>  
      <xacro:cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
    </link>

（6）刚体link如果要在gazebo上显示，必须使用gazebo标签，可以添加颜色，或者关闭重力：

    <gazebo reference="${prefix}_wheel_link">