skip to main content

Title: Robust Optimization-based Motion Planning for high-DOF Robots under Sensing Uncertainty
Motion planning for high degree-of-freedom (DOF) robots is challenging, especially when acting in complex environments under sensing uncertainty. While there is significant work on how to plan under state uncertainty for low-DOF robots, existing methods cannot be easily translated into the high-DOF case, due to the complex geometry of the robot’s body and its environment. In this paper, we present a method that enhances optimization-based motion planners to produce robust trajectories for high-DOF robots for convex obstacles. Our approach introduces robustness into planners that are based on sequential convex programming: We reformulate each convex subproblem as a robust optimization problem that “protects” the solution against deviations due to sensing uncertainty. The parameters of the robust problem are estimated by sampling from the distribution of noisy obstacles, and performing a first-order approximation of the signed distance function. The original merit function is updated to account for the new costs of the robust formulation at every step. The effectiveness of our approach is demonstrated on two simulated experiments that involve a full body square robot, that moves in randomly generated scenes, and a 7-DOF Fetch robot, performing tabletop operations. The results show nearly zero probability of collision for a reasonable range of the noise parameters for Gaussian and Uniform uncertainty.  more » « less
Award ID(s):
Author(s) / Creator(s):
; ;
Date Published:
Journal Name:
2021 IEEE International Conference on Robotics and Automation (ICRA)
Page Range / eLocation ID:
9724 to 9730
Medium: X
Sponsoring Org:
National Science Foundation
More Like this
  1. Seabed mapping is a common application for marine robots, and it is often framed as a coverage path planning problem in robotics. During a robot-based survey, the coverage of perceptual sensors (e.g., cameras, LIDARS and sonars) changes, especially in underwater environments. Therefore, online path planning is needed to accommodate the sensing changes in order to achieve the desired coverage ratio. In this paper, we present a sensing confidence model and a uncertainty-driven sampling-based online coverage path planner (SO-CPP) to assist in-situ robot planning for seabed mapping and other survey-type applications. Different from conventional lawnmower pattern, the SO-CPP will pick random points based on a probability map that is updated based on in-situ sonar measurements using a sensing confidence model. The SO-CPP then constructs a graph by connecting adjacent nodes with edge costs determined using a multi-variable cost function. Finally, the SO-CPP will select the best route and generate the desired waypoint list using a multi-variable objective function. The SO-CPP has been evaluated in a simulation environment with an actual bathymetric map, a 6-DOF AUV dynamic model and a ray-tracing sonar model. We have performed Monte Carlo simulations with a variety of environmental settings to validate that the SO-CPP is applicable to a convex workspace, a non-convex workspace, and unknown occupied workspace. So-CPP is found outperform regular lawnmower pattern survey by reducing the resulting traveling distance by upto 20%. Besides that, we observed that the prior knowledge about the obstacles in the environment has minor effects on the overall traveling distance. In the paper, limitation and real-world implementation are also discussed along with our plan in the future. 
    more » « less
  2. Trajectory optimization o↵ers mature tools for motion planning in high-dimensional spaces under dynamic constraints. However, when facing complex configuration spaces, cluttered with obstacles, roboticists typically fall back to sampling-based planners that struggle in very high dimensions and with continuous di↵erential constraints. Indeed, obstacles are the source of many textbook examples of problematic nonconvexities in the trajectory-optimization prob- lem. Here we show that convex optimization can, in fact, be used to reliably plan trajectories around obstacles. Specifically, we consider planning problems with collision-avoidance constraints, as well as cost penalties and hard constraints on the shape, the duration, and the velocity of the trajectory. Combining the properties of B ́ezier curves with a recently-proposed framework for finding shortest paths in Graphs of Convex Sets (GCS), we formulate the planning problem as a compact mixed-integer optimization. In stark contrast with existing mixed-integer planners, the convex relaxation of our programs is very tight, and a cheap round- ing of its solution is typically sufficient to design globally-optimal trajectories. This reduces the mixed-integer program back to a simple convex optimization, and automatically provides optimality bounds for the planned trajectories. We name the proposed planner GCS, after its underlying optimization framework. We demonstrate GCS in simulation on a variety of robotic platforms, including a quadrotor flying through buildings and a dual-arm manipulator (with fourteen degrees of freedom) moving in a confined space. Using numerical experiments on a seven-degree-of-freedom manipulator, we show that GCS can outperform widely-used sampling-based planners by finding higher-quality trajectories in less time. 
    more » « less
  3. Abstract

    Trajectory planning for multiple robots in shared environments is a challenging problem especially when there is limited communication available or no central entity. In this article, we present Real-time planning using Linear Spatial Separations, or RLSS: a real-time decentralized trajectory planning algorithm for cooperative multi-robot teams in static environments. The algorithm requires relatively few robot capabilities, namely sensing the positions of robots and obstacles without higher-order derivatives and the ability of distinguishing robots from obstacles. There is no communication requirement and the robots’ dynamic limits are taken into account. RLSS generates and solves convex quadratic optimization problems that are kinematically feasible and guarantees collision avoidance if the resulting problems are feasible. We demonstrate the algorithm’s performance in real-time in simulations and on physical robots. We compare RLSS to two state-of-the-art planners and show empirically that RLSS does avoid deadlocks and collisions in forest-like and maze-like environments, significantly improving prior work, which result in collisions and deadlocks in such environments.

    more » « less
  4. We propose a locomotion framework for bipedal robots consisting of a new motion planning method, dubbed trajectory optimization for walking robots plus (TOWR+), and a new whole-body control method, dubbed implicit hierarchical whole-body controller (IHWBC). For versatility, we consider the use of a composite rigid body (CRB) model to optimize the robot’s walking behavior. The proposed CRB model considers the floating base dynamics while accounting for the effects of the heavy distal mass of humanoids using a pre-trained centroidal inertia network. TOWR+ leverages the phase-based parameterization of its precursor, TOWR, and optimizes for base and end-effectors motions, feet contact wrenches, as well as contact timing and locations without the need to solve a complementary problem or integer program. The use of IHWBC enforces unilateral contact constraints (i.e., non-slip and non-penetration constraints) and a task hierarchy through the cost function, relaxing contact constraints and providing an implicit hierarchy between tasks. This controller provides additional flexibility and smooth task and contact transitions as applied to our 10 degree-of-freedom, line-feet biped robot DRACO. In addition, we introduce a new open-source and light-weight software architecture, dubbed planning and control (PnC), that implements and combines TOWR+ and IHWBC. PnC provides modularity, versatility, and scalability so that the provided modules can be interchanged with other motion planners and whole-body controllers and tested in an end-to-end manner. In the experimental section, we first analyze the performance of TOWR+ using various bipeds. We then demonstrate balancing behaviors on the DRACO hardware using the proposed IHWBC method. Finally, we integrate TOWR+ and IHWBC and demonstrate step-and-stop behaviors on the DRACO hardware. 
    more » « less
  5. Abstract Legged robots have a unique capability of traversing rough terrains and negotiating cluttered environments. Recent control development of legged robots has enabled robust locomotion on rough terrains. However, such approaches mainly focus on maintaining balance for the robot body. In this work, we are interested in leveraging the whole body of the robot to pass through a permeable obstacle (e.g., a small confined opening) with height, width, and terrain constraints. This paper presents a planning framework for legged robots manipulating their body and legs to perform collision-free locomotion through a permeable obstacle. The planner incorporates quadrupedal gait constraint, biasing scheme, and safety margin for the simultaneous body and foothold motion planning. We perform informed sampling for the body poses and swing foot position based on the gait constraint while ensuring stability and collision avoidance. The footholds are planned based on the terrain and the contact constraint. We also integrate the planner with robot control to execute the planned trajectory successfully. We validated our approach in high-fidelity simulation and hardware experiments on the Unitree A1 robot navigating through different representative permeable obstacles. 
    more » « less