trajectory_tracking_mpc

Distributed nonlinear model predictive control software for the trajectory tracking of an autonomous underwater vehicle.

Overview ——

A Computationally efficient implementation of NMPC for AUV to track a time-varying trajectory, by exploiting the fact that at low-speed applications the six DOF equations of motion of an underwater vehicle can be divided into two slightly coupled subsystems for diving and maneuvering, therefore the subproblems are solved in parallel which significantly reduces the computation time of the large NMPC problem.

Table of contents ——

• Prerequisites
• Dependencies
• Nodes
• Usage
• Implementation
• References

Prerequisites ——

• ROS2 - Eloquent.

Dependencies ——

• CppAD
• Ipopt 3.12.x
• Eigen3
• Ceres

Nodes ——

guidance_node

• Published topics:
  • /guidance/MMPC of type custom_ros_interfaces/msg/MMPC. Custom ros2 msg contains the required states by the maneuvering model predicive controller, namely AUV state, Reference state and Error state.
  • /guidance/DMPC of type custom_ros_interfaces/msg/DMPC. Custom ros2 msg contains the required states by the depth model predicive controller, namely AUV state, Reference state and Error state
• Subscribed topics:
  • /rexrov/pose_gt of type nav_msgs/msg/Odometry. This is the current state of the vehicle. The odometry msg includes pose and twist information.
  • /guidance/point of type geometry_msgs/msg/Point. This is a desired 3D waypoint..
• Parameteres:

maneuver_node

• Published topics:
  • /maneuver/tau of type geometry_msgs::msg::WrenchStamped. The output control forces, namely Fx , Fy and Mz.
• Subscribed topics:
  • /guidance/MMPC of type custom_ros_interfaces::msg::MMPC. Custom ros2 msg contains the required information for the maneuver MPC to work, namely the AUV-state, the Reference-state and the Error-state.
• Parameteres:

depth_node

• Published topics:
  • /depth/tau of type geometry_msgs::msg::WrenchStamped. The output control forces, namely Fz.
• Subscribed topics:
  • /guidance/DMPC of type custom_ros_interfaces::msg::DMPC. Custom ros2 msg contains the required information for the depth MPC to work, namely the AUV-state, the Reference-state and the Error-state.

Parameters —— | Parameter | Description | |——————|——————————————————————————————————————————–| | N | Prediction horizon. | | dt | Sampling time. | | w_ze | Cost function weight on the heave z tracking error. | | w_psie | Cost function weight on the pitch tracking error. | | w_we | Cost function weight on the heave speed w tracking error. | | w_Fz | Cost function weight on using the heave actuation. | | w_My | Cost function weight on using the pitch actuation. | | w_Fz_dot | Cost function weight on the rate of using the heave actuation. | | w_My_dot | Cost function weight on the rate of using the pitch actuation. | | Mz | Total heave mass, rigid body mass + added mass. | | Mtheta | Total pitch inertia,rigid body inertia Iyy + added inertia. | | LDz | Linear damping in heave. | | LDtheta | Linear damping in pitch. | | QDz | Quadratic damping in heave. | | QDtheta | Quadratic damping in pitch. |

thrust_merger_node

• Published topics:
  • /rexrov/thruster_manager/input_stamped of type geometry_msgs::msg::WrenchStamped. The combined output control forces, namely Fx , Fy , Mz and Fz.
• Subscribed topics:
  • /maneuver/tau of type geometry_msgs::msg::WrenchStamped. The control forces from the maneuvering sub-system.
    • /depth/tau of type geometry_msgs::msg::WrenchStamped. The control force from the diving sub-system.

Usage ——

• Launching the nodes:

  $ ros2 launch trajectory_tracking_mpc mpc.launch.py

• Sending a reference waypoint:

  $ ros2 topic pub --once /guidance/waypoint geometry_msgs/msg/Point "{x: 48.5 , y: 25.5, z: -4.0}"

Implementation ——

Trajectory Generation

The objective in trajectory tracking control is to force the system output η(t) = [x(t), y(t), z(t)] and ν(t) = [u(t), v(t), w(t)] to track a desired output ηd(t) = [xd(t), yd(t), zd(t)] and νd(t) = [ud(t), vd(t), wd(t)]. The choice of trajectory generator should reflect the dynamics of the vehicle such that a feasible trajectory is constructed. For instance, it is important to take into account physical speed and acceleration limitations of the vehicle. For this purpose we used a Quintic polynomial (5th order) trajectory generator for smooth and jerk-minimum motion. The software for this system is based on auv_gnc library, where we feed it with the start state [position, velocity and acceleration] and the end state as well as some physical constraints, then we get a variable with a data-type auv_guidance::BasicTrajectory which can be evaluted at any time-stamp to get the reference state basicTrajectory->computeState(t) which helds the reference position and velocity in 3D and basicTrajectory->computeAccel(t) which helds the reference acceleration.

Steering Law

The LOS guidance method is used for generating heading reference. In LOS guidance, the vehicle follows a LOS vector pointing from a reference point to a point (x_los , y_los ) located at distance ∆ ahead of the projection point of the vehicle along the path. Based on geometric calculations the reference heading is ψd = χd.

Distributed MPC.

The maneuvering subsystem includes the states ζ = [x, y, ψ, u, v, r] with control signal λ = [Fu , Fv , Fr ], while the depth subsystem includes the states ξ = [z, φ, θ, w, p, q] with control signal Λ = [Fw], therefore the total control signal is τ = [Fu , Fv , Fw , 0, 0, Fr ]. The success of this distribution is based on two assumptions must be true: (1) Low-speed motion, (2) The roll φ and pitch θ angles are ≈ 0, i.e., the vehicle is stable by default in these angles.

MPC uses a model of the controlled system to make predictions about future system outputs. It solves an optimization problem at each time step given a defined cost-function to find the optimal control action that drives the predicted system output to the desired reference as close as possible. That’s why it’s also called a a receding horizon control.

References ——

[1] Handbook of Marine Craft Hydrodynamics and Motion Control, Thor I. Fossen.

[2] C. Shen, Y. Shi, and B. Buckham. Distributed implementation for nonlinear model predictive tracking control of an AUV. IEEE/ASME Transactions on Mechatronics, under review, 2017.

[3] C. Shen, Y. Shi, and B. Buckham. Integrated path planning and tracking control of an AUV: A unified receding horizon optimization approach. IEEE/ASME Transactions on Mechatronics, 22(3):1163 – 1173, 2017.