Move to a Pose Control
In this section, we present the logic of PathFinderController that drives a car from a start pose (x, y, theta) to a goal pose. A simulation of moving to a pose control is presented below.
Position Control of non-Holonomic Systems
This section explains the logic of a position controller for systems with constraint (non-Holonomic system).
The position control of a 1-DOF (Degree of Freedom) system is quite straightforward. We only need to compute a position error and multiply it with a proportional gain to create a command. The actuator of the system takes this command and drive the system to the target position. This controller can be easily extended to higher dimensions (e.g., using Kp_x and Kp_y gains for a 2D position control). In these systems, the number of control commands is equal to the number of degrees of freedom (Holonomic system).
To describe the configuration of a car on a 2D plane, we need three DOFs (i.e., x, y, and theta). But to drive a car we only need two control commands (theta_engine and theta_steering_wheel). This difference is because of a constraint between the x and y DOFs. The relationship between the delta_x and delta_y is governed by the theta_steering_wheel.
Note that a car is normally a non-Holonomic system but if the road is slippery, the car turns into a Holonomic system and thus it needs three independent commands to be controlled.
PathFinderController class
Constructor
PathFinderController(Kp_rho, Kp_alpha, Kp_beta)
Constructs an instantiate of the PathFinderController for navigating a 3-DOF wheeled robot on a 2D plane.
Parameters:
- Kp_rho : The linear velocity gain to translate the robot along a line towards the goal
- Kp_alpha : The angular velocity gain to rotate the robot towards the goal
- Kp_beta : The offset angular velocity gain accounting for smooth merging to the goal angle (i.e., it helps the robot heading to be parallel to the target angle.)
Member function(s)
calc_control_command(x_diff, y_diff, theta, theta_goal)
Returns the control command for the linear and angular velocities as well as the distance to goal
Parameters:
- x_diff : The position of target with respect to current robot position in x direction
- y_diff : The position of target with respect to current robot position in y direction
- theta : The current heading angle of robot with respect to x axis
- theta_goal : The target angle of robot with respect to x axis
Returns:
- rho : The distance between the robot and the goal position
- v : Command linear velocity
- w : Command angular velocity
How does the Algorithm Work
The distance between the robot and the goal position, \(\rho\), is computed as
The distance \(\rho\) is used to determine the robot speed. The idea is to slow down the robot as it gets closer to the target.
Note that for your applications, you need to tune the speed gain, \(K_P{_\rho}\) to a proper value.
To turn the robot and align its heading, \(\theta\), toward the target position (not orientation), \(\rho \vec{u}\), we need to compute the angle difference \(\alpha\).
The term \(mod(2\pi)\) is used to map the angle to \([-\pi, \pi)\) range.
Lastly to correct the orientation of the robot, we need to compute the orientation error, \(\beta\), of the robot.
Note that to cancel out the effect of \(\alpha\) when the robot is at the vicinity of the target, the term
\(-\alpha\) is included.
The final angular speed command is given by
The linear and angular speeds (Equations (1) and (2)) are the output of the algorithm.
Move to a Pose Robot (Class)
This program (move_to_pose_robot.py) provides a Robot class to define different robots with different specifications. Using this class, you can simulate different robots simultaneously and compare the effect of your parameter settings.
Note: The robot class is based on PathFinderController class in ‘the move_to_pose.py’.
Robot Class
Constructor
Robot(name, color, max_linear_speed, max_angular_speed, path_finder_controller)
Constructs an instantiate of the 3-DOF wheeled Robot navigating on a 2D plane
Parameters:
- name : (string) The name of the robot
- color : (string) The color of the robot
- max_linear_speed : (float) The maximum linear speed that the robot can go
- max_angular_speed : (float) The maximum angular speed that the robot can rotate about its vertical axis
- path_finder_controller : (PathFinderController) A configurable controller to finds the path and calculates command linear and angular velocities.
Member function(s)
set_start_target_poses(pose_start, pose_target)
Sets the start and target positions of the robot.
Parameters:
- pose_start : (Pose) Start postion of the robot (see the Pose class)
- pose_target : (Pose) Target postion of the robot (see the Pose class)
move(dt)
Move the robot for one time step increment
Parameters:
- dt : <float> time increment
References
PathFinderController class
P. I. Corke, “Robotics, Vision and Control” | SpringerLink p102