This ROS2 package enables an F1/10 car to dock autonomously at a docking station using a Model Predictive Control (MPC) algorithm. The package employs a custom objective function with configurable constraints specified in the config.yaml file. The vehicle's kinematics are modeled using a bicycle model.
This ROS2 package operates as an action server designed to manage the docking process. Users can send desired positions to the server using a custom Docking message type.
The video showcases the F1/10 car performing the docking maneuver using the MPC-based control algorithm at a designated docking station.
docking_mpc.mp4
-
: x-position of the car -
: y-position of the car -
: Orientation angle of the car (heading) -
: Steering angle
-
: Linear velocity of the car -
: Steering rate (rate of change of steering angle)
-
: Wheelbase (distance between front and rear wheels)
The model uses the following continuous-time equations to describe the evolution of the state variables:
The objective function is central to the MPC, guiding the controller to make optimal decisions by balancing multiple factors such as position accuracy, orientation, steering angles, and control efforts.
-
Stage Cost:
The stage cost evaluates the performance at each step within the prediction horizon. It consists of the following terms:
-
Terminal Cost:
The terminal cost evaluates the performance at the end of the prediction horizon. In this project, the terminal cost is set to be equal to the stage cost:
-
Control Effort Penalties:
To avoid aggressive control actions, penalties are applied to the control inputs:
The overall objective function combines the terminal and stage costs:
where
Edit the config file:
nvim f1tenth_docking/config/config.yaml
Start the docking action server:
ros2 launch f1tenth_docking f1tenth_docking.launch.py
Mock the vesc servo pose:
ros2 topic pub -r 100 /vesc/core vesc_msgs/VescStateStamped '{state: {servo_pose: 0.0}}'
Mock the optitrack pose:
ros2 topic pub -r 100 /optitrack/rigid_body_0 geometry_msgs/PoseStamped '{pose: {position: {x: 1.0, y: 2.0, z: 3.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}'
Set the control loop setpoint:
ros2 action send_goal docking_action_server f1tenth_docking_interfaces/action/Docking "{setpoint: {x_pos: 497, y_pos: 2.0, theta: 0.0, delta: 0.0}}"