实物部署【Switching Sampling Space of Model Predictive Path-Integral Controller to Balance Efficiency and Safety in 4WIDS Vehicle Navigation】

博客地址：https://www.cnblogs.com/zylyehuo/
Switching Sampling Space of Model Predictive Path-Integral Controller to Balance Efficiency and Safety in 4WIDS Vehicle Navigation
GitHub项目链接

部署环境

• Ubuntu 20.04
• ROS Noetic
• gcc 9.4.0
• numpy 1.20.3
• numexpr 2.7.3
• cmake 3.12.4（3.5 及以上，不能用 cmake 4 系列）
• 松灵 ranger mini 3.0
• 禾赛 32线雷达
• xsense IMU

整体运行指令流程

nullspace_mpc_README.md

# 打开禾赛雷达
cd /home/yehuo/Lidar/Hesai
source devel/setup.bash
roslaunch hesai_lidar cloud_nodelet.launch lidar_type:="PandarXT-32" frame_id:="PandarXT-32"

# 时间同步命令
# sudo apt install linuxptp
ifconfig -a
sudo ptp4l -m -4 -i enp11s0 -S

# 打开imu
cd /home/yehuo/IMU/xsense/Xsens_MTi_ROS_Driver_and_Ntrip_Client/src/xsens_ros_mti_driver/build
sudo chmod 777 /dev/ttyUSB0
source devel/setup.bash
roslaunch xsens_mti_driver  xsens_mti_node.launch  # display.launch

# 打开点云地图
cd  /home/yehuo/location/location_ws
source devel/setup.bash
roslaunch pointcloudmap_load pointcloudmap_load.launch

# 启动定位程序
cd /home/yehuo/location/location_ws
source devel/setup.bash
roslaunch neu_localization  neu_localization.launch

# 启动四轮四转向车(遥控器使能)
cd /home/yehuo/agilexrobotics/agilexrobotics_ws
source devel/setup.bash
rosrun ranger_bringup bringup_can2usb.bash
roslaunch ranger_bringup ranger_mini_v2.launch

# 启动 nullspace_mpc 项目
cd /home/yehuo/nullspace_mpc_test
make navigation_nullspace_mpc

与原项目的区别

基于禾赛3d雷达进行定位，使用的是自己的定位程序

• 为了与原项目的程序相容，基于 pointcloud_to_laserscan 功能包将三维的点云数据改为二维的激光数据
• 此外，将实际场景的三维地图转变为二维地图

实物上需要启动 xsense IMU 的驱动
实物上需要启动底盘的驱动
实物上需要进行时间的同步

rqt_graph 比对

仿真

实物

rqt_tf_tree 比对

仿真

实物

  <node pkg="tf" type="static_transform_publisher" name="map2odom" args="0.0  0.0  0.0  0 0 0 1 map odom 100" />
  <!--发布 map 坐标系 → odom 坐标系 的静态变换 -->
  <node pkg="tf" type="static_transform_publisher" name="base_link2laser_link" args="0.0  0.0  0.0  0 0 0 1 base_link laser_link 100" />
  <!-- body_imu 改为 base_link ;base_link 改为 laser_link -->

具体改动代码

三维点云.pcd地图转换为二维栅格.pgm地图

点云pcd文件转二维栅格地图

cd /home/yehuo/pcd2pgm/pcd2pgm_ws

source devel/setup.bash

rosrun pcd_to_pgm pcd_to_pgm_node map/custom/drone_map_01.pcd map/custom/drone_map_01 0.05 0.1 2.0

/home/yehuo/nullspace_mpc_test/launch/navigation.launch

<launch>
  <!-- ##### SETTINGS { ##### -->

  <!-- often changed arguments -->
  <arg name="enable_gazebo" default="false"/>
  <arg name="gazebo_world_name" default="maze" /> <!-- empty, empty_garden, cylinder_garden, maze -->
  <arg name="joy_operation" default="true" />
  <arg name="local_planner" default="nullspace_mpc" /> <!-- mppi_3d_a, mppi_3d_b, mppi_4d, mppi_h, nullspace_mpc -->

  <!-- arguments -->
  <arg name="workspace" default="$(env HOME)/nullspace_mpc" />
  <arg name="rvizconfig" default="$(arg workspace)/data/rviz/navigation.rviz" />
  <arg name="open_rviz" default="true"/>
  <arg name="scan_topic"     default="/scan"/>  <!-- "/laser_link/scan" -->
  <arg name="cmd_vel_topic"  default="/cmd_vel"/>
  <arg name="odom_topic"     default="/odometry"/>      <!-- "/groundtruth_odom" -->
  <arg name="set_base_frame" default="base_link"/>
  <arg name="set_odom_frame" default="odom"/>
  <arg name="set_map_frame"  default="map"/>
  <arg name="initial_pose_x" default="0.0"/>
  <arg name="initial_pose_y" default="0.0"/>
  <arg name="initial_pose_yaw" default="0.0"/>
  <arg name="do_evaluation" default="true" />
  <arg name="scenario_config_path" default="file_not_specified"/>
  <arg name="eval_result_dir" default="file_not_specified"/>
  <arg name="controller_mode" default="default" doc="Mode for nullspace_mpc: choose 'default' or 'lite' to adjust performance"/>
  <arg name="show_gazebo_gui" default="false"/>  <!-- true -->
  <arg name="gazebo_headless" default="false"/>
  <rosparam param="/use_sim_time">false</rosparam> <!-- set use_sim_time true to syncronize time with gazebo --> <!-- true -->
  <!-- ##### } SETTINGS ##### -->

  <!-- ##### COMMON NODES { ##### -->

  <!-- launch gazebo world -->
  <group if="$(arg enable_gazebo)">
    <include file="$(arg workspace)/launch/gazebo_world.launch">
      <arg name="gazebo_world_name" value="$(arg gazebo_world_name)" />
      <arg name="use_joystick" value="$(arg joy_operation)" />
      <arg name="use_vel_driver" value="false" />
      <arg name="open_rviz" value="false" />
      <arg name="show_gazebo_gui" value="$(arg show_gazebo_gui)" />
      <arg name="gazebo_headless" value="$(arg gazebo_headless)" />
    </include>
  </group>

  <!-- map server -->
  <!-- <node pkg="map_server" type="map_server" name="map_server" args="$(arg workspace)/data/map/$(arg gazebo_world_name)/map.yaml" /> -->
  <node pkg="map_server" type="map_server" name="map_server" args="/home/yehuo/pcd2pgm/pcd2pgm_ws/map/custom/drone_map_01.yaml" />

  <!-- 3d map visualizer -->
  <!-- <group if="$(arg enable_gazebo)">
    <include file="$(find map_visualizer)/launch/map_visualizer.launch">
      <arg name="map_name" value="$(arg gazebo_world_name)" />
    </include>
  </group> -->

  <!-- amcl -->
  <!-- <node pkg="amcl" type="amcl" name="amcl">
      <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
      <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
      <param name="initial_pose_a"            value="$(arg initial_pose_yaw)"/>
      <remap from="scan"                      to="$(arg scan_topic)"/>
      <rosparam file="$(arg workspace)/config/amcl.yaml" command="load" />
  </node> -->

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
  </group>
  
  <!-- run pointcloud_to_laserscan node -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
        <remap from="cloud_in" to="/cloud_registered_body"/>
    </node>
  <!-- ##### } COMMON NODES ##### -->


  <!-- ##### LAUNCH SELECTED LOCAL PLANNER { ##### -->

  <!-- Nullspace MPC -->
  <group if="$(eval local_planner=='nullspace_mpc')">

      <!-- nullspace_mpc with controller_mode to adjust performance -->
      <include file="$(find nullspace_mpc)/launch/nullspace_mpc.launch">
          <arg name="controller_mode" value="$(arg controller_mode)"/>
      </include>

      <!-- reference_costmap_generator -->
      <include file="$(find reference_costmap_generator)/launch/reference_costmap_generator.launch" />

      <!-- launch global planner -->
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
          <rosparam file="$(arg workspace)/config/base_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local_nullspace_mpc.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base.yaml" command="load" />
          <remap from="cmd_vel" to="/no_use/cmd_vel"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>

      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- MPPI-3D (a) (exploring 3 dimensional space: vx, vy, omega) -->
  <group if="$(eval local_planner=='mppi_3d_a')">
      <include file="$(find mppi_3d)/launch/mppi_3d_a.launch" />

      <!-- reference_costmap_generator -->
      <include file="$(find reference_costmap_generator)/launch/reference_costmap_generator.launch" />

      <!-- launch global planner -->
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
          <rosparam file="$(arg workspace)/config/base_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base.yaml" command="load" />
          <remap from="cmd_vel" to="/no_use/cmd_vel"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>

      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- MPPI-3D (b) (exploring 3 dimensional space: vx, vy, omega) -->
  <group if="$(eval local_planner=='mppi_3d_b')">
      <include file="$(find mppi_3d)/launch/mppi_3d_b.launch" />

      <!-- reference_costmap_generator -->
      <include file="$(find reference_costmap_generator)/launch/reference_costmap_generator.launch" />

      <!-- launch global planner -->
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
          <rosparam file="$(arg workspace)/config/base_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base.yaml" command="load" />
          <remap from="cmd_vel" to="/no_use/cmd_vel"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>

      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- MPPI-4D (exploring 4 dimensional space) -->
  <group if="$(eval local_planner=='mppi_4d')">
      <include file="$(find mppi_4d)/launch/mppi_4d.launch" />

      <!-- reference_costmap_generator -->
      <include file="$(find reference_costmap_generator)/launch/reference_costmap_generator.launch" />

      <!-- launch global planner -->
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
          <rosparam file="$(arg workspace)/config/base_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base.yaml" command="load" />
          <remap from="cmd_vel" to="/no_use/cmd_vel"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>

      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- MPPI-H (switching MPPI-3D and MPPI-4D in real-time) -->
  <group if="$(eval local_planner=='mppi_h')">
      <include file="$(find mppi_h)/launch/mppi_h.launch" />

      <!-- reference_costmap_generator -->
      <include file="$(find reference_costmap_generator)/launch/reference_costmap_generator.launch" />

      <!-- launch global planner -->
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
          <rosparam file="$(arg workspace)/config/base_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base.yaml" command="load" />
          <remap from="cmd_vel" to="/no_use/cmd_vel"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>

      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- (not supported) move_base [base local planner] -->
  <group if="$(eval local_planner=='base')">
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
          <rosparam file="$(arg workspace)/config/base_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base.yaml" command="load" />
          <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>
      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- (not supported) move_base [teb local planner] -->
  <group if="$(eval local_planner=='teb')">
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
          <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
          <rosparam file="$(arg workspace)/config/teb_local_planner.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_common_global.yaml" command="load" ns="global_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_common_local.yaml" command="load" ns="local_costmap" />
          <rosparam file="$(arg workspace)/config/costmap_local.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/costmap_global.yaml" command="load" />
          <rosparam file="$(arg workspace)/config/move_base_teb.yaml" command="load" />
          <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
          <remap from="odom" to="$(arg odom_topic)"/>
          <remap from="scan" to="$(arg scan_topic)"/>
      </node>
      <!-- launch vel_driver to operate 4WIDS vehicle with /cmd_vel-->
      <include file="$(find vel_driver)/launch/vel_driver.launch" />
  </group>

  <!-- ##### } LAUNCH SELECTED LOCAL PLANNER ##### -->


  <!-- ##### { OPTIONAL NODES ##### -->
    <!-- evaluator -->
    <group if="$(arg do_evaluation)">
        <include file="$(find mpc_nav_evaluator)/launch/mpc_nav_evaluator.launch">
            <arg name="scenario_config_path" value="$(arg scenario_config_path)" />
            <arg name="eval_result_dir" value="$(arg eval_result_dir)" />
        </include>
    </group>
<!-- ##### } OPTIONAL NODES ##### -->


</launch>

/home/yehuo/nullspace_mpc_test/src/control/nullspace_mpc/config/nullspace_mpc.yaml

# topic names
## subscribing topics
odom_topic: /odometry   ## 仿真使用 /groundtruth_odom 实物使用 /odometry
ref_path_topic: /move_base/NavfnROS/plan
collision_costmap_topic: /move_base/local_costmap/costmap
distance_error_map_topic: /distance_error_map
ref_yaw_map_topic: /ref_yaw_map
## publishing topics
control_cmd_vel_topic: /cmd_vel
mppi_absvel_topic: /mpc/cmd/absvel
mppi_vx_topic: /mpc/cmd/vx
mppi_vy_topic: /mpc/cmd/vy
mppi_omega_topic: /mpc/cmd/omega
calc_time_topic: /mpc/calc_time
mppi_overlay_text_topic: /mpc/overlay_text
mppi_optimal_traj_topic: /mpc/optimal_traj
mppi_sampled_traj_topic: /mpc/sampled_traj
mppi_via_state_seq_topic: /mpc/via_state_seq
mpc_eval_msg_topic: /mpc/eval_info

# navigation params
navigation:
  xy_goal_tolerance: 0.5  # [m]
  yaw_goal_tolerance: 0.15 # [rad] set 6.28 to ignore angular error
  goal_snap_distance_for_via_pos: 0.3 # [m]
  goal_snap_distance_for_via_angle: 1.5 # [rad]

# target_system params
target_system:
  l_f: 0.5 # [m]
  l_r: 0.5 # [m]
  d_l: 0.5 # [m]
  d_r: 0.5 # [m]
  tire_radius: 0.2 # [m]

# controller params
controller:
  name: "nullspace_mpc"
  control_interval: 0.05 # [s]
  num_samples: 200 # number of samples
  prediction_horizon: 15 # [steps]
  step_len_sec: 0.125 # [s]
  param_exploration: 0.01
  param_lambda: 250.0
  param_alpha: 0.975
  idx_via_states: [4, 9, 15] # index of via states in the prediction horizon (indices must be less than prediction_horizon)
  sigma: [
    0.2, 0.25, 0.393, # noise for via state at step 4
    0.2, 0.25, 0.393, # noise for via state at step 9
    0.2, 0.01, 0.393, # noise for via state at step 15
  ]
  reduce_computation: true # if true, noise sampling is done only once and the same noise is used for all processes.
  weight_cmd_change: [0.0, 0.0, 0.0] # penalty weight for variation of [vx, vy, omega]
  weight_vehicle_cmd_change: [1.4, 1.4, 1.4, 1.4, 0.1, 0.1, 0.1, 0.1] # penalty weight for variation of [fl_steer, fr_steer, rl_steer, rr_steer, fl_vel, fr_vel, rl_vel, rr_vel]
  ref_velocity: 2.0 # [m/s]
  weight_velocity_error: 10.0
  weight_angular_error: 30.0
  weight_collision_penalty: 50.0
  weight_distance_error_penalty: 40.0
  weight_terminal_state_penalty: 0.0
  use_sg_filter: true # set true to use Savitzky-Golay filter for smoothing the control input
  sg_filter_half_window_size: 8 # value in the range of 1 ~ (prediction_horizon - 1) is allowed.
  sg_filter_poly_order: 2
# Note: 
## - to change the velocity limit of the controller, check ../include/nullspace_mpc/common_type.hpp
## - to edit the prediction model or the cost function, check ../include/nullspace_mpc/nullspace_mpc_setting.hpp