实物部署【Switching Sampling Space of Model Predictive Path-Integral Controller

博客地址：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

1. # 打开禾赛雷达 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 比对

仿真

实物

复制代码

具体改动代码

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

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

1. 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

1. false                          

复制代码

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

1. # 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

复制代码

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！

鼓励转贴优秀软件安全工具和文档！