METHOD AND SYSTEM FOR TRAJECTORY OPTIMIZATION FOR NONLINEAR ROBOTIC SYSTEMS WITH GEOMETRIC CONSTRAINTS

摘要

A method and system to determine trajectories including a path with a starting pose and an ending pose over a sequence of time intervals while satisfying dynamic constraints and geometric constraints on a robot arm having links. Determine the dynamic constraints based on dynamics of each link, and determine the geometric constraints as a first-order differentiable function by generating a coordinate grid of an environment, to determine Cartesian coordinates of an obstacle and end-points of each link. Use a multi-link dynamics model to formulate a non-linear optimization (NLO) program for the robot arm, the NLO program uses as inputs the starting and ending pose, the dynamic and geometric constraints, to optimize an objective function of the trajectories while satisfying the dynamic and geometric constraints and robot arm operating constraints, for each time interval. Control the motors of the robot drive to move the robot arm based upon the trajectories.