The Model Predictive Control (MPC) is an optimization-based feedback control technique, which was initially developed for the field of process industries. With the recent advancements in cost-effective computation, MPC was extended to the field of robotics for real-time control. This means that the optimization happens on the fly. MPC otherwise also called as the Receding Horizon Control (RHC) is utilized in robots for the purpose of path following and / or trajectory tracking.
neo_mpc_planner2 is exclusively developed for quasi-omnidirectional robots such as Neobotix MPO-700, but can also be used on mecanum-wheeled robots such as Neobotix MPO-500. MPO-700 is used for transporting various objects in a structured environment. Therefore this planner aims in catering to all the omni-directional robots used in a known environment. The controller was tested on an Intel i5 processor and was found that it can easily work at an average rate of 50 Hz. This would help to instantaneously react to suddenly occurring obstacles. Currently, the planner is designed in a way to stop the robot if there is an unknown obstacle in the predicted path.
The entire planner is built on the Navigation2 stack for ROS-2. Currently, we support omni-directional robots and later will be extended to differential robots.
If you are interested in the theoretical background about the MPC, then go to this link.
- With smac_planner (Simulation):
Python 3.8.10 Scipy 1.6.3 neo_nav2_py_costmap2D (https://github.com/neobotix/neo_nav2_py_costmap2D) neo_srvs2
controller_server:
ros__parameters:
# controller server parameters (see Controller Server for more info)
controller_plugins: ["FollowPath"]
controller_frequency: 30.0
controller_plugin_types: ["neo_mpc_planner::NeoMpcPlanner"]
goal_checker_plugins: ["general_goal_checker"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 100.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.03
yaw_goal_tolerance: 0.03
stateful: True
FollowPath:
plugin: "neo_mpc_planner::NeoMpcPlanner"
lookahead_dist_min: 0.4
lookahead_dist_max: 0.4
lookahead_dist_close_to_goal: 0.4
control_steps: 3
mpc_optimization_server:
ros__parameters:
acc_x_limit: 2.5
acc_y_limit: 2.5
acc_theta_limit: 3.0
min_vel_x: -0.7
min_vel_y: -0.7
min_vel_trans: -0.7
min_vel_theta: -0.7
max_vel_x: 0.7
max_vel_y: 0.7
max_vel_trans: 0.7
max_vel_theta: 0.7
# Translation error weight
w_trans: 0.82
# Orientation error weight
w_orient: 0.50
# Control error weight
w_control: 0.05
# Terminal weight
w_terminal: 0.05
# Footprint weight
w_footprint: 0
# Costmap weight
w_costmap: 0.05
# Waiting time before the robot can try a maneuver, after it had been stuck in the obstacle
waiting_time: 3.0
low_pass_gain: 0.5
# Optimization tolerance, smaller it is, slower the performance
opt_tolerance: 1e-3
# Time period, upto which MPC has to predict the control commands
prediction_horizon: 0.8
# Number of steps that the prediction horizon needs to be splitted into
control_steps: 3
In the scenarios, where the global planner tries to make the robot to move through a narrow path, it is suggested to the users to set a short lookahead_dist_min
and lookahead_dist_max
distance, so that the robot can slow down and navigate through the narrow path without ending up in scanner stop. Of course, the speed depends upon the predicition horizon and the control steps as well.
Note that the mpc_optimization server is a seperate node, since the optimization depends on the Scipy library. If mpc_optimization node is being used, make sure also to point the node to the parameter file. An example would be:
ros2 run neo_mpc_planner2 mpc_optimization_server.py --ros-args --params-file src/neo_simulation2/configs/mpo_700/navigation.yaml
In the near future we plan to migrate the optimization process to C++.
Feel free to open an issue for any feature requests or bugs.
Special mention: Some of the code from nav2_regulated_pure_pursuit controller has been reused, since the neo_mpc_planner2 works on the principle of pure-pursuit as well. Thanks to the Nav2 team for that.