Differential Drive

note

When tuning controllers, it is advised to always only modify one single parameter prior to each test. This allows the user to analyze the effect of this single parameter on the controller performance.

Tuning Flowchart

When attempting to tune the navigation controller, it is recommended to follow the following flowchart. Begin with setting up Initial Parameters for your UGV.

Differential Drive Tuning Flowchart

Condition Blocks: The condition blocks can be seen in the flowchart as RED decision blocks and either contain tests needed to be run or features of the software that are enabled/disabled.

Tuning Blocks: The tuning blocks can be seen in the flowchart as YELLOW blocks where it is expected that the user modifies a parameter prior to re-testing the controller.

Conditions Blocks

Velocity Deadband Tests

The "Velocity Deadband?" condition block asks the user to verify if there is a velocity deadband that the UGV motors must overcome in order to move the UGV. (ie. what is the minimum velocity that the UGV is required to receive, in order for it to move).

To verify this, the user should set up two missions:

1. A two Waypoint mission where the first Waypoint is located on the robot, and second Waypoint about 10-20 meters directly in front of the UGV. This will verify the requried linear motion of the UGV.
2. A two Waypoint mission where the first Waypoint is located on the robot, and second Waypoint about 10-20 meters directly behind the UGV. The first part of the navigation to this point (ie. the rotation to face the Waypoint) will verify the required angular motion of the UGV.

If the above tests demonstrate that the UGV is stuck in a velocity deadband and is not moving, then proceed to Tune Deadband.

If the above tests demonstrate that the UGV is NOT stuck in a velocity deadband (ie. it can smoothly start navigating the mission), proceed to the following condition block: Oscillation Tests.

Oscillation Tests (Straights)

The "Oscillations on Straights?" conditions block asks if there are any oscillations present during normal navigation along a straight path.

Tests

To verify the amount of oscilations around the reference path, the user should set up four missions, each of which should pass (or fall within the users requirements) prior to moving on from this section of the tuning. The start and end points of each of the tests should be identical and the start position (XY) should be noted/marked on the testing area. The first Waypoint should be placed on the robots current location and the second Waypoint should be placed 20-30m away from the start position. The four UGV orientations for each test are as follows:

1. Align the UGV with a 0° offset from the Waypoint.
2. Align the UGV with a 90° offset from the Waypoint.
3. Align the UGV with a -90° offset from the Waypoint.
4. Align the UGV with a 180° offset from the Waypoint.

See the image below to determine the placement of the UGV. The red arrow indicates the orientation of the UGV with respect to the Waypoint.

Oscillation Test Start Orientations

It is not necessary to run all of the offset tests after every parameter change. It is enough to progress systematically through the tests as they succeed. That is, if there are no oscillations during a 0° offset test, proceed to run the 45° test and so on and so forth. If the 0° offset test fails, then there is no need to run the other three tests before updating a parameter. The condition has been satisfied when all of the test results in minimal to no oscillations (or as is deemed acceptable).

Causes

Oscillations during navigation along straight paths can be caused by one or several of the following reasons. They are listed here in order of likelihood of occurence:

	Cause	Description
1	Untuned MPC parameters	The default MPC controller parameters have been tuned for CPR UGVs, and may not be the same for third-party OEM UGVs.
2	Controller delay	There may be delay present in your UGV between the time the navigation computes a command velocity and when the velocity is actually executed. This could appear as mechanical delay in your motors or even mechanical delay in the rotational components of UGVs control mechanisms . It is rarely noticed when electrical motors are used. The effect of controller delay is most commonly noticed when when entering a straight from a curve or after a pure rotation.
3	Deformable Tires	If your tires are deformable, then after coming out of a pure rotation, while the UGV may be facing the required direction of travel, a purely linear command velocity would cause the UGV to veer off to the left/right. This in turn would cause the controller to correct itself, and place it into an oscillatory motion.
4	Uneven weight distribution	If your UGV is carrying more weight on one side it may bias the UGVs motion when being command straight and veer the UGV off to one side. The controller would then consistently try to correct this bias by pulling it back towards the reference path, thus causing oscillations.

To determine if causes 3 and 4 may apply to you, it is a good idea to run the following test:

1. send the following command to rotate your UGV for one full rotation,

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.3" -r 10

2. stop the rotation by stopping the command.
3. send the following command to move the UGV purely forward,

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.5
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" -r 10

4. Analyse the motion and then stop the linear motion. If the UGV drifts left/right during step 3, then your UGV is likely to suffer from the effect of uneven weight distribution or deformable tires. (If not, there may be bigger issues). Further investigation/discussion with Clearpath would be required.

If the above tests demonstrate that the UGV is oscillating around the reference path, then proceed to Reduce Oscillations.

If the above tests demonstrate that the UGV is NOT oscillating or within an acceptable range of oscillation, proceed to onto the following condition block: Velocity Increase.

Linear Velocity Condition/Test

The "Increase Linear Velocity?" condition block asks if the user wants to increase the linear velocity that the UGV will travel along long straight paths. This does not mean that it will always drive at this velocity, but it is the velocity that it will accelerate to on straights, when starting a mission and/or when coming out of a curve.

To test this we want to create a mission that sends the UGV to a Waypoint directly in front of the UGVs current position. This requires two Waypoints: the first one located at the robots current position and the second Waypoint at least 20 meters away from the start position and will depend on the linear velocity you wish to acheive.

If during the above test, the velocity does not reach the desired velocity, proceed to Increase Linear Velocity and repeat the above test until the desired has been achieved.

If the above test results in the UGVs maximum velocity reaching the desired velocity, then proceed to checking the two subconditions: Goal Behavior and Cornering Behavior.

note

Only advance to the following condition block: Collision Avoidance if the UGVs maximum velocity has reached the desired velocity and all subconditions are also satisfied. Ie. you have tuned the corner and goal behavior to within your allowable limits.

Goal State Tests

The "Goalpoint Overshoot?" condition block asks the user to check if, at the end of a mission, the UGV overshoots the goalpoint. This condition should always be checked every time the velocity is increased. A similar test can be used as in the Velocity Increase section.

If the test results in the UGV overshooting the goal point, then proceed to Tune Goal Behavior.

If the test results in a smooth deceleration to the goal point, with the UGV completing its mission within the required tolerance, and NO overshoot, then proceed to re-verifying that your changes have not caused any oscillations by returning to the Oscillation Tests.

note

Only advance to the following condition block: Collision Avoidance if the UGVs maximum velocity has reached the desired velocity and all subconditions also are also satisfied. Ie. you have tuned the corner and goal behavior to within your allowable limits.

Corner State Tests

The "Overshoot/Oscillations in Corners?" condition block asks the user to check if the UGV overshoots the entry to corners, and/or if the UGV oscillates during the cornering. This condition should always be checked every time the velocity is increased.

A test should be designed so that the UGV will navigate a curve/corner. This type of mission can be set up regardless of whether or not you have set a specified turn radius for the path planner.

The Waypoints for the test mission should be placed such that there is sufficient change in orientation of the reference path that the controller will switch into its CORNER state. An example mission is provided below which demonstrates said Waypoint placement (although an extreme case with lots of Waypoints for vehicle with a large turn radius).

Example test mission (complex) for cornering tests

For the current case of differential drive UGVs who are able to turn on the spot, simpler test missions can be used that change the heading of the UGV more than 45°, at each Waypoint.

If the test results in the UGV overshooting the entrance to the corner/curve, then proceed to the first issue listed in Tune Corner State.

If the test results in the UGV oscillating during the corner/curve, then proceed to the second issue listed in Tune Corner State.

If the test results in the UGV navigating smoothly through all curves/corners, with no minimal oscillations and no overshoot, then proceed to re-verifying that your changes have not caused any oscillations during straights (Oscillation Tests).

note

Only advance to the following condition block: Collision Avoidance if the UGVs maximum velocity has reached the desired velocity and all subconditions are also satisfied. Ie. you have tuned the corner and goal behavior to within your allowable limits.

Collision Avoidance Condition

The final condition block asks the user if collision avoidance is enabled or disabled on the UGV.

If collision avoidance is DISABLED, then you have completed tuning your UGV. Congratulations!

When collision avoidance is ENABLED, it is recommended to begin with a straight line test as in Velocity Increase but with an obstacle placed roughly in the center of the predicted path (ie. at the midpoint between the start and goal locations). This obstacle should be at least 30 cm tall and at least 10 cm wide. The user should be ready to emergency stop the UGV if they notice it approaching the obstacle too fast or too closely.

If the test results in any unexpected behaviour, such as:

1) not stopping in front of the obstacle,

2) not navigating after stopping in front of the obstacle,

3) jittery motion navigating around the obstacle,

then proceed to the respective issue listed in Tune Collision Avoidance Behavior.

Tuning Blocks

Initialize Parameters

1. Update the footprint offsets of the UGV in the ${HOME}/cpr_outdoornav_launch/outdoornav_tuning.env file.

Differential Drive Tuning Flowchart

Environment Variable	Description	Sign
FOOTPRINT_OFFSET_POS_X	Distance from base_link to the furthest edge of any part/sensor at the front of the UGV.	+
FOOTPRINT_OFFSET_NEG_X	Distance from base_link to the furthest edge of any part/sensor at the left of the UGV.	-
FOOTPRINT_OFFSET_POS_Y	Distance from base_link to the furthest edge of any part/sensor at the rear of the UGV.	+
FOOTPRINT_OFFSET_NEG_Y	Distance from base_link to the furthest edge of any part/sensor at the right of the UGV.	-

2. Update the vehicle_length parameter to be the longitudinal length (ie. along the x-axis) of your UGV. This is not the sum of the above footprint x-axis magnitudes, but only the length of the UGV body.

3. Update the following parameters with your UGV specific maximums and minimums:

   • max_fwd_velocity
   • min_fwd_velocity
   • max_rev_velocity
   • max_ang_velocity
   • max_accel
   • max_decel
   • max_ang_accel
   • max_lateral_accel

4. Update the stiction_compensator_fwd and stiction_compensator_yaw to be equivalent to the minimum linear and angular velocities that the UGV requires it to move (eg. stiction_compensator_fwd would be equal to min_fwd_velocity).

Tune Velocity Deadband

To tune the velocity deadband, the user should at least start off by knowing the minimum linear and angular velocities that the UGV motors can generate.

If not already set from the Initial Parameters section, you should set the min_fwd_velocity to the minimum forward linear velocity that the UGV can handle.

For each iteration of the linear deadband tuning, increment the following parameters by 0.05 m/s , until smooth linear motion is acheived from standstill:

• min_fwd_velocity
• stiction_compensator_fwd

For each iteration of the angular deadband tuning, increment the following parameters by 0.02 rad/s , until smooth angular motion is acheived from standstill:

• stiction_compensator_yaw

Reducing Oscillation (Straights)

To tune the UGV behavior on straights when dealing with the simple cause of untuned MPC controller parameters, we list the following parameters along with likely ranges and their initial default values. The are listed in order of which parameter will have the most effect of improving the performance of the current tuning block.

Parameter	Likely Range*	Default
yaw_weight	90 - 120	18.0
y_weight	12 - 18	14.0
endpoint_multiplier	10 - 20	10
max_lookahead	2.5 - 3.5	2.5
x_weight	14 - 16	14.0
lookahead_factor	1.4 - 1.6	1.45
mpc_opt_maxiteration*	10 - 15	13
discretization_steps*	6 - 10	8

note

* These parameters should be changed with caution.

When you are experiencing oscillations due to cause #2 (controller delay) try the modifying the following parameters:

Parameter	Likely Range*	Default
enable_delay_compensation	true	false
controller_delay	depends on your delay in your system. (usually between 300 - 1000 will provide changes to the performance)	NA

No available solutions yet to resolve oscillations due to causes #3 and #4. Please contact Clearpath customer support at support@clearpathrobotics.com to discuss any related issue.

Increase Linear Velocity

The following instructions are only relevant if you are planning on increasing the velocity of the UGV above 2.0 m/s.

To increase the UGVs maximum velocity, the following parameters should be modified.

First, the mpc_horizon parameter should be set to: 1.0

The following parameters should then be modified to the desired velocity ($v_{des}$). Ie. if $v_{des}=4.0$ m/s, then you should modify both parameters to be 4.0.

Parameter	Value
max_fwd_velocity	$v_{des}$
max_lookahead	$v_{des}$

Finally, if you are increasing the velocity to a value greater than or equal to 4.0 m/s or if you are seeing that the velocity is being limited even after changing the above parameters, it is possible to try and increase the size (width and height) of the local costmap in file: ~/cpr_outdoornav_launch/onav_robots/params/platform/navigation/costmap_local.yaml. The limit in the velcoity is due to the fact that the lookahead distance at higher speeds ends up exceeding the size of the local costmap. By increasing it, we allow the controller to lookahead further and allow for that velocity increase.

Tune Goal State

A few different issues may have occurred that led you to tuning the goal behavior. We list a few of them and the possible parameters that can be modified to improve the performance in and around the goal point.

1) Overshooting the goal point can be caused by a few factors:

• Not decelerating fast enough or soon enough as the UGV approaches the goal.

To tune for this issue, some parameters are listed in order in which they will have the most effect of improving the performance of the current tuning block:

Parameter	Default
goal_horizon_threshold	2.0
goal_slowdown_multiplier	1.5
goal_horizon_threshold	0.8
endpoint_multiplier_goal	20
x_weight_goal	14.0
fwd_v_weight_goal	0.01
fwd_a_weight_goal	8.0

• The autonomy core not considering the goal complete because it has too high of a velocity as it starts entering the goal tolerance.

To to tune for this issue, modify the following parameter:

Parameter	Value	Default
max_speed_at_goal	depends on the deceleration of your UGV	0.8

2) Oscillations (or jittery behavior) around the goal point are typically caused by the the tolerance at the goal point being set too small. If they are too small and the UGV is unable to localize itself within the required position and yaw tolerances, then it will jitter and rotate to attempt to be a precise as you are expecting it to be. To resolve this, it is advised to slightly increase the goal tolerance parameters:

Parameter	Default
goal_tolerance_xy_rtk	0.1
goal_tolerance_yaw_rtk	0.25
goal_tolerance_xy_nortk	0.3
goal_tolerance_yaw_nortk	0.5

note

If your UGV has RTK corrections, then it is only required to modify goal_tolerance_xy_rtk and goal_tolerance_yaw_rtk. Otherwise, only modify goal_tolerance_xy_nortk and goal_tolerance_yaw_nortk.

A complete list of parameters that relate to the GOAL state can be found here, however, the above set of parameters is likely to satisfy the users requirements.

Tune Cornering State

A few different issues may have occured that led you to tuning the cornering behavior. We list a few of them and the possible parameters that can be modified to improve the performance approaching as well as in the corner.

1) Overshooting the corner only to enter the turn late

This issue occurs most often, when the controller is unable to slow the UGV down early and/or fast enough, prior to entering the corner. A reduction in velocity earlier in the approach to the corner will improve the cornering performance. Below some parameters are listed that will allow the user to improve the behavior entering corners:

Parameter	Default
corner_segment_slowdown	false
corner_lookahead	5.0
corner_detection_threshold	0.15
corner_slowdown_multiplier	1.5

2) Oscillations during the corner

To reduce the oscillations in the corners, some parameters are listed in order in which they will have the most effect of improving the performance:

Parameter	Default
endpoint_multiplier_corner	20.0
curvature_slowdown	0.15
controller_delay	0
y_weight	14.0
x_weight	14.0

The complete list of parameters that relate to the CORNER state can be found here, however, the above set of parameters is likely to satisfy the users requirements.

Tune Collision Avoidance

A few different issues may have occured that led you to tuning the collision avoidance. We list a few of them and the possible parameters that can be modified to improve the performance approaching as well as navigating around detected obstacles.

1) UGV not stopping in front of the obstacle

This issue would either occur if the collision avoidance has been disabled, or if the detection sensors are not populating the costmap with the obstacle(s). To enable/disable collision avoidance:

Parameter	Default
ENABLE_COLLISION_AVOIDANCE	true

For the case where the obstacle that you are using is not being detected by the detection sensors, make sure that it is neither too short nor too thin. Use RViz to check that the local costmap is being populated with the obstacle.

2) UGV not navigating after stopping in front of an obstacle

This issue is either due to the robot unable to replan around the obstacle, in which case the following parameters may help:

Parameter	Default
inflation radius	1.0
cost_scaling_factor	0.5
max_decel	0.5
max_accel	0.5

or that the obstacle has gotten inside of the safety footprints causing the navigation to place the UGV in a collision state:

Consult the Footprint Tuning instructions to increase the safety footprint sets. The user can also try increasing the stop distance from the UGV to an obstacle as it decelerates to a stop.

Parameter	Default
enable_stop_distance radius	false
obstacle_stop_distance	3.0

3) UGV has jittery motion navigating around the obstacle,

This issue is typically a problem when the safety footprints collide with the obstacle as it is trying to navigate the replanned path around the obstacle:

Consult the Footprint Tuning instructions to modify the safety footprint sets.

The user can also try increasing the stop distance of the UGV to an obstacle.

Parameter	Default
enable_stop_distance radius	false
obstacle_stop_distance	3.0

4) UGV suddenly reducing its velocity as it accelerates up to maximum

This issue is due to the safety footprints exceeding the length of the local costmap. Since the safety footprints increase in length as the velocity increases, the user must also increase the size of the local costmap. This allows the safety footprints to extend to their entire length without triggering a "false" obstacle detection due to the footprint exceeding the local costmap size. Increase the costmap using these parameters:

Parameter	Default
local_costmap/width	$2 * l_{max\_footprint} + 0.5$
local_costmap/height	$2 * l_{max\_footprint} + 0.5$

where l_{max_footprint} is the length of the largest safety footprint.