I Introduction
Motion planning for robots with multijointed arms is challenging. Due to the complicated geometric structure and nonlinear dynamics, timeconsuming computation is required to solve motion planning problem even in the simplest cases. Most existing works utilize the pathvelocity decomposition approach [pham2017admissible], in which motion planning problem is separated into easier subproblems, i.e., path planning and trajectory planning. The path planning problem focuses on the generation of collision free geometric path in the configuration space, while the trajectory planning problem focuses on the generation of time optimal velocity profile along the geometric path. Extensive research [lavalle2006planning, sucan2012open, verscheure2009time, reynoso2016convex] has been conducted for each subproblem, resulting a rich collection of algorithms. However, due to the inconsistency of performance metrics between motion planning problem and the subproblems, only suboptimal solution can be found by pathvelocity decomposition based approaches. Time optimal motion planning involving collision avoidance requirement and robot dynamics is still a challenging problem ([la2011motion, pham2014general]).
In order to avoid the inconsistency of performance metrics, this paper presents an optimal control based approach to address the path planning and trajectory planning problems simultaneously. The presented approach is able to generate time optimal trajectories without predetermining the geometric path while satisfying constraints involving robot dynamics. Similar works can be found in [schulman2013finding, chettibi2004minimum, diehl2006fast]. However either robot dynamics are ignored or long computation time is required in these works. In this paper, an efficient numerical method for trajectory optimization is utilized to solve the optimal control problem for robot motion planning. It is shown by numerical results that the solution can be found with short computation time even when complicated robot dynamics are involved. Experimental results have shown the feasibility of the planned motion.
The rest part of this paper is organized as follows: section II presents optimal control formulation for robot motion planning problems, section III presents an efficient numerical method for trajectory optimization, section IV presents numerical and experimental results of the proposed approach, and section V concludes this paper.
Ii Problem Formulation
A general optimal control problem can be posed as follows: determine the statecontrol function pair, , terminal time , that minimize the performance metric or cost function, while satisfying dynamic constraints, path constraints, and boundary conditions ([ross2012review]). The robot motion planning problem can be formulated as an optimal control problem by defining the cost function, dynamic constraints, path constraints, and boundary conditions.
The state and control in motion planning involving robot dynamics can be defined as:
(1) 
where ,
is the vector for joint positions, and
is the vector for joint torques, is the number of robot joints.Iia Cost Function
The quality of the planned motion strongly depends on the formulation of cost function. In this paper, the cost function is formulated as a summation of motion time and a regularization term for smoothness and naturalness of the generated motion:
(2) 
where is the jerk of joint motion. The regularization term is designed based on the minimumjerk model of human motion ([flash1985coordination]) and thus corresponds to the importance of the naturalness of the generated motion. is a weighting coefficient for the regulation term, and is a weight matrix designed to penalize the motion of joints with higher gear ratios. The weight matrix is defined as
(3) 
The case corresponds to the time optimal motion planning problem. Larger slows down the generated motion, but increases the naturalness and smoothness.
IiB Dynamic Constraints
The dynamic constraints is the robot dynamics. The equations of motion can be derived using Lagrangian’s equations or NewtonEuler approach:
(4) 
where is the inertia matrix, is the Coriolis and centrifugal force term, is the gravity term, and is the friction term. For multijointed robot arms, all of these terms are inherently nonlinear.
Letting , , the dynamic constraints can be formulated as state space model with state and control :
(5a)  
(5b) 
The state space model can be rewritten as:
(6) 
IiC Path Constraints
A set of path constraints can be formulated to accommodate various physical limitations of the robot actuators, as well as collision free conditions. The path constraints for robot motion planning include:
Position bounds:  (7a)  
Velocity bounds:  (7b)  
Torque bounds:  (7c)  
Torque rate bounds:  (7d) 
Collision free conditions are also included in the path constraints. Dues to the complicated geometric mapping between robot workspace and configuration space, it is difficult to represent collision free conditions analytically. To simplify the formulation, the robot links and obstacles can be approximated by a set of spheres for differentiable collision detection, as illustrated in Fig. 1. The approximation can be performed either manually or automatically using spheretree construction algorithms [bradshaw2004adaptive].
Suppose spheres are used to approximate robot links, and spheres are used to approximate obstacles. Let the center and radius of each sphere that representing robot links be , the center and radius of each sphere that representing obstacles be . Robot forward kinematics problem can be solved to determine the functional relationship between joint positions and the location of sphere centers . The collision free constraints can then be formulated as
Self:  (8a)  
Obstacle:  (8b) 
where is a set of indices indicating possible collision between two balls that approximate robot links.
When workspace boundaries are presented, additional path constraints are necessary. Let , , and be the workspace limits for , , and directions respectively. Let , . The path constraints for workspace boundary can be formulated as:
Workspace lower bound:  (9a)  
Workspace upper bound:  (9b) 
IiD Boundary Conditions
The boundary conditions for robot motion planning problem include:
Initial & final position  (10a)  
Initial & final velocity  (10b)  
Initial & final acceleration  (10c)  
Terminal time bounds :  (10d) 
where and are the initial and target joint positions, and are the minimum and maximum allowed terminal time.
Iii Efficient Numerical Method for Trajectory Optimization
Trajectory optimization is a technique for computing an openloop solution to an optimal control problem. Since no universal analytical solution can be found for nonlinear optimal control problems, a variety of numerical approaches have been developed for trajectory optimization in [rao2009survey, betts2010practical]. In most numerical approaches, the continuous time optimal control problem is firstly converted into discretized optimization problem in a procedure called transcription ([kelly2015transcription]
). The optimization problem is then solved by general purpose optimization solver. Polynomial interpolation is finally utilized to return an approximate solution to the continuous time optimal control problem.
In numerical methods for trajectory optimization, two parts are playing the key role: discretization and optimization. An efficient implementation can be designed by choosing these two components intelligently. In this paper, the pseudospectral method is chosen to transcribe the continuous time optimal control problem, and the interior point method with the support of automatic differentiation is chosen to solve the discretized optimization problem, as illustrated in Fig. 2.
Iiia Pseudospectral Method
The decision variables in the continuous time optimal control problem for robot motion planning include , , and . In the transcription procedure, and are discretized by their values at certain time as and , where are called knots, and is the number of knots. It is reported in previous research [rao2009survey, kelly2017introduction, trefethen2013approximation] that the solution accuracy increases exponentially fast with the increase of interpolation knots for pseudospectral methods. Thus high computational efficiency can be achieved using pseudospectral method since less discretization knots can be chosen under the same solution accuracy requirement. In addition, the approximate solution is guaranteed to be smooth since high order global polynomial interpolation is utilized in pseudospectral methods.
In this paper, the ChebyshevLobatto points (or Chebyshev points) are chosen to be the knots. Such choice can avoid the oscillation phenomenon in high order global polynomial interpolation. For , the knots are:
(11) 
Pseudospectral methods have provided a set of tools for polynomial interpolation, approximating integration terms using quadrature, and approximating derivatives using differential matrix.

Interpolation The polynomial interpolation in pseudospectral methods can be performed by barycentric interpolation, which can be formulated as a linear combination of Lagrangian polynomials. For state trajectory and control trajectory, the form is
(12) where is the th Lagrange polynomial. In barycentric interpolation, a special form of Lagrange polynomial is implemented to efficiently perform interpolation ([trefethen2013approximation, berrut2004barycentric]).

Quadrature Quadrature is the standard term for the numerical calculation of integrals. The integration of function can be approximately evaluated by quadrature rules in pseudospectral methods as:
(13) where is a set of quadrature weights. The quadrature weights can be explicitly defined to be
(14) When Chebyshev points are chosen, the corresponding quadrature rule (Clenshaw–Curtis quadrature) can be found in [waldvogel2006fast]:

Differentiation matrix Let the stacked state and robot dynamics at knots be
(15) The dynamic constraints can be posed as [rao2009survey]:
(16) where is the differential matrix that is used to compute the scaled time derivative of the polynomial approximation of ([berrut2004barycentric]). Let the stacked joint torques be , the stacked torque rates can be approximately calculated as:
(17)
IiiB Automatic Differentiation
Lots of optimization solvers are based on gradient descent algorithm. Derivative of objective function and constraints are frequently evaluated by numerical differentiation approaches, which perturbs input to the function in each dimension to obtain an approximation of the derivative using finite differences. However, numerical differentiation approaches are computationally expensive for functions with high dimensional input, and inevitably introduces roundoff errors. Symbolic differentiation is one way to avoid roundoff errors, however it frequently leads to inefficient code. Both numerical differentiation and symbolic differentiation are problematic in the calculation of higher order derivatives like Hessian.
To address the problems in numerical differentiation and symbolic differentiation, automatic differentiation is introduced. Automatic differentiation is a set of techniques to evaluate derivative of a function ([griewank2008evaluating]). The computational cost of automatic differentiation is lower than numerical differentiation or symbolic differentiation. A rich collection of automatic differentiation implementations can be found in [walther2009getting, bell2012cppad, bendtsen1996fadbad]. In this paper, CasADi [Andersson2018] is chosen for its good usability in MATLAB environment. Since computational cost of automatic differentiation is proportional to that for function evaluation, articulated body algorithm [featherstone2014rigid] is utilized in this work for efficient evaluation of robot dynamics.
Iv Numerical and Experimental Results
Motion planning of a 6axis industrial robot with dynamic constraints is considered as an example.
The industrial robot is supposed to work in a constrained workspace for pickandplace tasks. The geometric model of the industrial robot is shown in Fig. 3. The path constraints include joint position, velocity, torque, torque rate bounds, and collision free conditions. The sphere approximation of robot links, obstacles, and workspace boundary is shown in Fig. 4. The actuator limits are listed in Table I. The actuator limitations are designed to be conservative for safety.
Limits  J1  J2  J3  J4  J5  J6 

160  90  120  180  120  180  
170  90  230  180  100  180  
165  165  175  350  340  520  
[Nm]  1397  1402  383  45.2  44.6  32.5 
[Nm/s]  20948  21035  5741  678  669  488 
Iva Numerical Results
Several tests have been performed to evaluate the effect of different regularization weights and number of knots. It is observed that the geometric path can be adjusted automatically for collision avoidance, even if infeasible initialization is provided. Increasing is helpful for shortening the computation time, but results in slower motion. Shorter motion time can be obtained by using more knots, but the computation takes longer time. When 12 knots are chosen, the planned motion is reasonably close to the possible optimal solution with an acceptable computation time. Less knots can be used if shorter computation time is required. can be chosen to balance the timeoptimality requirement and the naturalness of the generated motion.
The proposed planning algorithm has also been tested using a group of different initial and target positions, as shown in Fig. 5. All the computation is performed on a laptop with a 2.1 GHz Intel® Core™ processor. The solver takes around 5.9 s for a onetime initialization for automatic differentiation by CasADi. The computation times and optimized motion times for the test cases are summarized in Table II.
time [s]  knots  case 1  case 2  case 3  case 4 

motion time  12  1.65  1.23  1.05  1.08 
8  1.98  1.23  1.09  1.11  
computation time  12  2.55  2.37  2.39  2.57 
8  1.28  1.75  0.84  1.32 
In all the test cases, good approximations of the time optimal trajectories are returned in about 23 seconds using 12 knots, and about 12 seconds using 8 knots. Existing works [chettibi2004minimum, diehl2006fast] require from 20 seconds to several minutes for computation, in which only robot dynamics are involved but not collision avoidance. The proposed approach is highly efficient comparing to these results.
IvB Experimental Results
The planned optimal trajectory of test case 2 in Fig. 5 has been used as motion reference in experiment at the Mechanical Systems Control laboratory at the University of California, Berkeley. The actual robot motions are captured from video record as shown in Fig. 6. As shown in the figure, the planned motion is collision free.
The scaled joint velocities and joint torques are shown in Fig. 7. As shown in the figure, the planned motion is feasible under conservative actuator limitations. The motion time is adjusted automatically from an initial value 10 s to about 1.2s. It is also observed that the planned motion is close to timeoptimal with at least one of the constraints is active.
V Conclusion
Motion planning involving complicated robot dynamics and geometric constraints is challenging. Since most approaches decompose motion planning to two subtopics and deal with them separately, only suboptimal solution can be found. This paper presents an optimal control based approach to address the path planning and trajectory planning problems simultaneously. An efficient numerical method for trajectory optimization is proposed as one practical solution for the nonlinear optimal control problem. Numerical results have shown that the motion planning problem can be solved with a short computation time and reasonable accuracy. Experimental results have verified the effectiveness and feasibility of the planning algorithm. It is worth investigating improvements to this approach and exploring possibilities to implement it in different robotic applications.
Comments
There are no comments yet.