利用move_base導航--42

摘要: 原創博客:轉載請標明出處:http://www.cnblogs.com/zxouxuewei/

各位博友好長時間又沒有寫博客了，突然發現上班和在學校是不一樣的，在公司的卻沒有時間寫博客了，不過還有好多小伙伴會遇到不一樣的問題，今天我在總結的同時，將遇到的問題也匯集一下：

一。首先統一硬件：

　　1>不過對於ros軟件平台下的機器人底盤來說，沒有什么是統一的，標准的，只有完成ros層的參數透傳和執行就可以了。不過今天我所用的硬件不是我之前博客中提到的兩輪差分的。 而是3輪全向的，具體是3個時代超群的直流無刷電機，3個全向輪，3個直流無刷電機驅動器（不過驅動器的速度反饋太慢了，所以我是直接將霍爾的接口在接到STM32 主控板上的，采用定時 器正交解碼后計算的速度，由於電機轉動一圈的霍爾跳變沿只有8個，所以，我們不能用時間片脈沖數的方式去計算速度，要通過，兩次跳變的時間和移動的距離計算速度。在我的測試下，最終效 果還是不錯的）

　　2>剩下的就是每一個小伙伴都要經歷的就是，底盤的運動學正逆解，具體的推算過程有好多論文可以看，所以也就不多說的，兩輪差分的比較簡單，3輪全向的可以看我的上一篇博 文，http://www.cnblogs.com/zxouxuewei/p/5871470.html（強烈建 議，有些人寫的論文公式都是錯的，具體一定要自己推算一下，不然后面楚翔素的變換不正常的的情況就會讓你特別痛苦）。

二。我的STM32部分代碼分享給大家，希望大家不要見笑，不過底層的東西沒有算法那么復雜，所以大家都可以做的，在此就貼出重要的部分代碼：

              1》我的3個輪子的對象描述。

typedef struct _kinematics_
{
　　float Target_Speed;                  //運動學正解后的速度 m/s
　　int Moto_Speed_Output;               //給定的轉速 rpm/s 實際的電機要乘以減速比 36
　　float Input_Speed;                   //電機控制器輸入捕獲的實際速度 m/s
　　unsigned char TIMCH1_CAPTURE_STA;    //輸入捕獲狀態
　　unsigned short TIMCH1_CAPTURE_VAL;   //輸入捕獲值
　　float xyz_Target_value;              //通過上位機或者手機遙控器給定的xyz方向上的速度
　　unsigned char D_Flag;
}_struct_Pos_;

_struct_Pos_ Kinematics_Positive1 = {0,0,0,0,0,0};
_struct_Pos_ Kinematics_Positive2 = {0,0,0,0,0,0};
_struct_Pos_ Kinematics_Positive3 = {0,0,0,0,0,0};

 

#define VX_VALUE 0.5f //(0.5f)
#define VY_VALUE 0.72f //(sqrt(3)/2.0)
#define L_value 0.219f // 0.2/角速度校准參數(0.915)

#define RADIUS_value 0.386
#define REDUCTION_RATIO 36 //減速比
/**********************************************
* describetion: 底盤運動學正解函數
* param: vx: x方向的速度
* 　　　　vy: y方向的速度
* 　　　　vz： z軸的速度
* return: none
* author: 周學偉
* date : 2016-9-16
*************************************************/

void Speed_Moto_Control(float vx,float vy,float vz)
{
　　　　Kinematics_Positive1.Target_Speed = (-VX_VALUE*vy + VY_VALUE*vx + L_value*vz);//正解函數，具體推算過程請查看我的博客
　　　　Kinematics_Positive2.Target_Speed = (-VX_VALUE*vy - VY_VALUE*vx + L_value*vz);
　　　　Kinematics_Positive3.Target_Speed = (vy + L_value*vz);

　　　　Kinematics_Positive1.Moto_Speed_Output = -(Kinematics_Positive1.Target_Speed * REDUCTION_RATIO / RADIUS_value); //單位m/s
　　　　Kinematics_Positive2.Moto_Speed_Output = -(Kinematics_Positive2.Target_Speed * REDUCTION_RATIO / RADIUS_value);
　　　　Kinematics_Positive3.Moto_Speed_Output = -(Kinematics_Positive3.Target_Speed * REDUCTION_RATIO / RADIUS_value);

　　　　Control_Moto_Speed(MOTO1,Kinematics_Positive1.Moto_Speed_Output);
　　　　Control_Moto_Speed(MOTO2,Kinematics_Positive2.Moto_Speed_Output);
　　　　Control_Moto_Speed(MOTO3,Kinematics_Positive3.Moto_Speed_Output);
}

逆解過程

movement_cul.upload_speed.Y_speed = -(2.0*Kinematics_Positive3.Input_Speed-Kinematics_Positive1.Input_Speed-Kinematics_Positive2.Input_Speed)/3.0;
movement_cul.upload_speed.X_speed = -(Kinematics_Positive1.Input_Speed - Kinematics_Positive2.Input_Speed)/1.442;
movement_cul.upload_speed.Z_speed = -(Kinematics_Positive1.Input_Speed + Kinematics_Positive2.Input_Speed + Kinematics_Positive3.Input_Speed)/(L_value*3);

    2》做完速度的輸入正解和逆解輸出后，下面進入機器人的線速度和角速度校准，參考我的博客：http://www.cnblogs.com/zxouxuewei/p/5482302.html#3525543

    3》下面我們准備導航所需要的包。

           a.ros-indigo-gampping :我們不需要修改包內的東西，所以直接安裝可執行文件就好了。

 sudo apt-get install ros-indigo-slam-gmapping

           b.安裝雷達的驅動（我的是robopack），直接將提供的ros驅動包拷貝到工作空間中，

           c.安裝導航定位包，navigation 進入git：https://github.com/ros-planning/navigation/tree/indigo-devel，下載和自己ros版本匹配的包，解壓到自己的工作空間中，

cd ~/catkin_ws
catkin_make

indigo的navigation包會出現一個依賴問題，：Orocos-bfl not found while installing navigation stack ROS indigo + Ubuntu 14.04

解決方法：
rosdep install --from-paths src --ignore-src --rosdistro indigo -y

       d.由於導航包在/cmd_val下發布的移動數據加速度會過於不友好，所以我們需要對速度做平滑處理，其實就是控制加速，一般通過濾波即可實現，在此我們采用turtlebot的平滑包即可，

　安裝平滑包yocs_velocity_smoother,具體的平滑算法和輸入切換請自己閱讀源碼。

apt-get install ros-indigo-yocs-velocity-smoother

三。1.所有的包准包好后，我們去准備啟動所需的launch文件，首先是機器人地盤的啟動文件ZHXWBOT_start.launch：

<launch>

    <param name="use_sim_time" value="false" />

    <node name="link_laser" pkg="tf" type="static_transform_publisher" args="0.15 0 0.15 0 0 0 base_link laser 50"/> 
    <node name="link_footprint" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 base_link base_footprint 50"/>

    <node pkg="odom_tf_package" type="tf_broadcaster_node" name="serial_send_recevice" output="screen"/>


    <include file="$(find odom_tf_package)/launch/include/rplidar_ros.launch.xml">
    </include>
  
  <arg name="node_name"             value="velocity_smoother"/>
  <arg name="nodelet_manager_name"  value="nodelet_manager"/>
  <arg name="config_file"           value="$(find odom_tf_package)/config/yocs_velocity_smoother.yaml"/>
  <arg name="raw_cmd_vel_topic"     value="cmd_vel"/>
  <arg name="smooth_cmd_vel_topic"  value="smoother_cmd_vel"/>
  <arg name="robot_cmd_vel_topic"   value="robot_cmd_vel"/>
  <arg name="odom_topic"            value="odom"/>
  
  <!-- nodelet manager -->
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>
  
  <!-- velocity smoother -->
  <include file="$(find yocs_velocity_smoother)/launch/velocity_smoother.launch">
    <arg name="node_name"             value="$(arg node_name)"/>
    <arg name="nodelet_manager_name"  value="$(arg nodelet_manager_name)"/>
    <arg name="config_file"           value="$(arg config_file)"/>
    <arg name="raw_cmd_vel_topic"     value="$(arg raw_cmd_vel_topic)"/>
    <arg name="smooth_cmd_vel_topic"  value="$(arg smooth_cmd_vel_topic)"/>
    <arg name="robot_cmd_vel_topic"   value="$(arg robot_cmd_vel_topic)"/>
    <arg name="odom_topic"            value="$(arg odom_topic)"/>
  </include>
</launch>

2.然后去准備建圖包的啟動文件gmapping.launch

<launch>

  <arg name="scan_topic" default="scan" />

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">

    <!--because my used rtabmap_ros -->
    <param name="odom_frame" value="/odom""/>
    <!--param name="odom_frame" value="/base_controller/odom""/-->
    <param name="map_update_interval" value="30.0"/>

    <!-- Set maxUrange < actual maximum range of the Laser -->
    <param name="maxRange" value="5.0"/>
    <param name="maxUrange" value="4.5"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.5"/>
    <param name="angularUpdate" value="0.436"/>
    <param name="temporalUpdate" value="-1.0"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="80"/>
  <!--
    <param name="xmin" value="-50.0"/>
    <param name="ymin" value="-50.0"/>
    <param name="xmax" value="50.0"/>
    <param name="ymax" value="50.0"/>
  make the starting size small for the benefit of the Android client's memory...
  -->
    <param name="xmin" value="-1.0"/>
    <param name="ymin" value="-1.0"/>
    <param name="xmax" value="1.0"/>
    <param name="ymax" value="1.0"/>

    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

3，導航包（move_base）和定位(amcl)的啟動文件:zxbot_amcl.launch

<launch>

  <param name="use_sim_time" value="false" />

  <!-- Set the name of the map yaml file: can be overridden on the command line. -->
  <arg name="map" default="map.yaml" />

  <!--node name="map_odom" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 map odom 50"/-->  

  <!-- Run the map server with the desired map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find odom_tf_package)/maps/$(arg map)"/>

  <!-- The move_base node -->
  <include file="$(find odom_tf_package)/launch/move_base_amcl.launch" />
  
  <!--zxw add Fire up AMCL-->
  <include file="$(find odom_tf_package)/launch/tb_amcl.launch" />
  

</launch>

move_base_amcl.launch:

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find odom_tf_package)/config/zxbotconfig/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find odom_tf_package)/config/zxbotconfig/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find odom_tf_package)/config/zxbotconfig/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find odom_tf_package)/config/zxbotconfig/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find odom_tf_package)/config/zxbotconfig/base_local_planner_params.yaml" command="load" />

    <rosparam file="$(find odom_tf_package)/config/nav_obstacles_params.yaml" command="load" />
  </node>
  
</launch>

tb_amcl.launch:

<launch>

  <arg name="use_map_topic" default="false"/>


  <arg name="scan_topic" default="scan"/>

  <node pkg="amcl" type="amcl" name="amcl" clear_params="true">


    <param name="use_map_topic" value="$(arg use_map_topic)"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type" value="diff"/>
    <param name="odom_alpha5" value="0.1"/>
    <param name="gui_publish_rate" value="10.0"/>
    <param name="laser_max_beams" value="60"/>
    <param name="laser_max_range" value="12.0"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="2000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="odom_alpha1" value="0.2"/>
    <param name="odom_alpha2" value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.2"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.25"/>
    <param name="update_min_a" value="0.2"/>

    <param name="odom_frame_id" value="odom"/>

    <param name="resample_interval" value="1"/>
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance" value="1.0"/>
    <param name="recovery_alpha_slow" value="0.0"/>
    <param name="recovery_alpha_fast" value="0.0"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

4.導航的配置參數如下：

base_local_planner_params.yaml

controller_frequency: 2.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false

TrajectoryPlannerROS:
   max_vel_x: 0.3
   min_vel_x: 0.05
   max_vel_y: 0.0  # zero for a differential drive robot
   min_vel_y: 0.0
   min_in_place_vel_theta: 0.5
   escape_vel: -0.1
   acc_lim_x: 2.5
   acc_lim_y: 0.0 # zero for a differential drive robot
   acc_lim_theta: 3.2

   holonomic_robot: false
   yaw_goal_tolerance: 0.1 # about 6 degrees
   xy_goal_tolerance: 0.15  # 10 cm
   latch_xy_goal_tolerance: false
   pdist_scale: 0.8
   gdist_scale: 0.6
   meter_scoring: true

   heading_lookahead: 0.325
   heading_scoring: false
   heading_scoring_timestep: 0.8
   occdist_scale: 0.1
   oscillation_reset_dist: 0.05
   publish_cost_grid_pc: false
   prune_plan: true

   sim_time: 2.5
   sim_granularity: 0.025
   angular_sim_granularity: 0.025
   vx_samples: 8
   vy_samples: 0 # zero for a differential drive robot
   vtheta_samples: 20
   dwa: true
   simple_attractor: false

costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.30
inflation_radius: 0.15
max_obstacle_height: 0.6
min_obstacle_height: 0.0
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

global_costmap_params.yaml

global_costmap:
   global_frame: /map
   robot_base_frame: /base_link
   update_frequency: 1.0
   publish_frequency: 0
   static_map: true
   rolling_window: false
   resolution: 0.01
   transform_tolerance: 0.5
   map_type: costmap

local_costmap_params.yaml

local_costmap:
   global_frame: /odom
   robot_base_frame: /base_link
   update_frequency: 1.0
   publish_frequency: 1.0
   static_map: false
   rolling_window: true
   width: 6.0
   height: 6.0
   resolution: 0.01
   transform_tolerance: 0.5
   map_type: costmap

四，准備好以上所有的啟動文件和配置參數后，我們開始創建地圖和導航，

1.創建地圖：

roslaunch odom_tf_package ZHXWBOT_start.launch   //啟動地盤控制器
roslaunch odom_tf_package gmapping.launch        
roscd odom_tf_package/maps/
rosrun map_server map_saver -f mymap


然后會產生以下地圖文件
mymap.pgm  mymap.yaml

2.開始導航

roslaunch odom_tf_package ZHXWBOT_start.launch //啟動地盤控制器
roslaunch odom_tf_package zxbot_amcl.launch map:=mymap.yaml
rosrun rviz rviz -d `rospack find odom_tf_package`/nav_test.rviz  

然后指定導航目標，開始自己慢慢玩吧，不過因為我的TF變換主要是里程計更新的，車體打滑或者地盤電機震盪都會積累誤差，所以我們必須添加視覺里成計或者閉環檢測。