1.How Obstacle Avoidance works
1.1 Penalty Clause
Obstacle avoidance is implemented as part of overall trajectory optimization. Obviously, optimization involves finding the minimum cost solution (trajectory) to a specified cost function (objective function). Simply put: if a planned (future) pose violates the desired separation from obstacles, then the cost of the cost function must increase. Ideally, the cost function value must be infinite in these cases, otherwise the optimizer may be better off rejecting these regions entirely. However, this will require the optimizer to deal with hard constraints (i.e. solving non-linear programs). teb_local_planner gives up the ability to consider hard constraints in favor of efficiency. Convert hard constraints into soft constraints to obtain a quadratic penalty term with finite cost.
The image above shows an example penalty clause (for obstacle avoidance). Allowed minimum Euclidean distance to obstacles (parameter
m
i
n
o
b
s
t
a
c
l
e
d
i
s
t
min_obstacle_dist
mino?bstacled?ist) is set to 0.2 meters. Therefore, distances below 0.2 meters result in non-zero costs. Now assume that the optimization problem contains more cost terms. Some of them conflict with each other, such as temporal optimality. Therefore, the optimizer may take into account a small violation (and therefore a small penalty) to minimize the overall combined cost function. There are two options here to adjust the behavior:
1. Adjust the optimization weight (scaling the individual cost, here is the parameter weight_obstacle). However, if a value is chosen that is too high, the optimization problem becomes pathological, leading to poor convenience behavior.
2. Change the parameters by adding “extra margins”. You can implicitly increase the cost value to 0.2m by adding a small extra margin in the min_obstacle_dist parameter. You can use a single parameter penalty_epsilon to move all penalties simultaneously, but be careful as doing so can greatly affect the optimization results.
1.2 Local optimal solution
Note that the optimizer itself can only find locally optimal solutions. Imagine that the robot might be wrapped sideways by two obstacles. The penalty term is indeed non-zero, but the optimizer gets stuck (reaching this local minimum) because moving the corresponding pose laterally to one of the obstacles further increases the total cost. You can easily try it using test_optim_node (see the tutorial to set up and test optimization, and turn off same-origin class planning). The behavior should be similar to the one in the image below:
The trajectory cannot jump over obstacles. Even the gesture itself is pushed away from the area between obstacles (red arrow). Obviously, this situation should be avoided in practice. Therefore, the homology class planning algorithm seeks (topologically) alternative solutions, while the feasibility check (see below) rejects such solutions before actually commanding the robot.
1.3 The relationship between potential and obstacles
The image below shows a snapshot of a common planning scenario:
The scene consists of a mobile robot approaching a polygonal obstacle while traveling to its current goal. A planned (discrete) trajectory consists of multiple robot poses. The planner’s goal is to schedule every two consecutive poses according to the desired temporal resolution (parameter dt_ref). Note that the actual resolution is not fixed/frozen as the optimizer needs to adjust the conversion time to seek time optimality.
For obstacle avoidance, the distance between the planned posture and the obstacle must be defined from below. The example trajectory in the figure consists of 8 variable poses (the starting pose and target pose are fixed). You may agree that in order to achieve a collision-free trajectory, multiple distance calculations are required (the optimizer calls the calculation of the cost function value multiple times). To speed up optimization, a dedicated correlation strategy is implemented.
For each obstacle (point/occupied cost map cell, line, polygon), locate the closest pose of the planned trajectory (see figure). Depending on the value of the parameter obstacle_poses_affected, the affected neighbors of the nearest pose are also taken into account. Only this selected subset of poses (here 3 poses, hence 3 penalty terms) are considered in subsequent optimization steps. Repeat the association process after no_inner_iterations (parameter) respectively. The value of obstacle_poses_affected at each outer optimization iteration slightly affects the smoothness of the trajectory around obstacles. Additionally, larger obstacles require more connection poses to avoid unmissable shortcuts. You can also choose a higher value (>Trajectory Length) so that all poses are connected to each obstacle.
Note that the robot footprint model is considered for distance calculations and is therefore critical for the required computational resources. Details are summarized in the following sections.
2.Robot Footprint Model Robot Footprint Model
For optimization purposes, the robot footprint model approximates the 2D outline of the robot. This model is critical to the complexity of distance calculations as well as the computational time. Therefore, the robot footprint model constitutes a dedicated parameter, rather than loading the footprints from the generic costmap_2d parameter. The optimization footprint model may differ from the cost map footprint model (the latter is used for feasibility checks, see next section).
The package outline model is selected and configured using a parameter server. You can add the following parameter structure to the teb_local_planner configuration file:
TebLocalPlannerROS: footprint_model: # types: "point", "circular", "line", "two_circles", "polygon" type: "point" radius: 0.2 # for type "circular" line_start: [-0.3, 0.0] # for type "line" line_end: [0.3, 0.0] # for type "line" front_offset: 0.2 # for type "two_circles" front_radius: 0.2 # for type "two_circles" rear_offset: 0.2 # for type "two_circles" rear_radius: 0.2 # for type "two_circles" vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18 ], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
The default schematic model type is “point”. Note that the footprint is published to ~/teb_markers and can be visualized in rviz (e.g. for verification).
Important: For car-like robots, pose [0,0] is at the rear axis (rotation axis)!
The following paragraphs describe all the different types:
2.1 Trace type: point
The robot is modeled as a single point. For this type, minimal computation time is required.
2.2 Trace type: circular
The robot is modeled as a simple circle with a given radius ~/footprint_model/radius. The distance calculation is similar to the calculation for point robots, with the exception that the radius of the robot is added to the min_obstacle_dist parameter of each function call. You can eliminate this extra addition by choosing a point robot and adding the radius to the minimum obstacle distance beforehand.
2.3 Diagram type: Line
Linear robots are suitable for robots that exhibit different extensions/lengths in longitudinal and transverse directions. Lines (segments) can be configured using the parameters /footprint_model/line_start and /footprint _model/line_end (each [x,y] coordinate). The robot (rotation axis) is assumed to be [0,0] (unit: meters). Please make sure to encapsulate the entire bot by further adjusting the parameter min_obstacle_dist (see example below). 
You can also look at the “two circles” model.
2.4 Schematic type: two circles
Another possibility to approximate the robot outline consists of defining two circles. Each circle is described by an offset and radius along the robot’s x-axis: /footprint_model/front_offset, /footprint _model/front_radius, ~/foot print_model/rear_offset, and ~/footprint_model/rear_radius. The offset may be negative.
Please refer to the picture below as an example: 
For each relevant robot pose, two distance calculations are required.
2.5 trace type: polygon
Complex models can be combined by defining closing polygons. The polygon is defined in terms of a list of vertices (providing x and y coordinates for each vertex). Assume that the robot’s rotation axis is located at [0,0] (unit: meters). Please do not repeat the first vertex as the polygon is automatically closed. Keep in mind that each additional edge significantly increases the required computation time! You can copy a footprint model from a cost chart common parameters file.
3 Feasibility check
After the optimizer returns the trajectory, and before sending velocity commands to the robot, a feasibility check is performed. The purpose of this check is to identify invalid/infeasible trajectories that the optimizer may produce (remember: soft constraints, local minima, etc.).
Currently, the algorithm iterates over the first n poses (n=~/pability_check_no_poses(parameter!)) starting from the current robot pose and checks if these poses are free of collisions. To detect if a collision occurs, a cost map footprint is used (see navigation tutorial)! Therefore, this verification model may be more complex than the package footprint used for optimization (see section above).
Since the optimizer may not fully converge, you should not choose too high a value for ~/probability_check_no_poses: figuratively, small obstacle violations in the (distant) future can be corrected as the robot moves toward the goal.
If you are driving in a tight environment, make sure the obstacle avoidance behavior (local planner and global planner) is configured correctly. Otherwise, the local planner may reject trajectories that are unfeasible (from its perspective), but the global planner may further consider the chosen (global) plan to be feasible in comparison: the robot may get stuck .
———-20231107
If there is a situation where I encounter an obstacle, I want to confirm the local starting and ending strategies:
Reference
1.Obstacle Avoidance and Robot Footprint Model