LOS guidance backstepping

Jaehyeong Hwang started writing on 28.11.2020
breaking down the code of los_guidance_backstepping.py

Module explanations line 9 ~ line 24

vortex_msgs.msgs

contains all custom ROS message types used in all workspaces

PropulsionCommand : Provides normalized motion command in float64 type and control mode in bool type

nav_msgs

defines the common messages used to interact with the navigation stack.

Odometry : represents an estimate of a position and velocity in free space.

geometry_msgs

provides messages for common geometric primitives such as points, vectors, and poses. These primitives are designed to provide a common data type and facilitate interoperability throughout the system.

Wrench : This represents force in free space, separated into its linear and angular parts.
PoseStamped : A Pose with reference coordinate frame and timestamp.
Pose : A representation of pose in free space, composed of position and orientation.

tf.transformations.euler_from_quaternion(quaternion, axes='sxyz')

Return Euler angles from quaternion for specified axis sequence.

tf.transformations.quaternion_from_euler(ai, aj, ak, axes='sxyz')

Return quaternion from Euler angles and axis sequence.

Dynamic_reconfigure

purposes on providing a standard way to expose a subset of a node's parameters to external reconfiguration

from los_guidance.cfg import AutopilotConfig

Describes parameters such as look-ahead distance, proportional gain, integrarl gain, derivative gain,saturation

actionlib

actionlib stack provides a standardized interface for interfacing with preemptable tasks. For example, moving the base to a target location. actionlib will import class SimpmleActionServer, functions register_goal_callback, register_preempt_callback, start, publish_feedback, set_succeeded, is_preempt_requested, set_preempted, accept_new_goal These are all described in https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/action_server.py

~~cannot find LosPathFollowingAction, LosPathFollowingGoal, LosPathFollowingFeedback~~

AutopilotBackstepping

This refers to BacksteppingDesign in the file backstepping_control.py, which defines acceleration proportional values and velocity proportional values that should be used in mass matrix M.class AutopilotBackstepping updates gains for the system inertia matrix M. System inertia matrix M=M_RB+M_A for underwater vehicle follows symmetry considerations

$\begin{array}{c} M ={\left[\begin{array}{cccc} m-X_{\dot{u}} & -X_{\dot{v}} & -X_{\dot{w}} \\ -X_{i} & m-Y_{\dot{v}} & -Y_{\dot{w}} \\ -X_{\dot{w}} & -Y_{\dot{w}} & m-Z_{\dot{w}} \\ -X_{\dot{p}} & -m z_{g}-Y_{\dot{p}} & m y_{g}-Z_{\dot{p}} \\ m z_{g}-X_{\dot{q}} & -Y_{\dot{q}} & -m x_{g}-Z_{\dot{q}} \\ -m y_{g}-X_{\dot{r}} & x_{g}-Y_{\dot{r}} & -Z_{\dot{r}} \\ -X_{\dot{p}} & m z_{g}-X_{\dot{q}} & -m y_{g}-X_{\dot{r}} \\ -m z_{g}-Y_{\dot{p}} & -Y_{\dot{q}} & m x_{g}-Y_{\dot{r}} \\ m y_{g}-Z_{\dot{p}} & -m x_{g}-Z_{\dot{q}} & -Z_{\dot{r}} \\ I_{x}-K_{\dot{p}} & -I_{x y}-K_{\dot{q}} & -I_{z x}-K_{\dot{r}} \\ -I_{x y}-K_{\dot{q}} & I_{y}-M_{\dot{q}} & -I_{y z}-M_{\dot{r}} \\ -I_{z x}-K_{\dot{r}} & -I_{y z}-M_{\dot{r}} & I_{z}-N_{\dot{r}} \end{array}\right]} \end{array}$

AutopilotPID : defines basic pid controller

from reference_model.discrete_tustin import ReferenceModel

From discrete_tustin.py class ReferenceModel, determines the reference model by using Tustin’s method. Tustin's method was used for transform continuous time system to discrete time system in reference model. I won't go into too much in this theory. But by choosing the damping ratio as 1.0 and natural frequency as 0.1 we can get a critically damped and rapid reference model. By using Matlab, we can get the transfer function.

Module explanations line 61 ~ line 74

updateState

is used in callback function, specified in line 213. It will update position and velocities.

Module explanations line 76 ~ line 84

setWayPoints

updates next way points.

Module explanations line 86 ~ line 88

Distance

calculates distance between current position and the next waypoint.

Module explanations line 90 ~ line 92

sphereofAcceptance

suggests condition of the distance that is allowable.

Module explanations line 93 ~ line 109

getEpsilonVector

resulted epsilon vector is the coordinates of the AUV in the path-fixd reference frame for a straight line going from the reference point to target position the path-tangential angle alpha is used for rotation matrix. transpose matrix of Rotation matrix is denoted as R_T and is used to calculate epsilon vector.

Module explanations line 113 ~ line 117

quat2euler

Describes transformation from quaternion to euler angle. Euler_from_quaternion From tf.transformations, euler angle yaw can be calculated.

Module explanations line 121 ~ line 150

lookaheadBasedSteering

lookahead-based steering is less computationally intensive than enclosure-based steering.

In order to get to the intersection point p_los,

manta has to turn its angle as parallel as the straight line segment. It is expressed as self.chi_p.
Then, it has to turn its heading directly to the p_los. The angle is expressed as self.chi_r

For lookahead-based steering, the course angle assignment is separated into two parts

which is a path-tangential angle, while,

$\chi_r(e) := arctan(\frac{-e}{\Delta})$

So the desired heading self.chi_d is calculated from the sum of path-tangential angle and velocity-path relative angle.

Module explanations line 152

~~_feedback, _result cannot find the source~~

Module explanations line 158 ~ line 184

rospy.init_node('los_path_following')

Initializes ROS node by specifying name of the node as los_path_following.

self.sub = rospy.Subscriber('/odometry/filtered',Odometry, self.callback, queue_size=1)

‘/odometry/filtered’ signifies the topic name, we are going to subscribe this topic. Odometry topic is going to be used for this. Queue_size is the size of outgoing message queue used for asynchronous publishing. A good queue size is different depending on variables. Briefly speaking, bigger queue size will only use more memory

self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1)

Node is publishing to the /manta/thruster_manager/input, using the message type Wrench, with queue size 1.

self.pub_desired = rospy.Publisher('/manta/los_desired', Odometry, queue_size=1)

As same as the code in line 164, this node is publishing to the /manta/los_desired , using Odometry message type with queue size 1

Module explanations line 168 ~ line 171

assigns requisite classes.

Module explanations line 174 ~ line 184

unclear

Module explanations line 189 ~ line 193

Geometric task should be satisfied. It is to force the system output to converge to a desired path parametrized by a continuous scalar variable

IN order to reference model work, we have to put heading angle as continuous. However arctangent function, which was used to obtain heading angle is discontinuous at -pi and pi. So before being input to reference model, \psi_ref is being wrapped. From line 197 to 198 Tustin method which was also used in pd_quaternion controller, is being used to calculate desired heading. Result of reference model \psi-d then will be un-wrapped from line 200 to 210.

tustin

def callback(self, msg):

After updating current position, velocities, and goal. Position was updated by quat2euler function described in line from 113 to 119 Velocities was updated by updateState function described in line from 61 to 74 Goal was updated by lookaheadBasedSteering function described in line from 121 to 150. As written in the description from line 230 to 233, assuring wrapping by executing fixHeadingWrapping function Desired speed u_d, desired acceleration, u_d_dot Desired heading psi_d, desired yaw rate r_d, rate of desired yaw rate r_d_dot are assigned by the result of reference model.

Module explanations line 243 ~ line 253

By using Odometry.msg included in nav_msgs of ROS, we can estimate position and velocity. Quaternion converted from euler angles are assigned to estimated position. Desired velocity u_d and yaw rate r_d are assigned to estimated velocity by using Twist(TwistwithCovariance message)

self.pub_desired.publish(los_msg)

los_msg is published to /manta/los_desired topic, that we described in line 165

Module explanations line 256

tau_d = self.autopilot.backstepping.controlLaw(self.los.u, self.los.u_dot, u_d, u_d_dot, self.los.v, self.psi, psi_d, self.los.r, r_d, r_d_dot)

tau

The desired tau is set like this. Which makes the Lyapunov function less than 0,which eventually makes the system UGAS(Uniformly Globally Asymtotically Stable).

Code break

autopilot is AutopilotBackstepping class in autopilot.py file, which looks like this.

Inside AutopilotBackstepping class, it uses BacksteppingControl class which is described in the file backstepping_control.py And the control law function returns tau,

Which is the result of Lyapunov’s Indirect method to decide whether the system is stable or not.

Module explanations line 257

PID is autopilot.py and depthController function look like this in the file.

Error e is derived by the difference between desired depth and current depth. Control force tau will be calculated by regulate function. regulate function is originated from PIDregulator class in PIDregulator.py

P = 25.0, I = 0.024, D = 3.5, Sat = 5.0 were used for pid controller.

Wrench() represents force separated into linear and angular, force and torque. Thrust force in x direction is first row of tau_d. Likewise thrust force in y and z direction are assigned by second row of tau_d and tau_depth_hold each. Third row of tau_d is assigned to torque in z direction. Thrust_msg is published to /manta/thruster_manager/input topic, that we described in line 164

HOME User Manual

Development Guidelines

Documentation

Theory and Resources

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

LOS guidance backstepping

Clone this wiki locally