Gazebo環境下VIO SLAM系統實現

簡介

迫於實體機器人不在手邊的情形，gazebo不失爲一種好的方案。但搜了下baidu和google，都沒有很全的關於自己搭建gazebo環境下相機及IMU完成SLAM系統的教程，這篇文章詳細介紹整個gazebo環境下的搭建過程，並實現了VINS-MONO及ORB-SLAM2開源方案的SLAM系統。同時，由於這個gazebo平臺還在開發中，後期會開源出來，再更新在這裏。

0. 準備工作

Ubuntu16.04或Ubuntu18.04
ROS-kinetic或ROS-melodic
Gazebo7.0（其他版本應該也可以實現）
可以運行的gazebo及其控制包（這部分沒有自己的機器人的可以直接使用開源方案https://github.com/ros-simulation/gazebo_ros_demos，雖然rrbot裏以xacro爲主，但是通過修改也可以實現，重點是確保有XXX_description/XXX_gazebo/XXX_control包）

1. 搭建gazebo環境

首先需要有自己的機器人平臺的URDF或XACRO文件，當然有基本的可實現機器人鍵盤遙控運動的gazebo包，關於這塊的教程網上還是比較多，大家可自己摸索實現準備工作中的第4點。

1.1 相機

由於本人研究的實體機器人使用的是realsense-d435i相機，因此相機這部分主要以d435i爲主來進行實現，包括dae模型文件、深度相機等。對於不同的需求可進行修改實現。
首先需要得到相機的模型，這裏參考https://github.com/pal-robotics-forks/realsense的模型dae文件（在realsense2_description/meshes中）和xacro文件（在realsense2_description/urdf中_d435.urdf.xacro）。同時還需要增加與機器人本體的關節，這裏額外增加一個type爲continuous的活動關節，後面SLAM中會用到，也是一個實現VINS-MONO過程中一個很關鍵的trick。
這裏強調一下，由於深度相機與單目相機之間存在位移，並不是在同一個點，因此建立了兩個link（camera_link和depth_link）以及對應的兩個joint。urdf部分代碼如下：

  <link name="base_link">
  <origin xyz="0.0 -0.0 0.0" rpy="0 0.0 0"/>
    <inertial>
      <mass value="0.01" />
      <inertia ixx="0.0000125" ixy="0.0" ixz="0.0" iyy="0.0000125" iyz="0.0" izz="0.0000125" />
    </inertial>
    <visual>
        <geometry>
            <box size="0.0001 0.0001 0.0001" />
        </geometry>
    </visual>
  </link>

  <joint name="revolute_joint" type="continuous">
  <origin
      xyz="0.0 -0.0 0.0"
      rpy="0 0 0" />
      <parent link="base_link" />
      <child link="revolute_link" />
      <axis xyz="0 0 1" />
      <dynamics damping="0.0" friction="0.0" />
  </joint>
<link name="revolute_link">
  <origin xyz="0.0 -0.0 0.0" rpy="0 0.0 0"/>
    <inertial>
      <mass value="0.01" />
      <inertia ixx="0.0000125" ixy="0.0" ixz="0.0" iyy="0.0000125" iyz="0.0" izz="0.0000125" />
    </inertial>
    <visual>
        <geometry>
            <box size="0.0001 0.0001 0.0001" />
        </geometry>
    </visual>
  </link>
 
  <joint name="gimbal_camera_joint" type="fixed">
      <origin xyz="-0.01 0.0 0.023" rpy="1.5707 0 -1.5707"/>
      <parent link="base_link"/>
      <child link="camera_link"/>
  </joint>

  <link name="camera_link">
        <visual>
            <origin xyz="-0.0 -0.0 0.0 " rpy="0 0 0" />
            <geometry>
              <mesh	filename="package://amphi_description/meshes/d435.dae" />
            </geometry>
            <material name="white">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
        <collision>
          <origin
            xyz="0 0 0"
            rpy="0 0 0" />
          <geometry>
            <mesh	filename="package://amphi_description/meshes/d435.dae" />
          </geometry>
        </collision>
  </link>

  <joint name="depth_color_joint" type="fixed">
      <origin xyz="-0.01 0.0 0.023" rpy="0 0 3.14159" />
      <parent link="gimbal_y_link"/>
      <child link="depth_link"/>
  </joint>

  <link name="depth_link">
    <origin xyz="-0.0 -0.0 0.0 " rpy="0 0 0"/>
    <inertial>
      <mass value="0.01" />
      <inertia ixx="0.0000125" ixy="0.0" ixz="0.0" iyy="0.0000125" iyz="0.0" izz="0.0000125" />
    </inertial>
    <visual>
        <geometry>
            <box size="0.0001 0.0001 0.0001" />
        </geometry>
    </visual>
  </link>

完成模型建立後，就需要增加gazebo部分，參考，這裏的libgazebo_ros_openni_kinect.so不特定，還有很多其他如librealsense_gazebo_plugin.so都可以實現。這裏可以設置圖像更新頻率：update_rate，圖像尺寸，imageTopicName（rgb），depthImageTopicName（深度圖）等參數，代碼如下：

<gazebo reference="depth_link">  
    <sensor type="depth" name="camera">
      <update_rate>40</update_rate>
      <camera>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
        </image>
        <clip>
            <near>0.01</near>
            <far>100</far>
        </clip>
      </camera>

      <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <cameraName>camera</cameraName>
        <alwaysOn>true</alwaysOn>
        <updateRate>40</updateRate>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <frameName>depth_link_optical</frameName>
        <baseline>0.1</baseline>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
        <pointCloudCutoff>0.4</pointCloudCutoff>
      </plugin>
    </sensor>
  </gazebo>

這部分代碼添加到urdf文件後，roslaunch啓動XXX_description中的display.launch打開RVIZ界面（剛開始啓動什麼都沒有，需要訂閱話題），XXX_gazebo中的gazebo.launch打開gazebo界面。如果你的gazebo環境中沒有物體，可以拖動添加一些簡單的物體到相機前方，也可以下載gazebo的模型庫（參考https://blog.csdn.net/qq_40213457/article/details/81021562），建立自己world。我建的gazbeo模型如下圖所示

這時在RVIZ下點擊add，在by topic欄下添加圖像（rgb或者深度圖或者點雲）話題，添加過程及最後獲取到的圖像如下圖。

1.2 IMU

在VINS-MONO中需要用到IMU來避免純視覺方案的失效情況，因此在URDF中增加imu傳感器。這部分相對於相機傳感器更簡單一點，直接增加gazebo部分即可，代碼如下：
這裏的camera_link是指imu的相對座標系，同樣可以設置更新頻率update_rate、話題名稱topicName。

  <gazebo reference="camera_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>200</update_rate>
      <visualize>true</visualize>
      <topic>__default_topic__</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <topicName>d435i/imu</topicName>
        <bodyName>camera_link</bodyName>
        <updateRateHZ>200.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0.0 0.0 0.0</xyzOffset>
        <rpyOffset>0.0 0.0 0.0</rpyOffset>
        <frameName>imu_link_</frameName>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

urdf文件添加完成後，啓動XXX_gazebo中的gazebo.launch打開gazebo界面，由於imu數據不能在rviz中顯示（可安裝rviz_imu_plugin，但是試過不起作用），因此通過rostopic echo /d435i/imu來查看imu數據，具體話題名稱可修改，如圖。

2.VINS-MONO

完成了傳感器部分，就可以通過傳感器發佈的話題來實現我們的SLAM系統。VINS-MONO可實現單目相機+IMU的SLAM，對於gazebo仿真平臺來說，相機及IMU內參是設定好的且不需要考慮加工誤差等，因此可以直接通過topic讀取（rostopic echo /camera/rgb/camera_info視自己設定的話題名而定）。但難點在於IMU與相機的外參（相對位置及座標系旋轉）無法確定，gazbeo中的IMU座標系無從獲取，本人也嘗試過Ethz的開源標定方案kalibr，但效果不理想，VINS-MONO還是會瞬間漂移，如下圖。PS：gazebo環境下的相機標定也有一些trick，包括相機如何移動，標定板如何解決，有需要這一塊工作的可以回覆我。而且，camera與IMU之間的時間同步也是個問題，對於實體realsense-d435i，有額外的realsense軟件包來實現時間同步，在gazebo環境下的camera與imu組合其實與普通camera+imu無差別，只是用了d435的外觀dae文件。

好在VINS-MONO自帶Camera-Imu外參標定以及時間校準功能，可以很好的解決普適性問題。

2.1 安裝及make

這一部分網上教程比較多，或者參考官方github。隨便列一篇教程。

2.2 參數修改

一般教程是針對數據集，因此使用自己的設備需要修改一些參數，主要修改VINS-Mono/config/euroc/文件夾下的yaml文件。這裏直接在euroc_config.yaml文件上進行修改。修改後就不再適用於官方的euroc數據集了，因此對於有一定ros基礎的同學，建議重新複製出自己yaml文件，再在launch文件中修改自己的yaml文件的路徑。
主要修改幾個地方：

imu_topic
image_topic
相機參數（image_width、image_height、distortion_parameters、projection_parameters）：獲取方法同前面rostopic echo
estimate_extrinsic：外參標定時設置爲2，外參標定結束並修改外參後設置爲0
extrinsicRotation以及extrinsicTranslation：修改爲外參標定過程中得到的數據
estimate_td：時間同步，設置爲1

修改後的yaml文件如下：

%YAML:1.0

#common parameters
imu_topic: "/d435i/imu"
image_topic: "/camera/rgb/image_raw"
output_path: "/home/shaozu/output/"

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 381.3625
   fy: 381.3625
   cx: 320.2
   cy: 240.5
   
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [-0.692596,  0.0080581,  -0.721281,
            0.033031,  -0.998534,  -0.042873, 
           -0.720569, -0.0535183,   0.691315]

           
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.0173203, -0.0361233, -0.0236887]

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.08          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.004         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.00004         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-6       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.81007     # gravity magnitude

#loop closure parameters
loop_closure: 1                    # start loop closure
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0             # useful in real-time and large project
pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: 0.0                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#rolling shutter parameters
rolling_shutter: 0                  # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

#visualization parameters
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4      # size of camera marker in RVIZ

2.3 外參標定

外參標定前需要保證estimate_extrinsic爲2。
外參標定過程中需要保持相機一直在旋轉狀態，因爲需要IMU的運動，且不能大幅度運動。在gazebo環境下，不能像在物理世界中一樣手拿着相機做連續運動，因此這裏用到前面urdf文件中設置的continuous旋轉關節，通過設定旋轉關節的旋轉速度可以保證相機以勻速旋轉用以外參標定。

2.3.1 相機旋轉關節控制器

這一部分先閱讀https://blog.csdn.net/banzhuan133/article/details/82717191的第3節（用ROS控制gazebo模型），已經會gazebo控制器的可以跳過。
首先需要在urdf中添加transmission標籤和ros_control插件。以本文爲例，其中joint name="revolute_joint"中的revolute_joint要與前面urdf中的continuous旋轉關節名稱對應；ros_control插件中的robotNamespace的rrbot指的是機器人名字，在urdf的第二行。代碼如下：

  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="revolute_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
       <robotNamespace>/rrbot</robotNamespace>   
       <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
       <legacyModeNS>true</legacyModeNS>  
    </plugin>
  </gazebo>

添加完urdf部分後，再修改rrbot_control包下config文件夾下的yaml文件，添加控制器代碼，同樣joint名字要對應，這裏的JointVelocityController不能修改，需要是速度控制。

  rrbot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  
  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint3_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: revolute_joint
    pid: {p: 1000 , i: 0.01, d: 1.0}

還需要修改rrbot_control包下launch文件夾下的launch文件rrbot_control.launch，在controller_spawner代碼行添加yaml文件中的控制器joint3_velocity_controller。修改後如下：

<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
	output="screen" ns="/rrbot" args="joint_state_controller
					  joint1_position_controller
					  joint2_position_controller
					  joint3_velocity_controller"/>

接下來，啓動gazebo中的gazebo.launch文件，並運行以下命令，可實現相機繞z軸以0.3（單位rad/s？）的速度旋轉。這裏需要保證gazbeo中，相機的周圍有特徵點，也就是需要豐富gazebo環境，不再是一個簡單的立方體。

rostopic pub -1 /rrbot/joint3_velocity_controller/command std_msgs/Float64 0.3

至此，實現在gazebo環境下相機的旋轉運動，用於外參標定。

2.3.1 外參標定過程

確保VINS-Mono/config/euroc/euroc_config.yaml中的參數estimate_extrinsic爲2。
gazebo環境：
運行

roslaunch rrbot gazebo.launch

VINS-MONO下：
運行

roslaunch vins_estimator euroc.launch

這時可以看到輸出指令"calibrating extrinsic param, rotation movement is needed"，如圖

然後，在新的終端下運行讓相機旋轉的命令：

rostopic pub -1 /rrbot/joint3_velocity_controller/command std_msgs/Float64 0.3

這一採集過程需要等待幾分鐘，可以通過運行VINS-MONO下的rviz界面查看圖像，如圖所示。

roslaunch vins_estimator vins_rviz.launch

這裏沒辦法上傳視頻，應該是相機一直在旋轉採集圖像，等待幾分鐘後roslaunch vins_estimator euroc.launch窗口輸出結果，如圖所示。

將結果

-0.561508  0.0300504    0.826925
0.0498908  -0.996293   0.0700826
0.825966   0.0806079   0.557927

0.0110133	0.027835	0.0234937

替換euroc_config.yaml中的extrinsicRotation和extrinsicTranslation中的data，替換後如下：

extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [-0.561508,  0.0300504,    0.826925,
			0.0498908,  -0.996293,   0.0700826,
			0.825966,   0.0806079,   0.557927]

           
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.0110133,	0.027835,	0.0234937]

至此，標定過程結束。

2.4 實驗效果

通過修改參數後，明顯漂移會減少很多。但是因爲VINS-MONO需要IMU不斷運動，由於本項目的機器人及gazebo環境比較大，使得機器人的速度運行很慢，導致VINS-MONO效果不是很好，後期還需要優化。但是經過外參標定後的VINS-MONO的漂移減少很多。

Gazebo環境下VIO SLAM系統實現