Graceful Controller

This implement a controller based on “A Smooth Control Law for Graceful Motion of Differential Wheeled Mobile Robots in 2D Environments” by Park and Kuipers, ICRA 2011.

Currently this does not implement section IV.B of the paper (which describes how to switch between poses along a path). Instead this controller has a naive approach which attempts to find the farthest pose in the path which is both A) less than some maximum lookahead distance away, and B) reachable (without collision) using our control law (as determined by a forward simulation). We call this the target_pose.

There is a more detailed write up of the motiviation behind creating yet another local controller on my blog

ROS Topics

As with nearly all navigation local controllers, our controller outputs both a global path and a local path as a nav_msgs::Path.

The “local path” is derived through a forward simulation of where the robot is expected to travel by iteratively applying the control law. Unlike the DWA controller, our forward simulation is NOT based on applying the same command throughout a fixed simulation time, but rather based on calling our control law multiple times as we forward simulate our path to the “target pose”. As long as the acceleration limits are correctly specified, and the robot controllers can execute the velocities commanded, this will be the true path that the robot will follow.

An additional topic that is unique to our controller is the “target_pose” topic which is a geometry_msgs::PoseStamped. This is published every time we compute the command velocity for the robot and tells you which pose in the global plan we are using for computing the control law. It is potentially helpful in debugging parameter tuning issues.

Parameters

The underlying control law has several parameters which are best described by the original paper, these include:

• k1 - controls convergence of control law (slow subsystem). A value of 0 reduces the controller to pure waypoint-following with no curvature. For a high k1 value, theta will be reduced faster than r.
• k2 - controls convergence of control law (fast subsystem). A higher value of k2 will reduce the distance of the path to the target, thus decreasing the path’s curvature.
• lambda - controls speed scaling based on curvature. A higher value of lambda results in more sharply peaked curves.
• beta - controls speed scaling based on curvature. A higher value of beta lets the robot’s velocity drop more quickly as K increases. K is the curvature of the path resulting from the control law (based on k1 and k2).

Several parameters are used for selecting and simulating the target pose used to compute the control law:

• max_lookahead - the target pose cannot be further than this distance away from the robot. A good starting value for this is usually about 1m. Using poses that are further away will generally result in smoother operations, but simulating poses that are very far away can result in reduced performance, especially in tight or cluttered environments. If the controller cannot forward simulate to a pose this far away without colliding, it will iteratively select a target pose that is closer to the robot.
• min_lookhead - the target pose cannot be closer than this distance away from the robot. This parameter avoids instability when an unexpected obstacle appears in the path of the robot by returning failure, which typically triggers replanning.
• acc_dt - this parameter is used to set the maximum velocity that the control law can use when generating velocities during the path simulation.

There are several major “features” that are optional and configured through one or more parameters:

• odom_topic - by setting this parameter to the name of the odometry topic, the controller will subscribe to the odometry topic and limit the control law velocities to those that are feasible based on the current robot velocity. Without odometry, performance will be degraded.
• use_vel_topic - in ROS1 there is no standard way of defining a “speed limit map”. If this parameter is set to true, the controller will subscribe to a std_msgs::Float32 topic called “max_vel_x” and use this to supercede our max_vel_x parameter.
• initial_rotate_tolerance - when the robot is pointed in a very different direction from the path, the control law (depending on k1 and k2) may generate large sweeping arcs. To avoid this potentially undesired behavior when the robot begins executing a new path, the controller can produce an initial in-place rotation to get the robot headed towards the path before it starts using the control law to follow it. The initial rotate tolerance is the maximum angular difference (in radians) between robot heading and the target pose. This feature can be disabled by setting initial_rotate_tolerance to 0.0.
• prefer_final_rotation - similar to the initial rotation issue many plans may cause large arcs at the end of a path because the orientation of the final pose is significantly different from the direction the robot would drive to follow the path as planned. By setting prefer_final_rotation to true, the orientation of the final pose will be ignored and the robot will more closely follow the path as planned and then produce a final in-place rotation to align the robot heading with the goal.
• latch_xy_goal_tolerance - similar to many other local controllers in ROS, this will prevent hunting around the goal by latching the XY portion of the goal when the robot is within the goal tolerance. The robot will then rotate to the final heading (while possibly being outside of the real XY goal tolerance).
• compute_orientations - if the global planner does not compute orientations for the poses in the path, this should be set to true.
• use_orientation_filter - since the controller is highly dependent on the angular heading of the target pose it is important the angular values are

File truncated at 100 lines see the full file