Chapter 4

Path Planning and Decision Making

Once the autonomous mobile robot (AMR) is aware of the environment using the perception and tracking workflows, the robot uses that information along with planning and decision-making algorithms to move from one place to another and execute tasks.

Path Planning

Path planning refers to the process of finding an optimal path and calculating steering commands for the AMR to move from one location to another. A path planning algorithm is also referred to as a planner, and the algorithm generates the shortest path that avoids any obstacles on the map. Typical planning algorithms include sampling-based methods such as rapidly exploring random tree (RRT) and RRT*, which are supported by Navigation Toolbox in MATLAB. The planner generates waypoints that are further smoothed for drivability using the Dubins and Reeds-Shepp motion models. Finally, path metrics are used to verify smoothness and distance from obstacles. You can select the best path using numerical and visual comparisons.

Decision-Making Through State Transitions

AMRs need to make decisions according to ever-changing conditions or states. For example, if there is an obstacle in front of it, an AMR needs to change its state to perform its next task. Taking another example, if the AMR’s battery is exhausted, the robot will have to initiate a set of state changes and plan several maneuvers to reach the charging station. Although the AMRs are designed to have these decision-making and state-changing capabilities, monitoring the AMR’s decisions and transitions can improve safety. You can use Stateflow to determine such conditions and states and represent them in a flowchart. Stateflow lets you implement state machines using Simulink blocks and describe the transitions of any system from one high-level operating mode to another using state transition diagrams.

In addition to the state transition diagrams, Stateflow provides flowcharts, state transition tables, and truth tables. With Stateflow, you can model combinatorial and sequential decision logic, which can be simulated as a block within a Simulink model or executed as an object in MATLAB. Using graphic animation, you can also analyze your logic while it is executing, which allows the operator to read the robot’s decision-making logic while it is executing tasks. This simplifies both task scheduling and debugging.

Path Following Control

The path provided by the path planning algorithm is a set of waypoints. A path following algorithm such as pure pursuit controller follows these waypoints by calculating the steering and velocity commands for the autonomous mobile robot (AMR). In addition, you can use algorithms such as vector field histograms to control the robot to avoid obstacles.

There are three types of workflows for path tracking and obstacle avoidance that you can use with MATLAB to control an AMR.

1. Pure Pursuit Controller for Path Tracking

Pure pursuit is a path tracking algorithm that computes the angular velocity command, which moves the AMR from its current position to a look-ahead point in front of the robot. When the AMR reaches that point, the algorithm then moves the look-ahead point on the path based on the current position of the AMR. This process repeats until the AMR reaches the last point of the path. Thus, the AMR is constantly chasing a point in front of it. The algorithm assumes a constant linear velocity, and you can change the AMR’s linear velocity at any point. The controllerPurePursuit object in the Navigation Toolbox provides a pure pursuit controller algorithm for path following purposes. This algorithm works with a specified list of waypoints and includes properties such as LookAheadDistance, which decides the distance from the AMR’s current position to the look-ahead point.

2. Model Predictive Control

Model predictive control (MPC) is a feedback control algorithm that uses a model to make predictions about future outputs of a process. This approach solves an online optimization algorithm to select the best control action for driving the predicted output to the reference. MPC can control multi-input multi-output (MIMO) systems that may exhibit interactions between inputs and outputs. Using MPC for path tracking allows for optimal tracking control under multiple constraints, such as a maximum speed or a minimum distance from objects. MPC is also called real-time optimal control or receding horizon control.

Simulating the predictive model for the MPC algorithm and setting the required parameters are important steps to ensure that the optimization selects the best control action. With Model Predictive Control Toolbox™, you can create an MPC controller by programming in MATLAB and set the required parameters graphically. You can also validate the design through numerical simulation of a closed-loop system using Simulink.

3. Obstacle Avoidance Using Reinforcement Learning

Reinforcement learning is another approach to path following and obstacle avoidance. Reinforcement learning is a machine learning method in which an agent (or robot) observes a state in an environment and decides an action to be taken. In the past, reinforcement learning did not have the capacity to handle high-dimensional sensor data such as images and point clouds. With the advent of deep learning, end-to-end deep reinforcement learning has attracted more attention as a feasible path-following approach. With deep reinforcement learning, AMRs can recognize obstacles from images and point clouds and learn paths that avoid the obstacles by trial and error. In addition, with continued learning, AMRs can respond flexibly to changes in the environment.

However, applying this approach directly in an actual AMR is difficult because sufficient learning for safe operation requires an enormous amount of time. Therefore, many approaches of this type use a simulator first. When the learning process has progressed sufficiently in the simulator, additional learning and verification can occur in the actual AMR. MATLAB and Simulink provide strong support for building simulation environments for training. Furthermore, the Reinforcement Learning Toolbox™ supports various reinforcement learning algorithms such as DQN and DDPG.