Probabilistic Collision Checking and Trajectory Planning

In the next benchmark, we plan trajectories using the different probabilistic collision detection algorithms which discussed in Section 7.3.3. We measure the minimum distance between the robot and the human obstacle along the computed trajectory as a safety metric, and the duration and length of the end-effector trajectory as efficiency metrics. The results for the planners with three different probabilistic collision detection algorithms are shown in Table 7.2. The enlarged bounding volumes have the largest safety margins, but the durations and lengths of the computed trajectories are longer than other approaches, since the overestimated collision probability makes the planner compute trajectories that are unnecessarily far from the obstacles. On the other hand, the approximating approach that uses the probability of the object center point underestimates the collision probability and causes several collisions in the planned trajectories, i.e., the minimum distance between the robot and human obstacle become negative. Our approach shows a similar level of safety with the approach using enlarged bounding volumes, while it also computes efficient trajectories that have shorter trajectory durations and lengths. These benchmarks demonstrate the benefits of our probabilistic collision checking on trajectory planning.

(a) A trajectory for zero-noise obstacles (b)δCL= 0.95andvt= 0.005I3×3

(c)δCL= 0.95andvt= 0.05I3×3 (d)δCL= 0.99andvt= 0.05I3×3

Figure 7.7: Robot trajectory with different confidence and noise levels: Static obstacles are shown in green, the estimated current and future human bounding volumes are shown in blue and red, respectively.

7.7 Conclusions and Limitations

We present a novel algorithm for trajectory planning for high-DOF robots in dynamic, uncertain environments. This include new methods for belief space estimation and probabilistic collision detection. Our approach is fast, and works well in our simulated and real robot results where it can compute efficient collision-free paths with a high confidence level. Our probabilistic collision detection computes tighter upper bounds of the collision probability as compared to prior approaches. We highlight the performance of our planner on different benchmarks with human obstacles.

Our approach has some limitations. Some of the assumptions used in belief space estimation in terms of Gaussian distribution and Kalman filter may not hold. Moreover, Our approach needs pre-defined shape representations of the obstacles. The trajectory optimization may get stuck at a local minima and may not converge to a global optimal solution. Furthermore, our approach assumes that the obstacles in the scene undergo rigid motion. There are many avenues for future work. Our

(a) A stationary human (b) The human arm swings slow (c) The human arm swings fast

Figure 7.8:Real Robot Experiment:7-DOF Fetch robot arm repeatedly moves between two points while avoiding collisions with the human. It is noticeable that the robot trajectory deviates more as the human motion becomes faster, in order to deal with the increased uncertainties in the human motion prediction.

Figure 7.9:Real Robot Experiment:The 7-DOF Fetch robot arm is serving a soda can on a table, while the robot avoids collisions with the human arm that may takes soda cans.

approach only takes into account the imperfect information about the moving obstacles. Uncertainties from control errors or sensor errors, which are rather common with the controllers and sensors, need to be integrated in our approach.

CHAPTER 8

Conclusions and Future Work

In this thesis, we have presented motion planning approaches for high-DOF robots in dynamic environments. We use optimization-based planning to efficiently compute feasible high-DOF robot motions. We present new techniques using incremental optimization, parallel computation, and efficient modeling of constraints to improve the performance and reliability of the motion planning. The work presented in this thesis addressed many of the important problems in motion planning, such as dynamically stable human-like motion planning, task constrained motion planning, and motion planning under uncertainties.

To summarize the main results presented in this thesis:

Incremental Trajectory Optimization: We present ITOMP, an optimization-based algorithm for motion planning in dynamic environments. ITOMP does not require a priori knowledge about global movement of moving obstacles and tries to compute a trajectory that is collision-free and also satisfies smoothness and dynamics constraints. In order to respond to unpredicted cases in dynamic scenes, ITOMP interleaves planning optimization and task execution. This strategy can improve the responsiveness and safety of the robot.

Hierarchical Trajectory Optimization of High-DOF Robots: We present an hierarchical planning approach for high-DOF robots. Our algorithm decomposes the high-dimensional motion planning problem into a sequence of low-dimensional sub-problems and computes the solution for each sub-problem in an incremental manner. We use constrained coordination and local refinement to incrementally compute the motion. In static environments, our algorithm offers up to 14X speedup while still generating smooth trajectories. In dynamic environments, we show that the algorithm can increase the success rate of the planning.

Planning Dynamically Stable Motion for Human-like Robots: We present an efficient ap- proach to compute dynamically stable motion of high-DOF robots using optimization-based motion planning algorithm. The stability of the motion is computed in a wrench space, and we compute the

friction force that creates an equilibrium between the forces exerted on the robot. Our formulation of contacts is general and can handle multiple contacts simultaneously. We highlight the performance of our algorithm using a human-like robot, and also demonstrate the applications of our approach in multi-robot planning and natural-looking motion generation of virtual characters.

Parallel Trajectory Optimization using GPUs: We present a novel parallel algorithm for real-time replanning in dynamic environments. The underlying planner uses an optimization-based formulation, and we parallelize the computation on many-core GPUs. We demonstrate the our parallel multi-trajectory optimization on GPUs improves the performance and success rate of planning. We derive bounds on how parallelization improves the responsiveness and the quality of the trajectory computed by our planner.

Constrained Trajectory Planning using Precomputed Roadmaps: We present an efficient parallel constrained planning algorithm for end-effector trajectory constraints. We use a two step approach : the precomputation step and the trajectory refinement step. In the precomputation step, we compute multiple trajectories that satisfy the collision-free and non-singular constraints from static obstacles. The trajectories are used as initial trajectories for the trajectory refinement step. Our planner optimizes the trajectories in the dynamic environment, using cost functions of the constraints. Therefore, our parallel planning algorithm tends to compute the trajectories that follow the given Cartesian trajectory of the end-effector in challenging environments.

Handling Environment Uncertainty using Probabilistic Collision Detection: We present a trajectory planning algorithm for high-DOF robots in dynamic, uncertain environments. This include new methods for belief space estimation and probabilistic collision detection. Our probabilistic collision detection computes tighter upper bounds of the collision probability as compared to prior approaches. We highlight the performance of trajectory optimization using the proposed probabilistic collision detection approach on different benchmarks with human obstacles in simulated environments as well as with a 7-DOF Fetch robot arm.