RDA: An Accelerated Collision Free Motion Planner for Autonomous Navigation in Cluttered Environments

Autonomous motion planning is challenging in multi-obstacle environments due to nonconvex collision avoidance constraints. Directly applying numerical solvers to these nonconvex formulations fails to exploit the constraint structures, resulting in excessive computation time. In this paper, we present an accelerated collision-free motion planner, namely regularized dual alternating direction method of multipliers (RDADMM or RDA for short), for the model predictive control (MPC) based motion planning problem. The proposed RDA addresses nonconvex motion planning via solving a smooth biconvex reformulation via duality and allows the collision avoidance constraints to be computed in parallel for each obstacle to reduce computation time significantly. We validate the performance of the RDA planner through path-tracking experiments with car-like robots in both simulation and real-world settings. Experimental results show that the proposed method generates smooth collision-free trajectories with less computation time compared with other benchmarks and performs robustly in cluttered environments. The source code is available at https://github.com/hanruihua/RDA_planner.Comment: Published in: IEEE Robotics and Automation Letters ( Volume: 8, Issue: 3, March 2023) (https://ieeexplore.ieee.org/document/10036019

A recursively feasible and convergent Sequential Convex Programming procedure to solve non-convex problems with linear equality constraints

A computationally efficient method to solve non-convex programming problems with linear equality constraints is presented. The proposed method is based on a recursively feasible and descending sequential convex programming procedure proven to converge to a locally optimal solution. Assuming that the first convex problem in the sequence is feasible, these properties are obtained by convexifying the non-convex cost and inequality constraints with inner-convex approximations. Additionally, a computationally efficient method is introduced to obtain inner-convex approximations based on Taylor series expansions. These Taylor-based inner-convex approximations provide the overall algorithm with a quadratic rate of convergence. The proposed method is capable of solving problems of practical interest in real-time. This is illustrated with a numerical simulation of an aerial vehicle trajectory optimization problem on commercial-of-the-shelf embedded computers

Fast Second-order Cone Programming for Safe Mission Planning

This paper considers the problem of safe mission planning of dynamic systems operating under uncertain environments. Much of the prior work on achieving robust and safe control requires solving second-order cone programs (SOCP). Unfortunately, existing general purpose SOCP methods are often infeasible for real-time robotic tasks due to high memory and computational requirements imposed by existing general optimization methods. The key contribution of this paper is a fast and memory-efficient algorithm for SOCP that would enable robust and safe mission planning on-board robots in real-time. Our algorithm does not have any external dependency, can efficiently utilize warm start provided in safe planning settings, and in fact leads to significant speed up over standard optimization packages (like SDPT3) for even standard SOCP problems. For example, for a standard quadrotor problem, our method leads to speedup of 1000x over SDPT3 without any deterioration in the solution quality. Our method is based on two insights: a) SOCPs can be interpreted as optimizing a function over a polytope with infinite sides, b) a linear function can be efficiently optimized over this polytope. We combine the above observations with a novel utilization of Wolfe's algorithm to obtain an efficient optimization method that can be easily implemented on small embedded devices. In addition to the above mentioned algorithm, we also design a two-level sensing method based on Gaussian Process for complex obstacles with non-linear boundaries such as a cylinder

Unsupervised learning for long-term autonomy

This thesis investigates methods to enable a robot to build and maintain an environment model in an automatic manner. Such capabilities are especially important in long-term autonomy, where robots operate for extended periods of time without human intervention. In such scenarios we can no longer assume that the environment and the models will remain static. Rather changes are expected and the robot needs to adapt to the new, unseen, circumstances automatically. The approach described in this thesis is based on clustering the robot’s sensing information. This provides a compact representation of the data which can be updated as more information becomes available. The work builds on affinity propagation (Frey and Dueck, 2007), a recent clustering method which obtains high quality clusters while only requiring similarities between pairs of points, and importantly, selecting the number of clusters automatically. This is essential for real autonomy as we typically do not know “a priori” how many clusters best represent the data. The contributions of this thesis a three fold. First a self-supervised method capable of learning a visual appearance model in long-term autonomy settings is presented. Secondly, affinity propagation is extended to handle multiple sensor modalities, often occurring in robotics, in a principle way. Third, a method for joint clustering and outlier selection is proposed which selects a user defined number of outlier while clustering the data. This is solved using an extension of affinity propagation as well as a Lagrangian duality approach which provides guarantees on the optimality of the solution

Edge Accelerated Robot Navigation with Hierarchical Motion Planning

Low-cost autonomous robots suffer from limited onboard computing power, resulting in excessive computation time when navigating in cluttered environments. This paper presents Edge Accelerated Robot Navigation, or EARN for short, to achieve real-time collision avoidance by adopting hierarchical motion planning (HMP). In contrast to existing local or edge motion planning solutions that ignore the interdependency between low-level motion planning and high-level resource allocation, EARN adopts model predictive switching (MPS) that maximizes the expected switching gain w.r.t. robot states and actions under computation and communication resource constraints. As such, each robot can dynamically switch between a point-mass motion planner executed locally to guarantee safety (e.g., path-following) and a full-shape motion planner executed non-locally to guarantee efficiency (e.g., overtaking). The crux to EARN is a two-time scale integrated decision-planning algorithm based on bilevel mixed-integer optimization, and a fast conditional collision avoidance algorithm based on penalty dual decomposition. We validate the performance of EARN in indoor simulation, outdoor simulation, and real-world environments. Experiments show that EARN achieves significantly smaller navigation time and collision ratios than state-of-the-art navigation approaches.Comment: 12 pages, 14 figures, 1 table, submitted to IEEE for possible publicatio

학습 기반 자율시스템의 리스크를 고려하는 분포적 강인 최적화

학위논문 (석사) -- 서울대학교 대학원 : 공과대학 전기·정보공학부, 2020. 8. 양인순.In this thesis, a risk-aware motion control scheme is considered for autonomous systems to avoid randomly moving obstacles when the true probability distribution of uncertainty is unknown. We propose a novel model predictive control (MPC) method for motion planning and decision-making that systematically adjusts the safety and conservativeness in an environment with randomly moving obstacles. The key component is the Conditional Value-at-Risk (CVaR), employed to limit the safety risk in the MPC problem. Having the empirical distribution obtained using a limited amount of sample data, Sample Average Approximation (SAA) is applied to compute the safety risk. Furthermore, we propose a method, which limits the risk of unsafety even when the true distribution of the obstacles movements deviates, within an ambiguity set, from the empirical one. By choosing the ambiguity set as a statistical ball with its radius measured by the Wasserstein metric, we achieve a probabilistic guarantee of the out-of-sample risk, evaluated using new sample data generated independently of the training data. A set of reformulations are applied on both SAA-based MPC (SAA-MPC) and Wasserstein Distributionally Robust MPC (DR-MPC) to make them tractable. In addition, we combine the DR-MPC method with Gaussian Process (GP) to predict the future motion of the obstacles from past observations of the environment. The performance of the proposed methods is demonstrated and analyzed through simulation studies using a nonlinear vehicle model and a linearized quadrotor model.본 연구에서 자율 시스템이 알려지지 않은 확률 분포로 랜덤하게 움직이는 장애물을 피하기 위한 위험 인식을 고려하는 모션 제어 기법을 개발한다. 따라서 본 논문에서 안전성과 보수성을 체계적으로 조절하는 새로운 Model Predictive Control (MPC) 방법을 제안한다. 본 방벙의 핵심 요소는 MPC 문제의 안전성 리스크를 제한하는 Conditional Value-at-Risk (CVaR)라는 리스크 척도이다. 안전성 리스크를 계산하기 위해 제한된 양의 표본 데이터를 이용하여 얻어진 경험적 분포를 사용하는 Sample Average Approximation (SAA)을 적용한다. 또한, 경험적 분포로부터 실제 분포가 Ambiguity Set라는 집합 내에서 벗어나도 리스크를 제한하는 방법을 제안한다. Ambiguity Set를 Wasserstein 거리로 측정된 반지름을 가진 통계적 공으로 선택함으로써 훈련 데이터와 독립적으로 생성된 새로운 샘플 데이터를 사용하여 평가한 out-of-sample risk에 대한 확률적 보장을 달성한다. 본 논문에서 SAA기반 MPC (SAA-MPC)와 Wasserstein Distributionally Robust MPC (DR-MPC)를 여러 과정을 통하여 다루기 쉬운 프로그램으로 재편성한다. 또한, 환경의 과거 관측으로부터 장애물의 미래 움직임을 예측하기 위해 Distributionally Robust MPC 방법을 Gaussian Process (GP)와 결합한다. 본 연구에서 개발되는 기법들의 성능을 비선형 자동차 모델과 선형화된 쿼드로터 모델을 이용한 시뮬레이션 연구를 통하여 분석한다.1 BACKGROUND AND OBJECTIVES 1 1.1 Motivation and Objectives 1 1.2 Research Contributions 2 1.3 Thesis Organization 3 2 RISK-AWARE MOTION PLANNING AND CONTROL USING CVAR-CONSTRAINED OPTIMIZATION 5 2.1 Introduction 5 2.2 System and Obstacle Models 8 2.3 CVaR-constrained Motion Planning and Control 10 2.3.1 Reference Trajectory Planning 10 2.3.2 Safety Risk 11 2.3.3 Risk-Constrained Model Predictive Control 13 2.3.4 Linearly Constrained Mixed Integer Convex Program 18 2.4 Numerical Experiments 20 2.4.1 Effect of Confidence Level 21 2.4.2 Effect of Sample Size 23 2.5 Conclusions 24 3 WASSERSTEIN DISTRIBUTIONALLY ROBUST MPC 28 3.1 Introduction 28 3.2 System and Obstacle Models 31 3.3 Wasserstein Distributionally Robust MPC 33 3.3.1 Distance to the Safe Region 36 3.3.2 Reformulation of Distributionally Robust Risk Constraint 38 3.3.3 Reformulation of the Wasserstein DR-MPC Problem 43 3.4 Out-of-Sample Performance Guarantee 45 3.5 Numerical Experiments 47 3.5.1 Nonlinear Car-Like Vehicle Model 48 3.5.2 Linearized Quadrotor Model 53 3.6 Conclusions 57 4 LEARNING-BASED DISTRIBUTIONALLY ROBUST MPC 58 4.1 Introduction 58 4.2 Learning the Movement of Obstacles Using Gaussian Processes 60 4.2.1 Obstacle Model 60 4.2.2 Gaussian Process Regression 61 4.2.3 Prediction of the Obstacle's Motion 63 4.3 Gaussian Process based Wasserstein DR-MPC 65 4.4 Numerical Experiments 70 4.5 Conclusions 74 5 CONCLUSIONS AND FUTURE WORK 75 Abstract (In Korean) 87Maste

Communication-based Decentralized Cooperative Object Transportation Using Nonlinear Model Predictive Control

This paper addresses the problem of cooperative transportation of an object rigidly grasped by N robotic agents. We propose a Nonlinear Model Predictive Control (NMPC) scheme that guarantees the navigation of the object to a desired pose in a bounded workspace with obstacles, while complying with certain input saturations of the agents. The control scheme is based on inter-agent communication and is decentralized in the sense that each agent calculates its own control signal. Moreover, the proposed methodology ensures that the agents do not collide with each other or with the workspace obstacles as well as that they do not pass through singular configurations. The feasibility and convergence analysis of the NMPC are explicitly provided. Finally, simulation results illustrate the validity and efficiency of the proposed method.Comment: European Control Conference 2018. arXiv admin note: text overlap with arXiv:1705.0142