## Planning algorithms basics

[1] RI 16-735 Robot Motion Planning, CMU

[2] MIT Course Number 16.410/16.413: Principles of Autonomy and Decision Making, Prof. Brian Charles Williams, Prof. Emilio Frazzoli

[3] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations,
MIT Press, Boston, 2005.

### What is motion planning[1]

One can formulate the motion planning problem as a nonlinear optimization problem as the following:

But optimization based planning may stuck at local minimum or the problem can be non-convex which result in being too computational expensive.

### What are path planning and trajectory planning

• Path planning: Path planning algorithms generate a pure geometric path, from an initial to a final point.
• Trajectory planning: Trajectory planning algorithms take a given geometric path and endow it with the time information. The path is time-dependent.
• Motion planning: Trajectory planning + controller

### Types of Motion planning problems

• Control inputs [latexpage]\$m\$ = or < degrees of freedom \$n\$
• Online(rapid-changing environments) or offline(static environments)
• Optimal or satisficing
• Exact or apprximate(just close to the goal)

### Performance criterias for planning algorithms

There is no single planner applicable to all motion planning problems.

• Basic and least performance requirement: Feasibility
1. Optimality
2. Completeness: always answers a path planning query correctly in bounded time.
3. Computational complexity

Ideally, we want a planning algorithm to be optimal and complete, but in reality, that’s not doable most of the time. Usually, we have to sacrifice Optimality and Completeness in order to reduce computational complexity.

Multiple-query versus single-query planning: If the robot is being asked to solve a number of motion planning problems in an unchanging environment, it may be worth spending the time building a data structure that accurately represents C-free. This data structure can then be searched to solve multiple planning queries efficiently. Single-query planners solve each new problem from scratch. For example, RRT use a tree representation for single-query planning in either C-space or state space, PRM are primarily C-space planners that create a roadmap graph for multiple-query planning.

“Anytime” planning: An anytime planner is one that continues to look for better solutions after a solution is already found. The planner can be stopped at any time, for example when a specified time limit has passed, and the best solution returned.

### Constraints

1. Kinematic constraints (configuration related) such as joint limits and obstacles, limit the configuration (position) of a robot.
2. Dynamics constraints (time-derivatives of configuration related) govern the time-derivatives of configuration (independent of obstacles). They include dynamics laws and bounds on velocity, acceleration, and applied force.
3. Strictly kinodynamic constraints (when decoupling between configuration and time-derivatives of configuration is not reasonableare obstacle-dependent constraints that govern configuration and its time-derivatives but do not fall into either of the previous categories. An example of such a constraint is a speed-dependent obstacle-avoidance margin.

### Planning algorithm 分类

1. Algebraic planners: Explicit representation of obstacles. Use complicated algebra (visibility computations/projections) to find the path. Complete, but impractical. (环境已知是可能的，offline算法。但是如何环境不是提前知道的，就不能应用。)
2. Discretization + graph search algorithms(Grid methods): 可以把环境按照Cell来分解(每个cell通常都不同的形状)，也可以是grid来分解(通常形状是统一的)。 These methods are relatively easy to implement and can return optimal solutions. But for a fixed resolution, the memory required to store the grid, and the time to search it, grow exponentially with the number of dimensions of the space. Analytic/grid-based methods do not scale well to high dimensions. Graph search methods (A*, D*, etc.) can be sensitive to graph size. Resolution complete. This limits the approach to low-dimensional problems.
3. Sampling-based planning algorithms: A generic sampling method relies on
• (1) a random or deterministic function to choose a sample from the C-space or state-space;
• (2) a function to evaluate whether the sample is in X_free;
• (3) a function to determine the “closest” previous free-space sample;
• (4) and a local planner to try to connect to, or move toward, the new sample from the previous sample. This process builds up a graph or tree representing feasible motions of the robot. Sampling methods are easy to implement, tend to be probabilistically complete, and can even solve high-degree-of-freedom motion planning problems. The solutions tend to be satisficing, not optimal, and it can be difficult to characterize the computational complexity. A major trend in recent years has been toward sampling methods, which are easy to implement and can handle high-dimensional problems. Sampling algorithms retain some form of completeness, e.g., probabilistic or resolution completeness.
• (1) Deterministic sampling (e.g sampling on a fixed grid)
• Janson, L., Ichter, B., & Pavone, M. (2018). Deterministic sampling-based motion planning: Optimality, complexity, and performanceThe International Journal of Robotics Research37(1), 46-61.
• (2) Stochastic sampling (e.g. Monte Carlo sampling)
4. Potential fields/navigation functions: Virtual attractive forces towards the goal, repulsive forces away from the obstacles. No completeness guarantees, unless “navigation functions” are available—very hard to compute in general.
5. Nonlinear optimization-based planning algorithms: While these methods can produce near-optimal solutions, they require an initial guess at the solution. Because the objective function and feasible solution space are generally not convex, the optimization process can get stuck far away from a feasible solution or a local minimum, let alone an optimal solution.

### Path/trajectory smoothing

Often the motions found by a planner are jerky. A smoothing algorithm can be run on the result of the motion planner to improve the smoothness.

### Configuration space VS workspace

Now, there are 2 ways(or spaces) you can explain the motion of this arm in.

1. Configuration Space: All the possible values of ‘theta1’ and ‘theta2’ it can take.
2. Workspace: The physical volume it can cover. That in this case, is a circle with the radius that is the sum of the link lengths of 2 arms (It obviously cannot go beyond that).

In the above picture, you can picture both the spaces. The left figure shows you the physical location of the robot(blue) and the obstacles(black). Now, all the white space that can be reached by the robot is its workspace.

The figure on the right is a plot of the configuration of the system in the left figure. These are all possible values of ‘theta1’ and ‘theta2’ this system can take avoiding the obstacles. This is therefore called the Configuration Space of the system.