Abstract
Today’s standard robotic systems often do not meet the industry’s demands for accurate high-speed robotic applications. Any machine, be it an existing or a new one, should be pushed to its limits to provide “optimal” efficiency. However, due to the high complexity of modern applications, a one-step overall optimization is not possible. Therefore, this contribution introduces a step-by-step sequence of multiple nonlinear optimizations. Included are optimal configurations for geometric calibration, best-exciting trajectories for parameter identification, model-based control, and time/energy optimal trajectory planning for continuous path and point-to-point trajectories. Each of these optimizations contributes to the improvement of the overall system. Existing optimization techniques are adapted and extended for use with a standard industrial robot scenario and combined with a comprehensive toolkit with discussions on the interplay between the separate components. Most importantly, all procedures are evaluated in practical experiments on a standard robot with industrial control hardware and the recorded measurements are presented, a step often missing in publications in this area.
1. Introduction
State-of-the-art robotic systems are equipped with highly sophisticated industrial hardware, capable of short sample times and offering high computational power. This increasing arithmetic performance may be used to improve positioning accuracy and dynamic accuracy and compute time/energy-optimal trajectories. The proper fundamentals are found in the underlying mathematical models.
This contribution focuses on giving an overview of various nonlinear optimizations. With each optimization, a certain aspect of an arbitrary robotic system is improved. Applying all of them in sequence will provide better results regarding positioning accuracy and dynamic accuracy and will reduce the cycle times. The basis for this optimization lies in the kinematic and dynamical modeling of the system. Therefore, Section 2 starts with the deviation of these models enhanced by a model-based control strategy. Special emphasis is laid on the implementation on an industrial system including time delays. In Section 3, the static position accuracy of the robot is improved by considering unavoidable tolerances in the robot kinematics. The main topic is obtaining optimal configurations for the identification process. The identification of the dynamic parameters, that are used for the model-based control, is described in Section 4. Also, the problem of optimal exciting trajectories is solved by nonlinear optimization techniques. Finally, Section 5 reviews time/energy optimal trajectory planning for continuous path trajectories and point-to-point trajectories.
Each section provides information on the used solvers for the various optimizations and also presents detailed results of the conducted experiments. Experiments are performed using an off-the-shelf robot, a Stäubli RX130L shown in Figure 3, with industrial control hardware and commercially available measurement hardware, to guarantee that all proposed methods are applicable for industry. For specifications of the articulated robot, please consult Table 1.
Previous works of the authors, which are also focused on industrial robotics, see, for example, [1], cover the basics like control and geometric path planning. Based on these works, a specific combination of algorithms is presented, each of which optimizes a specific aspect of the robotic system.
2. System Modeling and Control
2.1. Kinematic Modeling
The kinematic modeling can be divided in the direct kinematics problem and the inverse kinematics problem. The direct kinematics calculates the end-effector coordinates as a function of the joint coordinates and nominal geometric parameters as see Figure 3. Vector indicates the end-effector position, which can be calculated by a sequential summation of the relative connecting vectors. For the orientation, a description in Cardan angles is used. A sequential multiplication of the relative rotation matrices between two successive joints delivers the rotation matrix between end-effector frame and inertial frame from which the Cardan angles can be extracted. In this paper, also the notation tool center point (TCP) is used for the end-effector. Inverse kinematics calculates the joint coordinates as a function of end-effector coordinates and : Solutions for the inverse kinematics problem of such standard robotic systems can be found in many textbooks, for example, [2] or [3].
2.2. Dynamic Modeling
The equations of motion for multibody systems, like a robot, can be calculated by several methods. The method with minimal effort is the Projection Equation, see [4]. Linear momenta and angular momenta are projected into the minimal space (minimal velocities ) via the appropriate Jacobian matrices: All the values like the translational velocity or the rotational velocity of the center of gravity can be inserted in arbitrary coordinate systems . In contrast to , is the velocity of the used reference system. The matrix is the inertia tensor, while characterizes the vector product . and are impressed forces and moments acting on the th body. An evaluation of (3) yields the highly nonlinear equations of motion for the robot They are composed of the minimal coordinates , the configuration dependent, symmetric and positive definite mass matrix , the vector containing all nonlinear effects (like Coriolis, centrifugal, gravity, and friction forces), and the vector of the generalized actuating forces .
Detailed dynamical modeling is an essential task because it is the basis for model-based control and optimal trajectory generation.
2.3. Control
The hardware setup is composed of an industrial PC, communicating via Powerlink with six ACOPOS servo drives, which power the synchronous motors of the Stäubli robot. The servo drives use cascaded controllers for precise position control of the robot’s joints. However, the articulated robot is a highly nonlinear system due to the serially connected arm/joint units. Typically, using linear joint controllers on a highly nonlinear mechanical system will not lead to sufficiently accurate and dynamic end-effector motion.
2.3.1. Model-Based Control
This contribution suggests to effectively linearize the nonlinear mechanical system with a flatness-based feed-forward approach and use the servo drives’ linear cascaded controllers to compensate model deviations and external disturbances. Thus, they are summarized for the sake of completeness. Introducing the state , the equations of motion, (4), in state space read with the input vector of motor torques . With the flat output and its derivatives with respect to time, all state and input variables can be computed as a function of the flat output, see, for example, [5]. Consequently, the evolution of all system variables , , and is determined by a sufficiently smooth trajectory .
Thus, combining the superimposed feed-forward branch with the servo drives’ internal controllers yields the control law for the motor torques : Please note that the feed-forward term in (7) guides the system states along the desired trajectory, while the feed-back control law ensures stability against disturbances and modeling errors with the positive diagonal matrices and . The stability of the overall system, applying (7), is proven in [6]. Alternatively, the feed forward control law may also be computed by using the structural properties of the robot’s subsystem representation, as described in [7].
2.3.2. Implementation
In order to evaluate the quality of the suggested controls, the Stäubli RX130L robot is interfaced with the motion hardware. During this process, the robot’s mechanics, AC motors, and resolvers are not changed or manipulated in any way.
The industrial PC is using 400? as base sample time. Every sample step, the inverse kinematic problem is solved numerically to transform the desired end-effector movement into corresponding joint positions, velocities, and accelerations. With these reference joint variables (, and ), the mathematical model is evaluated to compute the feed-forward motor torques as stated in (7). Then, for each joint, a separate Ethernet telegram is generated containing the reference positions and corresponding feed-forward motor torques.
The servo drives are parameterized to accept cyclic position inputs for the cascaded controller. To do so, the cyclically arriving network telegrams are received and the desired reference positions are passed to the set value inputs of the cascaded controllers. Also, the computed feed-forward torques are transformed to corresponding motor currents and added to the servo drives’ current controllers set values. Figure 1 presents the corresponding block diagram.
2.3.3. Experimental Results
The quality of the implemented control strategy is validated by experiments with the Stäubli RX130L robot. Regarding industrial interests, the standardized trajectory found in the EN ISO NORM 9283, see [8], is used as reference trajectory. It consists of various elements: circles, straight lines, and squares in the typical workspace for an industrial application. The TCP velocity is chosen with 1?m/s and its acceleration with 5?m/.
Figure 2(a) depicts the the lag errors of the first three joints. The other axes show similar results. The positive influence of the feed-forward loop is clearly observable. To present insights on the TCP-accuracy during high dynamics, the TCP errors for the standard and the improved control law along the trajectory are presented in Figure 2(b). They are measured using a high-precision laser tracker from Leica at a sampling rate of 1?.
(a) Joint errors
(b) TCP errors
Obviously, the positive effects of the model-based feed-forward approach drastically reduce the TCP and joint errors. Mainly, they compensate static effects, for example, resulting from gravity, and dynamic effects, for example, arising from friction and high accelerations while the standard cascaded controller provides stiffness, stability, and robustness.
3. Static Position Accuracy
Industrial robotic systems usually offer a satisfying repeatability. However, the lack of sufficient accuracy of the end-effector is responsible for time-consuming adjustments while commissioning new tasks. The sources of these errors are nonmodeled manufacturing tolerances, assembly inaccuracies, and joint encoder offsets.
3.1. Kinematics with Error Parameters
The direct kinematics, which compute the end-effector position and orientation as function of the joint angles and nominal geometric parameters , can be extended by a set of error parameters regarding lengths, joint offsets, and assembly inaccuracies. For example, the connection vector between points and reads where is the nominal length of the connection (part of ) and are the position error values (part of ). Thus, the robot’s TCP position and orientation is For a standard six-axis industrial robot, as sketched in Figure 3, contains error parameters regarding length, 6 joint encoder offsets, angles for misaligned coordinate frames between each link, and 3 tolerances for defining the inertial coordinate system. This results in a set of 39 parameters. To remove all linear dependencies, a -decomposition is applied, see Section 4.1 for details. Applying this method yields the final error parameter .
3.2. Geometric Calibration
The calibration process calculates valid values for the error parameter that minimize the error between measured TCP-positions and TCP-positions determined using the corresponding joint angles : This highly nonlinear equation can be solved by an iterative method. Representing (10) as a Taylor-series around , neglecting higher order terms and demanding that the error converges towards zero, yields For measurements, (11) becomes which is an overdetermined equation system solvable by standard least-squares techniques. The minimum of the quadratic error is found with yielding the update law for the error parameter vector Initial values for can be assumed to be zero because the error parameters regarding length, and so forth, are considerably smaller than the nominal geometric parameters. To finally obtain good position accuracy, the identified parameters must be considered in the inverse kinematics algorithm. Any suitable numerical solution is viable and can handle the set of additional parameters.
3.3. Optimal Configurations for Calibration
It is obvious that the geometric calibration only identifies parameters well that are excited accordingly. To overcome this problem, the whole work- or joint-space could be discretized for taking the necessary measurements. While this approach is viable for small systems, it is heavily time and cost consuming for systems with multiple degrees of freedom. Such complex systems are common in today’s industrial applications. As a consequence, an optimal set of configurations, which guarantees well-excited error parameters, needs to be found.
3.3.1. Observation Index and Algorithm
In order to ensure a well-conditioned problem formulation and also well-excited parameters , the minimum singular value of the covariance matrix in (14) is used as observation index and optimization criterion and further described by . As, for example, [9–11] show, there exist various choices for valid observation indices with slightly different properties. The proposed algorithm, which has been presented in [12], allows the use of arbitrary observation indices.
Before introducing the optimization algorithm, two basic operations are defined: (i)Add a configuration: adding a new configuration with the Jacobian to the current information matrix yields which directly influences the change of the chosen observation index (ii)Remove a configuration: similar to the previous case, removing a configuration from the current information matrix is described by Additionally, the complete set of measurable configurations is called the configuration-set . An arbitrary subset of it is denoted by the set . These operations and definitions are the basics for the following optimization method. (1)Initialization: a given number of configurations is randomly selected from the initial configuration-set . This subset must lead to a nonsingular covariance-matrix to ensure a computation of the observation index . Should this not be the case for the chosen configurations, then the random selection process is simply repeated.(2)Remove and replace: the configuration that leads to the minimal decrease of the observation index, , is removed from the current configuration set . Then, the resulting subset is complemented by exactly that configuration of that maximizes . For both operations, the correct element is found by an exhaustive search over the respective set. Note that due to the simple way to calculate (see ((15)–(17))), the computational effort for these searches is not as dramatic as it may sound. This remove-and-replace strategy effectively sorts out configurations that do not improve the overall conditioning of the covariance matrix and replaces them with ones that do. The step is repeated as long as one remove-and-replace operation still improves the observation index. Otherwise, the next step is executed.(3)Add additional configurations: from the configuration-set exactly the one configuration (again, found by exhaustive search) that, by itself, maximizes , is added to the current subset . This step is iterated until the observation index cannot be further increased by such an operation or until a user-specified maximum number of desired configurations is reached. Otherwise, execute the final step.(4)Final remove and replace: analogously to step 2, each single configuration of the subset is evaluated. If any remove and replace operation as described above still increases the observation index, this exchange is executed. Otherwise, the procedure is terminated. The algorithm may be applied multiple times to prevent getting stuck at a local minimum. The solution with the maximum observation index is used for the experimental analysis. Nevertheless, finding these optimal configurations is time consuming and lasts about five minutes on a standard PC. Since these calculations are performed offline before any measurements are done, this is not a critical issue.
3.4. Experimental Results
To evaluate the performance of the proposed algorithm, detailed experimental results for the RX130L robot are presented. First, the robot’s joint-space is discretized by 13 equally large intervals per degree of freedom. This results in a set of configurations. In reality the coordinates of the end-effector are measured by the visual infrared tracker system 3D-Creator, manufactured by the Boulder Innovation Group. A probe emitting infrared light by diodes is mounted on the end-effector of the robot. This light is recorded by three CCD cameras and the position and orientation in world coordinates are calculated by stereovision. Robot configurations that are not visible by this system are removed. The remaining set consists of approximately 4000 configurations. Then, the algorithm is used to compute optimal configurations. As Figure 4 emphasizes, the optimal configurations differ strongly from each other to keep the condition number of the covariance matrix low, also leading to well-excited parameters. Compared to a linear scan of the workspace, the observation index is reduced by the factor 18.5 while keeping the number of necessary measurements very small.
For information on the position and orientation accuracy before (uncal., ) and after calibration (cal), see Table 2. The orientation errors are represented in Cardan angles.
4. System Identification
In this section, the identification of the dynamical parameters describing the robot behavior, see (4), is covered. This is clearly an essential task because it is the basis for model-based control and optimal trajectory generation.
4.1. Dynamic Parameter Identification
An important property of the equations of motion is the linearity with respect to dynamic parameters characterizing the robot links and motors. By introducing the information matrix ( degrees of freedom), (4) can be restructured to The parameter vector is the composition of the the joint parameters containing the mass , the center of mass , the elements of the inertia tensor , and Coulomb/viscous friction coefficients and for the th joint-arm unit. Since the dynamical parameters are confidential manufacturer data, all these parameters have to be identified. Usually, the information matrix and the parameter vector contain linear dependencies, which have to be removed before the identification process. A decomposition and reformulation leads to where is an orthogonal matrix and is an upper triangular matrix. The nonidentifiable parameters are those whose corresponding elements on the diagonal of the matrix are zero. In contrast, if , then the corresponding column in is independent. Let the independent columns be collected in the matrix and the corresponding parameters be collected in . The dependent columns and parameters are represented by and , respectively, such that is fulfilled, since is linearly dependent on by a matrix . In (21) can be shifted to the parameters leading to with the base parameters . For the calculation of , (20) is again evaluated with separated matrices and , respectively, yielding and therefore Combining (23) and (24), can be obtained as It contains geometric parameters like lengths, lengths squared, gear ratios, and so on, see [13] for details. The identification process can be performed by sequentially arranging measurements in (18) and (22) leading to The unknown base parameters are calculated with the least-squares method leading to and therefore the solution is directly available since .
4.2. Optimal Exciting Trajectories
Similar to the problem of finding optimal configurations for the geometric calibration, the dynamic parameter identification requires exciting trajectories. In contrast to the steady-state configurations for the calibration, the identification trajectories include an additional dimension, the continuous time. Again, the aim is to minimize the condition number () of the covariance matrix by choosing suitable trajectories. A promising approach is introduced with [14]. This section summarizes these ideas, presents solutions to the optimization problem, and verifies the results on the RX130L robot.
4.2.1. Trajectory Description and Optimization
An adequate description for the identification trajectory for the th joint is given with This Fourier-series is parameterized by the finite order , the coefficients and , the base angular frequency , and the joint offset . To guarantee a periodic excitation of the robot, is a common parameter for all joints. To speed up the optimization time, a discretization of (29) and derivatives for one period of the base frequency () is done. The signals are therefore evaluated at discrete time values ,??, where is the number of discretization points. This provides the following optimization problem: The constraints include the mechanical joint limits , the permitted joint velocities , the expected maximum joint accelerations , and a space of allowed end-effector positions , which is used to avoid collisions with the robot itself and its surroundings. Due to the flatness property of the system, see Section 2.3.1, also the motor torques can be calculated dependent on the joint coordinates and derivatives. In contrast to [15], this motor torques are also used in our formulation. The optimization problem is solved with the Matlab-Optimization-Toolbox (fmincon). The parameters order , angular frequency , and turned out to work well to get adequate dynamical parameters, see Figure 5(b). For numerical scaling purposes, the columns of should also be normalized. Solving the optimization problem yields the optimal joint trajectories.
(a) End-effector path during identification process
(b) Measured and simulated motor torques
4.3. Experimental Results
The measurements for the overdetermined equations system (28) are taken to estimate the dynamical base parameters . Figure 5(a) presents a plot of the end-effector path. Along the path, velocities up to 5?m/s and accelerations up to 12?m/ are achieved. To verify the quality of the identified parameters, the real robot’s motor torques are recorded and compared to the simulated ones on a reference trajectory which is defined in the EN ISO NORM 9283, see [8]. Figure 5(b) emphasizes, by presenting the measured and simulated motor torques, the good parameter identification for the RX130L robot. Joints four, five, and six show an equivalent quality.
5. Optimal Trajectory Generation
There are two types of trajectory generation problems that have to be considered. The first one is the continuous path (CP) trajectory and the second one is the point-to-point (PtP) trajectory planning problem. For the first one, the geometric path in world coordinates is defined. Typical maneuvers are straight lines, circles, and splines. For the PtP planning, only the coordinates for the start-point and the end-point of the trajectory are given. These points can directly be transformed to joint coordinates , and the motion planning can be performed in this coordinates. For both cases, physical constraints like motor speeds and torques have to be taken into account. Additionally, the computed trajectories have to result in continuous velocities, accelerations, and jerks. This helps that the gear elasticities are not overly excited, leading to a soft, vibration-less end-effector movement. Again, the equations of motion, derived in Section 2.2 and identified in Section 4, provide the basis for this approach.
5.1. Continuous Path Trajectory
CP applications appear in many robotic applications like arc welding, deburring, and laser cutting. Due to the nonlinear kinematic interrelation between Cartesian space and the robot’s joint space, high motor speeds and accelerations may occur, especially near kinematic singularities. The CP motion planning can be divided into two subproblems, namely, a geometric path planner, describing a path as a function of a (scalar) trajectory parameter and a dynamic path planner that solves the problem of finding an optimized time behavior , see also [16]. For example, a straight line in space is characterized by (39). The vector contains the end-effector position while describes the evolution of the orientation in Cardan angles, both are functions of . The trajectory parameter itself is a function of time and the corresponding trajectory velocity and acceleration are The trajectory parameter and its derivatives (trajectory speed and acceleration) represent the trajectory and its evolution in a demonstrative and easily interpretable manner. Figure 6 shows an example for a straight line, beginning at and ending at . With six degrees of freedom and six world coordinates, the analytical solution of the inverse kinematic problem, see for example, [3], for the RX130 robot is obtained which can be differentiated with respect to time, leading to Inserting the kinematic relations (33) and (34) into the equations of motion (4) yields the following description of the robot’s dynamics: This representation allows to compute the motor torques as a function of the trajectory position, velocity, and acceleration. For additional details, see [17]. Please note that the inverse kinematics does not necessarily have to be computed analytically. Also numerical calculations of the inverse kinematics can be used.
5.2. Optimization Strategy
To find an optimal evolution along a desired trajectory, the objective reads where the motor torques are calculated in (35). The design parameters and are manipulated to weight between time and/or energy optimality. However, the upper limit of the integral in (36) is unknown in advance, merely a result of optimization.
Thus, a transformation with leads to an objective function with constant integration limits and as integration variable. Equation (38) summarizes the result. The objective function is also subject to a set of differential equations, describing the evolution of time along the trajectory parameter . The derivatives with respect to are used to compute all trajectory variables, which are found in the constraints. For instance, the trajectory acceleration is computed with where a prime indicates the differential operator . The input in the set of differential equations needs to be computed by a numerical solver in order to minimize the objective function while satisfying all constraints.
Finalizing the following complete description: the motor torques, motor velocities, and accelerations are constrained. To compute an optimal solution for the optimization problem stated in (38), the MUSCOD-II software package is utilized. Based on [18], it is developed by the Interdisciplinary Center for Scientific Computing at the University of Heidelberg. Using the direct multiple-shooting method, see [19, 20], the original continuous optimal control problem is reformulated as a nonlinear programming (NLP) problem which is then solved by an iterative solution procedure, a specially tailored sequential quadratic programming (SQP) algorithm. For further details on the used software package, see [21].
5.3. Experimental Results—CP Trajectory
For a straight tool center movement with constant end-effector orientation, the trajectory is given by where index indicates the start-point and index the end-point, respectively. For a grid of 200 shooting intervals, the time optimal case , is computed. Minimizing energy would lead to not only smoother, but also longer trajectories for and is therefore not considered in this example. The resulting trajectory parameters , and are retransformed into the time domain to compute the corresponding reference joint angles and the constrained feed-forward torques . The corresponding velocity profile is depicted in Figure 7. The velocity-drop occurs due to passing closely to a singular configuration.
The experiment with the Stäubli robot is conducted analogously to the previous section. The reference angles and torques are transferred to the servo drives where the occurring torques and motor velocities are measured for verification. Figure 8 shows the torques and velocities of the axes that actively constrain the trajectory. For readability reasons, the signals are normalized to their maximum values. The figure also shows that at least one constraint is always active, which is a necessary condition for the time-optimal solution.
5.4. Point-to-Point Trajectory
As an extension to the previous section, the PtP optimization is not limited to geometrically defined paths. Consequently, the one-dimensional variation of the trajectory parameter becomes a more complex two-point boundary value problem of the joint states with the additional task of minimizing time and/or energy, summarized in (40). Analogously to (38), the available joint velocities and torques are limited. Additionally, the joint limits need to be considered and the desired initial and final configurations , , , and are introduced.
A compromise between time and energy optimality is adjusted with the parameters and . Also note that the highly nonlinear differential equations of motion are included in the set of constraints.
5.5. Experimental Results—PtP Trajectory
The optimization formulation for the point-to-point trajectory is solved with MUSCOD-II setting and (time optimality). While the one-dimensional variation problem of the previous section is computed in one second, the PtP trajectory takes about three minutes to be calculated with a standard workstation. After solving the optimization problem, the system states and torque inputs for the two-point boundary problem are known and may be utilized as reference values. The initial conditions , , , and are chosen equals to the start- and endpoint of the previous section’s straight-line trajectory. This allows a comparison between both methods. The overall time of the trajectory was reduced from ?s to ?s. Figures 9(a) and 9(b) show the optimized torques and velocities. Again, the most interesting signals are selected for reasons of readability, all normalized to their maximum values. Experiments using these results on the RX130L robot verify validity of the optimized trajectory. The maximum end-effector velocity and acceleration that are reached along this trajectory are 5.2? and 48.6?, respectively. To avoid high jerks in joint space, which would induce vibrations, the torque rates are limited to .
(a) Optimization results for the joint torques
(b) Optimization results for the joint velocities
6. Conclusions
This work summarizes various optimization strategies for robotic systems to improve the overall performance. The individual results of each section not only verify the chosen approach but also offer values for expected accuracies of modern medium scale robotic systems. The suggested ideas are also applicable to smaller or larger scale robots with an arbitrary amount of degrees of freedom.
It is important to note that the quality of the control as well as the trajectory optimization is only as good as the underlying set of (kinematic or dynamical) parameters. As a consequence, the optimal configurations for calibration and the optimal-exciting trajectories for the identification process provide the basis for control and trajectory generations. All proposed methods can be automated by designing according to algorithms and used during the commissioning of robot applications.
Acknowledgments
Support of the current work within the framework of the Austrian Center of Competence in Mechatronics (ACCM) is gratefully acknowledged. The support of the Interdisciplinary Center for Scientific Computing at the University of Heidelberg is acknowledged, especially to Moritz Diehl and Andreas Poschka who kindly gave support. One of the basic ideas of this contribution is to review and present details using industrial, not laboratory, equipment. However, the authors do not have a direct relation to any mentioned hardware and software products. This information is consciously included, enabling the reader to review technical specifications for further comparisons.
Supplementary Materials
The video shows the described trajectory optimizations In a first step (Constant Trajectory Velocity), the robot moves on a straight line with constant velocity to show the path. The robot has to pass near a singular kinematic configuration. In this configuration the robot has to move very fast to stay on the line. The next phase of the video (Time Optimal Trajectory), described in chapter 5.3, shows the experimental results for the time optimal movement of the robot on the straight line, without violating maximal joint velocities, accelerations and motor torques. For the last part of the video (Time Optimal Point to-Point Trajectory), the constraint of moving on a straight line is given up, while again using the above physical constraints. The calculations are described in chapter 5.5.