Optimal Path and Trajectory Planning for Serial Robots: Inverse Kinematics for Redundant Robots and Fast Solution of Parametric Problems [1st ed. 2020] 978-3-658-28593-7, 978-3-658-28594-4

Table of contents :
Front Matter ....Pages I-XX
Introduction (Alexander Reiter)....Pages 1-7
Numerical Basics (Alexander Reiter)....Pages 9-47
Kinematics of Serial Robots (Alexander Reiter)....Pages 49-91
Path and Trajectory Planning (Alexander Reiter)....Pages 93-135
Optimal Path Tracking (Alexander Reiter)....Pages 137-154
Conclusion (Alexander Reiter)....Pages 155-158
Back Matter ....Pages 159-191

Citation preview

Alexander Reiter

Optimal Path and Trajectory Planning for Serial Robots Inverse Kinematics for Redundant Robots and Fast Solution of Parametric Problems

Optimal Path and Trajectory Planning for Serial Robots

Alexander Reiter

Optimal Path and Trajectory Planning for Serial Robots Inverse Kinematics for ­Redundant Robots and Fast Solution of ­Parametric Problems

Alexander Reiter Johannes Kepler University Linz Linz, Austria Dissertation, Johannes Kepler University Linz, 2019

ISBN 978-3-658-28593-7 ISBN 978-3-658-28594-4  (eBook) https://doi.org/10.1007/978-3-658-28594-4 Springer Vieweg © Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, the authors and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer Vieweg imprint is published by the registered company Springer Fachmedien Wiesbaden GmbH part of Springer Nature. The registered company address is: Abraham-Lincoln-Str. 46, 65189 Wiesbaden, Germany

Acknowledgement This work has been supported by the COMET-K2 “Center for Symbiotic Mechatronics” of the Linz Center of Mechatronics (LCM) funded by the Austrian federal government and the federal state of Upper Austria. Alexander Reiter

Contents List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . .

XI

List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . .

XIII

List of Symbols and Abbreviations

. . . . . . . . . . . . .

XV

Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . XVII Kurzfassung . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Numerical Basics . . . . . . . . . . . . . . . . . . . . 2.1 Numerical Optimization . . . . . . . . . . . . . . . . . 2.1.1 Convexity . . . . . . . . . . . . . . . . . . . . . . . . 2.1.2 Constrained Optimization . . . . . . . . . . . . . . . 2.1.3 Parametric Sensitivities . . . . . . . . . . . . . . . . 2.2 B-Spline Curves . . . . . . . . . . . . . . . . . . . . . . 2.2.1 Evaluation Algorithm . . . . . . . . . . . . . . . . . 2.2.2 Derivatives w.r.t. the Path Parameter . . . . . . . . 2.2.3 Derivatives w.r.t. the Control Points . . . . . . . . . 2.2.4 Constrained Approximation . . . . . . . . . . . . . . 2.3 Optimal Control Problems and Direct Methods . . . . 2.3.1 Direct Single Shooting . . . . . . . . . . . . . . . . . 2.3.2 Direct Multiple Shooting . . . . . . . . . . . . . . . . 2.3.3 Direct Collocation . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . .

XIX 1

. . . . . . . . . . . . . .

9 10 10 11 15 32 32 35 37 37 40 42 43 45

3 Kinematics of Serial Robots . . . . . . . . . . . . . . . .

49

VIII 3.1 3.1.1 3.1.2 3.2 3.3 3.3.1 3.3.2 3.4 3.4.1 3.4.2 3.5 3.5.1 3.5.2 3.5.3

Contents Position and Orientation . . . . . . . . . . . . . . . . . Position . . . . . . . . . . . . . . . . . . . . . . . . . Orientation . . . . . . . . . . . . . . . . . . . . . . . Translation and Rotation Velocity . . . . . . . . . . . . Joint Space, Work Space, Task Space . . . . . . . . . . Joint Space . . . . . . . . . . . . . . . . . . . . . . . Work Space . . . . . . . . . . . . . . . . . . . . . . . Forward Kinematics . . . . . . . . . . . . . . . . . . . . Position-Level Forward Kinematics . . . . . . . . . . Velocity-Level and Higher-Order Forward Kinematics Inverse Kinematics . . . . . . . . . . . . . . . . . . . . Kinematically Redundant Manipulators . . . . . . . . Position-Level Inverse Kinematics . . . . . . . . . . . Velocity-Level and Higher-Order Inverse Kinematics .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

50 50 51 53 54 54 55 58 58 59 62 63 65 72

4 Path and Trajectory Planning . . . . . . . . . . . . . 4.1 Boundary Value Problem . . . . . . . . . . . . . . . . . . 4.2 Optimal Point-to-Point Trajectory Planning . . . . . . . 4.2.1 Single-Step Approach . . . . . . . . . . . . . . . . . . . 4.2.2 Multi-Step Approach . . . . . . . . . . . . . . . . . . . 4.2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . .

93 95 97 98 116 135

5 Optimal Path Tracking . . . . . . . . . . . . . . . . . 5.1 Joint Space Approaches . . . . . . . . . . . . . . . . . 5.2 Work Space Approaches . . . . . . . . . . . . . . . . . 5.2.1 Kinematically Non-Redundant Manipulators . . . . . 5.2.2 Kinematically Redundant Manipulators . . . . . . . .

. . . . .

137 138 138 138 139

6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . .

155

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . .

159

A B-Spline Curves . . . . . . . . A.1 Arc Length Parameterization A.2 B-Spline Software Package . . A.2.1 Open Challenges . . . . . .

175 176 178 178

. . . .

. . . .

. . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . .

. . . .

. . . .

Contents B Mechatronic Multibody Systems . . . . . . . . . . . . . . B.1 Solution of the Equations of Motion . . . . . . . . . . . . . B.1.1 Constraints . . . . . . . . . . . . . . . . . . . . . . . . . B.1.2 Solution of System of DAEs . . . . . . . . . . . . . . . . B.1.3 Divide & Conquer: Constraint Forces and Accelerations B.1.4 Solution by Time Integration . . . . . . . . . . . . . . . B.1.5 Conditional Constraints . . . . . . . . . . . . . . . . . . B.1.6 Solution Algorithm . . . . . . . . . . . . . . . . . . . . . B.2 Code Generation Utility . . . . . . . . . . . . . . . . . . . B.2.1 Current Limitations and Open Challenges . . . . . . . .

IX 181 183 183 185 185 186 187 188 188 190

List of Figures 1.1 SCARA4 Lab Prototype. . . . . . . . . . . . . . . . . . . 1.2 SCARA4 Planar Model. . . . . . . . . . . . . . . . . . . 1.3 Stäubli TX90L Industrial Manipulator On Linear Axis. 1.4 Comau Racer R3 Industrial Manipulator. . . . . . . . .

. . . .

4 4 4 5

2.1

A Convex Function. . . . . . . . . . . . . . . . . . . . . . .

11

3.1 3.2 3.3 3.4 3.5 3.6 3.7 3.8

Position Vector. . . . . . . . . . . . . . . . . . . . . . . . . . Reachable Work Space of SCARA4 Manipulator. . . . . . . Dexterous Work Space of SCARA4 Manipulator. . . . . . . Forward Kinematics of the SCARA4 Manipulator. . . . . . . Inverse Kinematics of the SCARA3 Manipulator. . . . . . . Inverse Kinematics of the SCARA4 Manipulator. . . . . . . Domains of the Jacobian. . . . . . . . . . . . . . . . . . . . Algorithmic Singularity in JSD of the SCARA4 Manipulator.

50 56 57 62 66 68 80 84

4.1 4.2

Furuta Pendulum. . . . . . . . . . . . . . . . . . . . . . . Swing-Up Motion of Furuta Pendulum: Results for Joint Angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Swing-Up Motion of Furuta Pendulum: Results for Joint Velocities. . . . . . . . . . . . . . . . . . . . . . . . . . . . . Swing-Up Motion of Furuta Pendulum: Results for Motor Torque. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . SCARA4 Minimum-Time Trajectory Planning. . . . . . . . SCARA4 Minimum-Time Motion: Results for Constrained Accelerations. . . . . . . . . . . . . . . . . . . . . . . . . . . SCARA4 Minimum-Time Motion: Results for Constrained Motor Torques. . . . . . . . . . . . . . . . . . . . . . . . . .

95

4.3 4.4 4.5 4.6 4.7

. . . .

101 101 101 103 107 109

XII 4.8 4.9 4.10 4.11 4.12 4.13 4.14 4.15 4.16 5.1 5.2 5.3 5.4 5.5 5.6 5.7 5.8 5.9 5.10 5.11

List of Figures SCARA4 Minimum-Energy Trajctory Planning. . . . . . . . SCARA4 Minimum-Energy Motion: Results for Constrained Motor Torques. . . . . . . . . . . . . . . . . . . . . . . . . . Comau Racer R3 Trajectory Planning: Experimental Setup. SCARA4 Path Planning: Optimal Picking Poses. . . . . . . SCARA4 Path Planning: Admissible Picking Positions. . . . SCARA4 Path Planning: Admissible Picking Paths. . . . . . Synchronization Tasks of Two Comau Racer R3. . . . . . . Synchronization Tasks of Two Comau Racer R3: sync1. . . Synchronization Tasks of Two Comau Racer R3: sync2. . .

110

SCARA4 Minimum-Time Path Tracking. . . . . . . . . . . . SCARA4 Minimum-Time Path Tracking: Path Parameter and Derivatives. . . . . . . . . . . . . . . . . . . . . . . . . . . . SCARA4 Minimum-Time Path Tracking: Joint Angles and Derivatives. . . . . . . . . . . . . . . . . . . . . . . . . . . . SCARA4 Minimum-Time Path Tracking: Motor Torques. . . Stäubli TX90L Minimum-Time Path Tracking. . . . . . . . Stäubli TX90L Minimum-Time Path Tracking: Tracking Error. Stäubli TX90L Minimum-Time Path Tracking: Motor Torques. Stäubli TX90L Minimum-Time Path Tracking: Path Parameter and Derivatives. . . . . . . . . . . . . . . . . . . Stäubli TX90L Minimum-Time Path Tracking: Joint Angles and Derivatives. . . . . . . . . . . . . . . . . . . . . . . . . . Stäubli TX90L Minimum-Time Path Tracking: Motor Torques. Stäubli TX90L Minimum-Time Path Tracking, Experiment: Tracking Error. . . . . . . . . . . . . . . . . . . . . . . . . .

143

B.1 Schematics of Autonomous Walking Robot (AWARO). . . .

112 113 118 125 126 127 133 134

145 146 146 148 149 149 152 153 153 153 182

List of Tables 2.1 2.2 2.3

Parametric QP: Results. . . . . . . . . . . . . . . . . . . . . Parametric NLP: Results. . . . . . . . . . . . . . . . . . . . Parametric NLP: Results. . . . . . . . . . . . . . . . . . . .

4.1 4.2 4.3 4.4

SCARA4 Minimum-Time Trajctory Planning: Results. . . . SCARA4 Minimum-Energy Trajctory Planning: Results. . . Comau Racer R3 Trajectory Planning: Results. . . . . . . . SCARA4 Path Planning: Computation Time and Cost Increase in Synchronization Tasks. . . . . . . . . . . . . . . . . . . . SCARA4 Path Planning: Computation Time and Cost Increase in Synchronization Tasks. . . . . . . . . . . . . . . . . . . . Synchronization Tasks of Two Comau Racer R3: Success Rates in Synchronization Tasks. . . . . . . . . . . . . . . . . . . . Synchronization Tasks of Two Comau Racer R3, sync1 : Computation Time and Cost Increase. . . . . . . . . . . . . Synchronization Tasks of Two Comau Racer R3, sync2 : Computation Time and Cost Increase. . . . . . . . . . . . .

108 111 115

SCARA4 Minimum-Time Path Tracking: Results. . . . . . . Stäubli TX90L Minimum-Time Path Tracking: Results. . .

147 153

4.5 4.6 4.7 4.8 5.1 5.2

28 29 31

126 127 131 132 132

List of Symbols and Abbreviations Abbreviation DMS DOF DSS EE EoM GNA iff IK JSD KKT LICQ NLP OCP PG QP RG w.l.o.g.  

Description direct multiple shooting degree(s) of freedom direct single shooting end-effector equations of motion generalized nullspace augmentation if and only if inverse kinematics joint space decomposition Karush-Kuhn-Tucker linear equality constraint qualification nonlinear programming optimal control problem projected gradient quadratic programming reduced gradient without loss of generality correct, successful incorrect, failed

Abstract This dissertation covers the topic of optimal path and trajectory planning for serial robots in general, and rigorously treats the challenging application of path tracking for kinematically redundant manipulators therein in particular. Furthermore, methods for fast computation of approximate optimal solutions to planning problems are presented. As the theoretical foundation relevant to this thesis, basics of numerical optimization, B-spline curves and their use for the solution of optimal control problems using direct methods are discussed. Furthermore, the kinematics of serial robots is reviewed in depth. In particular, the inverse kinematics problem regarding kinematically redundant manipulators is considered. Therein, numerical optimization is applied not to find any solution to the path tracking problem, but rather to solve it in an optimal manner. As one contribution of this thesis, a method is presented that is particularly useful for this task. Therein, differential inverse kinematics is augmented by a weighted linear combination of the Jacobian nullspace basis, which allows to directly exploit a system’s internal motion capabilities in accordance with the path tracking optimization goal. It is capable of resolving both, the path tracking task and the optimal inverse kinematics problem, simultaneously. The efficacy of this approach is demonstrated using numerical examples and experiments. Optimal control problems require a certain level of complexity of the mathematical-physical models used therein to accurately reflect the corresponding real-world problem. As a result, the numerical optimization problem used to represent such a task, is typically time-consuming to solve. Moreover, certain parts of the problem setup may be subject to change and

XVIII

Abstract

are therefore not known beforehand. A sudden parameter change in the time between computing the optimal solution and the task execution using this solution may result in a suboptimal, or even inappropriate system behavior. The goal is of course to obtain a solution to the modified problem. Computing an exact optimal solution is typically not possible due to large computation times, classical methods for finding approximate optimal solutions produce – if at all – inadmissible results for larger perturbations. As another contribution, this thesis presents a method that exhibits fast convergence to an admissible, nearly optimal solution in the case of larger parameter perturbations. Therein, a first-order approximation of an optimal solution for a perturbed optimization problem is obtained by means of parametric sensitivities. The possibility of an active set of constraints different from that of the nominal solution is addressed which is the main advantage over existing methods. The potential of this approach is shown by means of a comparison with established methods as well as numerical examples and corresponding experiments.

Kurzfassung Diese Doktorarbeit behandelt optimale Bahn- und Trajektorienplanung für serielle Roboter im Allgemeinen und Pfadfolgestrategien für kinematisch redundante Roboter im Speziellen. Außerdem werden Näherungsmethoden für optimale Lösungen solcher Planungsaufgaben entwickelt, die im Vergleich zur exakten Lösungen eine bedeutend geringere Rechenzeit benötigen. In dieser Arbeit werden – soweit für das Verständnis relevant – zunächst die Grundlagen zur numerischen Optimierung, zu B-Spline-Kurven, sowie die Vereinigung dieser Gebiete in Optimalsteuerungsproblemen in direkter Formulierung untersucht. Zusätzlich wird eine genaue Untersuchung der Kinematik von seriellen Robotern durchgeführt, wie sie in Pfadfolgeproblemen auftritt. Besonderes Augenmerk wird auf die Inverskinematik für kinematisch redundante Roboter gelegt. Dabei wird numerische Optimierung angewandt, um nicht nur irgendeine, sondern die optimale Lösung für dieses Problem zu finden. Als einer der Beiträge dieser Arbeit wird eine Methode vorgestellt, die für diese Aufgabestellung besonders gut geeignet ist, da darin optimales Pfadfolgen und das darin enthaltene Inverskinematikproblem simultan optimal gelöst werden können. Dabei handelt es sich um einen differentiellen Inverskinematikansatz, der mittels gewichteter Basisvektoren des Nullraums der Jacobi-Matrix erweitert wird. Diese Vorgangsweise erlaubt es, die Möglichkeit für interne Bewegungen eines Systems hinsichtlich des Optimierungsziels des übergeordneten Pfadfolgeproblems direkt auszunutzen. Die Effektivität dieses Ansatzes wird mittels einiger Rechenbeispiele und zugehöriger Experimente gezeigt. Optimalsteuerungprobleme benötigen einen gewissen Detailgrad der darin verwendeten mathematisch-physikalischen Modelle, um die zugehörige Aufgabenstellung ausreichend genau abbilden zu können. Mit wachsender Genauigkeit steigt allerdings häufig auch die zur Lösung des Problems notwendige Rechenzeit an. Zusätzlich kann sich die Aufgabenstellung durch

XX

Kurzfassung

sich ändernde Parameter beeinflusst werden, womit die Aufgabe zu einem parameterabhängigen Optimalsteuerungsproblem wird. Wenn Parameterschwankungen im Zeitraum zwischen der Bestimmung der optimalen Lösung und ihrer Ausführung liegen, so wird die neue Aufgabe im Allgemeinen durch diese Lösung nur suboptimal oder garnicht gelöst. Das Problem für die tatsächlich gültigen Parameter zu lösen ist oftmals aufgrund von Rechenzeiteinschränkungen nicht möglich. Einen Ausweg stellt eine Methode dar, die in kurzer Rechenzeit eine zulässige Näherung der optimalen Lösung des parameterabhängigen Optimalsteuerungsproblems liefert. Klassische Methoden sind hier hinsichtlich der erlaubten Größe der zulässigen Parameterstörung für diese Aufgabenstellung nur eingeschränkt nutzbar. Einen weiteren Beitrag dieser Arbeit stellt ein Ansatz dar, mit dessen Hilfe mit vergleichsweise geringem Rechenaufwand eine zulässige Näherung erster Ordnung der optimalen Lösung bestimmt werden kann. Diese wird mittels parametrischer Sensitivitäten aus dem Optimierungsproblem für nominale Parameterwerte berechnet. Der Ansatz zeichnet sich durch die Eigenschaft aus, dass auch eine Veränderung der Menge der aktiven Nebenbedingungen des Optimierungsproblems berücksichtigt werden kann, was einen Vorteil gegenüber bestehenden Methoden darstellt. Die Wirksamkeit dieser Methode wird durch Vergleichsrechnungen mit bewährten Methoden, sowie durch weitere numerische Beispiele und deren experimentelle Umsetzung demonstriert.

Chapter 1

Introduction Due to the rapid progress in developing solutions for industrial applications of robotic manipulators, today it is not sufficient to simply fulfill a given task. Now it is required to solve a given problem in an optimal manner. To this end, not only foundational knowledge about kinematics and dynamics of multi-body systems and robotics sub-disciplines emerging therefrom are required, it also necessitates expertise in numerical optimization. The present thesis provides contributions to sub-fields of kinematics and also of numerical optimization. In the former field, the inverse kinematics for a specific class of robotics is investigated. In the latter, numerical optimization, particular approximate solutions of parametric optimization problems are discussed. In order to facilitate understanding of these topics, basics regarding all required fields of knowledge are presented in brevity and references to state-of-the-art literature are provided. An important portion of the present thesis is devoted to kinematic redundancy. In short, it is a property of the kinematic structure that robots of certain types exhibit. It allows a robot to perform additional, internal motion while executing a prescribed motion of its end-effector. That is, for a given motion of a robot’s end-effector, the joint motion is ambiguous, thus enabling additional criteria to be pursued in addition to end-effector path tracking. The mention of kinematic redundancy in this thesis is twofold. On the one hand, it is discussed in the kinematics chapter in Section 3.5 where its basic properties and consequences are reviewed. In particular, © Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4_1

2

1 Introduction

inverse kinematics methods for such kinematic structures are examined. On the other hand, kinematic redundancy is exploited in course of optimal path tracking in Chapter 5 to increase the task execution performance. Outline In Chapter 2, numerical basics relevant for the rest of the work are presented. Therein, elementary knowledge regarding numerical optimization is repeated in Section 2.1. In particular, parametric optimization problems are discussed wherein an iterative scheme to compute approximate solutions to such problem is proposed as one contribution of this work. Furthermore, Bspline curves are reviewed in Section 2.2 which are then used for general optimal control problems in Section 2.3. Chapter 3 treats kinematic properties of serial robots, i.e. the problem of forward kinematics as well as the more challenging inverse kinematics. Therein, kinematically non-redundant and redundant manipulators are considered and solutions on position-, velocity- and higher derivative level are presented. A further contribution of this thesis is a particular approach to compute a solution to the inverse kinematics problem for kinematically redundant manipulators on derivative level. Optimal path and trajectory planning, cf. Chapter 4, are robotic applications of the above. Therein, the evolution of a robot’s joint positions (and by virtue of the kinematic chain) the end-effector, is computed to fulfill a specified task, e.g. point-to-point motion. This is accomplished using different strategies, a single-step strategy wherein the geometric and temporal aspects are treated simultaneously, and a multi-stage approach where these subtasks are solved subsequently. Furthermore, in order to allow for rapid computation of the above, the iterative approximation scheme for optimal solutions using parametric sensitivities is applied. Chapter 5 discusses optimal path tracking. In contrast to path and trajectory planning, therein, a path is prescribed and needs to be tracked by the robot’s end-effector in an optimal way. With the previously discussed inverse kinematics for kinematically redundant manipulators and optimal control theory, optimal solutions not only regarding the inverse kinematics

1 Introduction

3

resolution, but also for the path tracking problem are obtained. For kinematically non-redundant robots, neither the inverse kinematics, nor the path tracking problem are in the scope of this thesis. Chapter 6 provides final remarks on this thesis and concludes with a brief summary of the work presented herein. In Appendix A, an alternative parametrization of B-spline curves in terms of the arc length is discussed. A software package that allows for evaluation and construction of such functions is also proposed therein. Appendix B presents additional information about the dynamics of multi-body systems, their representation as a system of coupled differential-algebraic equations and their numerical solution. Furthermore, a software library that facilitates code generation regarding the numerical solution of such problems is introduced. Examples In order to ease understanding, theoretical parts are frequently complemented by numerical examples of typical applications. The robots featured therein are of various types, planar and spatial, kinematically non-redundant and redundant, as well as industrial manipulators and lab prototypes. The following brief overview provides an introduction to the robots mentioned throughout this work. Example 1.1: SCARA4 (description) The SCARA4 manipulator is a kinematically redundant lab prototype that is shown in Figures 1.1 and 1.2. It comprises of four links, serially connected by four actuated revolute joints as well as a prismatic joint with a gripper attachment. In this thesis, only the planar structure with four revolute joints is considered, cf. the reduced presentation in Figure 1.2. The vertical prismatic joint with the gripper attachment are not part of the model.

4

1 Introduction

Figure 1.1: SCARA4 Lab Prototype.

Figure 1.2: SCARA4 Planar Model.

Example 1.2: Stäubli TX90L (description) The Stäubli TX90L is a spatial industrial robot, cf. Figure 1.3 that consists of six revolute joints in a serial structure and features a spherical wrist. In this setup it is placed on top of a linear axis, i.e. a prismatic joint, establishing inherent kinematic redundancy (which can be disabled by simply locking the prismatic joint).

Figure 1.3: Stäubli TX90L Industrial Manipulator On Linear Axis (Contour Rendering).

Example 1.3: Comau Racer R3 (description) The Comau Racer R3 is a kinematically inherently non-redundant, spatial, industrial robot, cf. Figure 1.4 that consists of six revolute joints in a serial structure and features a spherical wrist.

1 Introduction

5

Figure 1.4: Comau Racer R3 Industrial Manipulator (Contour Rendering).

In this thesis a number of numerical examples are discussed including their respective computation times. These computations were implemented in MATLAB R2015b on Windows running on an Intel Xeon E3-1245V3 CPU. Aim & Contributions The two main sets of contributions in this thesis are the following: • Fast solution approximation using parametric sensitivities. In certain applications, the computation time available to find a solution to a complex optimization problem is limited, e.g. for a robotics path trajectory planning task by a real-time system with a fixed cycle time. To that end, in many cases, solutions to such problems cannot be obtained by traditional optimization schemes which gives rise to computationally less demanding methods that compute approximations to the solution of an optimization problem. Such an approach requires an existing solution to a nominal problem as an initial guess to which the actual situation presents a deviation or perturbation. A frequent limitation to such approaches is the size of the allowed deviation. This work proposes an iterative scheme using parametric sensitivities that allows for large perturbations. Of course, it cannot only be applied to above robotic tasks, cf. Chapter 4, but is applicable to more general problems, cf. the (successful) comparison to more established approaches in the academic examples from Section 2.1.3. Publications of the author in this field are the conference papers [112, 116].

6

1 Introduction • Optimal inverse kinematics and path tracking for kinematically redundant manipulators. Optimal path tracking applications typically include an inverse kinematics scheme that allows to compute joint angles that correspond to a prescribed end-effector configuration. While for kinematically non-redundant manipulators this does not pose a problem, for redundant system it is more involved as infinitely many joint configurations result in the same end-effector pose. This leads to the question about the configuration that is the best, and how best is understood in this case. While the user necessarily specifies the latter when setting up the optimal path tracking problem, the former can be solved in the numerical optimization therein. However, the combination of both tasks, optimal inverse kinematics resolution in optimal path tracking poses a challenging endeavor. As mentioned in the above overview, a novel inverse kinematics approach is presented in Section 3.5.3 of this thesis. It is called generalized nullspace augmentation and allows direct parameterization of the internal motion of the robot under consideration of the path tracking goal. This method is successfully applied in the optimal path tracking example in Chapter 5. This field was investigated by the author beginning with his Master’s thesis [109, 110] and the conference paper [117] resulting therefrom. In the conference papers [113, 114, 115] and the journal paper [111] research on this topic was intensified.

Minor contributions that are a side product of the work leading up to this thesis are: • B-spline software package. Another contribution is a library to evaluate and construct B-spline curves and surfaces which is documented in Appendix A. In a similar manner, a method for curve fitting of measurement data with periodic functions was contributed to [141] in the context of trajectory generation for a robotic replacement for therapeutic horseback riding. • Code generation software package. For applications such as numerical simulation, model-based control and parameter identification, typically the differential-algebraic equations governing a system’s behavior

1 Introduction

7

are derived symbolically. The contribution consists of a code generation utility that allows such models to be conveniently exported from the Maple computer algebra software into the MATLAB/Simulink simulation environment. This is discussed in Appendix B. The author’s publicactions in the field of modeling, model reduction and control of multi-body dynamics of mechanical systems and system complexity reduction are [63, 51, 84].

Chapter 2

Numerical Basics In this chapter, the mathematical and numerical foundation of the subsequent chapters will be established. First, a brief overview of optimization theory regarding gradient-based optimization approaches is given. It includes definitions of relevant terms, introduces formalisms which will be applied later and provides information about the subfield of parametric optimization problems. A more detailed treatment of these topics as well as applicactions can be found in textbooks, e.g. [39]. In the following sections, B-spline curves are discussed. These constitute a versatile, well-researched (most famously [103]), widely used function class not only in robotics but also in many other fields. Not only basic definitions, but also algorithms for evaluation and construction of these functions are provided. Therein, special attention will be paid to construction by constrained approximation (and interpolation) as this is an important tool used throughout the present thesis. Both topics, numerical optimization and B-spline curves, will then be combined in the subsequent chapters in order to facilitate optimal path and trajectory planning for robotic applications. It is not an aim and not in the scope of this thesis to provide comprehensive treatment of these well-researched subjects, but only to provide sufficient

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4_2

10

2 Numerical Basics

details for understanding the contribution of the presented work in the following chapters.

2.1

Numerical Optimization

In the following brief summary of numerical optimization basics, proofs for definitions are omitted as long as they are not constructive or otherwise beneficial to the main subjects of this thesis. Interested readers are referred to some of the many good works on this subject, e.g. on general numerical optimization [92], on convex optimization [16], the textbook [39]. The following definitions are largely adapted from these excellent works.

2.1.1

Convexity

Definition 2.1: Convex Set A set S ⊆ Rn is called convex if a line connecting the points x, y ∈ S lies entirely in S, i.e. x + k(y − x) = (1 − k)x + ky ∈ S ∀k ∈ [0, 1]. Definition 2.2: Convex Function A function f : S → T is called convex if its domain S is a convex set and f ((1 − k)x + ky) ≤ (1 − k)f (x) + kf (y) ∀k ∈ [0, 1] holds. Definition 2.3: Concave Function A function f : S → T is called concave if (−f ) is convex. Example 2.1: Convex Function Consider the plot of the function f (x) = x2 defined on the interval x ∈ S = R as shown in Figure 2.1. As S is obviously a convex set and f ((1 − k)x1 + kx2 ) ≤ (1 − k)f (x1 ) + kf (x2 ) ((1 − k)x1 + kx2 )2 ≤ (1 − k)x21 + kx22 (1 −

k)2 x21

+ 2(1 − k)kx1 x2 + k 2 x22 ≤ (1 − k)x21 + kx22 0 ≤ k(1 − k) (x1 − x2 )2 

 ≥0



 ≥0



2.1 Numerical Optimization

11

with x1 , x2 ∈ S holds ∀k ∈ [0, 1], f is a convex function. f (x) 1 0.8 0.6 0.4 0.2 0

−1 −0.8 −0.6 −0.4 −0.2

0

0.2

0.4

0.6

0.8

1

x

Figure 2.1: A Convex Function.

2.1.2

Constrained Optimization

With the vector of optimization variables w a nonlinear programming (NLP) problem is given as minimize J(w) w s.t. gi (w) = 0, i = 1, . . . , Neq gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq

(2.1.1a) (2.1.1b) (2.1.1c)

which minimizes the objective function J(w) w.r.t. w while satisfying the Neq equality constraints gi (w), i = 1, . . . , Neq and the Nineq inequality constraints gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq that are collected in the vector g(w) of dimension Ncon = Neq + Nineq . Therein, Neq is the number of equality constraints and Nineq counts the inequality constraints. For the following it will be assumed that J(w) and g(w) are twice continuously differentiable w.r.t. w. Definition 2.4: Feasible Set The feasible set Ω is given as the set of points w which satisfy the constraints (2.1.1b) and (2.1.1c), i.e. Ω = {w : gi (w) = 0, i = 1, . . . , Neq ; gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq }.

12

2 Numerical Basics

Definition 2.5: Convex Programming Problem An optimization problem (2.1.1) is convex if the feasible set Ω and the objective function J : Ω → R are convex. The feasible set is convex if the equality constraints are linear and the inequality constraints are concave. Definition 2.6: Active Constraints, Active Set A constraint gi is called active at w∗ iff gi (w∗ ) = 0, otherwise inactive. The vector collecting all active constraints is denoted ga which is of dimension N a . The index set A(w∗ ) ⊆ {1, . . . , Ncon } of active (in)equality constraints is called the active set. Note that this not only concerns inequality constraints (2.1.1c) but also all equality constraints (2.1.1b) as they are considered to be always active. The above definitions rely on the equality operator = to determine the activity state of a constraint. In practice it is difficult to correctly identify a constraint’s state due to numerical issues and limited accuracy in computer implementations. This topic will be later picked up when parametric sensitivities are discussed as the correct identification of the active set is of eminent importance. Definition 2.7: Linear Inequality Constraint Qualification (LICQ) For a solution w∗ of (2.1.1) and an active set A(w∗ ) the LICQ holds if the set of the gradients of the active constraints w.r.t. w, i.e.   ∂ ∗ ∗ g (w ) : i ∈ A(w ) , is linearly independent, i.e. i ∂w ⎡

ker⎣



⎤ ⎦

∂ a ∗ ∂ a g (w ) , . . . , g a (w∗ ) ∂w 1 ∂w N

= ∅.

2.1 Numerical Optimization

13

Definition 2.8: Lagrange Function The Lagrange function of the optimization problem (2.1.1) is given as L(w, λ) = J(w) + λ g(w)

(2.1.2)

with the vector of Lagrange multipliers λ of dimension Ncon (the same dimension as the vector of constraints g). Definition 2.9: Global Minimum A point w∗ that is a (strict) global solution to the NLP problem (2.1.1) is found if it is a (strict) global minimizer, i.e. J(w∗ ) ≤ ( ∗ 0, then w is a local solution. Note that these conditions are sufficient but not necessary, i.e. they do not need to be met by a strict local minimizer.

2.1.3

Parametric Sensitivities

2.1.3.1

Parametric Nonlinear Programming Problems

In most practical cases, NLP problems such as (2.1.1) do not only involve optimization variables w, but also – implicitly – other problem-specific parameters p. Consider, for example, one of the robots shown in the introductory Chapter 1. In an optimal trajectory planning task, a load applied to its end-effector (EE) could be regarded as a parameter that is subjected to uncertainties. How does an optimal motion trajectory depend on the EE load and how does it change for load variations?

16

2 Numerical Basics

While the solution w∗ of (2.1.1) is found for a specific, nominal parameter setting pnom , the question remains how the optimal solution w∗ depends on p and how it deviates for parameter changes. Research on such parametric, i.e. parameter-dependent, NLP problems has been mainly initiated by [47, 46, 119]. For the following investigations, consider the parametric NLP problem w∗ (p) = arg minimize J(w, p) w s.t. gi (w, p) = 0, i = 1, . . . , Neq

(2.1.5a) (2.1.5b)

gi (w, p) ≤ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.5c) that is specified analogously to (2.1.1) but now includes explicit dependencies of the vector of parameters p in the objective function J(w, p) and the vector g(w, p) that collects the equality constraints (2.1.5b) and the inequality constraints (2.1.5c). Also, the optimization result w∗ is now denoted with a dependency of the parameters p in order to emphasize their impact. By extending (2.1.2), the parametric Lagrange function is L(w, λ, p) = J(w, p) + λ g(w, p),

(2.1.6)

and similar to (2.1.3), the parametric KKT conditions are found to be

∂ L(w∗ , λ∗ , p) = 0 ∂w gi (w∗ , p) = 0, i = 1, . . . , Neq ∗

gi (w , p) ≤ 0, i = Neq + 1, . . . , Neq + Nineq λ∗i

λ∗i gi (w∗ , p)

(2.1.7a) (2.1.7b) (2.1.7c)

≥ 0, i = Neq + 1, . . . , Neq + Nineq

(2.1.7d)

= 0, i = 1, . . . , Ncon .

(2.1.7e)

2.1 Numerical Optimization 2.1.3.2

17

Derivation of Parametric Sensitivities

By considering the local solution w∗ , the KKT conditions (2.1.7a) to (2.1.7c) can be rewritten as ⎛

k(w∗ , λ∗ , p) =

 ⎞ ∂ ∗ ∗ ⎜ ∂w L(w , λ , p) ⎟ ⎝ ⎠ a ∗

g (w , p)

⎛

=

⎞    ∂ a ∂ ∗ a∗ ⎜ ∂w J(w , p) + ∂w g (w, p) λ ⎟ ⎝ ⎠ a ∗

g (w , p)

=0

(2.1.8)

wherein by virtue of strict complementarity only the active inequality constraints are included that are collected in the vector ga (w∗ , p). Inactive constraints are not relevant for the subsequent derivations. By computing the total derivative of (2.1.8) w.r.t. the vector of parameters p, the second-order expression ⎡

2

∂ d L(w∗ , λ∗ , p) k(w∗ , λ∗ , p) = ⎢⎣ ∂w2∂ a ∗ dp ∂w g (w , p) 

⎤  ⎤⎡ d ∂ a w∗ ⎥ g (w, p) ⎢ ⎥ dp ∂w ⎦⎣ d a ∗ ⎦

 K(w∗ ,λ∗ ,p)

⎡

+



O

  ⎤ ∂ d ∗ ∗ ⎢ dp ∂w L(w , λ , p) ⎥ ⎣ ⎦ d a ∗ dp g (w , p)



dp λ

=O

+ ...

(2.1.9)

is obtained that includes the KKT matrix K(w∗ , λ∗ , p) which is regular d w∗ , at a solution. From this equation, the parametric sensitivities, i.e. dp the partial derivatives of the optimal solution w.r.t. the parameters and d a∗ dp λ , the partial derivatives of the optimal active multipliers w.r.t. the parameters, are obtained by ⎡

⎤ d ∗ ⎢ dp w ⎥ ⎣ d a∗⎦ dp λ

⎡

−1

∗

∗

= −K (w , λ

 ⎤  d ∂ ∗ ∗ ⎢ dp ∂w L(w , λ , p) ⎥ ⎦. , p)⎣ d a ∗ dp g (w , p)

(2.1.10)

18

2 Numerical Basics

2.1.3.3

First-Order Approximation of Optimal Solutions to Perturbed Parametric NLPs

The above result gives rise to a first-order Taylor series approximation of the optimal solution (w∗ , λ∗ ) for perturbed parameters ppert , i.e. d ∗ w (ppert − pnom ) dp ˆ a∗ (ppert ) = λa∗ (pnom ) + d λa∗ (ppert − pnom ) . λa∗ (ppert ) ≈ λ    dp ˆ ∗ (ppert ) = w∗ (pnom ) + w∗ (ppert ) ≈ w

(2.1.11) (2.1.12)

Δp

Henceforth, the abbreviations (·)∗nom := (·)∗ (pnom ) and (·)∗pert := (·)∗ (ppert ) will be used to denote the optimal solution in the nominal and perturbed case, respectively. A similar notation will be used for the approximate quantities. 



∗ ˆ∗ ˆ pert Note that the solution estimate w ,λ pert obtained by means of the firstorder approximations (2.1.11) and (2.1.12) is not necessarily admissible w.r.t. the (in)equality constraints (2.1.5b) and (2.1.5c) imposed onto the original NLP problem. Therefore the question arises how large the admissible parameter perturbations Δp may become. The limitations of (2.1.11) and (2.1.12) are given by the change of the active set of constraints due to Δp. Note, that the vector of the estimate of optimal multipliers in the ˆ ∗ is found by calculating the active multiplier estimate perturbed case λ pert λa ∗ as in (2.1.12) and then inserting zeros for all inactive constraints.

Admissible Parameter Range Without Active Set Change Therein, three variants may occur wherein the impact of perturbations of single parameters on single constraints and their corresponding multipliers is considered: 1. The trivial case in which a constraint gi , i = 1, . . . , Ncon maintains its nominal activity state. ∗ / A(wnom ) may become active, i.e. enter 2. An inactive constraint gi , i ∈ the active set which corresponds to the constraint residue becoming

2.1 Numerical Optimization

19

zero. Therefore, a first-order approximation is employed, i.e. for only a perturbation of the j-th element of the parameter vector p 



∗ ˆ ∗ (ppert ), ppert ) ≈ gˆi (ppert ) gi wpert , ppert ≈ gi (w d ∗ ∗ , pnom ) + gi (wnom , pnom )(pj,pert − pj,nom ) gˆi (ppert ) = gi (wnom dpj (2.1.13) ∗

dwnom ∂ ∗ ∗ holds with dpdj gi (wnom , pnom ) = + ∂w gi (wnom , pnom ) dpj ∂ ∗ ∂pj gi (wnom , pnom ). Note the sensitivity of the optimization variables therein. By rearranging this equation an approximation pˆj,pert of the perturbed j-th parameter is found such that the approximation of the i-th constraint becomes active, i.e. gˆi (ppert ) = 0:

pˆj,pert = pj,nom −

∗ , pnom ) gi (wnom . d ∗ dpj gi (wnom , pnom )

(2.1.14)

If the denominator in the second term vanishes, the parameter approximation becomes infinite. This means that the constraint gi does not depend on pj and thus arbitrary perturbations of this parameter are admissible. 3. An active constraint gi , i ∈ A(w∗ ) may become inactive, i.e. leave the active set which corresponds to a multiplier becoming close to zero. Assuming only a perturbation of the j-th element of the parameter vector p, the first-order approximation (2.1.12) of the i-th multiplier yields ∗ ˆ ∗ (ppert ) = λ∗ (pnom ) + dλi (pnom ) (pj,pert − pj,nom ). λ i i dpj

(2.1.15)

Now the perturbed parameter pj,pert is computed that yields a correˆ ∗ (ppert ) = 0 that is considered inactive: sponding multiplier λ i pˆj,pert (λ∗i (pnom )) = pj,nom −

λ∗i (pnom ) dλ∗i (pnom ) dpj

.

(2.1.16)

20

2 Numerical Basics Again, if the denominator in the second term vanishes, the parameter approximation becomes infinite as the active multiplier λi does not exhibit a sensitivity w.r.t. pj and thus arbitrary perturbations of this parameter are admissible.

In [22] the authors present a first-order approximation of the parameter range that does not yield a change in the active set of constraints. In the referenced work this is called the sensitivity domain which is somewhat misleading as it refers not to the admissible domain of the sensitivities but to the parameter range. In the present work, the term admissible range of parameters will be used. A a hyperrectangular approximation of the admissible parameter range is given by 

pj,pert ∈Pj,pert = max({ˆ pj,pert (λ∗i (pnom )) < pj,nom } ∪ {−∞}), i



min({ˆ pj,pert (λ∗i (pnom )) > pj,nom } ∪ {∞}) , i = 1, . . . , Ncon . i

(2.1.17) As only the impact of single parameters on the active set are considered with the other parameters remaining at their respective nominal values, it is not necessarily admissible to superpose ranges of perturbations of multiple parameters in order to find the true admissible range. However, in [22] the authors further propose a multi-step first-order solution approximation that exploits knowledge of the admissible range of parameters: d ∗ d ∗ ˆ (ppert,adm − p)Δp2 w (pnom )Δp1 + w dp dp (2.1.18) ˆ ∗ (ppert,adm − p)Δp2 . ˆ ∗ (ppert ) = λ∗ (pnom ) + d λ∗ (pnom )Δp1 + d λ λ dp dp (2.1.19)

ˆ ∗ (ppert ) = w∗ (pnom ) + w

Therein, the perturbation Δp = (ppert − p) = Δp1 + Δp2 is divided in two parts, one up to the admissible range and the rest. Regarding the former part Δp1 = (ppert,adm − p), i.e. the perturbation within the admissible range, the approximations (2.1.11) and (2.1.12) are applied to compute the solution ap-

2.1 Numerical Optimization

21

proximation at the bounds of the admissible parameter range. For the latter part Δp2 = (ppert − ppert,adm ), i.e. the perturbation exceeding the admissid d ˆ∗ ˆ ∗ (ppert,adm − p), dp ble range, additionally sensitivities dp λ (ppert,adm − p) w are computed at the boundary point which are then used for a similar approximation. Note that these sensitivities are directional sensitivities, i.e. they are tangential to the hyperrectangle of constraint approximations. They are obtained by modifying the KKT system (2.1.9) in order to reflect the active set at the bounds of the admissible parameter range. Therefore knowledge of the correspondence of the admissible range limits and the responsible constraints is required which can be obtained as a byproduct by (2.1.14) and (2.1.16). Identification of Active Set So far, identifying the active set of constraints, i.e. Definition 2.6, relies on exact satisfaction of equalities. However, in practice, such comparisons are unsuitable due to round-off errors and other accuracy issue resulting from commonly used number systems in computer implementations. Therefore, it is more practical to check if a certain quantity is in a range given by the result of an identification function that corresponds to the considered optimization problem. In case of a perturbation the multipliers and the inequality constraint residues inform about the active set and possible changes thereof. As in numerical implementations comparisons against zero are impractical. Instead, a nonzero, but small threshold value, given by an identification function ρ(w, λ), should be used. The active set A(w, λ, p) = {i ∈ {1, . . . , Ncon } : gi (w, p) ≥ −ρ(w, λ, p)}

(2.1.20)

and the set of strictly active constraints A+ (w, λ, p) = {i ∈ A(w, λ, p) : λi ≥ ρ(w, λ, p)}

(2.1.21)

22

2 Numerical Basics

are determined using the identification function [43] ρ(w, λ, p) =

 ⎛   ⎞   ∂  ⎜ L(w, λ, p) ⎟  ⎝ ∂w ⎠ 

(2.1.22)

min(−g(w, p), λ)

wherein min(·, ·) : Rn × Rn → Rn is applied elementwise, or alternatively [95] ⎛

ρ(w, λ, p) =

⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝



 ∂ ∂w L(w, p, λ)

⎞ ⎟ ⎟

⎟ g1 (w, p) ⎟ ⎟ ⎟ ⎟ g2 (w, p) ⎟ ⎟ .. ⎟ . ⎟ ⎟ ⎟ (w, p) g ⎟ N eq ⎟   ⎟ min−gNeq +1 (w, p), λNeq +1  ⎟ ⎟ ⎟ ⎟ min −gNeq +2 (w, p), λNeq +2 ⎟ ⎟ .. ⎟ . ⎟  ⎠ min −gNeq +Nineq (w, p), λNeq +Nineq

(2.1.23)

1

For the parametric NLP problems considered in this section, the function from the referenced publications required extension by the parameters p, i.e. ρ(w, λ, p). A Special Case: Linear Constraint Perturbations In preparation of the next section, dealing with restoration of admissibility of solution approximations, consider the parametric NLP problem (2.1.5) with additional parameters p w∗ (p) = arg minimize J(w, p) w

(2.1.24a)

s.t. g(w, p, p) = g(w, p) − p ≤ 0, i = 1, . . . , Ncon . (2.1.24b) Therein, p are considered as linear constraint perturbations themselves, i.e. pnom = 0. The objective J is perturbation-free and the only perturbations appear linearly in the (in)equality constraints.

2.1 Numerical Optimization

23

For this special case, the parametric sensitivities (2.1.10) yield ⎤ d ∗ ⎢ dp w ⎥ ⎣ d ∗⎦ ⎡

dp λ

 ⎤  d ∂ ∗ ∗ ⎢ dp ∂w L(w , λ , p, p) ⎥ ⎦ , p)⎣ d a dp g (w, p, p) ⎡ ⎤ ⎡

−1

∗

∗

= −K (w , λ

O = −K−1 (w∗ , λ∗ , p)⎣ ⎦ −I 









(2.1.25) 

∂ ∂ ∂ a as ∂w L(w∗ , λ∗ , p, p) = ∂w J(w∗ , p) + ∂w g (w∗ , p, p) λa ∗ is independent of p and g is linear in p. Note that also the KKT matrix K is independent of the linear perturbation p, in fact, it is the same KKT matrix as in the original parametric NLP problem, cf. (2.1.9).

Restoring Admissibility The first-order approximations (2.1.11) and (2.1.12) yield a result that is not necessarily admissible w.r.t. the (in)equality constraints. However, for small perturbations (that do not change the active set) the stability of the solution can be exploited by the Newton-type iterative Algorithm 1 in order to restore admissibility, cf. [21]. Therein, inadmissible constraint residues are regarded as linear perturbations of these constraints which gives rise to the application of the sensitivity expressions from (2.1.25). Algorithm 1 assumes that a parametric NLP problem (2.1.5) has already been solved for a set of nominal parameters d d ∗ pnom and that parametric sensitivities dp w∗ and dp λ are readily available. Algorithm 1 Iterative Feedback Method From [21]. d ˆ ∗ (ppert ) ← w∗ (pnom ) + dp 1: w w∗ Δp  first-order approximation (2.1.11) ˆ ∗ (ppert ) ← λ∗ (pnom ) + d λ∗ Δp ˆ ∗ := λ 2: λ  first-order approximation dp

3: 4: 5: 6: 7: 8:

(2.1.12) ˆ ∗ , ppert p ← ga (w )  residueof active constraints (2.1.5b) and (2.1.5c)  ˆ ∗ , ppert do ˆ ∗, λ while p > ρ w  threshold (2.1.22) ∗

ˆ ∗ − dw ˆ∗ ← w w dp p dλ∗ ∗ ∗ ˆ ˆ λ ← λ − dp p end while ˆ return w

 sensitivities (2.1.25)

24

2 Numerical Basics

Note the difference in sign in the iterative update steps of Algorithm 1 compared to the approximations (2.1.11) and (2.1.12). These are a result of the Newton-type iteration scheme that reduces the residues in the active constraints. Convergence of this algorithm is guaranteed in a small neighborhood of the nominal parameter pnom due to the stability of the solution as shown in [20, 19]. As mentioned before, this method is limited to a constant set of active constraints. This issue can be addressed by means of the first-order predictor approach presented in [39]. Therein, an NLP is approximated by a quadratic programming (QP) problem 1 ∂ 2 L(wnom , λnom , pnom ) Δw+ Δw∗ = arg minimize Δw Δw 2 ∂w2 ∂ 2 L(wnom , λnom , pnom ) ∂J(wnom , pnom ) + Δw + Δp Δw ∂w ∂w∂p (2.1.26a) ∂g(wnom , pnom ) ∂g(wnom , pnom ) Δw + Δp ≤ 0 s.t. g(wnom , pnom ) + ∂w ∂p (2.1.26b) yields the optimal solution increment Δw∗ that gives an approximation of the optimization variables ˆ ∗ (ppert ) = wnom + Δw∗ w

(2.1.27)

in case of a parameter perturbation Δp. It can be shown that the KKT conditions of the quadratic approximation (2.1.26) are the same as in (2.1.7). The multipliers resulting from the QP problem (2.1.26) are the desired ˆ ∗ (ppert ). The active set of constraints may be different from approximations λ A(wnom , λnom , pnom ) and may converge to A(wpert , λpert , ppert ) in certain cases. However, in general it cannot be expected that the result obtained by the approximation is admissible w.r.t. the (in)equality constraints imposed onto the original NLP problem as the above QP does not include the original constraints but only their first-order approximation at the nominal solution. Another similar QP-based approach can be found in [71].

2.1 Numerical Optimization

25

In contrast to Algorithm 1, Algorithm 2 allows for an active set change which is an extension of Algorithm 1. In each iteration, the active set is updated by observing the current state of the constraints. As detailed above, a constraint can maintain its active state, i.e. remain active or inactive, an active constraint may become inactive and an inactive constraint may become active. Algorithm 2 Improved Iterative Feedback Method, termed iterative+. d ˆ ∗ (ppert ) ← w∗ (pnom ) + dp ˆ ∗ := w 1: w w∗ Δp  (2.1.11) d ∗ ∗ ∗ ∗ ˆ ˆ 2: λ := λ (ppert ) ← λ (pnom ) + dp λ Δp  (2.1.12) ˆ ∗ , ppert 3: p ← ga (w )  residue of active constraints (2.1.5b) and (2.1.5c)   ∗ ˆ∗ ˆ , λ , ppert do 4: while p > ρ w  threshold (2.1.22)   ∗ ˆ∗ ˆ , λ , ppert 5: determine active set A w  (2.1.20) 6: 7: 8: 9: 10:

determine updated sensitivities ∗ ˆ ∗ − dw ˆ∗ ← w w dp p ˆ ∗ − dλ∗ p ˆ∗ ← λ λ dp end while ˆ return w

 sensitivities (2.1.25)

The differences of iterative+, i.e. Algorithm 2, to Algorithm 1 are found in lines 5 and 6. In order to accommodate a changed active set, the new active set must be determined first. This is facilitated by means of (2.1.20) wherein the constraints are evaluated and compared with the identification function from (2.1.22). Then the KKT matrix is updated such that the constraint Jacobians therein reflect the current active set update which may lead to a change of dimensions. In the final steps of every iteration, the ˆ ∗ are computed as ˆ ∗ and multipliers λ approximate optimization variables w in the original scheme. In case of convergence of iterative+, the result is admissible w.r.t. the ˆ ∗ = w∗ constraints. However, it is not necessarily optimal, i.e. in general w ∗ ∗ ˆ = λ ˆ . and λ It should be noted, that, by adding constraints to the active set, the LICQ can be violated in certain cases. Therefore, possible LICQ violations may be avoided by either pre-selecting constraints such that their gradients

26

2 Numerical Basics

may never be linearly dependent which may not be suitable for practical applications. Alternatively, the optimization problem can be relaxed by removing linearly dependent constraints from the iteration scheme, e.g. by assigning priorities to constraints an applying a Gram-Schmidt process. 2.1.3.4

Comparison using Examples

The property of the methods presented above is analyzed using three showcase examples presented in [22] and [39]. Therein, rather simple parametric NLP problems are considered that already allow to capture the most relevant properties of the methods. Regarding the numerical solution, the implementation is facilitated by means of CasADi 3.4.5 [2] together with MATLAB R2015b. As solvers, qpOASES 3.2 [44] and Ipopt 3.12.3 [133] are employed for QP, and general NLP problems, respectively. In detail, CasADi code generation which may improve the execution time was not used for the functions implementing the formalism required for the above approximation schemes. This may especially improve the directional sensitivity-based scheme and the improved iterative method as in the other approaches the most effort is required by the optimization solvers which are readily available as compiled libraries. Regarding the computation time for the exact solutions of the perturbed problems, note that these were obtained using the nominal solution as the initial guess for the non-quadratic problems. Example 2.2: Parametric NLP 1, from [22] The parametric NLP (QP) w∗ = arg minimize J(w) = (w1 + 1)2 + (w2 − 2)2 w ⎛

⎞

g1 −w1 + p ⎠ ≤0 s.t. g(w, p) = ⎝ ⎠ = ⎝ g2 2w1 + w2 

(2.1.28a)

⎞

⎛



(2.1.28b)

with the optimization variables w = w1 w2 and the parameter p. Note that, as the objective function is quadratic and the inequality constraints are linear in w, the NLP is convex which eases its solution.

2.1 Numerical Optimization

27 



For a nominal parameter pnom = 1 the solution wnom = 1 2 with the 



multipliers λnom = 4 0

was found using qpOASES . The constraint 



residues at the nominal solution are g = 0 −2 . Thus only constraint g1 is found to be active while g2 is inactive, thus ga = g1 . Next, the parametric sensitivities at the nominal solution are determined. The Lagrange function is defined as L(w, λ, p) = J(w) + λ g(w, p) 2

2



= (w1 + 1) + (w2 − 2) + λ1 λ2

⎛



⎝

⎞

−w1 + p ⎠ . 2w1 + w2

The derivatives



∂ J(w, p) ∂w

⎛

⎞

⎡

⎤

∂ −1 0⎦ g(w, p) = ⎣ 2 1 ∂w

2(w1 + 1)⎠ =⎝ 2(w2 − 2)

and the KKT conditions ⎛

k(w∗ , λ∗ , p) =

 ⎞ ∂ ∗ ∗ ⎜ ∂w L(w , λ , p) ⎟ ⎝ ⎠ a ∗

g (w , p)

=

⎛

=

⎞    ∂ a ∂ ∗ a∗ ⎜ ∂w J(w , p) + ∂w g (w, p) λ ⎟ ⎝ ⎠ a ∗

g (w , p)

⎛

⎞

2(w1 + 1) − λ1 ⎟ ⎟ = =0 2(w2 − 2) ⎟ ⎠ −w1 + p wnom ⎜ ⎜ ⎜ ⎝

λnom

yield the parametric KKT system ⎡

2

∂ d L(w∗ , λ∗ , p) k(w∗ , λ∗ , p) = ⎢⎣ ∂w2∂ a dp ∂w g (w, p) ⎡

+

 ⎤  d ∂ ∗ ∗ ⎢ dp ∂w L(w , λ , p) ⎥ ⎣ ⎦ d a dp g (w, p)

=

⎤  ⎤⎡ d ∗ ∂ a ⎢ dp w ⎥ ∂w g (w, p) ⎥ ⎦⎣ d ∗ ⎦ + . . . O dp λ ⎡ ⎤ ⎡ ⎤ ⎡ 2 0 −1⎥ d ∗ ⎤ ⎢0⎥ ⎢ w ⎢ ⎥ ⎢ ⎥ ⎢ 0 0⎥ = 0 2 0 ⎥⎥⎦⎢⎣ dp d ∗⎦ + ⎢ ⎣ ⎦ ⎣ λ dp 1 −1 0 0 

28

2 Numerical Basics

and thus the parametric sensitivities ⎡

⎤ d ∗ w ⎢ dp ⎥ ⎣ d ∗⎦ dp λ

=

⎡

⎤−1 ⎡ ⎤

⎢ −⎢⎢⎣

⎢ ⎥ ⎢ ⎥ ⎢0⎥ ⎣ ⎦

2 0 −1⎥ 0 2 0 ⎥⎥⎦ −1 0 0

⎡ ⎤

0

1

=

1

⎢ ⎥ ⎢ ⎥ ⎢0⎥. ⎣ ⎦

2

The admissible range of parameter without a change of the active set of constraints is found to be Ppert = [−1, 2]. As for a perturbed parameter inside this range easily can be accounted for by application of Algorithm 1, only a perturbation outside this range is considered for evaluating the above algorithms. Table 2.1: Parametric QP from [22]: Results for ppert = 2.5. λ

w exact





2.5 1



g admissible tCPU in ms 

11 2 







·

12.1

directional QP

 

λ = 11 8 

 

12.5 11.5

iterative+



λ = 11 9



12.1

From the results in Table 2.1 it can be seen that for ppert = 2.5 the approach based on the QP approximation (2.1.26) converges to the exact optimal solution. The first-order approximations (2.1.18) and (2.1.19) based on directional sensitivities also yields the exact solution for the optimization variables as well as a deviation of the multipliers which is also the case for the iterative+ method. All methods produce results that are admissible w.r.t. the constraints imposed onto the optimization problem. For this example, the QP is the best solution approximation method as it yields the optimal solution (which is also admissible) and requires the least computation time. However, in this particular case, the original NLP problem is a QP problem of the same dimension as its QP approximation and thus requires similar computational effort to obtain an exact solution.

2.1 Numerical Optimization

29

Ultimately, computing the exact solution for this rather small problem (with or without perturbations) is the best option in this case. Example 2.3: Parametric NLP 2, (16.10) in [39] The parametric NLP 1 1 J(w) = (w1 − 1)2 + (w2 − 2)2 w∗ = arg minimize w 2 2 s.t. g(w, p) = 4w12 + w22 − p2 ≤ 0 

(2.1.29a) (2.1.29b)



with the optimization variables w = w1 w2 and the parameter p. For a nominal parameter pnom = 2.2 the optimal solution wnom =  0.6551 1.767 is found using Ipopt. The constraint g is active, the corresponding multiplier is λnom = 0.06582. The parametric sensitivities at the nominal solution are ⎛

⎞

d ∗ ⎝0.5203⎠ w = 0.4734 dp

d ∗ λ = −0.1516. dp

For a perturbed parameter ppert = 2.5 the results of the exact solution as well as for its approximations are reported in Table 2.2. Table 2.2: Parametric NLP (16.10) from [39]: Results for ppert = 2.5, tCPU in ms.

λ

J(w)

1.6221 × 10−4

2.042 × 10−3

·

·

219

0.9843 −30.08 × 10−3 2.067

0.8810 0 1.973

0.9556 −23.92 × 10−3 2.046

2.052 × 10−3





10.9

7.443 × 10−3





11.2

2.360 × 10−3





14.6

w

exact

0.9839 1.992



directional QP iterative+

act. set g admiss. tCPU



While the approximation based on directional derivatives is the fastest to compute, its result is not admissible w.r.t. the constraint g. The

30

2 Numerical Basics

QP-based scheme yields a result that is admissible, it fails to estimate the correct active set. Each approximate result requires less than 7 % of the solution to the exact NLP to compute. Although the improved iterative scheme requires the most computation time, it yields a result that is not only admissible but also yields an objective function value close to the exact solution of the perturbed problem and features the correct active set of constraints. However, it yields a multiplier that is negative and thus not admissible w.r.t. the KKT condition (2.1.7d) which is not necessary as it is only a solution approximation. Example 2.4: Parametric NLP 3, (16.12) in [39] The convex parametric NLP 1 1 1 J(w, p) = (w1 − p1 )2 + (w2 − p1 )2 + w1 w2 w∗ = arg minimize w 2 2 2 (2.1.30a) s.t. g(w, p) = w12 + 5w22 − p2 ≤ 0 

with the optimization variables w = w1 w2 







(2.1.30b) and the parameter 

p = p1 p2 . For a nominal parameter pnom = 0.25 0.25 the optimal 



solution is found at wnom = 0.1667 0.1667 with Ipopt. The only constraint is inactive, its residue is g = −0.08333 with the corresponding multiplier λnom = 0. The parametric sensitivities at the nominal solution are ⎡

⎤

d ∗ ⎣0.6667 0⎦ w = . 0.6667 0 dp d ∗ As the only constraint is inactive, sensitivities dp λ do not exist. Furd ∗ thermore, the second column of dp w is zero as p2 is only included in the inequality constraint g which is inactive at the nominal solution. The admissible range of parameters is Ppert = (−∞, 0.3125] × [0.1667, ∞). 



In this example, the perturbed parameter is set to ppert = 0.25 0 where only the second element changes that forces the inequality con-

2.1 Numerical Optimization

31

straint g to become active. The results of the exact solution as well as for its approximations are reported in Table 2.3. Table 2.3: [Param. NLP (16.12) from [39]: Results for p1,pert = 0.25, p2,pert = 0. tCPU in ms. w

exact

1 × 10−4 0



directional QP iterative+

λ

J(w, p)

act. set g admiss. tCPU

1369

62.47 × 10−3

·

·

547

61.10 × 10−3





13.9

25.3 × 10−3





11.1

20.83 × 10−3





30.2



0.1667 5 × 10−5 0.1667

0.2024 53.57 × 10−3 0.05952

5 × 10−3 5.215 6 × 10−4

While all methods correctly estimate the active set, the improved iterative scheme is the only one whose result is admissible w.r.t. the constraints as the others work only with first-order constraint approximations but without the constraint functions themselves. Due to this feature, in this example the improved iterative scheme is superior to the other methods and also to the exact solution as it requires only 6 % of its computation time and yields a similar result. Summarizing, the numerical examples presented above demonstrate that the improved iterative Algorithm 2 can act as a suitable alternative to the exact computation of solutions to perturbed parameter NLP problems. More complex applications will be presented in the context of fast robot path and trajectory planning required in the case of changing conditions in Chapter 4. For the solution of the above NLP examples, the interior-point solver Ipopt [133] was employed. Its enhancement, sIpopt [106], is also capable of computing solution approximations for parameter perturbations by means of parameter sensitivities. However, results obtained with sIpopt were found to be inadmissible, i.e. violate inequality constraints in the perturbed case. This behavior was confirmed by the developers [13] as the software – in constrast to the iterative+ method from Algorithm 2 in the present thesis – cannot properly handle active set changes.

32

2.2

2 Numerical Basics

B-Spline Curves

B-spline curves are piecewise polynomials that find wide application in the field of robotics, e.g. in path and trajectory planning. Archival publications in this field are by de Boor [33] and Cox [32]. Although the fundamentals of B-spline curves are well-known, they are reproduced here for convenience as far as they will be used at multiple points throughout this thesis. For a comprehensive overview of theory and applications of B-spline curves the interested reader is referred to e.g. [103] from which the notation regarding this matter was largely adopted in this thesis.

2.2.1

Evaluation Algorithm

This section is devoted to evaluating a B-spline curve q(t) in terms of its parameter t. Therefore, first its definition is provided in a recursive form which is useful for evaluation. Then, the formulation is further developed into a more compact notation that leads to computational benefits at the price of degraded numerical accuracy. Recursive Formulation, Summation Representation A B-spline curve q(t) of degree p is defined as q(t) =

n  j=0

Nj,p (t)dj

(2.2.1)

with the basis functions Nj,p (t) recursively defined as t − tj tj+p+1 − t Nj,p−1 (t) + Nj+1,p−1 (t) tj+p − tj tj+p+1 − tj+1 ⎧ ⎨ 1 t ≤ t < t j j+1 Nj,0 (t) = ⎩ 0 otherwise

Nj,p (t) =

(2.2.2a) (2.2.2b)

which are piecewise polynomials. It follows that Nj,p (t) = 0, t ∈ / [tj , tj+p+1 ), and consequently, only the functions Nj−p,p (t), . . . , Nj,p (t) are non-zero on the knot interval [tj , tj+p+1 ). This fact can be exploited in the evaluation of (2.2.1). The curve q(t) is defined for t ∈ [a, b] on which m + 1 knots

2.2 B-Spline Curves

33

tj , j = 0, . . . , m are defined. Regarding the distribution of knots, note the following terminology: • clamped = non-periodic = open vs. unclamped = periodic = closed: The term open refers to knots with a distribution in which the first and last knots have multiplicity p + 1, i.e. a = t0 = t1 = · · · = tp ≤ tp+1 ≤ . . . · · · ≤ tm−p−1 ≤ tm−p = · · · = tm−1 = tm = b. • uniform vs. non-uniform: Uniform describes a knot distribution in which intermediate knots are equally spaced, multiplicities greater than 1 are allowed for boundary knots. The above open distribution would be uniform if tj+1 = tj + Δt, j = p, . . . , m − p − 1 with Δt =

tm − t0 m − 2p

Most splines considered in the following are chosen to be open-uniform for simplicity. Relevant properties • The curve q(t) is arbitrarily continuously differentiable on each knot interval except for the knots (with multiplicity k) at which continuous differentiability is degraded to C p−k . • The outmost control points are interpolated , i.e. q(a) = d0 and q(b) = dn . • It can be shown, that the curve is locally contained in the range of the $surrounding control points (local convex hull property), i.e. % q(t) ∈ min dj−p , max dj holds for t ∈ [tj , tj+1 ). Consequently, gobj $

j

%

ally, q(t) ∈ min dj , max dj holds (global convex hull property). j

j

34

2 Numerical Basics

Matrix-Vector Representation By replacing the sum in (2.2.1) with a vector product, the curve can be computed as q(t) = n (t)d.

(2.2.3)

wherein the basis function vector is denoted n(t) and the vector of control points is d with 



n (t) = N0,p (t) N1,p (t) · · · Nn−1,p (t) Nn,p (t) and 



d = d0 d1 · · · dn−1 dn . Having the basis functions (2.2.2) explicitly evaluated, their polynomial form is found as Nj,p = nj,0 (t) + nj,1 (t)t + · · · + nj,p (t)tp ⎛



= nj,0 (t) nj,1 (t) · · ·

1

⎞

⎟ ⎜ ⎟ ⎜ ⎜ t ⎟ ⎟ ⎜ ⎜ . ⎟ . ⎟ nj,p (t) ⎜ ⎜ . ⎟ ⎟ ⎜ ⎜ n−1 ⎟ ⎟ ⎜t ⎠ ⎝ n 

(2.2.4)

t

  τ (t)

with the vector of parameter powers τ (t) and the coefficients nj,i , i = 0, . . . , p which are piecewise constant on each knot interval. Now (2.2.3) can be rewritten as q(t) = τ (t)N(t)d

(2.2.5)

2.2 B-Spline Curves

35

with the p + 1 × n + 1 coefficient matrix (time-dependency omitted) ⎡

N(t) =

⎢n0,0 ⎢ ⎢n0,1 ⎢ ⎢ . ⎢ . ⎢ . ⎣ n0,p

n1,0 n1,1 .. . n1,p

⎤

· · · nn,0 ⎥ ⎥ · · · nn,1 ⎥⎥ . . . ... ⎥⎥ ⎥ ⎦ · · · nn,p

that is constant on each knot interval. Note that it is also sparse due to the limited parameter domain with non-zero values of each basis function Nj,p (t). This matrix-vector representation offers the advantage that the coefficient matrices N(t) can be precomputed as they only depend on the degree and the knots. In fact, even shifting or scaling the parameter domain has no influence if the vector τ of parameter powers is scaled appropriately. The only operations required for evaluation of q(t) are finding the knot interval that contains the requested parameter (e.g. with binary search), t ∈ [tk , tk+1 ), computing τ , and performing the sparse vector-matrix-vector multiplication (2.2.5). However, this notation exhibits numerical disadvantages arising from the difference in magnitude of the coefficients stored in N(t). A possible consequence is degraded continuity at knots which deteriorates with increasing spline degree. For numerical accuracy, [103] suggests to use the recursive formulation (2.2.2) to evaluate B-spline curves by (2.2.1).

2.2.2

Derivatives w.r.t. the Path Parameter

Summation Representation Deriving a B-spline curve with degree p (2.2.1) w.r.t. the parameter t results in another B-spline n−1  d (1) q(t) = q (1) (t) = Nj+1,p−1 (t)dj dt j=0

(2.2.6)

36

2 Numerical Basics

with degree p − 1. The n control points of the resulting spline (in contrast to the n + 1 control points of the original spline) are (1)

dj =

p (dj+1 − dj ), tj+p+1 − tj+1

j = 0, . . . , n − 1

(2.2.7)

which are linear combinations of the control points of the original B-spline curve. Furthermore, the boundary knots are dropped, i.e. m − 1 knots (1) remain. The control points dj can be collected in the vector d(1) = 

(1)

d0

(1)

d1



(1) . . . dn−1 .

This scheme can of course be repeated for higher derivatives in a similar manner, e.g. to compute the control points d(2) of the second-derivative B-spline curve. Matrix-Vector Representation Derivation of (2.2.5) w.r.t. t is straightforward as effectively only the vector of parameter powers needs to be derived as the coefficient matrix is constant on each interval. With  d τ (t) = 0 dt ⎡ ⎢0 ⎢ ⎢1 ⎢ ⎢ ⎢ ⎢0 ⎢ ⎢ = ⎢⎢ ... ⎢ ⎢ .. ⎢. ⎢ ⎢. ⎢. ⎢. ⎣ 0 

1 · · · (n − 1)tn−2 ntn−1

 ⎤

· · · · · · · · · · · · · · · 0⎥ ⎛ 1 ⎞ ⎟ .. ⎥⎥ ⎜ ... ⎟ .⎥ ⎜ ⎜ t ⎟ ⎟ ⎥⎜ .. ⎥⎥ ⎜ 2 ⎟ ⎟ 2 ... .⎥ ⎜ ⎜ t ⎟ .. ⎥⎥ ⎜ .. ⎟ ... 3 ... ⎜ . ⎥⎥ ⎜ . ⎟ ⎟ ⎟ .. ⎥⎥ ⎜ ⎜ n−2 ⎟ ... ... ... ⎟ t .⎥ ⎜ ⎟ ⎜ ⎟ . . . n − 1 . . . ... ⎥⎥ ⎜ ⎜tn−1 ⎟ ⎥⎝ ⎠ ⎦ tn ··· ··· ··· 0 n 0

d2 τ (t) = TTτ (t) dt2 .. .

 T



  τ (t)

2.2 B-Spline Curves

37

the derivatives of (2.2.5) yield d q(t) = τ (t)T N(t)d dt d2 q(t) = τ (t)T T N(t)d dt2 .. .

(2.2.8a) (2.2.8b)

Of course, derivatives vanish for orders k exceeding the degree p, i.e. dk dtk q(t) = 0 for k > p.

2.2.3

Derivatives w.r.t. the Control Points

Deriving a B-spline curve q(t) w.r.t. the vector of control points d can be conveniently expressed by the matrix-vector representation discussed above, i.e. d q(t) = τ (t)N(t). dd

(2.2.9)

Derivatives of a B-spline curve w.r.t. the parameter t, cf. (2.2.8), can be derived w.r.t. the vector of control points in an analogous manner. For example, for the first derivative, the results is d d q(t) = τ (t)T N(t). dd dt

(2.2.10)

While the properties and applications described in this section are fairly commonly known in robotics, a software library that includes additional functionality was developed. A brief overview of features can be found in Appendix A.

2.2.4

Constrained Approximation

While the above sections detail how to evaluate a B-spline curve and its derivatives, this section is devoted to constructing a curve. Therein, constrained B-spline approximation is a valuable tool, especially in optimal path and trajectory planning. For example, for many practical optimization

38

2 Numerical Basics

problems, an initial guess that is at least admissible w.r.t. the constraints benefits the computation time required to solve the problem. B-splines can be used to construct such a guess by means of a constrained approximation problem. Therein, the control points collected in the vector d act as optimization variables and are adjusted such that they satisfy certain requirements. Pure approximation, i.e. computing a curve that fits best for a given set of points, can be formulated as a linear programming (LP) problem. Here, it is formalized as a QP in order to perform quadratic curve fitting. A given point q a,i should be fitted by the curve that assumes the value q(ti ) at a given parameter ti . Approximation points are collected in the vector qa should be fitted by the curve that assumes the values qa at the same respective parameter values. The same holds for the curve derivatives, e.g. for the first derivative qa(1) ≈ q(1) a . If at a given parameter ti the curve should be constrained to a prescribed interval, q c,min ≤ qc ≤ q c,max , pure approximation is turned into a constrained approximation problem. Constrained points are subsequently collected in vectors qc for the basic curve, qc(1) for its first derivative and so on. Smoothing of the curve is achieved by additionally minimizing the highest relevant derivatives in form of the corresponding derivative B-spline curve control points. Possible inputs for an approximation scheme is point data to be approximated, but also interpolation constraints. Also, limits of the magnitude of the spline and its derivatives can be efficiently included using the convex hull property. For a given spline degree and knot distribution, the goal is

2.2 B-Spline Curves

39

to find a suitable vector of control points d. Therefore the corresponding constrained approximation problem is stated as the QP    1 1 − qa(1) q(1) − qa(1) + · · · + minimize (qa − qa ) (qa − qa ) + q(1) a a d 2 2   1 1  (p−1) (p−1) (p−1) − qa qa − qa(p−1) + αd(p) d(p) (2.2.11a) + qa 2 2 (2.2.11b) s.t. qc,min ≤ qc ≤ qc,max (1)

(2.2.11c)

qc,min ≤ qc(p−1) ≤ q(p−1) c,max

(p−1)

(2.2.11d)

1q min ≤ d ≤ 1q max

(2.2.11e)

(1) 1q min

1q (1) max

(2.2.11f)

1q min ≤ d(p) ≤ 1q (p) max

(2.2.11g)

qc,min ≤ qc(1) ≤ q(1) c,max .. .

≤d

(1)

≤

.. . (p)

in which the mixed objective (2.2.11a) is to be minimized. Therein, the pointwise approximation residue for all derivatives from 0, i.e. (qa − qa ),   (p−1) (p−1) − qa , as well as the control points d(p) of up to p − 1, i.e. qa the p-th, the highest non-zero derivative is quadratically minimized. The latter term increases the smoothness of the result which can be adjusted with the weight α ≥ 0. For α = 0, pure constrained approximation is performed, which in practice typically yields high magnitudes of the spline derivatives. In (2.2.11b) to (2.2.11d) pointwise ineqality constraints of all derivatives are collected. Interpolation is achieved by setting both, the respective lower and upper bound to the required value which results in an equality constraint. The inequality constraints (2.2.11e) to (2.2.11g) represent convex hull constraints of the spline derivatives, i.e. the magnitude of the respective spline derivatives is limited by the specified bounds. The vector 1 denotes a vector of ones of appropriate dimension. The above constrained approximation QP problem (2.2.11) can be solved with QP solvers such as qpOASES [44]. Typically, it is therefore required to extract the optimization variables, i.e. in this case the control points d from the expressions listed in the QP. The implementation of such problems,

40

2 Numerical Basics

the matrix-vector notation (2.2.5) and (2.2.8) can be employed to express pointwise approximation and constraint data in (2.2.11a) and (2.2.11b) to (2.2.11d), respectively. For the convex hull part of the objective (2.2.11a) and the constraints (2.2.11e) to (2.2.11g) the relationship (2.2.6) can be exploited.

2.3

Optimal Control Problems and Direct Methods

In optimal control problems, the best control trajectory u(t) is sought that drives the system towards its goal which can be, e.g., a point-to-point motion of a robot or a path tracking application. Several types of constraints may therein be imposed onto the system such as input and state constraints. In this context, the word best is used synonymously with optimal. Thus not only a control trajectory u(t) that solves the main challenge is required, it needs to stand out from all other possible trajectories in a quantitative manner. The latter qualification can be expressed by minimizing an objective function within an numerical optimization problem, cf. Section 2.1. As a result of the selection of a control trajectory u(t), a state trajectory x(t) is found that explicitly describes the time evolution of the system under consideration. Frequently, both, control and state trajectories are treated simultaneously, enabling efficient solution algorithms. However, it is also possible to construct the time evolution of the state from the time evolution of the control by time integration of the forward dynamics. Conversely, the control trajectory can be reconstructed by means of inverse dynamics from the state trajectory (in the case of redundantly actuated systems further considerations are required). Textbooks on optimal control are e.g. [11] or [39].

2.3 Optimal Control Problems and Direct Methods

41

An optimal control problem (OCP) is formulated as &T

minimize x(t),u(t)

L(x(t), u(t))dt + E(x(T ))

(2.3.1a)

0

s.t. x0 − x(0) = 0

(2.3.1b)

r(x(T )) ≤ 0

(2.3.1c)

˙ x(t) − f (x(t), u(t)) = 0

(2.3.1d)

h(x(t), u(t)) ≤ 0, ∀t ∈ [0, T ]

(2.3.1e)

wherein the state x and the control u are functions of time t ∈ [0, T ] and serve as optimization variables. The terminal time is denoted T . The objective consists of the integral cost L(x(t), u(t)), referred to as Lagrange term, and the terminal cost E(x(T )), named the Mayer term. This particular combination results in a so-called objective of Bolza type. The OCP is subjected to initial and terminal constraints, (2.3.1b) and (2.3.1c), respectively, as well as the system dynamics (2.3.1d) and constraints (2.3.1e) that apply during the path. While (2.3.1b) is a constraint that is linear w.r.t. the (inital) state, the other constraints are in general nonlinear w.r.t. state and control. The terminal time T may be fixed as denoted in the above OCP, or free through reparametrization of t and with T as the terminal value of an additional state xT = t with its time derivative x˙ T = 1. The central paradigm of direct methods is that before optimization techniques are employed, the original infinite dimensional OCP (2.3.1) is discretized, yielding a finite dimensional problem. Therefore, such methods are also known as discretize first, then optimize approaches. Therein, the time domain is split into N time intervals with N + 1 increasing knots t0 = 0 < t1 < t2 < · · · < tN = T . Without loss of generality, in this thesis the considered time interval is [0, T ], other values can be realized by introducing a time shift. Regarding the state and control trajectories, x(t) and u(t), respectively, suitable parameterizations are required to complete the discretization. Below, the most common direct methods, i.e. single and multiple shooting, as well as collocation, will be discussed. Therein, the shorthand (·)j := (·)(tj ) is used for expressing points along state and control trajectories.

42

2.3.1

2 Numerical Basics

Direct Single Shooting

The concept of direct single shooting1 (DSS) originates from [60] where it was introduced in the context of general optimal control. While such OCPs are mostly defined in time domain, the concept of DSS can be applied to problem sets in different domains. The infinite dimensional OCP (2.3.1) in DSS formulation can be written as &T

minimize u(t)

L(x(t), u(t))dt + E(x(T ))

(2.3.2a)

0

s.t. r(x(T )) ≤ 0

(2.3.2b) &T

f (x(t), u(t))dt

x(t) = x(0) +

(2.3.2c)

0

h(x(t), u(t)) ≤ 0, ∀t ∈ [0, T ].

(2.3.2d)

The considered system is only influenced by the control u(t), therefore the constraint (2.3.1b) on the initial state is no longer included. The OCP (2.3.2) is constrained by the system dynamics (2.3.1d), included in (2.3.2c) as numerical simulation by forward time integration, starting at the initial state x(0). Therefore the state depends nonlinearly from the input. Thus, even constraints h linear w.r.t. the state may then depend nonlinearly from the states and from the input. The OCP (2.3.2) is discretized by parametrizing the control u(t) using variables 



wu = u 0 u 1 · · · u N .

1 In the textbook [11] the author reports (without reference), that the name shooting stems from a control example in which a cannon is required be optimally aimed to shoot at a target.

2.3 Optimal Control Problems and Direct Methods

43

The constraints (2.3.2d) can be typically not enforced over the whole interval [0, T ] but are checked on Nch discrete checkpoints tk ∈ [0, T ], k = 1, . . . , Nch . The resulting finitely dimensional OCP thus yields &T

L(x(t), u(t))dt + E(x(T ))

minimize w u

(2.3.3a)

0

s.t. r(x(T )) ≤ 0

(2.3.3b) &T

f (x(t), u(t))dt

x(t) = x(0) +

(2.3.3c)

0

h(x(tk ), u(tk )) ≤ 0, tk ∈ [0, T ], k = 1, . . . , Nch

(2.3.3d)

with the time integration in (2.3.2c) and (2.3.3c) replaced by a numerical scheme suitable for the system dynamics. Regarding the practical implementation of such a DSS OCP, for gradientbased optimization derivatives of the objective and the constraints are required which are best obtained by automatic differentiation. Control parameterizations wu may represent piecewise constant, or piecewise polynomial functions, e.g. B-spline curves from Section 2.2. In these cases the effects of each element of wu will be typically limited to a few time intervals which is reflected in the sparsity of the resulting derivative matrices. The above choices typically lead to increased sparsity compared to function classes for which parameters impact the control function over all time intervals, e.g. Fourier series.

2.3.2

Direct Multiple Shooting

In contrast to DSS, where the control variables are the only optimization variables, direct multiple shooting (DMS) additionally incorporates the states, cf. (2.3.1). This concept was introduced in [15]. Both, the variables representing the states and the controls at certain points in time are collected in the vector 



w = x 0 u 0 x 1 u 1 · · · x N u N .

44

2 Numerical Basics

Note the alternating order of states and controls which is motivated by improving the sparsity of the resulting derivatives that can be exploited by optimization solvers for computation time reduction. The OCP (2.3.1) is thus rewritten in finite dimensional form as minimize w

N 

&tj

j=1 tj−1

L(x(t), u(t))dt + E(xN )

s.t. x0 − x0 = 0 r(xN ) ≤ 0 xj −

(2.3.4a) (2.3.4b) (2.3.4c)

&tj

f (x(t), u(t))dt = 0, j = 1, . . . , N

(2.3.4d)

tj−1

h(xk , uk ) ≤ 0, k = 1, . . . , Nch .

(2.3.4e)

While the objective (2.3.4a) is equivalent to (2.3.1a), it was rewritten so that it now reflects the state trajectory split at the interval bounds. The equality constraint (2.3.4d) maintains continuity of the state integration between intervals that was previously ensured by the integration over the entire interval. Therein, the integral sign again represents a suitable numerical time integration scheme. Instead of enforcing the system dynamics as an equality constraint (2.3.1d), they are incorporated in (2.3.4d). The additional state variables allow for shorter integration time intervals, reducing numerical errors occurring for longer times. They also enable direct access to the system behavior, allowing to handle especially unstable systems and systems with high sensitivity w.r.t. the initial value well. While the dimension of the resulting KKT system is significantly increased compared to DSS, its sparsity is improved as intermediate states have no explicit dependency on all previous states and controls. The fact that the integration over an interval does not depend on the result of any of its predecessors enables simultaneous computation of all integrations which allows for parallel computing.

2.3 Optimal Control Problems and Direct Methods

45

While a basic DMS method can be implemented in less than 100 lines of code, an enhanced and particularly efficient tool to solve OCPs is MUSCOD-II [75].

2.3.3

Direct Collocation

As for the shooting methods, the time domain is discretized. It is split up into N intervals at the N + 1 collocation points t0 = 0 < t1 < t2 < · · · < tN = T . In collocation methods, the state integration process that has been handled by numerical integration schemes in the shooting methods is now written in terms of piecewise polynomials that interpolate the state along time. The time evolution of the state x(t) and the control u(t) are parametrized using wx and wu , respectively. The corresponding NLP problem reads minimize w ,w x

u

N 

&tj

j=1 tj−1

L(x(t), u(t))dt + E(xN )

(2.3.5a)

s.t. x0 − x0,0 = 0

(2.3.5b)

r(xN ) ≤ 0

(2.3.5c)

˙ j,i ) − f (x(tj,i ), u(tj,i )) = 0, j = 0, . . . , N − 1, i = 0, . . . , p x(t (2.3.5d) xj,p − x(tj,p ) = 0, j = 0, . . . , N − 1

(2.3.5e)

h(xk , uk ) ≤ 0, k = 1, . . . , Nch

(2.3.5f)

wherein elements of the state x(t) are represented by polynomials of degree p, thus p + 1 coefficients are available on each collocation interval [tj , tj+1 ), j = 0, . . . , N −1. These polynomials are used to interpolate the system dynamics at M + 1 knots tj,0 , . . . , tj,M on each subinterval, cf. (2.3.5d), as well as the state optimization variables at the collocation points tj , cf. (2.3.5e). For example, Legendre polynomials can be used for this interpolation task. The state values at each knot xj,i are collected as 



wx = x 0,0 x 0,1 · · · x 0,M x 1,0 · · · x N −1,M −1 x N −1,M .

46

2 Numerical Basics

The elements of the control u(t) are again parametrized by variables wu , e.g. stair function levels for piecewise constant parametrization, control values at collocation points that are linearly interpolated, or coefficients of polynomials, e.g. B-spline curves. The objective (2.3.5a) is again evaluated using a numerical integration scheme, preferably reusing the states and controls evaluated at the collocation points. Again, initial and terminal conditions are incorporated using the constraints (2.3.5b) and (2.3.5c), respectively. In addition, the (in)equality constraints (2.3.5f) are enforced at Nch check points which may also coincide with the collocation points. Direct Collocation with B-Spline Curves This section is devoted to a specific form of piecewise polynomials that can be used for state interpolation: B-spline curves, cf. Section 2.2. In this case the control points dxi , i = 1, . . . , n serve as the optimization variables 



wx = d x1 d x2 . . . d xn , parametrizing the time evolution of the state x(t). The control trajectory u(t) is still parametrized in terms of wu . The NLP problem (2.3.5) accordingly modified to accommodate B-spline curves (2.2.1) is minimize w ,w x

u

N 

&tj

j=1 tj−1

L(x(t), u(t))dt + E(xN )

s.t. x0 − x0 = 0

(2.3.6a) (2.3.6b)

r(xN ) ≤ 0

(2.3.6c)

x˙ j − f (xj , uj ) = 0, j = 0, . . . , N − 1

(2.3.6d)

h(xk , uk ) ≤ 0, k = 1, . . . , Nch

(2.3.6e)

A particularly notable advantage of B-spline curves is their convex hull property, i.e. that a curve and its derivatives (which are splines, too) are contained in the range of their respective control points. It can be

2.3 Optimal Control Problems and Direct Methods

47

conveniently exploited as linear path constraints to box-constrain the state variables or their derivatives. Note, that the total number of equality constraints (2.3.6b) to (2.3.6d) regarding the time evolution of the state must not exceed the total number of state control points, otherwise the NLP problem would be overconstrained. In this thesis, direct collocation with B-spline curves is used for trajectory planning in Chapter 4. Numerical Solution In order to compute a solution to a practical OCP, the considered system behavior first needs to be formalized in terms of the equations governing relevant physical processes in a dynamics model. Furthermore, the optimization goal needs to be expressed in terms of dynamics quantities. Then, the above OCP formulations are applied to obtain a formulation ready for numerical solution. Software frameworks used to implement such formulations are CasADi [2] or YALMIP [79]. The numerical solution is obtained with an appropriately selected solver program such as Ipopt [133] for general NLP problems, or qpOASES [44] for QP problems.

Chapter 3

Kinematics of Serial Robots Kinematics describes the motion of a system of one or more bodies by considering their position, velocity, acceleration and possibly higher derivatives, i.e. disregarding forces and torques acting upon them. As this field is well-researched, the present chapter does not aim to provide a comprehensive discussion of existing methods. Interested readers are referred to general robotics textbooks such as [127] while advanced readers may prefer publications focusing on kinematics such as [86]. However, the approaches most relevant for the successive chapters are discussed in great level of detail. Depending on the complexity of the considered task and the manipulator executing the desired motion, kinematically non-redundant and redundant robots are distinguished. While for non-redundant serial structures solutions to the forward and inverse kinematics problems are well known and can thus be considered solved, the inverse kinematics problem for redundant robots still poses a challenge in research. Consequently, a large portion of the following section is devoted to that inverse kinematics problem. It is treated with geometric, iterative and optimization-based methods for both, kinematically non-redundant, and also redundant structures. After well-known differential inverse kinematics methodologies are revisited as an introduction, in particular the joint space decomposition method is © Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4_3

50

3 Kinematics of Serial Robots

reviewed in detail as it poses a convenient approach to optimization-based redundancy resolution methods, cf. the optimal path tracking applications in Chapter 5. One of the main contributions of this thesis is contained in Section 3.5.3 that generalizes differential inverse kinematics approaches for kinematically redundant robots. Therein, velocity Jacobian nullspace augmentation, most notably by means of the projected gradient method, but also joint space decomposition are covered within a unifying framework. In this section, at first, basic definitions and concepts are presented as detailed as required for the subsequent presentation of kinematics relationships on position, velocitiy and higher derivative level in forward and inverse form are discussed thereafter.

3.1 3.1.1

Position and Orientation Position

The position of a point (B) relative to a point of reference (A) is denoted by the position vector rAB , cf. Figure 3.1. (B)

(A)

Figure 3.1: Position Vector from Point (A) to Point (B).

For practical use, such position vectors need to be expressed in coordinates w.r.t. a frame of reference. An elementwise representation of rAB w.r.t. a frame denoted R using Cartesian coordinates is ⎛ R rAB

=

⎞ x ⎜R AB ⎟ ⎜ ⎟ ⎜ R yAB ⎟ ⎝ ⎠ R zAB

∈ R3

(3.1.1)

3.1 Position and Orientation

51

wherein R xAB , R yAB and R zAB are the distances between A and B measured in the x, y and z directions of the R frame, respectively. The index on the left, here R, denotes the frame in whose coordinates the vector is represented in. Other forms of position representations commonly used in robotics use cylindrical or spherical coordinates. However, for the applications discussed in this thesis description by means of Cartesian coordinates is sufficient.

3.1.2

Orientation

Rotation Matrix Orientation describes the relative rotation between two coordinate frames which can be uniquely represented by a rotation matrix RRS . Such a construct not only characterizes the relative rotation between the two coordinate frames indexed with R and S, it is also a coordinate transformation between these frames. Therefore, the rotation matrix RRS =

'

(

R (S e1 ) R (S e2 ) R (S e3 )

∈ SO(3)

(3.1.2)

comprises three mutually orthogonal column vectors that are the unit vectors aligned with the principal directions of frame S resolved in coordinates of the R system. It is an element of the special orthogonal group in three dimensions SO(3). Note that this representation is redundant, i.e. its nine parameters are more than necessary to fully describe the rotation. Example 3.1: Coordinate Transformation using Rotation Matrix For a position vector from point A to point B in coordinate representation w.r.t. the frame 2, i.e. 2 rAB , the representation w.r.t. the frame 1 is obtained as 1 rAB

= R12 2 rAB .

(3.1.3)

In the larger part of this thesis this rotation matrix representation will be used for expressing general coordinate transformations as well as orientations of coordinate frame, in particular a robot’s EE orientation. However, for

52

3 Kinematics of Serial Robots

orientation error considerations, also an angle-axis representation will be utilized, cf. Section 3.5.2. Therein, a relative orientation is represented by a rotation with an angle ϕ about an axis denoted by a unit vector uR pointing in its direction, thus this representation consists of four parameters of which only three are independent as |uR | = 1. For the identical transformation R = I the angle yields ϕ = 0, thus no rotation occurs. Note that the angle-axis representation is not unique as R(uR , ϕ) = R(−uR , −ϕ) Quaternions Another way of uniquely representing orientation are quaternions which extend the complex numbers. As this representation will not be used in the present work, only this brief overview is given and the reader is referred to textbooks such as [86]. Similar to complex numbers that can represent planar rotations on the unit circle in the complex plane, quaternions represent spatial rotations on the unit sphere. Using four numbers, a quaternion ⎛

Q = (q0 , q) with q =

⎞ q ⎜ 1⎟ ⎜ ⎟ ⎜q2 ⎟, ⎝ ⎠

qi ∈ R, i = 1, . . . , 3

q3 can be used to represent any point in SO(3). Therein, q0 is called the scalar compontent, and q is called the vector component of the quaternion Q. It is called a unit quaternion if q = 1 which was assumed in the introductory explanation using the unit sphere. Successive Rotations In contrast to the above representations that provide a unique orientation representation, spacial rotations can also be expressed by means of three successive elementary rotations about alternating axes of the accordingly

3.2 Translation and Rotation Velocity

53

moving coordinate frame, or of a spatially fixed frame. These rotations are quantified by sets of three angles denoted ⎛

ϕ=

⎞ ϕ ⎜ 1⎟ ⎜ ⎟ ⎜ϕ2 ⎟, ⎝ ⎠

(3.1.4)

ϕ3 such as Euler angles, Kardan angles about axes of moving frames, or rollpitch-yaw angles about axes of fixed frames. There are in total 12 different rotation orders. For the inverse problem, i.e. finding angles ϕi , i = {1, 2, 3}, all of these representations suffer from singularity issues if rotations about the first and last axis cannot be distinguished.

3.2

Translation and Rotation Velocity

The rate of change the position vector of point B, denoted in coordinates w.r.t. an inertially fixed frame of reference 0, is given by its time derivative 0 vB

d 0r dt B = 0 r˙ B . =

(3.2.1)

For a frame of reference R that is fixed to a body in motion, this relationship can be reused as R vB

d (R0RR rB ) dt d d = RR0 (R0R ) 0 rB + RR0 R0R R rB    dt dt    = RR0

˜ 0R =:R ω

d ˜ 0R 0 rB Rr + Rω dt B ˜ 0R 0 rB = R r˙ B + R ω =

I

(3.2.2)

54

3 Kinematics of Serial Robots

with the spin tensor1 of the angular velocity R ω0R of the frame R w.r.t. the inertially fixed frame 0. Note that the angular velocity ω is in general different from the time derivative of an orientation representation by means of a vector ϕ comprising three angles, such as Euler angles, cf. Section 3.1.2. The translation velocity of a point (E) on a body and the angular velocity of a body-fixed frame, indicated by the index E, are collected in the twist vector ⎞

⎛

ωE ⎠ . VE = vE ⎝

3.3 3.3.1

(3.2.3)

Joint Space, Work Space, Task Space Joint Space

The joint space of a robot composed of nR revolute joints and nP prismatic joints is given as the variety C ⊆ RnP × TnR . For prismatic joints, the corresponding joint coordinate qi ∈ R describes the joint position along a directed axis. In the case of revolute joints, the joint coordinate qi ∈ T gives the joint position on the topological space of a 1-torus T1 , sometimes denoted as 1-sphere S 1 . In practice, a robot’s joint space is obtained by sweeping its admissible joint ranges and constructing C by evaluating the above variety. Example 3.2: SCARA4 (joint space) For the SCARA4 manipulator depicted in Figure 1.2, the joint space is given by C = T4 with joint limits neglected. When the respective lower and upper joint limits qi,min and qi,max , i = 1 . . . 4, respectively, need to be considered, it reduces to C = ⎛ ⎞ 

ωx

⎡

0

⎜ ⎟ ⎢ 1 spin tensor: ω ˜ = ⎝ ωy ⎠ = ⎣ ωz

ωz

−ωy

4 )

i=1

[qi,min , qi,max ] ⊂ T4 . ⎤

−ωz ωy ⎥ 0 −ωx ⎦ ωx 0

3.3 Joint Space, Work Space, Task Space

55

Example 3.3: Stäubli TX90L (joint space) In case of the Stäubli TX90L industrial robot mounted on a linear axis shown in Figure 1.3, the joint space is given by the variety C = R × T6 with joint limits neglected. When the respective lower and upper joint limits qi,min and qi,max , i = 1 . . . 7, respectively, need to be considered, it reduces to C =

3.3.2

7 )

[qi,min , qi,max ] ⊂ R × T6 .

i=1

Work Space

As described in [86], the work space of a robot is the set of EE configurations, i.e. EE position rE and EE orientation RE , that can be reached by some choice of joint positions q, i.e. W = {(rE (q), RE (q))|q ∈ C} ⊂ SE(3).

(3.3.1)

The reachable work space is the set of positions the EE can assume with some orientation, i.e. WR = {rE (q)|q ∈ C} ⊂ R3 .

(3.3.2)

Such points may include intermediate points of via paths, where the orientation of the EE is often irrelevant. Determining WR is a simple task as only the joint coordinates need to be swept in their entire admissible range while recording the respective resulting EE positions. Example 3.4: SCARA4 (reachable work space) In order to find the reachable work space WR of the SCARA4 manipulator (with joint limits neglected), it is sufficient to first consider all points the EE can assume along one radial line (here assumed as the 0 x axis w.l.o.g.). The rest of the reachable work space is then found by circularly sweeping these reachable points around the first joint. While the innermost reachable point is at radius l1 − l2 + l3 − l4 and thus coincident with point (0), the outermost reachable point is at radius rout = l1 + l2 + l3 + l4 , cf. Figure 3.2.

56

3 Kinematics of Serial Robots 0y

WR

rout (E) (0)

0x

Figure 3.2: Reachable Work Space of SCARA4 Manipulator.

The dexterous work space is the set of positions the EE can assume with arbitrary orientation, i.e. WD = {rE (q)|∀RE (q) ∈ SO(3)∃q ∈ C} ⊂ R3 .

(3.3.3)

Dexterous points are relevant for, e.g. machining tasks in which one particular point is required to be accessible from all directions, or robotbased photography for which arbitrary EE orientation is also of significance. In contrast to the rather effortless task of finding WR , deriving WD is more involved. A notable academic example is an unconstrained manipulator with a spherical wrist. For this particular kinematic structure, the dexterous work space is composed of all points that lie in the reachable work space of the wrist point (not the EE), thus WD ⊂ WR . However, real manipulators often do not have a dexterous work space at all, i.e. WD = ∅, due to joint limits or self-collisions.

3.3 Joint Space, Work Space, Task Space

57

Example 3.5: SCARA4 (dexterous work space) In order to find the dexterous work space WD of the SCARA4 manipulator (with joint limits and self-collisions neglected), it is again sufficient to consider all points the EE can assume along the 0 x with arbitrary orientation. The rest of the dexterous work space is found by circularly sweeping these dexterous points about the first joint axis. The innermost dexterous point is coincident with point (0) (radius l1 − l2 − l3 + l4 ), the outermost reachable point is at radius l1 + l2 + l3 + l4 , cf. Figure 3.3. 0y

WD

rout

(E) (0)

0x

Figure 3.3: Dexterous Work Space of SCARA4 Manipulator.

In general, understanding of work space is especially relevant for path placement and path planning tasks in course of the preparations required for operating a robot system. However, not all subtasks therein require knowing an explicit representation of the entire spaces. In many cases, it is sufficient to only be able to check whether or not a given EE configuration (or part thereof) is in a specific type of work space. If an EE position is in the dexterous work space, the existence of a corresponding joint configuration is guaranteed.

58

3 Kinematics of Serial Robots

Path planning involves choosing a suitable sequence of EE configurations such that a given task is fulfilled. A subtask therein may be path placement, i.e. a full path is prescribed and its relative position and orientation w.r.t. the robot needs to be determined. This is a common process in industrial applications in which a given contour needs to be tracked such as grinding, or polishing. In this case, the evolution of the joint configuration for all points along a desired path need to be determined. While in many cases both, EE position and orientation, are prescribed, cf. path tracking in Chapter 5, this is not necessary. In certain cases, e.g. intermediate steps in path planning, some elements of the EE configuration are deliberately left free, e.g. in order to allow for path optimization. Consider in this context, for example, a machining task in which the rotation about the tool axis is left free, cf. [115]. In any case, once path planning is completed for a specific task, the operation of the manipulator is restricted to the task space WT ⊆ W. WT denotes the EE space necessary to execute a given task. However, before any planning schemes regarding paths or trajectories can commence, elementary kinematic relationships between the a robot’s base (which in this thesis is always considered fixed) and its EE must first be established.

3.4 3.4.1

Forward Kinematics Position-Level Forward Kinematics

The forward kinematics f : C → W of a robot is a mapping from its joint space C to its work space W. More specifically, a robot’s EE configuration is given in terms of the Cartesian EE position rE ∈ R3 and the EE orientation, denoted as the rotation matrix R0E ∈ SO(3). While the former, rE , is the position vector of the EE w.r.t. an inertially fixed point, the latter, R0E , is the coordinate transformation between a coordinate frame fixed to the EE body and an inertially fixed frame of reference. Both quantities are

3.4 Forward Kinematics

59

expressed depending on the joint configuration of the robot, denoted as the vector of joint positions q ∈ C, i.e. zE (q) = (R0E (q), rE (q)) = f (q) ∈ SE(3).

(3.4.1)

Note that not only the joint positions are relevant for the result of the forward kinematics mapping, also constant geometric parameters along the serial kinematic structure of the manipulator under consideration such as link lengths are required. In addition, the EE orientation may be expressed in terms of three angles defining successive rotations, cf. (3.1.4), i.e. ⎛

⎞

ϕE (q)⎠ = f (q) ∈ SE(3). zE (q) = rE (q) ⎝

3.4.2

(3.4.2)

Velocity-Level and Higher-Order Forward Kinematics

On velocity-level the forward kinematics describes the pose-dependent relationship between the EE velocity, i.e. its angular velocity ωE and ˙ This is conveniently its translation velocity vE , and the joint velocity q. expressed by the EE twist ⎛

VE =

⎝

⎞

˙ ⎠ ωE (q, q) . ˙ vE (q, q)

(3.4.3)

˙ the geometric Jacobian matrix As VE is linear w.r.t. the joint velocity q, J(q) =

∂VE ∂ q˙

(3.4.4)

can be used to find the equivalent forward kinematics representation ˙ VE = J(q)q.

(3.4.5)

60

3 Kinematics of Serial Robots

It should be noted that for practical calculations the EE twist VE is expressed w.r.t. a frame of reference, thus the resulting Jacobian J is also frame-dependent. With the EE pose expressed using angles describing successive rotation ϕE , e.g. Euler angles, and the position vector rE , the analytical Jacobian ⎛

∂ ϕ (q)

⎞

E ∂zE (q) ⎜ ∂q ⎠ JA (q) = = ⎝ ∂ rE (q) ⎟ ∂q ∂q

(3.4.6)

is used to express the velocity-level forward kinematics as ⎛ ⎝

⎞

ϕ˙ E ⎠ ˙ = JA (q)q. vE

(3.4.7)

Note that, in general, the EE rotation velocity expressed by the derivatives of the successive angles ϕ˙ E is different from the angular EE velocity ωE . Consequently, the analytical Jacobian JA is in general different from the geometric Jacobian J, cf. (3.4.5). A linear transformation between ϕ˙ E and ωE and thus JA and J can be found. For alternative holistic kinematics representation using screw coordinates, the reader is referred to textbooks such as [86]. The following example means to illustrate forward kinematics by regarding a planar manipulator. Example 3.6: SCARA4 (forward kinematics) Consider the planar SCARA4 manipulator depicted in Figure 3.4. Therein, a relative coordinate description is chosen, i.e. the joint angles q2 , q3 , q4 are relative w.r.t. the respective previous link. The EE configuration is expressed by means of the rotation matrix R0E , i.e. the coordinate transformation from the frame E attached to the EE body to the inertially fixed frame 0, and the position vector 0 rE from the origin to the EE resolved in coordinates of the I frame: zE (q) = (R0E (q), 0 rE (q))

3.4 Forward Kinematics

61

with ⎡

R0E (q) =

4 *

− sin

qi ⎢cos ⎢ i=1 ⎢ 4 * ⎢ ⎢ sin qi ⎢ i=1 ⎣

i=1 4 *

cos

i=1

0

⎛

⎤

4 *

qi 0⎥ ⎥ ⎥ ⎥ 0⎥⎥ ⎦

qi

0

and

0 rE (q)

1

=

⎞ i 4 * * li cos qj ⎟ ⎜ ⎜i=1 j=1 ⎟ ⎜ ⎟ ⎜* ⎟ 4 i * ⎜ ⎟. l sin q ⎜ ⎟ i j ⎜ i=1 ⎟ j=1 ⎝ ⎠

0

The velocity forward kinematics of this planar model is expressed as the twist ⎛ 0 VE

=

⎞ ˙ ⎠ ω (q, q) ⎝0 E

˙ 0 vE (q, q)

= J(q)q˙ with 0 ωE ∈ R and 0 vE ∈ R2 which is represented in terms of the 0-frame with ˙ 0 ωE (q, q)

4 

=

i=1

q˙i

and

⎞ i 4 i * * * l sin q q ˙ ⎜− ⎟ i j j ⎜ i=1 j=1 j=1 ⎟ ⎜ ⎟ ⎜ * ⎟ i i 4 * * ⎝ li cos qj q˙j ⎠ j=1 j=1 i=1 ⎛

˙ 0 vE (q, q)

=

d0 rE (q) = dt

and therefore ⎡

J(q) =

∂ 0 VE = ∂ q˙

⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

1 −

4 *

li sin qj i=1 j=1 4 i * * li cos qj i=1 j=1 ···

−

4 *

i *

4 *

···

li sin qj i=2 j=1 i 4 * * li cos qj j=1 i=2

1

··· − ···

···

1 i *

li sin

··· ⎤

1 i *

qj −l4 sin

i=3 j=1 4 i * * li cos qj i=3 j=1

l4 cos

⎥ ⎥

4 *

qj ⎥⎥⎥. j=1 ⎥

4 *

j=1

qj

⎥ ⎦

62

3 Kinematics of Serial Robots

By regarding the Jacobian J it is obvious that columns become linearly dependent if any of the joint angles qi = 0. However, if only one or two columns are linearly dependent, the Jacobian is still full rank which is limited by the number of its lines. Ex

0y

(E)

l4 Ey

l3 l1

l2

(0)

q4 q3 q2 q1 0x

Figure 3.4: Forward Kinematics of the SCARA4 Manipulator.

3.5

Inverse Kinematics

Inverse kinematics (IK) represents a mapping from a robot’s EE configuration to its joint positions, i.e. given the position and orientation of the EE, the joint configuration is determined accordingly. While the forward kinematics of a serial robot can be derived straightforwardly, the IK is typically more involved as in general a system of nonlinear equations needs to be solved. Alternatively, the kinematics for some structures situation can be analyzed by regarding the geometric properties of the kinematic structure. Of course, as for forward kinematics, derivatives w.r.t. to time such as velocities and accelerations can also be considered accordingly. Overviews of IK methods are provided in [90, 125, 23]. In this thesis, the kinematics in general, but especially the IK of kinematically redundant manipulators are of particular interest.

3.5 Inverse Kinematics

3.5.1

63

Kinematically Redundant Manipulators

Kinematical redundancy of serial robots describes the property that the robot has more degrees of freedom than necessary to assume a given EE configuration. Several types of redundancy have been documented in the past, a concise overview of useful defintions is given in [31] from which the following statements are reproduced for convenience. The most noteworthy fact about IK for kinematically redundant manipulators is that in general there is an infinite amount of solutions to the IK problem. Definition 3.1: (Inherent) Kinematical Redundancy A manipulator is considered kinematically redundant if its work space dimension is lower than its joint configuration space dimension, i.e. m := dim W < n := dim C, and the work space is contained in the configuration space in its entirety, i.e. W ⊂ C. Sometimes, such a structure is also called inherently kinematically redundant as this property is built into the robot and does not depend on the task to be executed. From this definition it is obvious that a spatial robot is kinematically redundant if n = dim C > 6 and a planar manipulator is kinematically redundant if n = dim C > 3. Definition 3.2: Degree of Kinematical Redundancy The degree of kinematical redundancy is given by r = dim C − dim W = n − m. Example 3.7: Stäubli TX90L (Kinematical Redundancy) The (spatial) Stäubli TX90L industrial robot from Figure 1.3 is inherently kinematically redundant as it has 7 joints with n = dim C = 7.

64

3 Kinematics of Serial Robots

Example 3.8: SCARA4 (Kinematical Redundancy) The (planar) SCARA4 robot from Figure 1.2 is inherently kinematically redundant as it has 4 joints with n = dim C = 4. Its degree of kinematical redundancy is r = dim C − dim W = 4 − 3 = 1. Example 3.9: SCARA3 (Kinematical Redundancy) The (planar) SCARA3 robot that will be later considered in Figure 3.5 is not inherently kinematically redundant as is has 3 joints with n = dim C = 3. Definition 3.3: Task Redundancy A manipulator is considered kinematically redundant w.r.t. its assigned task if its task space dimension is lower than its joint configuration space dimension, i.e. l = dim T < n = dim C. A consequence of this definition is of course, that all inherently kinematically redundant manipulators are also task redundant, but not necessarily vice versa. Definition 3.4: Degree of Task Redundancy The degree of task redundancy is given by rT = dim C − dim T = n − l. Example 3.10: SCARA3 (Kinematical Redundancy) Although the (planar) SCARA3 robot from Figure 3.5 is not inherently kinematically redundant, it is task redundant if its task is limited to pure positioning of the EE without regarding its orientation, i.e. l = dim T = 2. In this case the degree of redundancy is rT = dim C − dim T = n − l = 3 − 2 = 1. In the following, IK schemes for kinematically non-redundant and redundant structures are presented. Therein, for many cases, IK resolution schemes for non-redundant kinematics are a special case of the redundant ones. Approaches operating on position-level as well as differential IK methods, i.e. methods on velocity, acceleration-level, etc., are detailed below.

3.5 Inverse Kinematics

3.5.2

65

Position-Level Inverse Kinematics

On position-level, an IK scheme yields joint configuration that depend on the EE configuration. As apparent from forward kinematics, cf. Section 3.4, kinematics relationships are nonlinear in most cases, not allowing for a straight-forward inversion and therefore often requiring iterative, numerical methods. However, in certain cases a symbolic solution can be found by regarding the geometric properties of the structure. Also depending on the robot’s structure, more than one joint configuration can correspond to a particular admissible EE configuration. Depending on the manipulator and the desired EE configuration, there may be one single solution, or multiple or no solutions at all to the IK problem on position-level. However, for kinematically redundant structures, an infinite amount of configurations correspond to a given EE pose. Of course, now the question arises which configuration is the best and should therefore be desired. And also, what does the best actually mean in this case? Closed-Form Solutions In rare cases, such an IK relationship can be found in closed-form, i.e. in terms of functions and mathematical operations, excluding iterations and time integration. This is facilitated by means of thorough investigation of a manipulator’s geometry, which of course is only applicable for kinematically non-redundant structures. In the following, two particular types of manipulators are considered that are of great relevance for robotics applications. A prominent example for such a case is a planar 3 degree of freedom (DOF) manipulator with only revolute joints for which a closed-form IK solution exists.

66

3 Kinematics of Serial Robots

Example 3.11: SCARA3 (Closed-form Inverse Kinematics) Consider the planar manipulator depicted in Figure 3.5 that consists of three revolute joints with relative angles q1 , q2 , q3 and three links of lengths l1 , l2 , l3 . 0y

(E)

l3 yE

q3

ϕE

q2

l2 up

yG

l1 q1

0x

down (0)

xG xE

Figure 3.5: Inverse Kinematics of the SCARA3 Manipulator.

For a given EE configuration zE = (ϕ0E , 0 rE ), denoted by means of the EE position 0 rE and the EE orientation given by the angle ϕ0E , the   corresponding joint configurations q = q1 q2 q3 are found by q1 = α + β q2 = arctan(yG − l1 sin q1 , xG − l1 cos q1 ) − q1 q3 = ϕ0E − q1 − q2 ⎛

⎞

⎛

⎞

xG cos ϕ0E ⎠ with the auxiliary quantities ⎝ ⎠ = 0 rE − l3 ⎝ ,α = yG sin ϕ0E   2 arctan(yG , xG ), l2 = x2G + yG , β = ± arccos

l12 +l2 −l22 2l1 l

.

Note, that there are in general up to two joint configurations that solve the IK problem. In the above case, exactly two solutions exist, i.e. the

3.5 Inverse Kinematics

67

depicted up/down configuration of the first two links. They collapse into a single one for EE configurations when the first two links assume their maximum stretch. Trivially, no solution exists for desired points outside the manipulator’s work space in which case the IK problem is ill-posed. For a spatial manipulator with 6 DOF sufficient conditions for the existence of a closed-form IK solution are [127] • three consecutive axes of revolute joints intersect in the same point, or • three consecutive axes of revolute joints are parallel. A notable structure that fulfills the first of the above criteria is a 6 DOF standard industrial robot with a spherical wrist, cf. the robot on the linear axis from Figure 1.3. Not only because of the EE positioning and orientation capabilities, but also due to the simplicity of the corresponding IK resolution problem, manipulators with this structure are widely used in many applications. For a thorough investigation of the IK of such structures the reader is referred to [62]. Inverse kinematics problems for kinematically redundant structures can also be solved on position-level wherein some restrictions apply. Joint Space Decomposition The concept of joint space decomposition (JSD) was originally introduced in [134]. Since its introduction, JSD has been applied in the field of optimal path tracking for industrial manipulators, e.g. in [135, 50, 100, 115] as well as inverse kinematics of mobile platforms [105]. The key idea is a separation of the joint coordinates q into two disjoint coordinate sets: the non-redundant joint coordinates qnr and the redundant coordinates qr . The relationship can be expressed by means of a selector matrix S, i.e. ⎞ q ⎝ nr ⎠ ⎛

qr

⎤ S ⎣ nr ⎦q. ⎡

=

Sr

68

3 Kinematics of Serial Robots

Of course, this decomposition alone does not enable solving the IK problem. However, a solution for the non-redundant coordinates parametrized in terms of the redundant coordinates can be found, i.e. qnr = qnr (R0E , rE , qr). For a non-redundant coordinate subset with certain geometric properties, even a closed-form solution may be available as the following introductory example will demonstrate. Example 3.12: SCARA4 (JSD on position-level) Consider the kinematically redundant planar manipulator depicted in Figure 3.6. 0y

(E)

l4 yE

l3 l2

q3

ϕ0E

q2

q1

l1 (0)

q4

0x

xE

Figure 3.6: Inverse Kinematics of the SCARA4 Manipulator.

The EE configuration zE = (ϕ0E , 0 rE ) is given in terms of the EE position 0 rE and the EE orientationgiven by the angle ϕ0E . The vector of joint angles is given by q = q1 q2 q3 q4 . In order to apply JSD to this manipulator, the last joint angle is chosen to act as the

3.5 Inverse Kinematics

69

redundant coordinate qr , thus the first three joints are the non-redundant coordinates qnr which is formalized by ⎛

⎞

⎡

q 1 0 0 ⎜ 1⎟ ⎢ ⎢ ⎟ ⎜q2 ⎟ = ⎢0 1 0 qnr = ⎜ ⎝ ⎠ ⎣ q3 0 0 1 

 Snr

⎛ ⎞ ⎤ q1 ⎟ 0⎥⎜ ⎜ ⎟ ⎜q2 ⎟ ⎟ 0⎥⎥⎦⎜ ⎜ ⎟ ⎜q3 ⎟ ⎠ ⎝ 0 

q4

⎛

⎞

q ⎜ 1⎟ ⎟ ' (⎜ ⎜q 2 ⎟ ⎟ and qr = q4 = 0 0 0 1 ⎜ ⎜ ⎟. ⎟  ⎜  ⎝q 3 ⎠ Sr q4

With qr treated as a (known) parameter, the problem is reduced to the IK problem of a planar 3R manipulator as discussed in⎛Example 3.11. ⎞ cos q r⎠ Therein the substitutions ϕ0E − qr → ϕ0E and 0 rE − l4 ⎝ → 0 rE sin qr are used to accommodate the changes of the present example w.r.t. the previous one. A common application of JSD is the case in which a structure with known closed-form IK solution is extended by additional joints, e.g. the above Example 3.12, or a 6 DOF standard industrial robot with a spherical wrist on top of a linear axis, cf. Figure 1.3. While the above methods yield a solution to the IK problem in a single step (which requries a closed-form solution of any kind), the next approach is an iterative one. Iterative Method The definition of the velocity Jacobian (3.4.4) gives rise to an iterative IK scheme that operates on position-level. It can be used for point-wise computation of a joint configuration q that corresponds to an EE configuration zE (q). Starting at a joint configuration that not necessarily already agrees with the desired EE configuration, the EE configuration error ⎛

⎞

er (q) ⎠ e(q) = ⎝ eR (q)

(3.5.1)

occurs. It consists of the EE position error er (q) = rE,d − rE (q) and its orientation error eR (q). While the former is simply obtained from the EE

70

3 Kinematics of Serial Robots

position vector difference, the latter is more involved. With the desired and actual EE orientation in rotation matrix representation, i.e. '

R0E,d = 0 (E e1,d ) '

(

0 (E e2,d ) 0 (E e3,d )

R0E (q) = 0 (E e1 (q))

(

0 (E e2 (q)) 0 (E e3 (q))

(3.5.2) (3.5.3)

the rotation matrix error yields Re (q) = R0E,d R 0E (q) ˜R (q) = exp 0 e

(3.5.4) (3.5.5)

˜R = log Re and its explicit with the spin tensor of the orientation error2 0 e [80] angle-axis representation, cf. Section 3.1.2, 0 eR (q)

=

1 (E e1 (q)) × 0 (E e1,d ) + 0 (E e2 (q)) × 0 (E e2,d ) + . . . 2 0  (3.5.6a) + 0 (E e3 (q)) × 0 (E e3,d )

= 0 uR (q) sin ϑ(q)

(3.5.6b)

with the orientation error axis uR and the orientation error angle ϑ. Note that this representation is w.r.t. the inertially-fixed frame of reference and only unique for |ϑ| < π2 . With the EE position error e(q) established, it can be related to the approximate discrete change of the joint configuration Δq by e(q) ≈ J(q)Δq.

(3.5.7)

By inverting this relationship, the joint configuration q + Δq is calculated that approximates the desired EE configuration zE (q). From the forward kinematics relationship (3.5.7) the iterative scheme Algorithm 3 is obtained. Therein, the joint angles q are updated until the EE error is sufficiently reduced. 2 Note, that the time derivative of e(q) does in general not correspond to the EE velocity ˙ as the orientation part is different [127]. difference 0 VE,d − 0 VE (q,q)

3.5 Inverse Kinematics Algorithm 3 Iterative Position-Level IK Scheme 1: initialize q ← qinit 2: compute error e(q) 3: while |e| > ethresh do 4: if redundant then 5: compute update: Δq = J+ (q)e(q) 6: else 7: compute update: Δq = J−1 (q)e(q) 8: end if 9: update q ← q + Δq 10: end while 11: return q

71

 desired accuracy  cf. (3.5.15)

In the above Algorithm 3 a full-(row)rank Jacobian was assumed. However, in general, in both, the non-redundant and the redundant case, the manipulator may assume joint configurations that yield a rank-deficient Jacobian. In such a case, the respective system of equations has no unique solution. This means that, no Δq can be found (in the current configuration) that corresponds to the given e(q) in the sense of (3.5.7). This does not mean, that zE (q) cannot be reached at all from the current joint configuration, it only means that it cannot be reached directly with the update q + Δq. Note, that (3.5.7) is only a linearized form of the actual forward kinematics relationship (3.4.1). Therefore even an updated joint configuration q + Δq with a Δq that solves (3.5.7) only approximates, but not necessarily agrees with the desired EE configuration zE (q). A possible way to mitigate this issue is to obtain a solution to (3.5.7) by means of singular value decomposition of J. Futhermore, as discussed after the required velocity kinematics foundation has been established, it can be avoided by application of damped least-squares solutions, cf. (3.5.15) for the non-redundant and (3.5.10) for the redundant case. See also [87] for further analysis.

72

3.5.3

3 Kinematics of Serial Robots

Velocity-Level and Higher-Order Inverse Kinematics

While IK schemes on position-level are complicated as they comprise highly nonlinear functions due to the manipulator’s kinematic structure, on differential level a linear relationship between EE and joint quantities exists. Not only the resolution of the differential IK problem is of interest, but also joint positions that correspond to the desired EE pose are relevant. However, the methods considered in this section only provide such information via ˙ By nature numerical time integration of the resulting joint velocities q. of computer implementations of numerical integration schemes, errors are introduced that accumulate over (simulation) time yielding a non-negligible deviation from the desired EE path. Therefore, a stabilizing feedback loop is introduced that reduces these undesired effects by means of closed-loop inverse kinematics (CLIK), originally introduced in [137]. It is worth to emphasize that the errors occuring in this stabilization schemes do not originate from control errors of a real system but are of numerical nature as a result of integration schemes used in the presented IK algorithms, cf. [124]. In certain joint configurations, so-called kinematic singularities, of the considered manipulator, the Jacobian J from (3.4.5) becomes rank-deficient. Singularities occur if the EE mobility is reduced, i.e. the EE cannot assume translational or angular velocities in certain directions. Not only the work space bounds are prone to singular configurations, but also poses well within the work space may result in such effects. In singular configurations, infinitely many solutions to the IK problem exist. Differential Inverse Kinematics Differential IK (DIK) represents the computation of the instantaneous joint motion in terms of joint velocities q˙ or higher derivatives due to prescribed EE motion, i.e. of EE velocities VE or higher derivatives such ˙ E . The basic concept of IK resolution on velocity level as accelerations V was introduced by [104] and [136].

3.5 Inverse Kinematics

73

For kinematically non-redundant manipulators, the IK problem has as many equations as variables. It is found by considering the forward kinematics ˙ i.e. scheme (3.4.5) and solving for the joint velocities q, q˙ = J(q)−1 VE,d .

(3.5.8)

Note, that only the solution of a system of linear equation is required rather than an explicit representation of the Jacobian inverse J(q)−1 . Around singular configurations, the numerical condition of the above system of linear equation degrades, resulting in high joint velocities. In singular configurations, the EE mobility is degraded, J is rank-deficient and thus the above system of linear equations cannot be uniquely solved. An approximate solution can be obtained by relaxing the path tracking requirement in favor of moderate joint velocities by solving the unconstrained QP problem 1 1 minimize J = e˙ e˙ + αq˙ q˙ q˙ 2 2

(3.5.9)

wherein a weighted combination of the squared EE velocity error e˙ and the squared joint velocities q˙ is minimized. From the first-order necessary condition

∂J ∂ q˙



 1 ∂  ˙ (VE,d − J(q)q) ˙ + αq˙ q˙ (VE,d − J(q)q) 2 ∂ q˙ = −J VE,d + J Jq˙ + αq˙ = 0

=

the solution 

q˙ = J J + αI

−1

J VE,d

(3.5.10)

is obtained. It is a minimum as the second-order sufficient condition     2 H = ∂∂ q˙J2 = J J + αI > 0 is satisfied. The result (3.5.10) is also called the damped least squares solution of the IK problem. While the above holds for kinematically non-redundant robots, in the redundant case, the IK problem is underdetermined as there are fewer equations

74

3 Kinematics of Serial Robots

(for EE quantities) than variables (joint velocities). It is formulated by means of the linear equality-constrained QP 1 ˙ = q˙ Wq˙ minimize J(q) q˙ 2 ˙ = VE,d − J(q)q˙ = 0 s.t. g(q)

(3.5.11a) (3.5.11b)

wherein the squared joint velocities, weighted by the diagonal matrix W > 0, are minimized in (3.5.11a) with the path tracking constraint (3.5.11b). The Lagrange function corresponding to this QP yields ˙ λ) = J(q) ˙ + λ g(q) ˙ L(q,

(3.5.12)

with the Lagrange multipliers λ. From the first-order necessary conditions follows ⎛ ⎝

⎞

˙ λ) ⎠ ∂L(q, = Wq˙ + J λ = 0 ∂ q˙

(3.5.13)

and thus (3.5.13) → q˙ = −W−1 J λ

(3.5.14a)

−1

(3.5.14a) → Jq˙ = −JW J λ 

(3.5.14b)

 −1 −1

(3.5.14b), (3.5.11b) → λ = − JW J

(3.5.14c)

VE,d

the solution 

−1

(3.5.13), (3.5.14c) → q˙ = W−1 J (q) J(q)W−1 J (q) 

 J+ W (q)



2



VE,d

(3.5.15)



is found. It is a minimum of (3.5.11a) as H = ∂∂ q˙L2 = W > 0. Therein, J+ W a generalized inverse that is weighted by W. With the substitution W = I it is also called the right Moore-Penrose inverse that is denoted J+ (q). A detailed overview of other generalized inverses is found in [10]. It should be noted that a closed, i.e. circular, work space path does not necessarily yield a closed joint space path [68]. Thus, such IK schemes are

3.5 Inverse Kinematics

75

not suitable for the unsupervised execution of repetitive tasks [5]. This issue will be addressed in the augmented IK schemes below where such additional criteria can be incorporated. Furthermore, as in the non-redundant case, this methodology fails in when the manipulator is in a singular configuration. Moreover, it is shown in [6] that such an IK scheme does not avoid singularities even if started in non-singular configurations. However, for this IK solution, the path tracking constraint is again relaxed by exploiting the EE velocity error e˙ as a slack variable that is treated as an additional optimization variable, i.e. 1 1 minimize J(w) = e˙ e˙ + αq˙ q˙ w 2 2 s.t. g(w) = VE,d − J(q)q˙ − e˙ = 0 

(3.5.16a) (3.5.16b) 

with the vector of optimization variables w = q˙ e˙ . The corresponding Lagrange function yields L(w, λ) = J(w) + λ g(w) 1 1 ˙ = e˙ e˙ + αq˙ q˙ + λ (VE,d − Jq˙ − e). 2 2 

from which the first-order necessary conditions

∂ L(w,λ) ∂w



(3.5.17)

=0

αq˙ − J λ = 0

(3.5.18a)

e˙ − λ = 0

(3.5.18b)

are derived. From (3.5.18a) follows (3.5.18a) → αq˙ − J λ = 0

(3.5.19a)



(3.5.19a) → αJq˙ − JJ λ = 0

(3.5.19b)



˙ − JJ λ = 0 (3.5.19b), (3.5.16b) → α(VE,d − e)

(3.5.19c)



(3.5.19c), (3.5.18b) → α(VE,d − λ) − JJ λ = 0

(3.5.19d) 



−1

(3.5.19d) → λ = α JJ + αI

VE,d (3.5.19e)

76

3 Kinematics of Serial Robots

and thus the IK solution 

−1

(3.5.19a), (3.5.19e) → q˙ = J (q) J(q)J (q) + αI

VE,d

(3.5.20)

is obtained which is called the damped least squares solution of the IK problem for kinematically redundant manipulators. It is indeed a minimum of (3.5.16a) as theHessian of the Lagrange function (3.5.17) is positive  ∂2L definite, i.e. H = ∂w = diag(αI, I) > 0. 2 It should be noted that both damped least squares IK schemes (3.5.10) and (3.5.20) do not strictly solve the original problem of finding suitable joint velocities such that Jq˙ = VE,d holds as it is not included as a strict constraint. This issue can be mitigated by setting the regularization variable α = 0 only in the vicinity of singularities. Augmented/Extended Jacobian Method In the augmented Jacobian method [42, 121], the issue of the in general non-square Jacobian J of rank m < n is addressed. Therein, r functional constraints gi (q) = 0, i = 1 . . . r, collected in the r-vector g(q) = 0, are sought to be satisfied in addition to the path tracking requirement. This is achieved by concatenating the forward kinematics scheme (3.4.3) with the time derivative of above constraints, i.e. ⎛

⎞

˙ ⎟ ω (q, q) ⎜ E V ⎝ E⎠ = ⎜ ⎜ vE (q, q) ⎟ ˙ ⎟ ⎝ ⎠ 0 d g(q) dt ⎛

⎞

⎡

⎤

J(q) ⎦ ˙ = ∂ q, ∂q g(q) ⎣ 

 Jaug

(3.5.21)



such that the augmented Jacobian Jaug is square. As a result, the corresponding IK problem yields a single solution ⎞

⎛

q˙ =

V ⎝ E,d ⎠ J−1 aug 0

(3.5.22)

3.5 Inverse Kinematics

77

instead of infinite solutions outside of singularities. These arise not only where the EE mobility is limited, i.e. rank J < m, but also artificially. Furthermore, note that (3.5.21) only holds if the trajectory is started in a point that satisfies the additional constraints g(q) = 0 such as singularity or obstacle avoidance [4], or including a manipulator’s admissible joint range. A special case is the extended Jacobian technique [5] wherein the condition 



∈ / ker J holds. The extended Jacobian technique is not only used in industrial robotics as considered in this thesis, but also for mobile, non-holonomic manipulators, e.g. in [131, 129]. ∂ ∂q g

In the application [74] of the characteristic length method [3], the Jacobian is augmented such that its condition number is minimized. Therein, an SQP method is employed to find point-wise optimal poses for a 6 DOF robot performing a 5 DOF task. While in the above derivation the result is obtained by solving a system of linear equations of dimension n, in the version proposed in [24], JSD is applied in order to reduce the dimension to r. As discussed in Section 3.5.3, JSD provides an additional source of algorithmic singularities. Transposed Jacobian Method This control-oriented DIK scheme was firstly presented in [123] and later applied to kinematically redundant manipulators in [122]. The joint velocities q˙ are found as the position error weighted with the velocity Jacobian of the current joint configuration, i.e. q˙ = J (q)We

(3.5.23)

with the position error e = e(zE,d , q) and the diagonal matrix W = diag(w1 , . . . , wm ) with weights wi > 0, i = 1, . . . , m.

78

3 Kinematics of Serial Robots

In the special case of a resting EE, i.e. z˙ E,d = 0 and a manipulator in non-singular configuration, stability of this method can be shown by means of the direct Lyapunov method with the function 1 V (e) = e Ke 2

(3.5.24)

with K = K > 0 and thus V > 0 ∀ e = 0 and V (e = 0) = 0 and its time derivative ˙ = e Ke˙ V˙ (e, e)

(3.5.25a)



˙ = e K(VE,d − VE (q, q)) ˙ = e K(VE,d − J(q)q) 



= e K VE,d − J(q)J (q)We = −e KJ(q)J (q)We ≤ 0.

(3.5.25b)

For kinematically non-redundant manipulators J(q)J (q) > 0 and thus ˙ < 0 holds for e = 0, i.e. the DIK scheme (3.5.23) is asymptotically V˙ (e, e) stable. In the redundant case, it is only stable as for We ∈ ker J (q) pure internal motion occurs and the EE position error e is not further reduced until the prescribed EE velocity is changed, the EE gets stuck. For the general case of a moving EE the selected Lyapunov candidate function (3.5.24) is not suitable as e KVE,d is indefinite. Jacobian Nullspace Augmentation by Projected Gradient A simple means to exploiting the self motion capabilities of kinematically redundant manipulators is the projected gradient method. This IK solution is obtained by solving the linear equality constrained QP 1 ˙ = (q˙ 0 − q) ˙ (q˙ 0 − q) ˙ minimize J(q) q˙ 2 ˙ = VE,d − J(q)q˙ = 0 s.t. g(q)

(3.5.26a) (3.5.26b)

3.5 Inverse Kinematics

79

wherein the error w.r.t. the desired joint velocities q˙ 0 is minimized as a secondary goal to tracking the prescribed EE velocities VE,d . The Lagrange function corresponding to the QP (3.5.26) yields ˙ λ) = J(q) ˙ + λ g(q) ˙ L(q, 1 ˙ (q˙ 0 − q) ˙ + λ (VE,d − Jq) ˙ = (q˙ 0 − q) 2   1 ˙ = q˙ 0 q˙ 0 − 2q˙ 0 q˙ + q˙ q˙ + λ (VE,d − Jq) 2

(3.5.27)

From the first-order necessary conditions to multipliers λ can be calculated: ⎛ ⎝

⎞

˙ λ) ⎠ ∂L(q, = −q˙ 0 + q˙ − Jλ = 0 ∂ q˙

(3.5.28a)

(3.5.28a) → JJ λ = J(q˙ − q˙ 0 ) 

 −1

(3.5.28b), (3.5.26b) → λ = JJ

(3.5.28b)

(VE,d − Jq˙ 0 ).

(3.5.28c)

With the velocities isolated from (3.5.28a) and the result (3.5.28c) for the Lagrange multipliers λ, the IK scheme yields (3.5.28a) → q˙ = J λ + q˙ 0 

−1



 −1

(3.5.28c) → = J JJ = J JJ 

 J+

(VE,d − Jq˙ 0 ) + q˙ 0 ⎛



VE,d +

⎜ ⎜ ⎜I ⎝



⎞  −1

− J JJ 



 J+

 

q˙ = J+ (q)VE,d + I − J+ (q)J(q) q˙ 0 .  N(q)





⎟

⎟q ˙ J⎟ ⎠ 0

(3.5.29)

with the nullspace projector N : Rn → ker J. This can be straightforwardly shown by pre-multiplication of N with J: 

−1 



JN = J I − J JJ 

 −1

= J − JJ JJ 

 I



J

(3.5.30)

J = O.

(3.5.31)

80

3 Kinematics of Serial Robots

The resulting IK scheme (3.5.29) is a minimum of (3.5.26) as the Hessian of  2 the Lagrange function (3.5.27) is positive definite, i.e. H = ∂∂ q˙L2 = I > 0. With the relationship (3.5.29) established now regard Figure 3.7. It shows an illustration of the domains occuring for transformations using the velocity Jacobian J. It transforms joint velocities q˙ into its image im J resulting in corresponding EE velocities. Non-zero joint velocities that lie in the Jacobian nullspace ker J do not result in EE motion. This inherent property of kinematical redundacy can now be exploited by purposefully assigning nullspace joint velocities Nq˙ 0 that do not change the EE behavior but improve the joint configuration. Again, the question comes up, what improve means in this context. If not only an instantaneous velocity relationship is considered but a whole path, i.e. a sequence of points, is treated with IK methods, an improved configuration allows the manipulator to track that path so that additional goals are fulfilled. These typically range from a minimum traversal time over motion reduction of the individual joints to energy-efficient operation if additionally considering the system dynamics. q˙ VE

ker J

0 im J

Figure 3.7: Domains of the Jacobian.

The scheme (3.5.29) is understood as an IK method that tracks the desired EE velocities VE,d while locally approximating the joint velocities q˙ 0 using its internal motion capabilities. As for the choice of a suitable target q˙ 0 ,

3.5 Inverse Kinematics

81

often the gradient of a pose-dependent, scalar quantity H(q), a so-called performance index, w.r.t. the joint configuration q is used, i.e. ⎛

⎞

∂H(q) ⎠ : (3.5.29) with q˙ 0 = ∂q ⎝

 

+

 N(q)

⎛



q˙ = J (q)VE,d + I − J (q)J(q) +



⎝

⎞

∂H(q) ⎠ . ∂q

(3.5.32)

Therefore this particular family of nullspace augmentation is called projected gradient (PG) method. This methodology was originally introduced in [78]. Popular performance indices H are the kinematic [139] and dynamic [138, 27] manipulability3 . In [24], it is shown that the PG approach is identical to that of the extended/augmented Jacobian method in certain cases. A large number of survey of performance measures was conducted in the past, a comprehensive study that was published recently is [99].An attempt to construct such performance measures in a unified manner was made in [40]. A further survey and a guideline to synthesizing resolution criteria are found in [77]. Regarding the use of performance indices in a PG IK scheme, only quantities with dependencies of the joint configuration q are relevant. Many indices are isotropic, i.e. they represent quantities that do not distinguish between spatial directions. The kinematic manipulability, for example, describes the EE mobility without considering the direction of the pursued path. However, for path tracking only the prescribed path is relevant, thus it is questionable whether mobility in directions lateral to the path need to be considered. Task-dependent indices such as directional manipulability [30, 28, 142], i.e. the projection of the manipulability in the path direction, can be appropriately used in such applications. 3 Note on kinematical redundancy: It is theoretically sound to include the projected gradient of kinematic manipulability in an DIK resolution scheme as a local optimization criteria, i.e. the configuration is drawn to a local maximum of manipulability. It is a common statement, e.g. in [126], that it represents a distance from singular configurations. However, in general it does not provide a metric to do so, i.e. it cannot be used to quantify how singular a configuration is.

82

3 Kinematics of Serial Robots

The idea of the result (3.5.32) that tracks a prescribed EE trajectory as its primary goal while (locally) pursuing an additional, secondary objective H(q) gives rise to IK schemes that incorporate priorities of different tasks, e.g. [59, 81, 89]. Therein, tasks of lower priority are projected onto the Jacobian nullspace of higher prioritized tasks. Therein, methods exist that make a trade-off between accuracy and strict task prioritization, e.g. [29]. Joint Space Decomposition Similar to the position-level approach discussed in Section 3.5.2, the differential version of JSD is based on explicit separation of the joint space C of a manipulator [134]. Such a decomposition is mostly performed by dividing the set of joint coordinates into two groups, the non-redundant and the redundant coordinates. The same decomposition of course holds for the joint velocities and higher derivatives which yields the non-redundant and the redundant joint velocities, i.e. q˙ nr and q˙ r , respectively. Thus the velocity forward kinematics (3.4.5) is rewritten as VE = J(q)q˙ = Jnr (q)q˙ nr + Jr (q)q˙ r

(3.5.33)

with the corresponding columns of the full geometric Jacobian J arranged in Jnr which is square and Jr . Assuming the redundant joint velocities q˙ r as another input next to the desired EE twist VE,d , the DIK scheme (3.5.8) is rearranged as q˙ nr = Jnr (q)−1 (VE,d − Jr (q)q˙ r ).

(3.5.34)

As for the non-redundant case4 , the above formulation requires Jnr to be full rank, i.e. the manipulator is outside of singularities. In addition to the conventional types of singularities typically arising due to reduced EE mobility, the JSD formulation also exhibits algorithmic singularities. These result from the particular choice of redundant coordinates and do 4 Note that for a non-redundant manipulator the redundant joint set is empty, therefore (3.5.34) becomes (3.5.8).

3.5 Inverse Kinematics

83

not limit the EE motion. Such singularities are artificial and can therefore be avoided by selecting a decomposition that is suitable for the current joint configuration of the considered manipulator, i.e. with a full rank Jnr . The example below will demonstrate that algorithmic singularities are an issue that can occur at any point along a trajectory. Moreover, choosing a decomposition that is feasible in a particular point does not imply that it is feasible in all points of a trajectory. As motivated in [115], the need for seamless switching between different decompositions arises in order to avoid algorithmic singularities. Note that the joints selected to be in the non-redundant set are required to be capable of performing the prescribed EE velocity motion without the redundant set. Example 3.13: SCARA4 (JSD algorithmic singularities) In this example, JSD is performed for the inherently kinematically redundant SCARA4 manipulator that is in the configuration depicted below in Figure 3.8. For a task that not only involves the EE position but also its orientation, i.e. the manipulator has a degree of kinematical redundancy of r = 1, cf. Example 3.8, thus the selection of each joint yields a possible choice regarding JSD. For the particular configuration depicted in Figure 3.8, only two of the choices, namely qr = q1 and qr = q4 yield a full rank non-redundant Jacobian Jnr . The other choices will result in an algorithmic singularity of Jnr due to q3 = 0, i.e. the columns corresponding to q˙2 and q˙3 are linearly dependent. If this particular configuration occurs in course of a trajectory with joint 2 or 3 selected as redundant, changing to a decomposition with joint 1 or 4 as redundant allows to continue resolving the IK problem at that point.

84

3 Kinematics of Serial Robots 0y

q4

l4 q2 l3 l2 l1

q1 0x

Figure 3.8: Algorithmic Singularity in JSD of the SCARA4 Manipulator.

In order to avoid such algorithmic singularities that are only introduced by the specific selection of redundant coordinates for a particular EE task configuration, in [45] the idea of a generalization of the above approach is conveyed. Therein, an additional velocity task is pursued for which additional rows are added to the original Jacobian which is then full rank. Although this methodology seems similar to the augmented Jacobian method, the added part does not represent a conventional task but is rather used to create a full-rank Jacobian. One of the remaining issues is the choice of the additional task. Furthermore, for the resolution of the first-order augmented Jacobian IK scheme, a second-order pseudoinverse-based scheme is required, which increases the computational burden of the methodology. Applications of DIK JSD are not only found in robots with fixed base [41] but also in mobile robotics [37, 83]. One particularly interesting work therein is [37], where DIK JSD is used in conjunction with the reduced gradient method that is discussed below. For higher derivatives, e.g. acceleration, jerk and jounce, first time derivatives of (3.5.33) are considered, i.e. ˙ E = Jnr (q)¨ ˙ V qnr + Jr (q)¨ qr + J(q) q˙ (3.5.35a) ... ... ¨ E = Jnr (q)qnr + Jr (q)qr + J(q, ¨ q, ˙ ˙ q ¨ )q˙ + 2J(q, ˙ q V q)¨ (3.5.35b) ... ... ... ... ... ... ¨ ˙ ˙ q ¨ , q)q˙ + 3J(q, q, ˙ q ¨ )¨ ˙ q. VE = Jnr (q)qnr + Jr (q)qr + J(q, q, q + 3J(q, q) (3.5.35c)

3.5 Inverse Kinematics

85

JSD schemes similar to (3.5.34) are then found from solving these equations for the desired non-redundant quantities, i.e. 



˙ ¨ nr = J−1 ¨ r − J˙ q˙ q nr VE,d − Jr q   ... ... −1 ¨ ¨ q˙ − 2J¨ ˙q qnr = Jnr VE,d − Jr qr − J ....

qnr =

J−1 nr

 ...

(3.5.36a)  ˙ ...

...

...

¨ q − 3Jq . VE,d − Jr qr − Jq˙ − 3J¨

(3.5.36b) (3.5.36c)

Therein, terms were collected and simplified as far as possible. Note that these derivatives are indeed consistent, i.e. (3.5.34) is the time derivative of (3.5.36a) and so on which is now exemplarily shown: (3.5.34) →

 d d  −1 q˙ nr = Jnr (VE,d − Jr q˙ r ) dt dt d d  −1  (VE,d − Jr q˙ r ) = Jnr (VE,d − Jr q˙ r ) + J−1 nr dt dt   d ˙ ˙ r − Jr q ˙ ˙ r ) +J−1 ¨r (Jnr ) J−1 = −J−1 nr nr (VE,d − Jr q nr VE,d − Jr q    dt q˙ nr



˙ ˙ nr − J˙ r q˙ r − Jr q ˙ ¨r = J−1 nr VE,d − Jnr q   −1 ˙ ˙ ¨ nr = Jnr VE,d − Jr q ¨ r − Jq˙ . (3.5.36a) : q



Reduced Gradient Method One particularly noteworthy flavor of JSD is the reduced gradient (RG) method [36]. Therein, JSD is performed and the redundant coordinates are determined by a PG method. In contrast to conventional PG-based approaches, the computationally expensive generalized inverse is avoided. By rewriting the JSD IK scheme (3.5.34) as ⎞ q˙ ⎝ nr ⎠ ⎛

q˙ =

⎡

q˙ r

⎤

⎛

=

J−1 (VE,d ⎝ nr ⎡

q˙ r

⎞

− Jr q˙ r )⎠ ⎤

J−1 −J−1 Jr = ⎣ nr ⎦VE,d + ⎣ nr ⎦q˙ r I 0

(3.5.37)

86

3 Kinematics of Serial Robots

the redundant joint velocities q˙ r are found such that the time derivative of a joint pose-dependent performance index H(q), cf. Section 3.5.3, is maximized, i.e. d H(q). dt

maximize J(q˙ r ) = q˙ r

⎡

Note, that

(3.5.38)

⎤

−J−1 nr Jr ⎦ is an orthogonal complement to J. With I

⎣

d ∂H(q) H(q) = q˙ dt ∂q ∂H(q) ∂H(q) q˙ nr + q˙ r (3.5.34) → = ∂qnr ∂qr ∂H(q) −1 ∂H(q) = J (VE,d − Jr q˙ r ) + q˙ r ∂qnr nr ∂qr ⎡ ⎤ ∂H(q) −1 ∂H(q) ⎣−J−1 nr Jr ⎦ = Jnr VE,d + q˙ r I ∂qnr ∂q

(3.5.39)

and thus the first-order necessary condition to (3.5.38) follows from (3.5.39) as ⎛

⎞

 ∂H(q) ⎠ ∂J(q˙ r )  ⎝ = −(J−1 = 0. nr Jr ) I ∂ q˙ r ∂q

(3.5.40)

Thereby, the joints are driven towards an optimal configuration q in which 







∈ ker −(J−1 nr Jr ) I is satisfied. As a consequence the redundant velocities q˙ r are chosen as ∂H(q) ∂q



q˙ r =

−(J−1 nr Jr )

⎛



I

⎝

⎞

∂H(q) ⎠ ∂q

(3.5.41)

3.5 Inverse Kinematics

87

which satisfies the second-order sufficient condition to (3.5.38) ⎛

∂ 2 J(q˙ r ) = ∂ q˙ r2

⎞

⎜ ⎜ ∂2 ⎜ ⎜ ∂H(q) −1 ⎜ Jnr VE,d 2 ∂ q˙ r ⎜ ⎜ ∂qnr ⎝

⎡

⎤

⎟ ⎟

 q˙  r



⎠

⎟ ∂H(q) ⎣−J−1 nr Jr ⎦ ⎟ = 2I > 0 + q˙ r ⎟ ⎟ I ∂q ⎟ 

(3.5.42) in which case the EE motion is only governed by the first term in (3.5.37). With the result (3.5.41) the RG scheme (3.5.37) is rewritten as ⎡

q˙ =

⎤ J−1 ⎣ nr ⎦V E,d

0 0

⎤

⎛

⎞

  ∂H(q) ⎠ −J−1 nr Jr ⎦ ⎝ + −(J−1 J ) I nr r I ∂q ⎣ ⎡

⎡

=

⎤ J−1 ⎣ nr ⎦V E,d

⎡

+

−1 −1 ⎢Jnr Jr (Jnr Jr ) ⎣ −(J−1 nr Jr )   NRG

⎤⎛

⎞

−J−1 nr Jr ⎥⎝ ∂H(q) ⎠ ⎦ ∂q I

(3.5.43)



This method was not only applied to planar [36] but also to spatial and even mobile non-holonomic manipulators [37]. It can be observed that NRG is an orthogonal complement of the Jacobian which can be straightforwardly shown by multiplication, i.e. '

JNRG = Jnr 

⎡

J−1 Jr (J−1 Jr ) Jr ⎢⎣ nr −1nr −(Jnr Jr ) (

⎤

−J−1 nr Jr ⎥ ⎦ I



−1 −1 −1 = Jnr J−1 nr Jr (Jnr Jr ) − Jr (Jnr Jr ) −Jnr Jnr Jr + Jr = 0.

It should be noted that the RG method does not only exhibit the same issues regarding artificial singularities as JSD, cf. Section 3.5.3, but also requires the challenging choice of a pose-dependent performance index H(q) as in PG schemes discussed in Section 3.5.3. Generalized Jacobian Nullspace Augmentation As mentioned above, one of the major shortcomings of the projected gradient methods discussed in Section 3.5.3 is that the self-motion property of

88

3 Kinematics of Serial Robots

kinematically redundant manipulators is only exploited by means of a gradient of a performance index w.r.t. the instantaneous joint configuration q that is projected onto the Jacobian nullspace. Any q˙ ∗ that solves VE,d = Jq˙ ∗ can be augmented by means of a linear combination of basis vectors of the Jacobian nullspace, i.e. q˙ = q˙ ∗ +

r  i=1

γi ai (q)

= q˙ ∗ + A(q)γ

(3.5.44) (3.5.45)

with the nullspace basis vectors ai ∈ ker J(q) columnwisely collected in the nullspace basis matrix A(q), and the weights γi , collected in the weights vector γ. Note that any nullspace augmentation can be rewritten in that form, e.g. a PG as 

⎛



I − J(q)J (q) +

⎝

⎞

∂H(q) ⎠ = A(q)γ. ∂q

(3.5.46)

A pseudoinverse-based solution for the IK problem on velocity-level thus yields q˙ = J+ VE,d + A(q)γ.

(3.5.47)

Compared to JSD, for GNA the explicit selection of a redundant set of joint coordinates is unnecessary and thus artificial singularities do not occur. Furthermore, GNA does not require the selection of a (pose-dependent) performance index as for PG and RG methods. A similar approach has been proposed in archival literature [88, 107, 25, 98]. In the extended Jacobian approach from [96], the weights γi are used as an additional task that is explicitly prescribed. Similar to JSD, where the redundant coordinates qr or their time derivatives have to be appropriately determined, for GNA an open challenge remains the choice of the weights γ. Chapter 5 details how they can be selected in

3.5 Inverse Kinematics

89

an optimal manner w.r.t. an optimization goal in path tracking applications of this IK approach. Example 3.14: GNA (Minimization of Joint Velocities) Exemplarily a very common optimization goal is pursued: minimization of joint velocities for which the IK solution is well-known, cf. (3.5.15). Starting from the velocity-level IK scheme (3.5.47), the weights γ are required to be chosen such that the squares of q˙ are minimized, i.e. the corresponding unconstrained quadratic optimization problem reads 1 ˙ J(γ) = q˙ (γ)q(γ) = (q˙ ∗ + A(q)γ) (q˙ ∗ + A(q)γ). minimize γ 2 (3.5.48) The corresponding first-order necessary conditions are



∂ J(γ) ∂γ

= A q˙ ∗ − A Aγ = 0 

−1

→ γ = − A A

(3.5.49)

A q˙ ∗

which gives q˙ = J+ VE,d + Aγ 

−1

= J+ VE,d − A A A 



−1

= I − A A A

A J+ VE,d

 ∗

A q˙ .

(3.5.50)

Assuming an IK solution q˙ ∗ = J+ VE,d for which it is well-known that ˙ cf. (3.5.15), by expanding J+ it can be easily it already minimizes q, verified that the result obtained by (3.5.50) and (3.5.15) are indeed equivalent, i.e. 



−1

q˙ = I − A A A 

 −1

= J JJ





−1

A J JJ 



−1

VE,d − A A A

VE,d 

A J     (JA) =O

−1

JJ

VE,d = J+ VE,d .

90

3 Kinematics of Serial Robots

In this particular case, γ = 0 holds which can be seen from (3.5.49) as A J+ = O. By virtue of time derivation of (3.5.47), the consistent IK solutions for acceleration, jerk, and jounce are respectively 



˙ ˙ ˙ E,d − J˙ q˙ + JAγ + Aγ+ ¨ = J+ V q 





−1 +Aγ˙ + NJ˙ JJ VE,d nullspace

...

(3.5.51a) 



¨ q˙ − 2J¨ ˙ q + JAγ ¨ ˙ + 2JA ˙ γ˙ + 2A ˙ γ˙ + Aγ+ ¨ ¨ E,d − J q= J V + 2J˙ Aγ ⎫  −1 ⎪ ˙ E,d + ⎬ +A¨ γ + 2NJ˙ JJ V    −1  −1 −1 nullspace ¨ − 2J˙ JJ ˙ ⎭ +N J JJ˙ − 2J˙ JJ JJ VE,d , ⎪ JJ +

(3.5.51b) ....

 ...

...

˙ ...

¨ q − 3Jq + 6J˙ A ˙ γ˙ + +3J˙ Aγ ¨ + 3JA ¨ γ+ q = J+ VE,d − Jq˙ − 3J¨ ˙  ... ... ˙ γ + 3A¨ ˙ γ + 3A ¨ γ˙ + Aγ+ ¨ Aγ ˙ + JAγ + 3JA¨ +3J 



−1 ... ¨ E,d + + 3NJ˙ JJ V +Aγ    −1    ¨ − 6J˙ JJ JJ˙ − 6J˙ JJ −1 JJ ˙ JJ −1 V ˙ E,d + +N 3J ...  −1  −1   ¨ − 6J˙ JJ J˙ J˙ − 3J ¨ JJ −1 JJ˙ + +N J − 3J˙ JJ JJ 















−1 −1 ˙ + 6J˙ JJ −1 JJ˙ JJ −1 JJ˙ + +6J˙ JJ JJ˙ JJ JJ  −1   ˙ + JJ ˙ + ˙ + JJ˙ + 6J˙ JJ −1 JJ +6J˙ JJ JJ   −1  −1 −1 ˙ − 3J˙ JJ ¨ −3J˙ JJ JJ VE,d JJ JJ

⎫ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎭

null space

(3.5.51c) d + with the nullspace projector N = I − J+ J and the identity dt J =   −1  −1 ˙J JJ − J+ d JJ JJ for the time derivative of the pseudoinverse. dt The above IK expressions are simplified as far as possible and denoted in a representation that explicitly shows terms in the primal and dual space of the Jacobian J. It is particularly noteworthy, that, in contrast to JSD, cf. (3.5.35), in general the above IK expressions cannot be obtained by simply rewriting the corresponding forward kinematics expressions.

3.5 Inverse Kinematics

91

Joint Space Decomposition with Generalized Nullspace Augmentation JSD, cf. Section 3.5.3, and GNA, cf. Section 3.5.3, can be combined in order to reduce the computational burden. This is facilitated by relying on the inverse of the non-redundant Jacobian Jnr instead of a generalized inverse of the full Jacobian J, i.e. by rewriting (3.5.34) as ⎞ q˙ ⎝ nr ⎠ ⎛

q˙ =

q˙ r

⎛

=

J−1 (VE,d ⎝ nr

q˙ r

⎞

− Jr q˙ r )⎠

⎡

=

⎤ J−1 ⎣ nr ⎦V E,d

0

⎡

⎡



⎤

⎤

−J−1 nr Jr ⎦ + q˙ r I ⎣

 ∈ ker J

J−1 = ⎣ nr ⎦VE,d + A(q)γ. 0



(3.5.52)

Note, that this formulation still requires the non-redundant part to be able to complete the task. In addition, it also exhibits the disadvantage of algorithmic singularities due to the particular choice of the decomposition. Other Approaches Artificial neural networks (ANN) and its application in robotics have been undergoing extensive research in the last few decades which was mainly performed by the computer science community. Although many methods in this field only operate under closed-set conditions, i.e. the situations faced in real-world operation are already known from the training set wherein they are included, ANN can also act as a controller instance. Therein, environmental perception of relevant parameters to the operation conditions, e.g. computer vision, or fusion of heterogenous sensoric information, plays an important role. In such open-set conditions, dictated by real-world operation, the parameters influencing the employment of robotics devices undergo permanent change, so the training data does not include occuring in practise. The survey paper [128] provides a comprehensive overview of current challenges and limitations of the application of ANN in robotics in general. Genetic algorithms, i.e. population-based stochastic methods, such as [54] are also used in this context.

Chapter 4

Path and Trajectory Planning A path represents a geometric entity, think, for example, of all points in space a point of a rock sweeps through when thrown. In a robotic motion, it can exist in the joint space as the sequence of joint positions, and also in the work space as the sequence of configurations the EE assumes. The correspondence between a joint space path and a work space path is given by the forward (and inverse) kinematics of the considered manipulator, cf. Section 3.4.1. A trajectory describes the temporal evolution of an object traversing a path, think of the motion a rock performs when thrown. That is, in contrast to a purely geometric path, a trajectory exhibits a time dependency. Typically, trajectories are given in terms of the time evolution of a robot’s joint angles while moving along a given joint space path, or its EE configuration tracking a work space path. In this chapter, path and trajectory planning problems are considered. Due to practical requirements in robotics, in most use cases it is not sufficient to only satisfy boundary constraints of a path or a trajectory, e.g. initial and terminal joint configurations. It is necessary to find an interpolation that is also admissible in terms of the technological limitations of the machine executing the task. This demand alone rules out simple planning methods as shown below. Furthermore, not only efficacy, but also © Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4_4

94

4 Path and Trajectory Planning

efficiency of the generated plans in terms of select quality criteria, e.g. the shortest transportation path, a minimum-time or minimum-energy execution of a trajectory, is increasingly relevant for robot operation. Numerical optimization can be used for both1 , ensuring admissibility of the results, and also optimality regarding a specified goal. In the next Section 4.1 a very simple trajectory planning approach is presented. Therein, a boundary value problem (BVP) is solved to find an interpolation of desired initial and terminal states of a system. In Section 4.2 point-to-point planning regarding geometric paths and also trajectories is considered. Therein, approaches that perform geometric and temporal planning in a single step (just as the BVP mentioned above), and also multi-stage approaches with successive computation of the spatial and temporal aspects are presented. Furthermore, the aspect of computational efficiency is discussed in this section. Due to the numerical complexity of the optimization problems arising from typical planning tasks and the resulting high computation times, they are only applicable in offline settings. For a robot that is employed in a process in which frequent changes occur, e.g. consider a task in which objects are picked from a conveyor belt whose positions vary slightly, such planning approaches are not feasible. However, with the approximation methods for optimal solutions presented in Section 2.1.3, the computational performance can be substantially improved (at the cost of slight suboptimality). Approximation schemes based on parametric sensitivities have been previously applied in robotics research [69, 112, 116]. In this section, the efficacy of the proposed methods is shown using various numerical examples. While therein mostly fully actuated industrial robots are considered, i.e. each joint is driven by its own actuator, the introductory example is an underactuated model. 1 Note, that such approaches can also be exploited when only admissible, but not optimal results are required. In this case, optimization is performed w.r.t. a dummy objective.

4.1 Boundary Value Problem

4.1

95

Boundary Value Problem

Problem Statement In order to the use of optimal, constrained trajectory planning, as discussed above, first a simple example is considered. Example 4.1: Swing-Up Motion of Furuta Pendulum (BVP) Consider the Furuta pendulum depicted in Figure 4.1. q2

q1 gearbox motor u

Figure 4.1: Furuta Pendulum.

The goal of this example is to compute control and joint trajectories to successfully perform a swing-up motion of the pendulum. With the   two joint angles q = q1 q2 , the pendulum exhibits two DOF while it has only one control input u, namely the motor torque, thus the system is considered underactuated. Furthermore, the system is constrained by the maximum torque and speed of the motor, i.e. |u| ≤ umax and |q˙1 | ≤ q˙1,max . Moreover, the torque is required to be continuous and zero at the bounds.

96

4 Path and Trajectory Planning

Implementation The swing-up procedure is thus formalized as the two-point boundary value problem x˙ = f (x, u) 

(4.1.1a)

q(0) = q0 = 0 π ˙ q(0) = q˙ 0 = 0



(4.1.1b) (4.1.1c)

q2 (T ) = q2,T = 0 ˙ ) = q˙ T = 0 q(T 

(4.1.1d) (4.1.1e)



with the state x = q q˙ and the additional boundary conditions u(0) = u(T ) = 0 due to the torque continuity requirement. Similar to [56], the control function can be constructed as a consine series, i.e.

u(t) = a0 + a1 cos



5  πt iπt + pi−1 cos T T i=2 

(4.1.2) 

with the free parameters collected in p = p1 p2 p3 p4 . With this particular choice of a function for u(t) the infinite dimensional BVP (4.1.1) is transcribed into a finite dimensional problem. The control trajectory u(t) also largely determines the properties of the resulting joint trajectories. From the torque boundary conditions a0 = −p1 − p3 and a1 = −p2 − p4 follow. As the time evolution of the control u and thus the state x can be determined from p, the goal of this example is to compute these parameters such that the boundary conditions are satisfied. This can be facilitated with standard techniques for solving DAEs, e.g. with the collocation-based BVP solver bvp4c [67]. Discussion Of course the function class for u(t) is not restricted to the above cosine series (4.1.2). Alternatively, for example, a B-spline curve, cf. Section 2.2, with 6 control points could be used to represent u(t).

4.2 Optimal Point-to-Point Trajectory Planning

97

As the technological constraints of the systems, e.g. a limit of the motor torque, are not considered in the above DAE formulation, it becomes apparent that these may be violated by the result. Therefore, this approach is very unflexible as it is not able to handle typical inequality constraints occuring in technical systems. In [57] inequality constraints are handled by means of saturation functions and augmentation of the dynamic system by additional states. This procedure again yields a DAE formulation that can be solved using standard frameworks, e.g. with bvp4c. Alternatively, such tasks can be handled by means of the OCP techniques discussed in Section 2.3. This topic will later be revisited in Example 4.2 where it will be treated as an inequality-constrained OCP in the next Section 4.2.

4.2

Optimal Point-to-Point Trajectory Planning

Trajectory planning is the task of finding suitable time evolutions of a robot’s motion in order to solve a particular task, such as transportation of items or material processing. Therefore, a suitable evolution of the manipulator’s joint angles q(t) must be found, which inherently specifies the motion of the EE due to forward kinematics, cf. Section 3.4. Planning tasks are constrained by the physical and technological limitations of the robot, e.g. those of joint angles, motor turning velocities, or motor torques. Further constraints are due to the work process the manipulator is employed in, e.g. geometric EE path constraints, restriction of the entire robot to a confined work space. Summarizing, a trajectory planning problem is composed of both, geometric and kinematic as well as dynamic aspects. Examples for point-to-point trajectory planning tasks are not only found in typical robotic transportation, but also in the field of ball catching, e.g. [120], (airborne) object interception, e.g. [73], missile guidance, or space maneuvers [48]. Therein, often fast computation of the trajectories is required, which is typically achieved by omitting or simplifying constraints, e.g. by neglecting friction

98

4 Path and Trajectory Planning

effects. Therefore, the resulting trajectories often lack admissibility and actual optimality. Other methods for instantaneous motion planning, e.g. [70, 66], and also model predictive control techniques suffer from similar limitations in some cases, e.g. [55]. The paper [26] considers optimal splinebased interpolation of initial and terminal states in joint space. In [35], B-spline robot trajectories are computed wherein parametric sensitivities are exploited to speed up calculations. However, instead of the control points which are used in the presents thesis, the knot intervals are the optimization variables. Trajectory planning methods can be classified in single-step approaches that solve the entire problem in a single optimization stage, and in multistep approaches which, for example, treat the kinematic and dynamic subproblems sequentially. In the following, both types will be discussed.

4.2.1

Single-Step Approach

As path planning and also motion planning are performed in a single optimization step, the OCP (2.3.1) must be reformulated in order to accommodate point-to-point (PTP) trajectory planning. The trajectory planning OCP reads &T

minimize q(t),u(t)

L(q(t), u(t))dt + E(q(T ))

(4.2.1a)

0

s.t. q0 − q(0) = 0, r(q(T )) ≤ 0,

˙ q˙ 0 − q(0) = 0, . . .

(4.2.1b)

˜r(q(T ), q(T ˙ )) ≤ 0, . . .

(4.2.1c)

qmin ≤ q(t) ≤ qmax ,

˙ q˙ min ≤ q(t) ≤ q˙ max , . . .

(4.2.1d)

umin ≤ u(t) ≤ umax

(4.2.1e)

˙ M(q(t))¨ q(t) + g(q(t), q(t)) = Q(q(t), u(t))

(4.2.1f)

h(q(t), u(t)) ≤ 0, ∀t ∈ [0, T ]

(4.2.1g)

wherein q(t) describes the time evolution of the robot’s joint angles and u(t) represents the input trajectory, i.e. actuator forces and torques. The objective (4.2.1a) is kept very general in order to account for goals frequently used in robotics such as minimum energy, minimum time, or minimum

4.2 Optimal Point-to-Point Trajectory Planning

99

component wear. For such a PTP problem, particular interest lies in the specification of initial and terminal conditions (4.2.1b) and (4.2.1c), respectively. The in general nonlinear function r represents the terminal conditions on position-level, ˜r is used for formalize a velocity-level constraint. Therein, typically not only the initial and terminal joint angles are considered, but also their time derivatives which are here (and in the following) omitted for brevity. The inequality constraints (4.2.1d) and (4.2.1e) represent limitations imposed onto the joint angles (and their derivatives) and onto the inputs, respectively. In the original OCP, these were included in the path constraints (2.3.1e) but are explicitly mentioned here due to their relevance in this application. While previously the system dynamics was considered in (2.3.1d) in state-space representation, here it is equivalently denoted as the equations of motion in (4.2.1f). Therein, M(q) is the generalized mass ˙ is a vector of in general nonlinear terms including general matrix, g(q, q) Coriolis, centrifugal and friction forces. The generalized input forces Q(q, u) are in practice often given by scaled inputs u. Furthermore, the motion is subjected to actual path constraints (4.2.1g) that impose restrictions arising from the task to be executed, e.g. work space constraints in multi-robot cooperation environments, maximum EE velocity in machining tasks, or limited EE acceleration to avoid sloshing in liquid transportation. The following examples will demonstrate possible applications of the singlestep approach. Example 4.2: Swing-Up Motion of Furuta Pendulum (OCP) Here, Example 4.1 is revisited in the context of OCP. Implementation The trajectory planning task is formalized as a direct multiple shooting OCP, cf. (2.3.4), not only to solve the BVP considered previously, but also to minimize the torque required to drive the pendulum and

100

4 Path and Trajectory Planning

incorporate technonogical constraints of the system. The corresponding NLP is derived from (4.2.1) and reads minimize w ,w x

u

&tj

N 

u2 (t)dt

(4.2.2a)

j=1 tj−1

s.t. q0 − q0 = 0, q2,T

q˙ 0 − q˙ 0 = 0 ˙ )=0 − q2 (T ) = 0, q˙ T − q(T

xj −

(4.2.2b) (4.2.2c)

&tj

f (x(t), u(t))dt = 0, j = 1, . . . , N

(4.2.2d)

tj−1

− Pmax ≤ P (xk , uk ) = uk q˙1,k ≤ Pmax (4.2.2e) uk RA + q˙1,k kM ≤ UA,max , k = 1, . . . , Nch − UA,max ≤ UA,k = kM (4.2.2f) 



wherein the desired initial configuration is q 0 = 0 π , see (4.2.2b). As prescribed in (4.2.2c), the desired terminal configuration is q2 (T ) = q2,T . The motion is required to be executed as a rest-to-rest motion, i.e. the joint velocities at the beginning and the end of the motion are zero, cf. (4.2.2b) and (4.2.2c). The execution time is fixed to T = 2.6 s, the time domain is discretized in N = 261 uniform intervals. As the setup is sufficiently stiff to be considered rigid for the swing-up operation, it is adequate to use a piecewise linear function for the motor torque trajectory u(t) in order to avoid excitation of unmodeled stiffness effects of the system. Results The above NLP problem (4.2.2) was implemented using CasADi and solved with Ipopt. The resulting joint trajectories, velocities and torques are shown in Figures 4.2 to 4.4. It can be observed, that the result accomplishes the swing-up motion, i.e. a rest-to-rest motion that drives both joint angles to zero, and is admissible w.r.t. the technological constraints of the system (the plots of UA and P are neglected for the sake of brevity).

4.2 Optimal Point-to-Point Trajectory Planning q in rad

q1

101

q2

4 2 0 t in s 0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

2.2

2.4

2.6

Figure 4.2: Swing-Up Motion of Furuta Pendulum: Joint Angles q(t). q˙ in rad/s

q˙1

q˙2

5 0 −5 −10

t in s 0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

2.2

2.4

2.6

˙ Figure 4.3: Swing-Up Motion of Furuta Pendulum: Joint Velocities q(t). u in Nm

0.4 0.2 0 −0.2 −0.4

t in s 0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

2.2

2.4

2.6

Figure 4.4: Swing-Up Motion of Furuta Pendulum: Motor Torque u(t).

The trajectories computed with the above scheme were successfully experimentally verified with the lab prototype shown in Figure 4.1. Similar to [56], a combined feed-forward/feed-back control structure is used. It comprises a model-based feed-forward part with the control trajectory resulting from the NLP problem (4.2.2), and PD feed-back control which is linear quadratic design [72]. Details about the implementation and experiments on the lab prototype are documented in [140].

102

4 Path and Trajectory Planning

The following two examples will demonstrate trajectory planning OCP in the case of synchronization tasks of robotic manipulators with their environment. In the field of robotics, such synchronization or rendezvous problems have been researched in ball catching. Similar tasks have been previously investigated in the (non-robotic) context of space landing and docking maneuvers and object interception. Example 4.3: SCARA4 (minimum-time trajectory planning) Problem Statement Consider the synchronization task depicted in Figure 4.5. Therein, the SCARA4 manipulator known from the previous examples is required to pick items from a conveyor belt that is moving with constant velocity vB . It is assumed, that the object to be picked has the same velocity, i.e. vObj = vB . In this example, only the computation of the pick trajectory is considered. Therein, the lateral placement of an item, denoted with its distance Δx from the base of the robot, is uncertain and therefore can only be determined immediately before the start of the picking process. While the initial rest configuration at q0 is the same for all pick processes, the terminal joint configuration q(T ) varies depending on the position of the item on the conveyor at pick time T . The pick motion needs to be executed in minimum time, i.e. the item needs to be picked as soon as possible. The motion is subjected to physical constraints regarding the joint angles q, velocities q˙ and motor torques MMot or ¨ . Moreover, the work space is constrained such that the accelerations q EE is not allowed to enter the area behind the conveyor as well as the area right of the detection line.

4.2 Optimal Point-to-Point Trajectory Planning

103

x object

detection line

vB

terminal configuration q(T )

Δx y

initial configuration q0

Figure 4.5: SCARA4 Minimum-Time Trajectory Planning.

Implementation The general OCP (4.2.1) is rewritten in order to fit the present minimumtime example and reads now &T

1dt

minimize q(t),T

(4.2.3a)

0

s.t. q0 − q(0) = 0,

˙ q˙ 0 − q(0) = 0,

¨ (0) = 0 q

(4.2.3b)

rE (q(T )) − rObj (T ) = 0, vE (q(T )) − vObj = 0, qmin ≤ q(t) ≤ qmax ,

ωE (q(T )) = 0,

˙ q˙ min ≤ q(t) ≤ q˙ max ,

¨ (T ) = 0 (4.2.3c) q

¨ min ≤ q ¨ (t) ≤ q ¨ max q (4.2.3d)

MMot,min ≤ MMot ≤ MMot,max

(4.2.3e)

x ⎝ min ⎠

(4.2.3f)

⎞

⎛

ymin

⎛

≤ rE (q(t)) ≤ ⎝

⎞

xmax ⎠ ymax

wherein the objective (4.2.3a) only consists of a Lagrange term L(q(t), u(t)) = L = 1. While the initial conditions are considered in (4.2.3b) in terms of joint space quantitites, the terminal conditions

104

4 Path and Trajectory Planning

can be found in (4.2.3c) in work space notation. Box constraints for joint angles, velocities, accelerations and torques are found in (4.2.3d) and (4.2.3e), respectively. The work space limits are imposed using (4.2.3f). With the choice for q(t) to be B-spline curves, OCP (4.2.3) is once again rewritten in order to exploit various properties of this function class. Therefore, each qi (t), i = 1, . . . , n = 4 is a B-spline curve of degree p = 3 and NCP = 20 control points. The control points of each B-spline curve describing the time evolution of the i-th joint angle are collected in the vector dqi ∈ RNCP . Furthermore, the respective j-th control points of all curves are collected in the vector dq,j ∈ Rn . Thus, the j-th control point of the i-th curve is denoted dqi ,j . The same notation holds for time derviatives of the curves. All control points and also the terminal time T are collected in the vector of optimization variables 



w = dq1 ,0 · · · dqn ,0 dq1 ,1 dq2 ,1 · · · dqn ,1 · · · dqn ,0 dqn ,1 · · · dqn ,NCP −1 T . The corresponding optimization problem reads minimize T w

(4.2.4a)

s.t. q0 − dq,0 (w) = 0,

q˙ 0 − dq,0 ˙ (w) = 0,

dq¨ ,0 (w) = 0 (4.2.4b)

rE (q(T ) = dq,NCP −1 (w)) − rObj (T ) = 0,

(4.2.4c)

˙ ) = dq,N vE (q(T ), q(T ˙ CP −2 (w)) − vObj = 0, ˙ )) = 0, ωE (q(T ), q(T dq¨ ,NCP −3 (w) = 0

(4.2.4d) (4.2.4e)

4.2 Optimal Point-to-Point Trajectory Planning

105

qi,min 1 ≤ dq,j (w) ≤ qi,max 1 ≤ 0, i = 1, . . . , n, j = 3, . . . , NCP − 1 (4.2.4f) q˙i,min 1 ≤ dq,j ˙ (w) ≤ q˙i,max 1 ≤ 0, i = 1, . . . , n, j = 2, . . . , NCP − 2 (4.2.4g) q¨i,min 1 ≤ dq¨ ,j (w) ≤ q¨i,max 1 ≤ 0, i = 1, . . . , n, j = 1, . . . , NCP − 3 (4.2.4h) ⎞ x ⎝ min ⎠ ⎛

ymin

⎞ x ⎝ max ⎠ ⎛

≤ rE (q(t)) ≤

ymax

(4.2.4i)

wherein 1 denotes a vector of ones of appropriate size. In this minimumtime OCP, cf. (4.2.4a), the initial rest state is prescribed in (4.2.4b). The pick point, i.e. the EE configuration at which the pick process begins, is assumed at rE (q(T )) (gripper above object).The greatest difficulty in this example is posed by the fact that the target item is moving, i.e. the (longitudinal) position rObj (t) of the item on the belt varies over time. Only at the pick time the object and EE positions conincide, and therefrom the terminal joint angles q(T ) are determined which is reflected in the nonlinear equality constraints (4.2.4c). Note, that due to the kinematic redundancy of the manipulator w.r.t. to this positioning task this relationship is ambigous, i.e. multiple joint configurations satisfy these constraints. The corresponding velocity relationship is formulated by means of the nonlinear equality constraints with the velocity-level forward kinematics (4.2.4d). Also the terminal joint acceleration needs to be zero in order to enable a smooth picking process, i.e. (4.2.4e). As for the corresponding initial condition, this constraint is implemented by exploiting the boundary point interpolation property of B-spline curves resulting in linear equality constraints. The OCP (4.2.4) is subjected to box constraints limiting the joint angles in (4.2.4f), the velocities in (4.2.4g) and accelerations in (4.2.4h). In addition to the fact, that control points of curve derivatives are linear combinations of the control poitns of the original curves, these constraints exploit the convex hull property of B-spline curves, resulting in linear inequality constraints. The work space constraints are formulated with the box constraints

106

4 Path and Trajectory Planning

(4.2.4i) which are due to the forward kinematics relationship nonlinear inequality constraints. Note, that these constraints are unlikely to be reached as they are far off the initial and final EE positions. Although this approach is termed single-step, the preparation of the solution of the above NLP problem (4.2.4) comprises multiple steps. In order to properly initialize the NLP problem and thus enable its efficient solution, multi-stage trajectory planning with relaxed constraints is performed. First, in order to determine a terminal joint configuration, the iterative IK scheme Algorithm 3 is employed, the terminal velocity is found by application of (3.5.15). The trajectories are then planned with a B-spline approximation scheme similar to (2.2.11), incorporating the geometric constraints of (4.2.4). In order to speed up the computation, the work space constraints (4.2.4i) are excluded from the initial guess computation, which ultimately yields a convex QP. As the torque constraints are nonlinear, the solution of the above NLP becomes more involved and thus more time-consuming if they are included. Alternatively, pure (linear) joint acceleration constraints can be used as an approximation. It is difficult to provide general ¨ , but also acceleration limits as the torques not only depend on the q ˙ on the current joint configuration q as well as on the joint velocities q. To this end, the actual torques must be underestimated, so the results are more conservative. A practical method is to assume acceleration limits, solve the above NLP problem and check the admissibility w.r.t. the torque constraints afterwards (and adjust the acceleration limits if necessary). Results In the above Figures 4.6 and 4.7 it can be seen that the velocity and torque constraints are active during most of the trajectory except for the initial acceleration and terminal deceleration phases in which the prescribed boundary points are dominant.

4.2 Optimal Point-to-Point Trajectory Planning q (normalized) 1 0.5 0 −0.5 −1

q1

q2

q3

107

q4

t in s 0

5·

10−2

0.1

0.15

q˙ (normalized) 1 0.5 0 −0.5 −1

0.2 q˙1

0.25 q˙2

0.3 q˙3

0.35

0.4

0.45

0.5

q˙4

t in s 0

5·

10−2

0.1

0.15

¨ (normalized) q 1 0.5 0 −0.5 −1

0.2 q¨1

0.25 q¨2

0.3 q¨3

0.35

0.4

0.45

0.5

q¨4

t in s 0

5·

10−2

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

¨ (t)), Figure 4.6: SCARA4 Minimum-Time Motion (Constrained Accelerations q Δx = 0. Limits: qi,max : 1.65 rad, q˙1,max = q˙2,max : 2.51 rad/s, q˙3,max = q˙4,max : 5.81 rad/s, q¨1,max = q¨2,max : 12.6 rad/s2 , q¨3,max = q¨4,max : 29.1 rad/s2 .

Note that the terminal motor torques are not zero despite the terminal ¨ (T ) = 0 due to the non-zero terminal accelerations are restricted to q velocity and the viscous friction effects included in the model for the system dynamics. While the above friction torque is proportional to the joint speeds, another effect result from the stiction model. Regard therefore the jitter behavior visible in the torque curves in Figure 4.7, especially of M3 and M4 where q˙3 and q˙4 change their respective signs. Moreover, it is apparent that the torques violate their respective constraints at multiple times along the trajectory between the Nch torque check points. This issue can be mitigated by increasing the number

108

4 Path and Trajectory Planning

of torque check points Nch , but cannot be completely avoided by this approach. For practical applications, one needs to check whether the peak torque constraint and the thermal constraints are violated during these times. Table 4.1 shows the optimal terminal times T and computation times tCPU for all considered cases Δx. The selected acceleration bounds appear to be too low, also investigation of the motor torques resulting from the trajectories reveals that the torque constraints are violated during multiple times which supports this indication. However, proper choice of acceleration bounds as a replacement for torque bounds is complex due to the aforementioned reasons. Furthermore, T decreases with the distance from the robot base as the robot is unfolded by centrifugal forces whereas the computation time rises because the KKT system grows in size due to the increasing amount of active constraints. Table 4.1: SCARA4 Minimum-Time Trajctory Planning: Results. ¨ constrained q

constrained MMot

Δx

T

tCPU

T

tCPU

1.00 0.95 0.90 0.85 0.80 0.75 0.70 0.65 0.60

0.6229 0.5889 0.5566 0.5328 0.5131 0.4966 0.4812 0.4674 0.4542

0.2876 0.3410 0.4481 0.4409 0.4631 0.4576 0.4142 0.4842 0.5240

0.7553 0.7328 0.7169 0.6969 0.6724 0.6520 0.6303 0.6083 0.5929

310.2455 373.2159 347.5644 562.5836 359.5886 346.5954 521.2011 463.7495 460.2238

4.2 Optimal Point-to-Point Trajectory Planning q (normalized) 1 0.5 0 −0.5 −1

q1

q2

q3

109

q4

t in s 0 5·

10−2 0.1

0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65

q˙ (normalized) 1 0.5 0 −0.5 −1

q˙1

q˙2

q˙3

q˙4

t in s 0 5·

10−2 0.1

0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65

MMot (normalized) 1 0.5 0 −0.5 −1

M1

M2

M3

M4

t in s 0 5·

10−2 0.1

0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65

Figure 4.7: SCARA4 Minimum-Time Motion (Constrained Motor Torques MMot (t)), Δx = 0. Limits: qi,max : 1.65 rad, q˙1,max = q˙2,max : 2.51 rad/s, q˙3,max = q˙4,max : 5.81 rad/s, M1,max = M2,max : 0.336 Nm, M3,max = M4,max : 0.142 Nm.

The next example will be a variation of the previous, now minimizing the joint velocities (and thus the energy dissipated via viscous friction). Example 4.4: SCARA4 (minimum-energy trajectory planning) Problem Statement Consider the previous Example 4.3 with the cost function rewritten such that the minimum-time trajectory planning task is converted into a minimum-(pseudo-)energy task, i.e. (4.2.4a) becomes minimize JE = w

4  i=1

di d q˙i dq˙i .

110

4 Path and Trajectory Planning

Minimizing JE , i.e. the sum of squares of the control points of the ˙ minimizes the B-spline curves, representing the joint velocities q(t), joint velocities due to the convex hull property. The sum of squared joint velocities is sometimes referred to as kinetic pseudo-energy. Furthermore, the velocities are weighted with the respective viscous friction coefficients di , cf. Rayleigh’s dissipation function. As in the previous example, the terminal time T is left free and will thus be optimally adjusted. Discussion Figure 4.8 shows a typical scenario in which the robot’s EE is synchronized with an object on the conveyor belt. Note, that in this example the farmost path in the x direction is limited by the path constraints (4.2.4i). Furthermore, the minimum-energy paths tend to be longer than the minimum-time ones due to the significantly higher execution times which can be seen in Table 4.2. Again, paths further away from the robot base yield higher energy dissipation due to decreasing execution times. As the rest of the optimization problems is the same, the trajectories for both kinds of constraints are the equal.

x object

detection line

vB

Δx y

terminal configuration q(T ) initial configuration q0

Figure 4.8: SCARA4 Minimum-Energy Trajctory Planning.

4.2 Optimal Point-to-Point Trajectory Planning

111

Table 4.2: SCARA4 Minimum-Energy Trajctory Planning: Results. Δx

JE

T

tCPU

1.00 0.95 0.90 0.85 0.80 0.75 0.70 0.65 0.60

14.56 13.05 11.94 11.10 10.46 9.96 9.60 9.33 9.16

3.307 3.431 3.526 3.600 3.656 3.697 3.726 3.746 3.758

13.83 18.29 31.98 18.30 17.95 18.02 23.93 28.68 31.72

For this example, only the torque-constrained plots are shown in Figure 4.9, as neither the (artificial) acceleration limits, nor the motor torque limits are reached due to the low speed of the operation. Again, note the sudden drops and increases of the motor torques in Figure 4.9. These are a results of stiction effects included in the dynamics model of the robot. q (normalized)

q1

1 0.5 0 −0.5 −1

q2

q3

q4

t in s 0

0.2 0.4 0.6 0.8

1

q˙ (normalized)

1.2 1.4 1.6 1.8 q˙1

1 0.5 0 −0.5 −1

q˙2

q˙3

2

2.2 2.4 2.6 2.8

3

2

2.2 2.4 2.6 2.8

3

q˙4

t in s 0

0.2 0.4 0.6 0.8

1

1.2 1.4 1.6 1.8

112

4 Path and Trajectory Planning MMot (normalized)

M1

1 0.5 0 −0.5 −1

M2

M3

M4

t in s 0

0.2 0.4 0.6 0.8

1

1.2 1.4 1.6 1.8

2

2.2 2.4 2.6 2.8

3

Figure 4.9: SCARA4 Minimum-Energy Motion (Constrained Motor Torques MMot (t)), Δx = 0. Limits: qi,max : 1.65 rad, q˙1,max = q˙2,max : 2.51 rad/s, q˙3,max = q˙4,max : 5.81 rad/s, M1,max = M2,max : 0.336 Nm, M3,max = M4,max : 0.142 Nm.

Example 4.5: Comau Racer R3 (synchronization trajectory planning with parametric sensitivities) Problem Statement In this example that was originally published in the author’s work [116], a rendezvous problem of two identical Comau Racer3 industrial robots A and B is considered, cf. Figure 4.10. In contrast to robot A, robot B is mounted on a linear axis and has therefore an increased work space. Robot B is used to simulate the synchronization target, i.e. it remains in the depicted pose during the operation, only the linear axis moves with constant velocity. Robot A’s task is to perform a minimum-energy motion to synchronize both robots’ end-effectors at the terminal time T = 0.8 s. The motion starts from rest and is acceleration-free at the beginning and the end, ¨0 = q ¨ N = 0. The setup is considered to be used in an i.e. q˙ 0 = q industrial process where the exact rendezvous position and velocity in the direction of the linear axis is only known at run time, immediately before executing the motion. Changes of Δx and Δvy = −0.05 ms , cf. Figure 4.10, result in a perturbation of robot A’s desired terminal state   (qf , q˙ f ) that is represented by the parameters p = p qN p q˙ N . In this particular example, the target moves along a straight line in work space. The terminal joint configuration qf of robot A varies nonlinearly with the perturbation Δx due to the kinematic chain of the serial manipulator.

4.2 Optimal Point-to-Point Trajectory Planning

113

Another consequence is the change of robot A’s terminal joint velocity q˙ f which is influenced by both, the position perturbation Δx and the velocity perturbation Δvy .

vy + Δ Δx B A

Figure 4.10: Comau Racer R3 Trajectory Planning: Experimental Setup.

Thus, linear work space motion of the EE does not constitute a special case. Implementation In the collocation scheme J(w) = minimize minimize w w

N  k=0

q˙ k q˙ k + αM k Mk

s.t. 1qi,min ≤ dqi ≤ 1qi,max

(4.2.5b)

1q˙i,min ≤ dq˙i ≤ 1q˙i,max ...

...

1q i,min ≤ d...qi ≤ 1q i,max

(4.2.5a)

(4.2.5c) ∀i = 1 . . . n

Mk,min ≤ Mk ≤ Mk,max ∀k = 0 . . . N ¨0 ¨0 = q q0 = q0 , q˙ 0 = q˙ 0 , q ¨N ¨N = q qN = pqN , q˙ N = pq˙ N , q

(4.2.5d) (4.2.5e) (4.2.5f) (4.2.5g)

114

4 Path and Trajectory Planning

the joint trajectories qi (t) = n (t)dqi are represented by B-spline curves with a degree of p = 3 and 50 control points and solved for a nominal set of parameters pnom . The cost function as well as the inverse dynamics for the inequality constraints regarding the motor torques M are evaluated 0 for a time grid of tk = kΔt, k = 0 . . . N with Δt = tf −t N . The trajectory planning problem was implemented using CasADi [2] and solved with Ipopt [133]. The cost function J(w) (4.2.5a) penalizes the joint velocities and motor torques with a weight α and is minimized w.r.t. w. Further on, the NLP is subjected to (in)equality constraints (4.2.5b) to (4.2.5g) and depends on the parameters p. The initial and terminal conditions (4.2.5f) and (4.2.5g) can be conveniently expressed in terms of the boundary control points which are interpolated by the B-spline curve. In the above formulation the column vector 1 of appropriate size consists of 1 entries. Due to computation time constraints, an exact solution of (4.2.5) is not feasible, but a nearly-optimal solution can be obtained by means of the improved iterative feedback method from Section 2.1.3. Therefore, an offline optimal solution for nominal parameters pqN = qN and pq˙ N = q˙ N with the corresponding parameter sensitivities in the optimal point is obtained beforehand. While small perturbations have no effect on the active set, for larger perturbations the active set of the optimal solution to the perturbed problem may be different from that of the nominal problem.

4.2 Optimal Point-to-Point Trajectory Planning

115

Results Table 4.3: Comau Racer R3 Trajectory Planning: Results (perturbations Δx, relative cost increase JΔJ , number of iterations). pert

Collocation Δx in m ΔJ/Jpert # iter −0.04 +5.65% 4 −0.02 +2.6% 3 −0.01 +1.48% 2 +0.01 +0.39% 2 +0.02 +5.13% 3 +0.04 +8.35% 4 Due to the particular choice of the cost function and the terminal time, the joint velocities q˙ as well as the motor torques QM rarely reach their respective limits in this example. The limiting factor is found to be the ... joint jerks q. Table 4.3 shows an overview of the results wherein the relative cost increase of J w.r.t. the exact cost function value in the perturbed case Jpert as well as the number of iterations of Algorithm 2 are reported. The method was found to converge for perturbations up to Δx = ±0.04 m in a low number of iterations. The results were successfully validated in experiments featuring the described setup, i.e. the planned trajectories were executed with negligible joint tracking error. Evaluation of the computation time required to solve the above NLP problems reveals that such formulations can hardly be used in practical applications. However, by revisiting the methods to approximate solutions of parametric NLPs discussed in Section 2.1.3 and especially the academic examples considered in Section 2.1.3.4, these trajectory planning examples seem to form a suitable basis for the optimal solution approximations as demonstrated in the above examples.

116

4.2.2

4 Path and Trajectory Planning

Multi-Step Approach

The staggered methodology consists of the following main stages that are executed in an iterative manner: • geometric planning, i.e. determining the geometric path that the EE (and the robot’s joints) will pursue during execution, and • dynamic planning, i.e. finding the time evolution of the joint angles such that the dynamics of the robotic system are considered. Both of the above stages can be further decomposed in substages, e.g. the geometric planning stage may include determining the initial and terminal joint configuration for a given EE pose. That way, the problem complexity of path planning is lowered, allowing efficient joint space formulations of path planning strategies to be applied. Pose Planning Subproblem (Optimal Inverse Kinematics) Before the kinematic aspect of trajectory planning is treated, first the geometric joint path needs to be determined. While in many cases the boundary conditions regarding the joint path, i.e. the initial and terminal joint configurations, are specified, some path planning tasks require computing such a suitable joint configuration for a prescribed EE configuration. Of course, a corresponding joint configuration needs to be admissible w.r.t. the joint limits as well as other geometric constraints such as collisions with the robot’s environment. For kinematically non-redundant robots, the number of possible configurations that meet the EE pose constraints is finite. Furthermore, some of the solutions of a corresponding IK problem violate joint limits and must therefore be excluded from the search space. Other configurations may interfere with the process the manipulator is used in. Due to the low number of admissible solutions, an exhaustive determination of the best, i.e. in some sense optimal, configuration is often indeed viable. However, for manipulators that are kinematically redundant, infinitely many joint configurations correspond to the prescribed EE configuration.

4.2 Optimal Point-to-Point Trajectory Planning

117

In order to select the best, numerical optimization is employed wherein the optimal joint configuration can be selected in order to pursue goals such as maximizing dexterity, centering the joint angles, or maximizing force transfer capabilities, possibly in specified directions. A corresponding optimization problem can be formulated as minimize J(q) q

(4.2.6a)

s.t. zE,d − zE (q) = 0

(4.2.6b)

qmin ≤ q ≤ qmax

(4.2.6c)

h(q) ≤ 0

(4.2.6d)

wherein a pose-dependent quantity captured in J(q) is minimized. The problem is subjected to the forward kinematics constraint regarding the EE pose (4.2.6b), i.e. in general EE position and orientation, as well as box constraints of the joint limits, cf. (4.2.6c). Additionally, further constraints (4.2.6d) may be incorporated, e.g. in order to avoid work space violations of other parts of the robot than the EE. Example 4.6: SCARA4 (terminal configuration) Problem Statement (Part 1) In this example, the EE position is prescribed for which a corresponding joint configuration is sought. While the kinematic redundancy of the considered manipulator increases the difficulty of this positioning problem, it also enables pose optimization regarding specific criteria. In this case it is chosen to maximize the dexterity of the EE in the form / of the kinematic manipulability [139] H(q) = abs det(J(q)J (q)).

118

4 Path and Trajectory Planning

Implementation (Part 1) The NLP problem (4.2.6) thus changes into 



H 2 (q) = abs det J(q)J (q) maximize q

(4.2.7a)

s.t. rE,d − rE (q) = 0

(4.2.7b)

qmin ≤ q ≤ qmax

(4.2.7c)

wherein a joint configuration q is sought that maximizes the squared kinematic manipulability in the objective (4.2.7a). The equality constraint (4.2.7b) enforces the prescribed EE position rE,d and (4.2.7c) are the box constraints of the joint angles. It is assumed that the EE orientation is irrelevant in this example. The situation is depicted in Figure 4.11 wherein one particular optimal pick configuration. 0x

conveyor belt

discretization point

vB

WR

0y

Figure 4.11: SCARA4 Path Planning: Optimal Picking Poses for the SCARA4 Manipulator. Red: Nominal Solutions. Green: Admissible Approximate Solutions.

Results (Part 1) The above NLP problem (4.2.7) is solved for all points on the uniform grid shown in Figure 4.11 to determine the respective optimal joint configuration. As the initial guess, all angles were set to zero (joints centered). Of course, only points within the manipulator’s reachable work space WR , cf. Section 3.3.2, will yield a corresponding joint con-

4.2 Optimal Point-to-Point Trajectory Planning

119

figuration. Due to the manipulator’s dexterity owed to its kinematic redundancy, indeed all discretization points on the conveyor are reachable. However, it is assumed that desired points closer to the origin, will not yield a solution due to the joint angle limitations (4.2.7c). The circles shown in Figure 4.11 represent EE configurations for which an admissible corresponding joint configuration can be found. An implementation that solves the above problem on the test platform takes, 111 ms to 1.55 s (238 ms on average) to converge, rendering this optimization approach unsuitable to act as a part of a real-time path or trajectory planning approach. Problem Statement (Part 2) Next, this example will investigate whether the iterative+ method based from Section 2.1.3 can be applied for such purposes if a solution to (4.2.7) is considered a nominal solution to a perturbed problem. Therefore, the desired EE configuration is regarded as the parameter that is subjected to perturbations. Implementation (Part 2) NLP (4.2.7) is thus rewritten as 



maximize J(q) = H 2 (q) = abs det J(q)J (q) q

(4.2.8a)

s.t. g1 (q, p) = p − rE (q) = 0

(4.2.8b)

g2 (q) = qmin − q ≤ 0

(4.2.8c)

g3 (q) = q − qmax ≤ 0

(4.2.8d)

in which the parameter perturbation occurs linearly in (4.2.8b). This equality constraint as well as the inequality constraints (4.2.8c) and (4.2.8d) are collected in g (q, p) = g 1 g 2 g 3 ≤ 0. Results (Part 2) In order to allow for a comprehensive comparison of the methods discussed in Section 2.1.3, each of the solution points from the first part

120

4 Path and Trajectory Planning

of this example is regarded as a nominal solution. Now the differences to all other points are used as possible pertubations of the desired EE configuration. Intuitively, one may suggest that the closer a desired point is to the nominal position, the more accurate the solution approximation gets which indeed holds for most cases. Two particular cases are depicted in Figure 4.11 where the red points are positions at which a nominal solution was obtained and the sourrounding green points represent all points for which convergence is achieved using the iterative+ method. Summarizing all results, the amount of admissible perturbation points by the iterative approach grows around the center of the manipulator’s work space and thins out towards the bounds where hardly any points are reachable, cf. the outer right solution. The iterative+ method converges for considerable perturbations around a nominal point, as not only the scheme for larger perturbations but also the QP-based approach do not yield admissible results. Two particular situations are shown in Section 2.1.3. The computation time for a successful approximation process using the improved iterative method varies between 3.5 ms and 79 ms with an average of 11.5 ms. Next, the actual path planning process for the synchronization task is considered. Path Planning Subproblem With the initial and terminal configurations either fixed or determined by optimizing the pose, the path planning task is to find an evolution of the joint angles to optimally interpolate these given boundary constraints. Moreover, since in most cases the motion direction at the bounds is defined, also the path derivatives are prescribed and must be fulfilled. In addition, intermediate constraints regarding the work space are typically part of such a planning problem. Such a joint path can be parametrized using a path parameter s ∈ [0, 1] with s = 0 at the beginning and s = 1 at the terminal point in order to synchronize the motion of the robot’s joints.

4.2 Optimal Point-to-Point Trajectory Planning

121

Consider the NLP path planning problem minimize J(q(s))

(4.2.9a)

q(s)

s.t. q(0) = qinit , 

 q (0) = qinit ,   q (0) = qinit ,

q(1) = qterm

(4.2.9b)



 q (1) = qterm  q (1) = qterm

(4.2.9c) (4.2.9d)

.. . qmin ≤ q(s) ≤ qmax ⎛

⎞

x ⎜ min ⎟ ⎜ ⎟ ⎜ ymin ⎟ ⎝ ⎠ zmin

⎛

⎞

x ⎜ max ⎟ ⎟ ⎜ ymax ⎟ ≤ rE (q(s)) ≤ ⎜ ⎝ ⎠ zmax

(4.2.9e) (4.2.9f)

formulated in terms of the joint paths q(s) in which a suitable parametrization w.r.t. the path parameter s is required. While there are many function classes that are capable of smoothly interpolating these points, B-spline curves are a typical choice due to their versatility, cf. Section 2.2. The objective that is minimized in (4.2.9a) is typically a geometric property of the path, e.g. the integral over the path curvature, or the resulting work space path length. The boundary conditions regarding the joint position and its derivatives are considered in (4.2.9b) to (4.2.9d). Constraints regarding higher derivatives can be formulated in a similar manner. Joint limits are incorporated as the box constraints (4.2.9e). The admissible work space is here formulated as being a block-shaped object which is expressed as the box constraints (4.2.9f) in terms of the EE position (forward kinematics). Further constraints may include more complex work space shapes, or work space constraints of other parts of the robot such as intermediate links, and also possible EE orientation limitations for transporting objects that must not be tilted, among others. The following examples show how optimal path planning is performed for pick and place tasks. Therein, not only the often computationally involved exact solution of such problems but also fast solution approximation using parametric sensitivities, i.e. the iterative+ method, is considered.

122

4 Path and Trajectory Planning

Example 4.7: SCARA4 (path planning) Problem Statement and Proposed Solution This example will continue the pick motion planning for the SCARA4 manipulator started in Example 4.6. Assuming the terminal joint configuration is either prescribed or otherwise determined such as in the previous example, the question about an admissible interpolation of the robot’s initial and terminal configurations arises. B-spline curves in terms of the path parameter s are selected as the function class for the joint paths q(s) due to their convenient properties exploitable in constrained approximation as detailed below. The shape of these curves is not only influenced by the values of the respective control points, but also depending on the distribution of knots across the path parameter range. In this example it is assumed to be sufficient to only manipulate the control points by considering them the optimization variables. The robot with n = 4 joints considered in this example is assumed to be rigid, thus paths that are twice continuously differentiable are acceptable. Therefore, a B-spline curve with degree p = 3, NCP = 20 control points and open-uniform knot distribution is used for each joint path. The path planning objective is to generate joint paths with maximum smoothness, i.e. minimizing the highest non-zero derivative.

4.2 Optimal Point-to-Point Trajectory Planning

123

Rewriting NLP problem (4.2.9) yields J(w) = minimize w

n  i=1

d qi (w)dqi (w)

s.t. dq,0 (w) − qinit (p) = 0

(4.2.10a) (4.2.10b)

dq,NCP −1 (w) − qterm (p) = 0

(4.2.10c)

 dq ,0 (w) − qinit (p) = 0

(4.2.10d)

 dq ,NCP −2 (w) − qterm (p) = 0

(4.2.10e)

 dq ,0 (w) − qinit (p) = 0  dq ,NCP −3 (w) − qterm (p)

(4.2.10f) =0

(4.2.10g)

qi,min 1 ≤ dq,j (w) ≤ qi,max 1, i = 1, . . . , n, j = 3, . . . , NCP − 4 (4.2.10h) fx (q(sk )) ≤ xmax

(4.2.10i)

zmin ≤ fz (q(sk )), k = 1, . . . , Nch

(4.2.10j)

with 1 being a vector of ones of appropriate size. Therein the (quadratic) objective is expressed as the squared sum of control points of the highest non-zero derivative spline in (4.2.10a). Note, that this NLP problem is designed to be benign in order to find paths, easily useable with path tracking methods. Furthermore, it should be exploitable by solution approximation methods using parametric sensitivities in order to allow for rapid path planning. Derivatives of B-spline curves w.r.t. the path parameter s yield Bspline curves with control points that are a linear combination of the control points of the original curve. Joint level boundary constraints are therefore trivially expressed as linear equality constraints (4.2.10b) to (4.2.10g) by virtue of the boundary control point interpolation property. The initial conditions are given by the depicted rest position of the manipulator. Therein, the terminal position-level threshold qterm was determined in the previous example. The terminal slope constraint  qterm depends on the joint configuration as well as on the known constant target velocity of the object to be picked. At the initial and the terminal point, the second path derivative is required to be zero, i.e.

124

4 Path and Trajectory Planning

  qinit = qterm = 0. Box constraints regarding the joint positions are formulated in terms of the convex hull property of each joint angle qi (s), i = 1, . . . , n in (4.2.10h). As the present example features a planar path planning problem, the work space constraints (4.2.9f) are reduced to (4.2.10i) and (4.2.10j) wherein f{x,z} represent the respective forward kinematics functions in x and z direction. These are the only nonlinear constraints in the above NLP. They are verified at Nch equally spaced points in the range of the path parameter s ∈ [0, 1]. The initial and terminal constraints are expressed in terms of the parameter vector 

    p = q init qinit qinit q term qterm qterm



that is later used for solution approximation with parametric sensitivities. Results Figure 4.12 shows the grid of reachable picking positions known from the previous Example 4.6 with circles for each position for which an admissible path was found by virtue of (4.2.10). In addition, one particular optimal EE path is shown in red color. It is used as the nominal solution to (4.2.10) that serves as the basis for approximations of optimal paths to other picking position. These are displayed as blue circles in contrast to the black ones that cannot be reached by an approximation. It is apparent, that many picking positions can be reached by an appoximation which is mainly due to the benign NLP problem (4.2.10), regard therefore the objective and the mostly linear constraints. The reasons for failures of convergence are twofold: On the one hand, there is the particular combination of work space constraints and joint limits, on the other hand some of the terminal joint configurations determined in the preceding example are not suitable as some joint angle qi,term may be at its lower or upper bound and the  corresponding joint slope qi,term is non-zero. Although these terminal configurations are admissible w.r.t. the NLP (4.2.8) of the previous example, they have to be effectively discarded. Modification of (4.2.8)

4.2 Optimal Point-to-Point Trajectory Planning

125

with narrowed joint angle bounds may mitigate this issue. Alternatively (or additionally) the objective function may be augmented by a term that penalizes joint displacement and centers the angles, i.e. (4.2.8a) is modified to J(q) = H 2 (q) + αq q with the weight α. 0x

conveyor belt

discretization point

vB

WR

0y

Figure 4.12: Admissible Picking Positions (Black) with One Nominal Path (Red) and All Possible Picking Position Reachable by Path Approximation (Blue).

Figure 4.13 shows all paths obtained by solution approximation with parametric sensitivities using the improved iterative method discussed in Algorithm 2. It is apparent that they show the same principal behavior.

126

4 Path and Trajectory Planning 0x

discretization point

conveyor belt vB

WR

0y

Figure 4.13: Nominal Path (Red) and All Paths to Possible Picking Position Reachable by Path Approximation (Green).

For the nominal solution shown in Figures 4.12 and 4.13, the success rate is 78.8% (152/193 samples) which is the highest of all terminal configurations. An overview of the computational performance in this particular case – the average, median and maximum computation times tCPU,avg , tCPU,med and tCPU,max , respectively, and the average, median and maximum relative cost increases ΔJrel,avg , ΔJrel,med and ΔJrel,max , respectively – can be found in Table 4.4. Most paths can be computed with no or one iterations, which yields low computation times on average and also little to no cost increase. Table 4.4: SCARA4 Path Planning: Computation Time and Cost Increase in Synchronization Tasks (152 Samples).

exact iterative+

tCPU,avg

tCPU,med

tCPU,max

ΔJrel,avg

ΔJrel,med

ΔJrel,max

6.43 s

6.31 s

12.4 s

-

-

-

85.9 ms

4.4 ms

1.8 s

0.7%

0

30.3%

For an overview of the total performance, all feasible 37442 samples were analyzed, returning a success rate of 57.7% (21618/37442 samples). An overview of the total computational performance is shown in Table 4.5.

4.2 Optimal Point-to-Point Trajectory Planning

127

Increased cost, i.e. curvature, is shown for the paths right to the initial position while increase computation times were found for the longer paths ending on the far left of the conveyor. Table 4.5: SCARA4 Path Planning: Computation Time and Cost Increase in Synchronization Tasks (21618 Samples). tCPU,avg

tCPU,med

tCPU,max

ΔJrel,avg

ΔJrel,med

ΔJrel,max

exact

6.38 s

6.24 s

63.7 s

-

-

-

iterative+

99 ms

10.6 ms

3.5 s

26.5%

0

1 031%

Example 4.8: Comau Racer R3 (path planning) sync2

sync1

Figure 4.14: Synchronization Tasks of Two Comau Racer R3 Industrial Robots (adapted from [65] with the author’s permission).

Now the example from [65] is revisited wherein real-time computation of admissible synchronization trajectories for an industrial 6R robot is performed, cf. Figure 4.14. Problem Statement One subtask therein is to perform optimal path planning under geometric constraints such that the joint path curvature is minimized. The joint evolutions are expressed by means of B-spline curves that are parameterized using a path parameter s ∈ [0, 1], i.e. q = q(s). Successful synchronization of a manipulator’s EE with a target imposes require-

128

4 Path and Trajectory Planning

ments on the initial and terminal joint configuration, q(0) and q(1), d d q(0) and ds q(1) as well as on the path curvature on the path slope ds d2 d2 q(0) and q(1), respectively. Note, that in [65] the curvature 2 2 ds ds κ(s) =



1+

d2 ds2 q(s)  2 3/2 d q(s) ds 2

d of a single curve q(s) is approximated with κ(s) ≈ ds 2 q(s) which is also used in the referenced publication to allow for comparison of the results.

Further constraints are imposed on the joint angles due to the joints’ physical turn limits. For collision avoidance, the EE motion is required to be confined within an admissible region such that no collision with the second robot occurs beyond xmax = 0.6 m as well as the floor occurs at zmin = 0.3 m. In the cited work, in order to reduce the computation time, the path planning problem is solved in its unconstrained form as only a QP remains for which a closed-form exists. Admissibility w.r.t. the constraints is then restored with a two-stage iterative process. Firstly, admissibility w.r.t. the joint limits is repaired by iteratively scaling initial and final conditions. Then, the work space limits are considered by iteratively adjusting the curves’ control points. As numerical examples, two synchronization tasks are performed, termed sync1 and sync2. In each task, both robots’ EEs move along predefined, cyclic trajectories, for sync1 a circular EE motion, for sync2 an arc-like motion. At any point in time, the Comau Racer R3 performing either the circular or arc-like EE motion can be commanded to synchronize its EE with the EE of the other robot. Therefore infinitely many possible initial and terminal state pairs for the trajectory of the considered robot exist and it is not reasonable to precompute and store these synchronization trajectories. However, although no issues occurred in the example from [65], this methodology exhibits two shortcomings that possibly limit its applica-

4.2 Optimal Point-to-Point Trajectory Planning

129

bility in practical applications: Firstly, in the second iterative process, the joint limits may become violated. This issue can be mitigated by another pass of the first iterative part which however may destroy the admissibility w.r.t. the work space constraints. As this enhancement is not included in the cited reference, further research may be required to investigate the properties of such a multi-stage iterative approach. Secondly, optimality is generally neglected in the iterative processes and therefore the full potential of the manipulator’s performance is not exploited. Proposed Solution By revisiting the methods to approximate solutions of parametric NLPs discussed in Section 2.1.3, this path planning example seems to be efficiently solvable with the improved iterative approximation approach from Algorithm 2. In order to apply Algorithm 2, first the optimization problem needs to be set up. As in the original example in [65], the B-spline control points serve as optimization variables w, thus dim w = nNCP . Therefore, the NLP problem describing the geometric path planning task reads minimize J(w) = w

n  i=1

d qi (w)dqi (w)

(4.2.11a)

s.t. dq,0 (w) − qinit (p) = 0

(4.2.11b)

 dq ,0 (w) − qinit (p) = 0

(4.2.11c)

dq ,0 (w) −

 qinit (p)

=0

dq,NCP −1 (w) − qterm (p) = 0  dq ,NCP −2 (w) − qterm (p) = 0  dq ,NCP −3 (w) − qterm (p) = 0

(4.2.11d) (4.2.11e) (4.2.11f) (4.2.11g)

130

4 Path and Trajectory Planning qi,min 1 − dq,j (w) ≤ 0

(4.2.11h)

dq,j (w) − qi,max 1 ≤ 0, i = 1, . . . , n, j = 3, . . . , NCP − 4 (4.2.11i) fx (q(sk )) − xmax ≤ 0

(4.2.11j)

zmin − fz (q(sk )) ≤ 0, k = 1, . . . , Nch

(4.2.11k)

wherein the path curvatures is sought to be minimal, cf. the objective function in (4.2.11a). The initial and terminal conditions, cf. (4.2.11b) to (4.2.11g), are conveniently expressed as linear equality constraints (boundary control point interpolation) by means of the parameters 



    p = q init qinit qinit qterm qterm qterm

which are set according to the synchronization task at hand. The joint angle limits are incorporated as (4.2.11h) and (4.2.11i). Note, that only the intermediate control points dqi ,3 , . . . , dqi ,NCP −4 are not influenced by the boundary conditions which specify the first and last 3 control points. The robot’s forward kinematics functions are denoted fx and fz for the inertial x and z direction in the work space constraints (4.2.11j) and (4.2.11k), respectively. Only the NCP − d − 5 knot spans between td+3 and tNCP −3 are not impacted by the boundary conditions and thus need to be checked which is facilitated by means of uniformly distributed check points. Due to this careful selection of constraints, the NLP problem is kept as small as possible. Furthermore, LICQ violations between inequality and equality constraints are avoided but they may arise within the work space limits if both constraints become active in certain poses of the manipulator. However, these cases are practically irrelevant as the EE is unlikely to assume a position along this boundary line as the target path is far away. Results In order to allow for comprehensive evaluation of this example, both, the initial and the target path of the considered manipulators, are discretized in 32 points each and all resulting 1024 combinations of

4.2 Optimal Point-to-Point Trajectory Planning

131

initial and terminal configurations were investigated. The B-spline curves representing the joint angle paths q(s) of degree d = 3, describing the joint paths, have NCP = 18 control points each. In order to provide a reference for all approximate results, exact solutions to the perturbed problems are computed with the nominal solution as the initial guess. Solutions do not exist for all possible combinations due to the joint bounds limiting the admissible terminal joint configuration. Also, some of the results found using this example exhibit joint configuration with one or more joint angles at the respective lower or upper bounds. Such configurations may be problematic for path or trajectory planning as discussed in Example 4.7. Table 4.6 gives an overview of the success rate for both paths. Not all tasks, only 768 out of 1024, were feasible due to the imposed joint angle limits (4.2.11h) and (4.2.11i). However, it can be seen that nearly all of the feasible synchronization tasks can be fulfilled using approximate solutions, i.e. with the iterative+ approach. Note, that admissibility w.r.t. the constraints of the optimization problem is a property inherent to this method. Table 4.6: Success Rates in Synchronization Tasks. sync1 (768 samples)

iterative+

sync2 (768 samples)

# successful

%

# successful

%

741

96.48%

763

99.35%

Tables 4.7 and 4.8 show details of the computation time (average tCPU,avg , median tCPU,med , maximum tCPU,max ) of the exact solution of the path planning tasks, as well as the relative cost increase (average ΔJrel,avg , median ΔJrel,med , maximum ΔJrel,max ) when using the improved iterative approach with one particular set of initial and terminal configuration used as the nominal solution. The iterative+ approach is significantly faster than the exact solution and yields admissible, but suboptimal results. More detailed analysis shows a correlation between computation

132

4 Path and Trajectory Planning

time and relative cost increase, i.e. the longer the methods typically takes to converge, the more expensive the resulting solution is. Note, that the high cost increase in this example is mainly due to the broad variety of requested synchronization paths that are mostly unresembling to the nominal solution, e.g. consider a terminal desired path slope that is directly opposed to that of the nominal solution. Table 4.7: sync1 : Computation Time and Cost Increase in Synchronization Tasks (768 Samples). tCPU,avg

tCPU,med

tCPU,max

ΔJrel,avg

ΔJrel,med

ΔJrel,max

exact

676 ms

658 ms

911 ms

-

-

-

iterative+

11.1 ms

10.6 ms

57.7 ms

467%

203%

14 600%

Table 4.8: sync2 : Computation time and cost increase in synchronization tasks (768 samples). tCPU,avg

tCPU,med

tCPU,max

ΔJrel,avg

ΔJrel,med

ΔJrel,max

exact

712 ms

632 ms

1.19 s

-

-

-

iterative+

4.3 ms

0.9 ms

22.4 ms

174%

26%

60 200%

For practical implementation of the approximation methods, comprehensive investigation of a larger number of possible nominal solutions may be useful in order to improve convergence and optimality as well as to reduce computation times. Multiple exact solutions to perturbed parameter sets can be stored, e.g. at points on a grid of the work space. Then, the closest exact solution to a perturbation can be selected to act as a suitable nominal solution.

4.2 Optimal Point-to-Point Trajectory Planning 0.9

0.8

0.8

0.7

0.7

0.6 0.5

0.5

z

z

0.6

0.4

0.4 0.3

0.3

0.2 0.8 0.6 0.4

0.8 0.6

0.2

0.2

0

y

−0.2

0.2

0.4

0.6

z

z

0.5 0.4 0.3

0.2

y

−0.2

0.4 0.2 x

0

0.6

−0.2

0.2

0.6

0.4

x

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.8 0.6

0

0.2

y

x

0.8

0.4

0.4

0.6

0.7

0.2 0 0.6

133

0.4

0.2

y

0 −0.2

0.2

0.4

0.6

x

Figure 4.15: Synchronization Tasks of Two Comau Racer R3, sync1 : Nominal Path (Red) and Some Approximations of Optimal Paths (Blue). Measures in m.

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 1 0.8

z

4 Path and Trajectory Planning

z

134

0.6

0.4

0.2

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 1 0.8

0.6

0.4

y

0.6

0.6

x

0.2

0 0.2 0.4

x

0.4

0.2

y

z

z

y

0 0.2 0.4

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 1 0.8

0.6

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 1 0.8

0.6

0.4

y

0 0.2 0.4

0.6

x

0.2

0 0.2 0.4

0.6

x

Figure 4.16: Synchronization Tasks of Two Comau Racer R3, sync2 : Nominal Path (Red) and Some Approximations of Optimal Paths (Blue). Measures in m.

As demonstrated in these optimal control examples, due to the high computational burden and the resulting notable computation time, these approaches are hardly useful in real-time applications. Instead, methods that yield faster convergence are required, possibly at the cost of exactness of the solution regarding optimality. These approaches feature a trade-off between reduced computation time and quality of the resulting trajectory w.r.t. the trajectory optimization goal. The iterative+ approach has been confirmed a viable method to achieve this goal. Online solutions are required in cases in which the task to be executed is only known shortly before its execution, leaving only little time for optimal trajectory planning. Examples are pick & place application where the location of the parts to be picked is not known in advance, or general changes to the operation environment such as unpredictably moving work space boundaries occur.

4.2 Optimal Point-to-Point Trajectory Planning

135

Path Tracking Subproblem In the second stage, the previously obtained geometric path is used as the basis to find a suitable time evolution of s = s(t) that considers the dynamic requirements. The topic of path tracking is discussed in Section 5.1 of this thesis.

4.2.3

Summary

Summarizing, an OCP such as (4.2.1) includes both, geometric and dynamics planning subtasks, it represents a trade-off between the quality of results and its use for real-time applications. Note, that such a staggered strategy, although computationally efficient, may yield different, possibly degraded results in terms of the optimization goal compared to those of comprehensive single-step methods. This is an effect of the optimization goal of the kinematic part often being different than that the originally desired one as no time-dependent goals (such as minimum traversal time) or dynamic objectives can be pursued. The examples in this chapter have shown, that the iterative+ method based on parametric sensitivities is especially suitable for fast path and trajectory re-planning.

Chapter 5

Optimal Path Tracking This chapter discusses the important task of optimal path tracking, i.e. situations where the path of the is prescribed while its EE trajectory is not. Therein, joint space and work space path tracking approaches are distinguished, cf. Section 5.1 and Section 5.2, respectively. In the former case, the sequence of a robot’s joint positions is given and a corresponding temporal interpolation is required to be found such that a given criteria is optimal. Kinematic redundancy is irrelevant for joint space path tracking approaches. In the latter setting, a robot’s EE follows a prescribed path. Here the property of kinematic redundancy is crucial for the complexity of path tracking methods. For non-redundant manipulatators usually the path tracking problem can be transformed into a more simple parameterized joint space formulation, cf. Section 5.2.1. Redundant structures prove to be more challenging in this regard, cf. the IK schemes from Section 5.2.2. In this thesis, the discussion about path tracking methods concerns mainly kinematically redundant manipulators. An additional family of methods can be summarized as tube following, e.g. [38, 94]. Therein, although an EE path is prescribed, slight deviations from it are allowed in order to increase the task execution performance, e.g. reduce the execution time. However, such approaches cannot be strictly regarded as path tracking methods, but may be termed path following.

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4_5

138

5.1

5 Optimal Path Tracking

Joint Space Approaches

A very early work that investigates exploiting a robot’s capabilities in joint path tracking is [61]. Therein, a joint trajectory q(t) is prescribed and a time warp t(τ ), i.e. a reparameterization of time t, is used such that the robot’s limits such as those of motor speeds and velocities are not violated. If time-optimality is required, the methods from [102, 101] can be employed. The latter guarantees successful solution in case of a well-posed path tracking problem. However, it requires a readily available parameterization of the joint paths in terms of a path parameter s, i.e. q(s). Finally, the temporal evolution of s(t) is determined such that the resulting joint angle trajectory q(s(t)) is optimal. The path tracking problem in joint space is not in the scope of this thesis.

5.2

Work Space Approaches

In this section, approaches are discussed in which a path for a manipulator’s EE is given, e.g. the EE orientation R0E and the EE position rE are prescribed in terms of a path parameter s. The robot is required to track this path such that not only admissibility but also optimality criteria are satisfied.

5.2.1

Kinematically Non-Redundant Manipulators

A key element in solving the optimal path tracking problem is the solution of the IK problem. For kinematically non-redundant manipulators, a given work space path can be straightforwardly converted into a joint space path by means of IK. After the IK resolution step, the problem can be solved with joint space trajectory planning techniques discussed in the previous Section 5.1. Therein, a phase-plane representation of the path tracking task can be formulated, yielding a convex optimization problem [132]. Furthermore, dynamic programming [93, 64] can be employed to find the optimal path parameter trajectory.

5.2 Work Space Approaches

139

As the present thesis is focused on path tracking for kinematically redundant manipulators, investigating this type of path tracking is not one of its aims.

5.2.2

Kinematically Redundant Manipulators

Kinematically redundant manipulators are increasingly employed in path tracking applications because of their larger work space and better kinematic dexterity compared to non-redundant robots, and the ability to avoid singularities. In [82, 88] the path tracking problem for kinematically redundant manipulators is formulated in terms of a two point boundary value problem. Therein, a PG method locally maximizing the kinematic manipulability is employed that allows to reduce the dimension of the problem which is then solved with calculus of variations. Further approaches using PG methods in combination with a directed manipulability index are found in [30, 28, 142]. As discussed in Section 3.5.3, it is difficult to select an appropriate performance index that locally corresponds to the global trajectory optimization goal. JSD was previously applied for time-optimal path tracking in [50, 135]. However, these phase-plane-based methods exhibit severe shortcomings as. For example, the terminal joint velocities at the end of a path cannot be prescribed, i.e. internal motion at the terminal point cannot be avoided. Furthermore, the approaches were only applied to manipulators with degree of redundancy r = 1 which does not confirm their general applicability. In [8], the dimension of the optimization problem is reduced by employing pseudoinverse-based IK methods which gives rise to the application of phase plane path tracking methods. Therein, several simplifications are made such as leaving out velocity-dependent and gravitational terms in the dynamics model of the considered manipulators. In the recent multi-step approach [1], first the IK problem is solved on a pure geometric level, i.e. without time-dependency, in a PG-like manner in order to obtain a joint path parameterization q(s). Then, standard time-optimal planning, c.g. [14, 102, 101] is applied to find a suitable s(t). However, in the derivation only the EE position is considered and

140

5 Optimal Path Tracking

also in the included examples no computation times are reported. It is especially unclear how the method could be applied in a case in which the EE orientation is required as well. Thus it is unclear if the presented approach can be applied in a practical example. Time-optimality is an important optimization criterion because of economical reasons. When executing such minimum-time trajectories, the robot’s capabilities are typically used to their full potential. As a consequence, additional, possibly previously unaccounted effects such as oscillation due to excited structural elasticity appear, cf. Example 5.2. As argued in [7, 12], higher-order continuity of trajectories is relevant for avoiding undesired vibrations and mechanical stress in the structure of the manipulator. In this section, the path tracking problem for kinematically redundant manipulators will be investigated. Therein, IK methods discussed in Section 3.5.3, namely JSD and GNA, are applied in order to resolve the redundancy. Thus, the actual motion of the EE along the prescribed EE path, and hence of the robot, can be determined so to be optimal in some sense. Using the time needed to accomplish the motion as the optimality criterion, two examples are used to show the efficacy of the proposed approaches. Minimum-time Path Tracking In order to express minimum-time path tracking as an OCP, (2.3.1) is rewritten as &T

minimize x(t),u(t),T

1dt = T

(5.2.1a)

0

s.t. x0 − x(0) = 0

(5.2.1b)

r(x(T )) ≤ 0

(5.2.1c)

˙ x(t) − f (x(t), u(t)) = 0

(5.2.1d)

h(x(t), u(t)) ≤ 0, ∀t ∈ [0, T ]

(5.2.1e)

While in the examples in the previous chapter a B-spline-based approach was selected, here the respective implementations of the optimization prob-

5.2 Work Space Approaches

141

lems closely feature the direct multiple shooting approach discussed in Section 2.3.2. To this end, the time domain [0, T ] is discretized into N uniform intervals [tk , tk+1 ], k = 0, . . . , N − 1 with t0 = 0 and tN = T . A shorthand notation is used such that (·)k = (·)(tk ). The joint position/angles q(t) and the path parameter s(t) are represented by the optimization variables 



w = x 0 u 0 x 1 u 1 · · · x N u N T . Note, that the time grid is being scaled when T changes in w. The choice of the IK resolution method determines the state vector x and the control vector u, e.g. in the case of C 4 trajectories x k = ... (3) (4) (4) (5) (5) ¨ r,k , q r,k , qr,k ), u k = (sk , qr,k ) for JSD, and (sk , s˙ k , s¨k , sk , sk , q k , q˙ r,k , q ... (3) (4) (5) (4) ¨ k , γ k ), u k = (sk , γk ) for GNA. x k = (sk , s˙ k , s¨k , sk , sk , q k , γ k , γ˙ k , γ Therein, the IK expressions (3.5.34) and (3.5.36) for JSD and (3.5.47) and (3.5.51) for GNA are used. The control trajectories are piecewise constant. Therefore, the NLP problem corresponding to minimum-time path tracking reads minimize T w

(5.2.2a)

s.t. s0 = 0, sN = 1 (i) q0

=

(i) (i) q0 , qN

=

(5.2.2b) (i) qf , i

= 0, . . . , ν

(5.2.2c)

xk+1 − RK4(xk , uk ) = 0

(5.2.2d)

IK: JSD or GNA

(5.2.2e)

0 ≤ sk ≤ 1

(5.2.2f)

s˙ k ≥ 0

(5.2.2g)

(i)

(i)

(i) , i = 0, . . . , ν + 1 qmin ≤ qk ≤ qmax

(5.2.2h)

τk = M(qk )¨ qk + g(qk , q˙ k )

(5.2.2i)

τmin ≤ τk ≤ τmax

(5.2.2j)

wherein the terminal time T is minimized, c.f. (5.2.2a).

142

5 Optimal Path Tracking

The initial and final conditions regarding the path parameter and the joint coordinates are given by (5.2.2b) and (5.2.2c). Therein, the overlined quantities, e.g. qf , represent desired values at the respective bounds. The equality constraint (5.2.2d) enforces the compatibility at the boundaries of the shooting intervals. On each interval, the state xk is time integrated under the influence of the control uk . In this example, this is facilitated by means of a fourth-order Runge-Kutta scheme denoted RK4. Choosing an IK method used for path tracking, i.e. JSD or GNA, is reflected in (5.2.2e). (5.2.2f) and (5.2.2g) constrain the time evolution of the path parameter s(t). (5.2.2h) represents box constraints imposed on the joint coordinates and its time derivatives denoted with their respective orders i = 0 for ˙ q(0) = q(t), i = 1 for q(1) = q(t), etc. The system dynamics is considered as the robot’s equations of motion in inverse dynamics form (5.2.2i) and the torque constraint (5.2.2j). Initial Guess The above NLP problem (5.2.2) is in general non-convex and nonlinear due to the system dynamics and the effect modeled therein. Therefore, the selection of an initial guess is important, i.e. the closer it is to a solution to the problem, the lower the computational load. Admissibility w.r.t. the constraints also improves the quality of an initial guess. A possible choice for the initial guess is to compute the joint angles by time integration of the non-augmented IK solution (4)

(3)

(3)

(3)

¨kq ˙ k − 3J ¨ k − 3J˙ k qk ). qinit,k = J+ k (VE,d,k − Jk q Thereby, the path tracking requirement is fulfilled, and also the box constraints regarding the joint quantities and the motor torques (5.2.2h) to (5.2.2j) are satisfied when initialized with a large enough terminal time T . This also yields the initial guess for the redundant coordinates qr and their time derivatives for JSD. For GNA, the scaling factors γ are initialized with zeros.

5.2 Work Space Approaches

143

Example 5.1: SCARA4 (higher-order path tracking for kinematically redundant manipulators) Task Description In this example, which was published in the author’s previous publications [114, 115], the kinematically redundant SCARA4 lab prototype from Figure 5.1 is considered. The goal is to compute joint trajectories q(t) with a continuity requirement up to C 4 such that the EE performs a minimum-time rest-to-rest motion, tracking the quarter circle path shown in Figure 5.1 in counterclockwise direction. Therein, JSD as well as the GNA approach will be used to formulate the IK as a means to satisfy the path following requirement. y terminal configuration q(T )

path rE (s)

x

initial configuration q0

Figure 5.1: SCARA4 Minimum-Time Path Tracking.

In order to reduce complexity, only the planar motion of the manipulator is relevant for this trajectory planning task. The four-link manipulator is inherently kinematically redundant which is further increased as only the EE position coordinates and not the orientation are considered task space coordinates of the path tracking task at hand, i.e. the degree of redundancy is rE ∈ T ⊂ R2 with l = dim T = 2 < n. The path is

144

5 Optimal Path Tracking

specified as desired EE position in terms of the path parameter s ∈ [0, 1], i.e. rE (s). The forward kinematics is given by rE (q). To that end, the EE twist has only translational components, i.e. VE = vE and the Jacobian yields J = ∂∂vq˙E . Optimization Problem The formulation of the optimization problem reflecting the problem described above follows the direct multiple shooting approach (5.2.2) using N = 200 uniform time intervals. Therein, the boundary conditions for the path parameter trajectory are specified in (5.2.2b). As a restto-rest motion is required, the initial and final constraints (5.2.2c) are (3) (4) (3) (4) ¨ 0 , q0 , q0 , q˙ N , q ¨ N , qN , qN = 0. The specified as q0 = q0 and q˙ 0 , q final joint configuration qN is subject to the optimization. In (5.2.2h), q and its time derivatives are constrained except for the acceleration which is indirectly limited by the torque constraints (5.2.2i) and (5.2.2j). Regarding the IK resolution, for JSD all six possible decompositions of the joint set are tested, as well as GNA. Initial Guess For the present example, a B-spline curve of degree 6 is used for s, the duration of the trajectory is conservatively initialized with tf,init = 3 s. Results The optimization problem (5.2.2) was implemented using CasADi [2] and solved with Ipopt. Table 5.1 shows the trajectory planning results for the above example. Therein, optimal trajectory durations for all joint space decompositions as well as for GNA are reported. The numbers denote the choice of redundant joints, i.e. JSD (1,2) means q r = (q1 , q2 ). Additionally, the corresponding computation times tCPU are listed. It should be mentioned that the resulting minima may not be global as (5.2.2) is non-convex.

5.2 Work Space Approaches

145

Figures 5.2 to 5.4 show the optimal C 4 time evolutions of the path parameter s, the joint angles q and the motor torques τ , respectively. Note that the axes of all joint quantities are normalized w.r.t. their respective limits and the time axes are normalized w.r.t. T . The visible steps in the motor torques in Figure 5.4 are due to Coulomb friction effects, i.e. they occur at sign changes of the corresponding joint velocities, c.f. Figure 5.3. The optimal final configuration of the robot in the C 4 case is depicted in Figure 5.1. The time evolutions are only reported for GNA as the results for JSD are very similar which is mainly due to the common initial guess. As expected, the trajectory duration is slightly increased with the order of continuous differentiability. The optimal trajectory duration is very similar in all cases of a particular choice. The convergence, i.e. the number of iterations, is similar for JSD and GNA. However, the computational burden and thus the solution time is significantly higher for GNA due to the complexity of the IK expressions. Not all joint space decompositions yielded an admissible result. For the decomposition with q r = (q2 , q4 ) the Jacobian Jnr is singular in the initial configuration. JSD switching algorithms that choose a suitable separation and thus avoid algorithmic singularities should be therefore investigated. The optimization problem with q r = (q3 , q4 ) does not converge within 1000 iterations. 1 0.5 0 −0.5 −1

s

s

2 · 10−1 s˙

4 · 10−2 s¨

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Figure 5.2: SCARA4 Minimum-Time Path Tracking: Path Parameter s and Derivatives.

1

146 1 0.5 0 −0.5 −1

1 0.5 0 −0.5 −1

5 Optimal Path Tracking q/qmax

q1

q2

q3

q4

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

˙ q˙ max q/

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(3)

1 0.5 0 −0.5 −1

q(3) /qmax

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(3)

1 0.5 0 −0.5 −1

q(3) /qmax

t/tf 0

0.1 (5)

1 0.5 0 −0.5 −1

q(5) /qmax

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Figure 5.3: SCARA4 Minimum-Time PathTracking: Joint Angles q and Derivatives. (3) Symmetric Limits: qi,max = π2 rad, q˙ max = π2 , π2 , π, π rad/s, qi,max = (4)

(5)

103 rad/s3 , qi,max = 104 rad/s4 , qi,max = 105 rad/s5 . 1 0.5 0 −0.5 −1

τ /τmax

τ1

τ2

τ3

τ4

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

5.2 Work Space Approaches

147

Figure 5.4: SCARA4 Minimum-Time Path Tracking: Motor Torques τ . Symmetric  Limits: τmax = (0.336, 0.336, 0.142, 0.142) Nm. Table 5.1: SCARA4 Minimum-Time Path Tracking: Results (times in seconds).

Mode JSD (1,2) JSD (1,3) JSD (1,4) C2 JSD (2,3) JSD (2,4) JSD (3,4) GNA

T #iter tCPU 0.9635 199 31.1 0.9635 215 33.7 0.9635 170 26.0 0.9635 210 32.6 infeasible no convergence 0.9635 166 69

Mode JSD (1,2) JSD (1,3) JSD (1,4) C3 JSD (2,3) JSD (2,4) JSD (3,4) GNA

T #iter tCPU 1.0030 203 43.4 1.0030 197 42.2 1.0030 225 47.9 1.0030 138 29.4 infeasible no convergence 1.0030 148 327

Mode JSD (1,2) JSD (1,3) JSD (1,4) C4 JSD (2,3) JSD (2,4) JSD (3,4) GNA

T #iter tCPU 1.0747 245 67.9 1.0747 221 61.2 1.0748 216 58.8 1.0747 240 63.4 infeasible no convergence 1.0747 279 5578

148

5 Optimal Path Tracking

Example 5.2: Stäubli TX90L (path tracking for kinematically redundant manipulators) Problem Statement Consider the redundant 7 DOF manipulator in Figure 5.5. It will serve as another use case of higher order optimal path tracking. This example has been largely adopted from the author’s previous publications [114, 115]. tool axis

Figure 5.5: Stäubli TX90L Minimum-Time Path Tracking (from [114]).

The manipulator consists of a 6 DOF industrial robot (Stäubli TX90L) mounted on a linear axis. The EE is required to move along an ellipse as shown in Figure 5.5. The tool axis is required to stay perpendicular to the ellipse, but the EE can freely rotate about the tool axis. This corresponds to a five axis manipulation task, such as grinding, milling, spray painting, or gluing. The robot is operated with a model-based control scheme consisting of a nonlinear feed-forward and linear feedback control. A time optimal solution was computed, where the joint trajectories are required to be C 2 continuous, i.e. twice continuously differentiable. This solution trajectory satisfies the limits on velocity, acceleration, and actuator forces/torques according to the robot specification (reduced to 90% to account for some control margin).

5.2 Work Space Approaches qlag,1

q1

2

149 q2

q3 qlag,1 q4 q5

q6

qlag,2...7 200

q7

1

100

0 −1 5 · 10−2

0

0.1

0.15

0.2

0.25

0.3

0 −100 t in s

Figure 5.6: Stäubli TX90L Minimum-Time Path Tracking, Experiment C 2 Trajectories: Tracking Error of Joint Positions qlag , qlag,1 in mm, qlag,2...7 in mdeg. τ /τmax

τ1

τ2

τ3

τ4

τ5

τ6

τ7

1 0 −1

t in s 0

5 · 10−2

0.1

0.15

0.2

0.25

0.3

Figure 5.7: Stäubli TX90L Minimum-Time Path Tracking, Experiment C 2 trajectories: Measured Motor Torques τ . Symmetric torque limits according to the robot specification.

The problem becomes obvious from Figure 5.6 that shows the tracking error qlag = qd −q with qd being the desired joint position. More crucial than this low tracking accuracy is the violation of the actual motor torque limits as apparent from Figure 5.7 that causes an emergency stop of the robot at around 0.07 s. The latter is due to the bounded but discontinuous jerk and the unbounded jounce of the trajectories. This example investigate the application of trajectories with higher continuity such that above problems are avoided. Task Description The goal is to determine joint trajectories such that a prescribed elliptic EE path is tracked in a minimum time rest-to-rest motion while the last axis of the spherical wrist (tool axis) is required to stay perpendicular to the ellipse (the EE is free to rotate about this axis). The path is defined in closed-form in terms of a path parameter s ∈ [0, 1]. The joint trajectories are required to be continuously differentiable up to four times

150

5 Optimal Path Tracking

in order to avoid undesired excitation of elasticity effects in the robot structure. The manipulator depicted in Figure 5.5 consists of a serial chain with one prismatic joint and six revolute joints, i.e. C = R × T6 . The corresponding joint coordinate vector is q = (q1 , . . . , q7 ), and n = dim C = 7. The EE pose is described by coordinates zE = (R0E , rE ) ∈ SE(3), and the work space is W ⊂SE(3), m = dim W = 6. The manipulator is considered kinematically redundant with degree (n − m) = 1. The task space T is of dimension l = dim T = 5 as the EE position and only the rotation about two axes are considered. Therefore, the robot is task redundant with degree rT = (n − l) = 2. For convenience, the angular EE velocity is expressed in the local coordinate frame where only two components are relevant, i.e. E ω E = (ωx , ωy ).   Thus the EE twist V E = E ω E , v E consists of only five elements. Optimization Problem See the previous Example 5.1. Initial Guess The initial guess is computed as in the previous Example 5.1 with the terminal time conservatively initialized to Tinit = 5 s. Results The optimization results for the spatial example are found in Table 5.2. JSD was performed for all 21 possible combinations of redundant coordinates. Table 5.2 only shows the results for separations with the largest (worst) and the smallest (best) final time. Not all choices converged, some encountered algorithmic singularities. The corresponding occurrences as well as all computation times tCPU are also listed. Again, note that the resulting minima may not be global as (5.2.2) is non-convex. The NLP problem (5.2.2) was implemented using CasADi 3.1 [2] available at the time of the initial submission of [115] and solved with Ipopt. Computation times for GNA are higher than for JSD as the evalua-

5.2 Work Space Approaches

151

tion of the constraint functions, especially the IK, is computationally expensive. The C 2 and C 3 trajectories obtained by JSD and GNA are very similar which is mainly due to the common initial guess. Figures 5.8 to 5.10 show the optimal C 4 time evolutions of the path parameter s, the joint angles q and the motor torques τ , respectively, that were obtained with JSD. Note that the axes of all joint quantities are normalized w.r.t. their respective limits as are the time axes w.r.t. T . The visible steps in the motor torques in Figure 5.10 are again due to Coulomb friction effects, i.e. they occur at sign changes of the corresponding joint velocities, c.f. Figure 5.9. The C 4 trajectory obtained using JSD was experimentally verified using the robot depicted in Figure 5.5 that uses PD joint control with feedforward torques. The recorded lag error shown in Figure 5.11 is deemed negligible for most applications, therefore the trajectory planning result is admissible. Note that these results heavily depend on the accuracy of the model used to compute the feed-forward torques by means of inverse dynamics. Therein, knowledge of accurate model parameters is crucial. Details about the identification process for the robot considered in this example are found in [91]. This example indicates the importance of a certain continuity of the time optimal trajectory. The necessary order of continuity of the joint trajectories, and thus of the EE trajectory, can be deduced by regarding the robot as a dynamical control system. A realistic model of an industrial robot is that of a rigid link manipulator with serial-elastic actuators, i.e. the links are modeled as rigid bodies connected by ideal joints to which elastic motor/gear units are attached [118]. This is a differentially flat control system with the EE position as its flat output. It was shown that it is differential and feedback linearizable [34, 52, 97]. Moreover, the vector relative degree is 4. Consequently, the joint (and thus EE) trajectory and its time derivatives up fourth order determine the actuator forces/torques. In other words, the prescribed trajectories must be C 4 continuous for the actuator forces/torques to be continuous.

152

5 Optimal Path Tracking

If C 4 continuity is not satisfied, then vibrations of the elastic components of the robot (gear/motor unit) will be excited. Time-optimal trajectories make full use of the dynamic capabilities of the robot so that actuator forces/torques, determined from the feed-forward control, are close to the robot-specific limits. When vibrations excited, due to insufficiently continuous trajectories, feedback control forces/torques occur in addition to the feed-forward terms (which are already close to the respective limits). As a further consequence, the path tracking performance is degraded. This can be seen in Figure 5.7 normalized w.r.t. the specified limits. 1 0.5 0 −0.5 −1

s

s

4 · 10−1 s˙

5 · 10−2 s¨

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Figure 5.8: Stäubli TX90L Minimum-Time Path Tracking: Path Parameter s and Derivatives. 1 0.8 0.6 0.4 0.2 0

(q − qmin )/(qmax − qmin )

q1

q2

q3

q4

q5

q6

q7

t/tf 0

1 0.5 0 −0.5 −1

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

˙ q˙ max q/

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(3)

1 0.5 0 −0.5 −1

q(3) /qmax

t/tf 0

0.1

5.2 Work Space Approaches

153

(4)

1 0.5 0 −0.5 −1

q(4) /qmax

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(5)

1 0.5 0 −0.5 −1

q(5) /qmax

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Figure 5.9: Stäubli TX90L Minimum-Time Path Tracking: Joint Angles q and Derivatives Normalized w.r.t. Their Respective Limits (q1,min = 0, q2,...,7,min = (−π, −2.3, −2.5, −4.7, −2, −3.5) rad, q1,max = 2.55 m, q2,...,7,max = (π, 2.6, 2.5, 4.7, 2.4, 3.5) rad. Symmetric derivative limits: (3) (4) (5) q˙ max = (4, 7, 7, 7, 8.7, 7.9, 12.6) rad/s, qi,max = 103 rad/s3 , qi,max = 104 rad/s4 , qi,max = 5 5 10 rad/s .). 1 0.5 0 −0.5 −1

τ /τmax

τ1

τ2

τ3

τ4

τ5

τ6

τ7

t/tf 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Figure 5.10: Stäubli TX90L Minimum-Time Path Tracking: Motor Torques τ .  Normalized w.r.t. Symmetric Limits: τmax = (69, 42, 42, 17.5, 4.5, 3.4, 2.2) Nm. qlag,1...2

q1

q2

qqlag,1 q4 qlag,2 q5 3

q6

qlag,3...7

q7

1000

100

0

0

−1000 0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

−100 t/tf

Figure 5.11: Stäubli TX90L Minimum-Time Path Tracking: Tracking Error of Joint Positions qlag , qlag,1 in μm, qlag,2...7 in mdeg.

154

5 Optimal Path Tracking

Table 5.2: Stäubli TX90L Minimum-Time Path Tracking: Results (times in seconds).

C2

Mode JSD (best) JSD (worst) JSD (issues) GNA

C3

Mode JSD (best) JSD (worst) JSD (issues) GNA

C4

Mode JSD (best) JSD (worst) JSD (issues) GNA

T #iter tCPU 0.7557 (1,2) 398 551.0 0.7601 (3,5) 256 291.5 (1,{3,4,6}), (2,{3..7}), (3,{4,6}), (4,6), (5,{6,7}), (6,7) 0.7586 248 2521 T #iter tCPU 0.8827 (1,5) 1045 1854.4 0.9037 (2,3) 533 1307.6 (1,{3,4,6,7}), (2,{5,6,7}), (3,{4..7}), (4,{6,7}), (5,{6,7}), (6,7) 0.8827 587 14287 T #iter tCPU 1.0253 (1,7) 152 319.2 1.0812 (1,2) 261 545.7 (1,{3,4,6}), (2,{5,7}), (3,{4,6}), (4,6), (5,{6,7}), (6,7) 1.094 270 2515

For GNA, due to the problem size in the C 4 case, cf. the IK expressions (3.5.51), a limited-memory quasi-Newton approximation of the Hessian matrix was used instead of exact derivatives. Convergence was achieved for relaxed optimality tolerances.

Chapter 6

Conclusion As its title suggests, this thesis is concerned with optimal path and trajectory planning for serial manipulators. For the treatment of this subject, the mathematical background regarding numerical optimization and therein especially parametric problems, B-spline curves and optimal control problems is required which is provided in this thesis. In addition, robot kinematics, i.e. forward and inverse kinematics on position, velocity and higher derivative level, is also examined rigorously in this thesis as a preparation for more advanced topics. On the one hand, point-to-point tasks are treated such that trajectories are computed that are optimal in some given sense, e.g. so that a robot consumes minimum energy during a transportation task. On the other hand, optimal trajectory planning includes optimal path tracking tasks in which the task space is reduced to a prescribed path, i.e. the EE is required to track a given path in minimum time. Therein, inverse kinematics is used to resolve the path given in terms of EE coordinates into joint coordinates. For kinematically redundant robots this is especially of interest as this class of robots exhibits internal motion capabilities which is optimally exploitable in that regard.

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4_6

156

6 Conclusion

Optimal Path and Trajectory Planning The present thesis covers particular aspects of robotic path and trajectory planning. Therein, especially point-to-point problems are considered for which initial and final conditions are specified along with technological limits of the robot employed in a process or the process itself. To that end, the corresponding numerical optimization problem may either cover the entire process at once, i.e. the inputs are only initial and final conditions as well as technical constraints and the result are joint trajectories, cf. Examples 4.3 and 4.4. Or, as an alternative, a multi-step approach can be pursued wherein the geometric and the dynamics aspects are treated subsequently. In the first, geometric part of this iterative process, path planning is performed, consider for instance Examples 4.6 and 4.7. Therein for a kinematically redundant manipulator first the terminal configuration is determined by maximizing a joint configuration-dependent performance index, and then a geometrically admissible joint path is planned from a given initial configuration. Therefore, the geometric initial and final conditions as well as geometric work space constraints are required. In the second step, the robot’s EE is required to track the path which gives rise to path tracking methods depending on the type of robot employed in the process. Therein, the constraints regarding kinematics, e.g. velocities, and dynamics, e.g. input torque, are required. The process is complete once an admissible, optimal solution is found. Otherwise, the geometric path needs to be re-planned or the optimization problem relaxed. In this context, also the topic of fast computation of approximations to optimal solutions using parametric sensitivities is discussed. Therein, given a solution to an optimization problem for a nominal set of parameters, parametric sensitivities are employed to compute an approximation for the case of parameter perturbations. A novel iterative method is proposed that allows large perturbations that inflict a change of the active set of constraints compared to the nominal problem. This represents an advantage over established method which is shown in comparative examples. Moreover, the efficacy of fast planning methods using this iterative approach is shown by means of numerical examples in simulations and experiments. Future

6 Conclusion

157

work will investigate whether the methods described in [108] can improve the convergence of the presented active set update approach.

Optimal Path Tracking for Kinematically Redundant Robots In this thesis, two methods regarding inverse kinematics in optimal path tracking for kinematically redundant robots are presented: JSD and GNA. While the former is an existing approach and known for simple cases, it is extended in the present thesis and the author’s previous publications. The latter is a novel method that was proposed by the author. While in JSD the joint set is separated into two parts, allowing a parameterized IK solution at modest computational expense, there are several shortcomings. A notable drawback therein are algorithmic singularities, rendering particular decomposition combinations unusable. An open challenge is a switching strategy between various decomposition combinations during an optimization process if such an algorithmic singularity occurs, resulting in a multi-stage optimization scheme. The challenge therein lies in the initialization of subsequent stages due to major differences between the formulation arising from different decompositions. GNA allows IK treatment on derivative-level. Therein, an IK solution is augmented by weighted nullspace basis vectors whose weights are computed to be optimal w.r.t. the objective of the path tracking problem. While the methods directly exploits the redundancy and does not suffer from algorithmic singularities, its computational demands are higher than those of JSD which is subject to further research. The efficacy of these approaches is shown using numerical examples and experiments. Furthermore, regarding high-performance path tracking applications, this thesis addresses the IK methods JSD and GNA in higher derivative-level formulation, i.e. on acceleration, jerk, or jounce-level, etc. These particular methods are applied in order to ensure continuous differentiability of the resulting joint trajectories and thus avoid unnecessary excitation of elastic effects unmodeled in the system dynamics. For both considered IK methods, JSD and GNA, the required computational effort and therefore time required

158

6 Conclusion

to compute a solution grows with the derivative order. This can be possible overcome by a recursive IK formulation. Alternatively, this task can be formalized with a modified OCP, cf. Section 2.3 based on the system’s forward dynamics. Note, that this approach requires time derivatives of the EoM which is computationally involved as well. Comparative investigations about the computational trade-off between an OCP formulation and an IK-based approach as proposed in this thesis are an open challenge. Dynamic programming can also be used to express and solve such path tracking problems. Such approaches are successfully applied for path tracking with non-redundant robots as the dynamics can be uniquely re-written in terms of a path parameter, reducing the problem to finding a single trajectory. For kinematically redundant structures this is not straightforwardly possible, as the IK resolution is ambiguous and only a parameterized solution can be obtained. In both, the JSD and the GNA methods, the number of states grows linearly with the degree of kinematic redundancy. However, in dynamics programming, the computational complexity grows exponentially with the number of states which may limit this idea’s practical feasibility, especially regarding higher derivative levels. In this thesis, single, stationary, serial robots are considered. Future investigations might include collaboration tasks of multiple serial robots, forming a kinematically parallel structure. Therein, the concept of nullspace augmentation by weighting nullspace basis vectors may be applied in the context of actuation redundancy, which is dual to the kinematic redundancy treated in the previous chapters. Another possible application lies in the field of mobile robotics. Consider, for example, a robotic arm attached to a mobile platform such as a car, a flying drone or a spaceship. As the degree of kinematic redundancy is increased, optimal inverse kinematics within the optimal path tracking framework from Section 5.2.2 is enabled.

Bibliography [1] Khaled Al Khudir and Alessandro De Luca. “Faster Motion on Cartesian Paths Exploiting Robot Redundancy at the Acceleration Level”. In: IEEE Robotics and Automation Letters 3.4 (2018), pp. 3553– 3560. [2] Joel A E Andersson et al. “CasADi – A software framework for nonlinear optimization and optimal control”. In: Mathematical Programming Computation (2018). [3] Jorge Angeles. “The design of isotropic manipulator architectures in the presence of redundancies”. In: The International Journal of Robotics Research 11.3 (1992), pp. 196–201. [4] John Baillieul. “Avoiding obstacles and resolving kinematic redundancy”. In: Robotics and Automation. Proceedings. 1986 IEEE International Conference on. Vol. 3. IEEE. 1986, pp. 1698–1704. [5] John Baillieul. “Kinematic programming alternatives for redundant manipulators”. In: Robotics and Automation. Proceedings. 1985 IEEE International Conference on. Vol. 2. IEEE. 1985, pp. 722–728. [6] John Baillieul, John Hollerbach, and Roger Brockett. “Programming and control of kinematically redundant manipulators”. In: Decision and Control, 1984. The 23rd IEEE Conference on. Vol. 23. IEEE. 1984, pp. 768–774. [7] Pierre-Jean Barre et al. “Influence of a jerk controlled movement law on the vibratory behaviour of high-dynamics systems”. In: Journal of Intelligent and Robotic Systems 42.3 (2005), pp. 275–293.

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4

160

Bibliography

[8] Francesco Basile and Pasquale Chiacchio. “A contribution to minimum-time task-space path-following problem for redundant manipulators”. In: Robotica 21.2 (2003), pp. 137–142. [9] Joachim Baumgarte. “Stabilization of constraints and integrals of motion in dynamical systems”. In: Computer methods in applied mechanics and engineering 1.1 (1972), pp. 1–16. [10] Adi Ben-Israel and Thomas NE Greville. Generalized inverses: theory and applications. Vol. 15. Springer Science & Business Media, 2003. [11] John T Betts. Practical methods for optimal control and estimation using nonlinear programming. Vol. 19. Siam, 2010. [12] Corrado Guarino Lo Bianco. “Minimum-jerk velocity planning for mobile robot applications”. In: IEEE Transactions on Robotics 29.5 (2013), pp. 1317–1326. [13] Lorenz T Biegler. Personal Communication. Jan. 2018. [14] James E Bobrow, Steven Dubowsky, and JS Gibson. “Time-optimal control of robotic manipulators along specified paths”. In: The international journal of robotics research 4.3 (1985), pp. 3–17. [15] Hans Georg Bock and Karl-Josef Plitt. “A multiple shooting algorithm for direct solution of optimal control problems”. In: IFAC Proceedings Volumes 17.2 (1984), pp. 1603–1608. [16] Stephen Boyd and Lieven Vandenberghe. Convex optimization. Cambridge university press, 2004. [17] Hartmut Bremer. Dynamik und Regelung mechanischer Systeme. Teubner Studienbücher, 1988. [18] Hartmut Bremer. Elastic Multibody Dynamics: A Direct Ritz Approach. Heidelberg: Springer Verlag, 2008. [19] Christof Büskens. “Echtzeitoptimierung und Echtzeitoptimalsteuerung parametergestörter probleme”. In: (2002). [20] Christof Büskens. “Optimierungsmethoden und sensitivitätsanalyse für optimale steuerprozesse mit steuer-und zustandsbeschränkungen”. PhD thesis. Universität Münster, 1998.

Bibliography

161

[21] Christof Büskens. “Real-time solutions for perturbed optimal control problems by a mixed open-and closed-loop strategy”. In: Online optimization of large scale systems. Springer, 2001, pp. 105–116. [22] Christof Büskens and Helmut Maurer. “Sensitivity analysis and realtime optimization of parametric nonlinear programming problems”. In: Online Optimization of Large Scale Systems. Springer, 2001, pp. 3–16. [23] Samuel R Buss. “Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods”. Unpublished survey. Oct. 2009. url: http://www.math.ucsd.edu/ ~sbuss/ResearchWeb/ikmethods/index.html. [24] Pyung Chang. “A closed-form solution for inverse kinematics of robot manipulators with redundancy”. In: IEEE Journal on Robotics and Automation 3.5 (1987), pp. 393–403. [25] Y-C Chen and Ian D Walker. “A consistent null-space based approach to inverse kinematics of redundant robots”. In: Robotics and Automation, 1993. Proceedings., 1993 IEEE International Conference on. IEEE. 1993, pp. 374–381. [26] T Chettibi et al. “Minimum cost trajectory planning for industrial robots”. In: European journal of mechanics. A, Solids 23.4 (2004), pp. 703–715. [27] P Chiaacchio and Mariano Concilio. “The dynamic manipulability ellipsoid for redundant manipulators”. In: Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No. 98CH36146). Vol. 1. IEEE. 1998, pp. 95–100. [28] Pasquale Chiacchio. “Exploiting redundancy in minimum-time path following robot control”. In: American Control Conference, 1990. IEEE. 1990, pp. 2313–2318. [29] Stefano Chiaverini. “Task-priority redundancy resolution with robustness to algorithmic singularities”. In: IFAC Proceedings Volumes 27.14 (1994), pp. 453–459. [30] Stephen L Chiu. “Task compatibility of manipulator postures”. In: The International Journal of Robotics Research 7.5 (1988), pp. 13–21.

162

Bibliography

[31] E Sahin Conkur and Rob Buckingham. “Clarifying the definition of redundancy as used in robotics”. In: Robotica 15.5 (1997), pp. 583– 586. [32] Maurice G Cox. “The numerical evaluation of B-splines”. In: IMA Journal of Applied Mathematics 10.2 (1972), pp. 134–149. [33] Carl De Boor. “On calculating with B-splines”. In: Journal of Approximation theory 6.1 (1972), pp. 50–62. [34] A De Luca. “Decoupling and feedback linearization of robots with mixed rigid/elastic joints”. In: Int. J. Rob. Nonlin. Cont. 8 (1998), pp. 965–977. [35] Alessandro De Luca, Leonardo Lanari, and Giuseppe Oriolo. “A sensitivity approach to optimal spline robot trajectories”. In: Automatica 27.3 (1991), pp. 535–539. [36] Alessandro De Luca and Giuseppe Oriolo. “The reduced gradient method for solving redundancy in robot arms”. In: Robotersysteme 7.2 (1991), pp. 117–122. [37] Alessandro De Luca, Giuseppe Oriolo, and Paolo Robuffo Giordano. “Kinematic modeling and redundancy resolution for nonholonomic mobile manipulators”. In: Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Conference on. IEEE. 2006, pp. 1867–1873. [38] Frederik Debrouwere et al. “Time-optimal tube following for robotic manipulators”. In: Advanced Motion Control (AMC), 2014 IEEE 13th International Workshop on. IEEE. 2014, pp. 392–397. [39] Moritz Diehl and Sébastien Gros. Numerical Optimal Control (preliminary draft). University of Freiburg, 2017. url: https://www. syscop.de/files/2017ss/NOC/script/book-NOCSE.pdf. [40] Kees van den Doel and Dinesh K Pai. “Performance measures for robot manipulators: A unified approach”. In: The International Journal of Robotics Research 15.1 (1996), pp. 92–111.

Bibliography

163

[41] Rajiv V Dubey, James A Euler, and Scott M Babcock. “An efficient gradient projection optimization scheme for a seven-degreeof-freedom redundant robot with spherical wrist”. In: Robotics and Automation, 1988. Proceedings., 1988 IEEE International Conference on. IEEE. 1988, pp. 28–36. [42] Olav Egeland. “Task-space tracking with redundant manipulators”. In: IEEE Journal on Robotics and Automation 3.5 (1987), pp. 471– 475. [43] Francisco Facchinei, Andreas Fischer, and Christian Kanzow. “On the accurate identification of active constraints”. In: SIAM Journal on Optimization 9.1 (1998), pp. 14–32. [44] H.J. Ferreau et al. “qpOASES: A parametric active-set algorithm for quadratic programming”. In: Mathematical Programming Computation 6.4 (2014), pp. 327–363. [45] Enrico Ferrentino and Pasquale Chiacchio. “Redundancy Parametrization in Globally-Optimal Inverse Kinematics”. In: International Symposium on Advances in Robot Kinematics. Springer. 2018, pp. 47–55. [46] Anthony V Fiacco. Introduction to sensitivity and stability analysis in nonlinear programming. Elsevier, 1983. [47] Anthony V Fiacco. “Sensitivity analysis for nonlinear programming using penalty methods”. In: Mathematical programming 10.1 (1976), pp. 287–311. [48] Angel Flores-Abad et al. “Optimal Capture of a Tumbling Object in Orbit Using a Space Manipulator”. In: Journal of Intelligent & Robotic Systems 86.2 (2017), pp. 199–211. [49] M Galassi et al. GNU Scientific Library Reference Manual , ISBN 0954612078. GSL. url: https://www.gnu.org/software/gsl/. [50] Miroslaw Galicki. “Time-optimal controls of kinematically redundant manipulators with geometric constraints”. In: IEEE Transactions on Robotics and Automation 16.1 (2000), pp. 89–93.

164

Bibliography

[51] Hubert Gattringer et al. “Dynamical Analysis of a Tip Balancing Cube”. In: PAMM 17.1 (2017), pp. 147–148. [52] Hubert Gattringer et al. “Recursive methods in control of flexible joint manipulators”. In: Multibody System Dynamics 32.1 (2014), pp. 117–131. [53] Christoph Glocker. “Dynamik von Starrkörpersystemen mit Reibung und Stößen”. PhD thesis. ETH Zürich, 1995. [54] Maria da Graça Marcos, JA Tenreiro Machado, and T-P AzevedoPerdicoúlis. “Trajectory planning of redundant manipulators using genetic algorithms”. In: Communications in nonlinear science and numerical simulation 14.7 (2009), pp. 2858–2869. [55] Knut Graichen and Nicolas Petit. “Incorporating a class of constraints into the dynamics of optimal control problems”. In: Optimal Control Applications and Methods 30.6 (2009), pp. 537–561. [56] Knut Graichen, Michael Treuer, and Michael Zeitz. “Swing-up of the double pendulum on a cart by feedforward and feedback control with experimental validation”. In: Automatica 43.1 (2007), pp. 63–71. [57] Knut Graichen et al. “Handling constraints in optimal control with saturation functions and system extension”. In: Systems & Control Letters 59.11 (2010), pp. 671–679. [58] M Hajžman and P Polach. “Application of stabilization techniques in the dynamic analysis of multibody systems”. In: Applied and Computational Mechanics 1.2 (2007), pp. 479–488. [59] Hideo Hanafusa, Tsuneo Yoshikawa, and Yoshihiko Nakamura. “Analysis and control of articulated robot arms with redundancy”. In: IFAC Proceedings Volumes 14.2 (1981), pp. 1927–1932. [60] GA Hicks and WH Ray. “Approximation methods for optimal control synthesis”. In: The Canadian Journal of Chemical Engineering 49.4 (1971), pp. 522–528. [61] John M Hollerbach. “Dynamic scaling of manipulator trajectories”. In: Journal of Dynamic Systems, Measurement, and Control 106.1 (1984), pp. 102–106.

Bibliography

165

[62] M. Husty, M. Pfurner, and H. Schröcker. “A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator”. In: J. Mechanism and Mach. Theory 42.1 (Jan. 2007), pp. 66–81. [63] Matthias Jörgl, Philip Hörmandinger, and Andreas Müller. “Dynamical Modeling and Swing-Up Control of a Self-balancing Cube”. In: Advances in Robot Design and Intelligent Control: Proceedings of the 25th Conference on Robotics in Alpe-Adria-Danube Region (RAAD16). Vol. 540. Springer. 2016, p. 144. [64] Dominik Kaserer, Hubert Gattringer, and Andreas Müller. “Nearly Optimal Path Following with Jerk and Torque Rate Limits Using Dynamic Programming.” In: IEEE Robotics and Automation Letters (2018). [65] Dominik Kaserer, Hubert Gattringer, and Andreas Müller. “Online Robot-Object Synchronization With Geometric Constraints and Limits on Velocity, Acceleration, and Jerk”. In: IEEE Robotics and Automation Letters 3.4 (2018), pp. 3169–3176. [66] Robert Katzschmann et al. “Towards online trajectory generation considering robot dynamics and torque limits”. In: IEEE/RSJ IROS 2013. IEEE. 2013, pp. 5644–5651. [67] Jacek Kierzenka and Lawrence F Shampine. “A BVP solver based on residual control and the Maltab PSE”. In: ACM Transactions on Mathematical Software (TOMS) 27.3 (2001), pp. 299–316. [68] Charles A Klein and Ching-Hsiang Huang. “Review of pseudoinverse control for use with kinematically redundant manipulators”. In: IEEE Transactions on Systems, Man, and Cybernetics 2 (1983), pp. 245–250. [69] Matthias Knauer and Christof Büskens. “Real-Time Trajectory Planning of the Industrial Robot IRB6400”. In: PAMM: Proceedings in Applied Mathematics and Mechanics. Vol. 3. 1. Wiley Online Library. 2003, pp. 515–516. [70] Torsten Kröger. On-line Trajectory Generation in Robotic Systems: Basic Concepts for Instant. Reactions to Unforseen Events. Springer, 2012.

166

Bibliography

[71] Vyacheslav Kungurtsev and Moritz Diehl. “SQP Methods for Parametric Nonlinear Optimization”. In: Optimization Online (2014), pp. 1–35. [72] Huibert Kwakernaak and Raphael Sivan. Linear optimal control systems. Vol. 1. Wiley-Interscience New York, 1972. [73] Roberto Lampariello et al. “Trajectory planning for optimal robot catching in real-time”. In: Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE. 2011, pp. 3719–3726. [74] J Leger and J Angeles. “A Redundancy-resolution Algorithm for Fivedegree-of-freedom Tasks via Sequential Quadratic Programming”. In: TrC-IFToMM Symposium on Theory of Machines and Mechanisms„ Izmir. 2015. [75] Daniel B Leineweber. “Efficient reduced SQP methods for the optimization of chemical processes described by large sparse DAE models”. PhD thesis. VDI Verlag Düsseldorf, 1999. [76] Renhui Li. “Strukturierte Modellbildung und Implementierung von mechanischen Systemen”. MA thesis. Johannes Kepler University, 2009. [77] Zhi Li et al. “Synthesizing redundancy resolution criteria of the human arm posture in reaching movements”. In: Redundancy in Robot Manipulators and Multi-robot systems. Springer, 2013, pp. 201– 240. [78] Alain Liégeois. “Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms”. In: IEEE Transactions on Systems Man and Cybernetics 12 (1977), pp. 868–871. [79] J. Löfberg. “YALMIP : A Toolbox for Modeling and Optimization in MATLAB”. In: In Proceedings of the CACSD Conference. Taipei, Taiwan, 2004. [80] J Luh, M Walker, and R Paul. “Resolved-acceleration control of mechanical manipulators”. In: IEEE Transactions on Automatic Control 25.3 (1980), pp. 468–474.

Bibliography

167

[81] Anthony A Maciejewski and Charles A Klein. “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments”. In: The international journal of robotics research 4.3 (1985), pp. 109–117. [82] Daniel P Martin, John Baillieul, and John M Hollerbach. “Resolution of kinematic redundancy using optimization techniques”. In: IEEE Transactions on Robotics and Automation 5.4 (1989), pp. 529–533. [83] Mustafa Mashali, Redwan Alqasemi, and R Dubey. “Task priority based dual-trajectory control for redundant mobile manipulators”. In: Robotics and Biomimetics (ROBIO), 2014 IEEE International Conference on. IEEE. 2014, pp. 1457–1462. [84] Johannes Mayr et al. “Exploiting the Equations of Motion For Biped Robot Control with Enhanced Stability”. In: Multibody Dynamics. Springer, 2016, pp. 299–321. [85] Andreas Müller. “Forward dynamics of variable topology mechanisms—The case of constraint activation”. In: Proceedings of the Institution of Mechanical Engineers, Part K: Journal of Multi-body Dynamics 230.4 (2016), pp. 442–454. [86] Richard M Murray. A mathematical introduction to robotic manipulation. CRC press, 1994. [87] Yoshihiko Nakamura and Hideo Hanafusa. “Inverse kinematic solutions with singularity robustness for robot manipulator control”. In: Journal of dynamic systems, measurement, and control 108.3 (1986), pp. 163–171. [88] Yoshihiko Nakamura and Hideo Hanafusa. “Optimal redundancy control of robot manipulators”. In: The International Journal of Robotics Research 6.1 (1987), pp. 32–42. [89] Yoshihiko Nakamura, Hideo Hanafusa, and Tsuneo Yoshikawa. “Taskpriority based redundancy control of robot manipulators”. In: The International Journal of Robotics Research 6.2 (1987), pp. 3–15. [90] Dragomir N Nenchev. “Redundancy resolution through local optimization: A review”. In: Journal of robotic systems 6.6 (1989), pp. 769–798.

168

Bibliography

[91] Matthias Neubauer, Hubert Gattringer, and Hartmut Bremer. “A persistent method for parameter identification of a seven-axes manipulator”. In: Robotica 33.5 (2015), pp. 1099–1112. [92] Jorge Nocedal and Stephen J Wright. Numerical Optimization. 2nd ed. Springer, 2006. [93] M Oberherber, H Gattringer, and A Müller. “Successive dynamic programming and subsequent spline optimization for smooth time optimal robot path tracking”. In: Mechanical Sciences 6.2 (2015), pp. 245–254. [94] Matthias Oberherber et al. “A Task Space Approach for Planar Optimal Robot Tube Following”. In: Proceedings of the 13th International Conference on Informatics in Control, Automation and Robotics. SCITEPRESS-Science and Technology Publications, Lda. 2016, pp. 327–334. [95] Christina Oberlin and Stephen J Wright. “Active set identification in nonlinear programming”. In: SIAM Journal on Optimization 17.2 (2006), pp. 577–605. [96] Yonghwan Oh, WK Chung, and Y Youm. “General task execution of redundant manipulators with explicit null-motion control”. In: Industrial Electronics, Control, and Instrumentation, 1996., Proceedings of the 1996 IEEE IECON 22nd International Conference on. Vol. 3. IEEE. 1996, pp. 1902–1908. [97] G. Palli, C. Melchiorri, and A. De Luca. “On the Feedback Linearization of Robots with Variable Joint Stiffness”. In: Proc. IEEE Int. Conf. Rob. Aut. (IROS), Pasadena, CA, USA, May 19-23. 2008. [98] Jonghoon Park, WanKyun Chung, and Youngil Youm. “Weighted decomposition of kinematics and dynamics of kinematically redundant manipulators”. In: Robotics and Automation, 1996. Proceedings., 1996 IEEE International Conference on. Vol. 1. IEEE. 1996, pp. 480– 486. [99] Sarosh Patel and Tarek Sobh. “Manipulator performance measures-a comprehensive literature survey”. In: Journal of Intelligent & Robotic Systems 77.3-4 (2015), pp. 547–570.

Bibliography

169

[100] Martin Pfurner. “Closed form inverse kinematics solution for a redundant anthropomorphic robot arm”. In: Computer Aided Geometric Design 47 (2016), pp. 163–171. [101] Hung Pham and Quang-Cuong Pham. “A new approach to timeoptimal path parameterization based on reachability analysis”. In: IEEE Transactions on Robotics (2018). [102] Quang-Cuong Pham. “A general, fast, and robust implementation of the time-optimal path parameterization algorithm”. In: IEEE Transactions on Robotics 30.6 (2014), pp. 1533–1540. [103] Les Piegl and Wayne Tiller. The NURBS book. Springer Science & Business Media, 2012. [104] Donald Lee Pieper. “The kinematics of manipulators under computer control”. PhD thesis. Stanford Univ., Dept. Of Computer Science, 1968. [105] François G Pin et al. “Motion planning for mobile manipulators with a non-holonomic constraint using the FSP (full space parameterization) method”. In: Journal of robotic systems 13.11 (1996), pp. 723–736. [106] Hans Pirnay, Rodrigo López-Negrete, and Lorenz T Biegler. “Optimal sensitivity based on IPOPT”. In: Mathematical Programming Computation 4.4 (2012), pp. 307–331. [107] Ron P Podhorodeski, Andrew A Goldenberg, and Robert G Fenton. “Resolving redundant manipulator joint rates and identifying special arm configurations using Jacobian null-space bases”. In: IEEE transactions on robotics and automation 7.5 (1991), pp. 607–618. [108] Daniel Ralph and Stephan Dempe. “Directional derivatives of the solution of a parametric nonlinear program”. In: Mathematical programming 70.1-3 (1995), pp. 159–172. [109] Alexander Reiter. “An explicit approach for time-optimal trajectory planning for kinematically redundant robots”. MA thesis. Johannes Kepler University Linz, Institute of Robotics, Dec. 2014.

170

Bibliography

[110] Alexander Reiter. Time-Optimal Trajectory Planning for Redundant Robots: Joint Space Decomposition for Redundancy Resolution in Non-Linear Optimization. Springer, 2016. [111] Alexander Reiter, Hubert Gattringer, and Andreas Müller. “On Redundancy Resolution in Minimum-Time Trajectory Planning of Robotic Manipulators Along Predefined End-Effector Paths”. In: Informatics in Control, Automation and Robotics. Springer, 2018, pp. 190–206. [112] Alexander Reiter, Hubert Gattringer, and Andreas Müller. “Realtime computation of inexact minimum-energy trajectories using parametric sensitivities”. In: International Conference on Robotics in Alpe-Adria Danube Region. Springer. 2017, pp. 174–182. [113] Alexander Reiter, Hubert Gattringer, and Andreas Müller. “Redundancy Resolution in Minimum-time Path Tracking of Robotic Manipulators.” In: ICINCO (2). 2016, pp. 61–68. [114] Alexander Reiter, Andreas Müller, and Hubert Gattringer. “Inverse kinematics in minimum-time trajectory planning for kinematically redundant manipulators”. In: Industrial Electronics Society, IECON 2016-42nd Annual Conference of the IEEE. IEEE. 2016, pp. 6873– 6878. [115] Alexander Reiter, Andreas Müller, and Hubert Gattringer. “On Higher-Order Inverse Kinematics Methods in Time-Optimal Trajectory Planning for Kinematically Redundant Manipulators”. In: IEEE Transactions on Industrial Informatics (2018). [116] Alexander Reiter, Andreas Müller, and Hubert Gattringer. “Rapid Nearly-Optimal Rendezvous Trajectory Planning Using Parameter Sensitivities”. In: ROMANSY 22–Robot Design, Dynamics and Control. Springer, 2019, pp. 515–522. [117] Alexander Reiter et al. “An explicit approach for time-optimal trajectory planning for kinematically redundant serial robots”. In: PAMM 15.1 (2015), pp. 67–68.

Bibliography

171

[118] Roland Riepl, Hubert Gattringer, and Hartmut Bremer. “Decentralized Control of an Industrial Robot with Elastic Gears”. In: PAMM 10.1 (2010), pp. 51–52. [119] Stephen M Robinson. “Perturbed Kuhn-Tucker points and rates of convergence for a class of nonlinear-programming algorithms”. In: Mathematical programming 7.1 (1974), pp. 1–16. [120] Seyed Sina Mirrazavi Salehian, Mahdi Khoramshahi, and Aude Billard. “A dynamical system approach for softly catching a flying object: Theory and experiment”. In: IEEE Trans. on Robotics 32 (2016), pp. 462–471. [121] Lorenzo Sciavicco and Bruno Siciliano. “A dynamic solution to the inverse kinematic problem for redundant manipulators”. In: Robotics and Automation. Proceedings. 1987 IEEE International Conference on. Vol. 4. IEEE. 1987, pp. 1081–1087. [122] Lorenzo Sciavicco and Bruno Siciliano. “A solution algorithm to the inverse kinematic problem for redundant manipulators”. In: IEEE Journal on Robotics and Automation 4.4 (1988), pp. 403–410. [123] Lorenzo Sciavicco and Bruno Siciliano. “Coordinate transformation: A solution algorithm for one class of robots”. In: IEEE transactions on systems, man, and cybernetics 16.4 (1986), pp. 550–559. [124] Bruno Siciliano. “A closed-loop inverse kinematic scheme for on-line joint-based robot control”. In: Robotica 8 (1990), pp. 231–243. [125] Bruno Siciliano. “Kinematic control of redundant robot manipulators: A tutorial”. In: Journal of intelligent and robotic systems 3.3 (1990), pp. 201–212. [126] Bruno Siciliano and Oussama Khatib. Springer Handbook of Robotics. Springer, 2016. [127] Bruno Siciliano et al. Robotics: Modelling, Planning and Control. Springer-Verlag London, 2009. [128] Niko Sünderhauf et al. “The limits and potentials of deep learning for robotics”. In: The International Journal of Robotics Research 37.4-5 (2018), pp. 405–420.

172

Bibliography

[129] Krzysztof Tchoń and Janusz Jakubiak. “Extended Jacobian inverse kinematics algorithms for mobile manipulators”. In: Journal of Field Robotics 19.9 (2002), pp. 443–454. [130] Bipop Team. Siconos. INRIA Grenoble Rhone Alpes. url: http: //siconos.gforge.inria.fr/4.1.0/html/users_guide/index. html. [131] Gaurav Tevatia and Stefan Schaal. “Inverse kinematics for humanoid robots”. In: Robotics and Automation, 2000. Proceedings. ICRA’00. IEEE International Conference on. Vol. 1. IEEE. 2000, pp. 294–299. [132] Diederik Verscheure et al. “Time-energy optimal path tracking for robots: a numerically efficient optimization approach”. In: Proceedings of the 10th international workshop on advanced motion control. 2008, pp. 727–732. [133] Andreas Wächter and Lorenz T Biegler. “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming”. In: Mathematical programming 106.1 (2006), pp. 25– 57. [134] Charles Wampler. “Inverse kinematic functions for redundant manipulators”. In: Robotics and Automation. Proceedings. 1987 IEEE International Conference on. Vol. 4. IEEE. 1987, pp. 610–617. [135] Mitsuru et al. Watanabe. “Time optimal path-tracking control of kinematically redundant manipulators”. In: JSME International Journal Series C Mechanical Systems, Machine Elements and Manufacturing 47.2 (2004), pp. 582–590. [136] D. E. Whitney. “Resolved Motion Rate Control of Manipulators and Human Prostheses”. In: IEEE Transactions on Man-Machine Systems 10.2 (Feb. 1969), pp. 47–53. [137] William A Wolovich and H Elliott. “A computational technique for inverse kinematics”. In: Proc. 23rd IEEE Conference on Decision and Control. Vol. 23. IEEE. 1984, pp. 1359–1363. [138] Tsuneo Yoshikawa. “Dynamic manipulability of robot manipulators”. In: Transactions of the Society of Instrument and Control Engineers 21.9 (1985), pp. 970–975.

Bibliography

173

[139] Tsuneo Yoshikawa. “Manipulability of robotic mechanisms”. In: The international journal of Robotics Research 4.2 (1985), pp. 3–9. [140] Ralf Zehetner. Trajektorienplanung und Regelung des Furuta-DoppelPendels. Bachelor’s Thesis, JKU Linz. June 2018. [141] Jakob Ziegler et al. “Generating realistic trajectories for robotic hippotherapy from 3D captured horseback motion”. In: The 5th Joint International Conference on Multibody System Dynamics. June 2018. [142] Leon Zlajpah. “Dexterity measures for optimal path control of redundant manipulators”. In: Proceedings of International Conference on Automatisation and Control (ICAR). 1996.

Appendix A

B-Spline Curves In Section 2.2, the numerical basics regarding B-spline curves have been reviewed. Therein, not only the simple evaluation, but also the optimizationbased construction of such functions have been discussed. In this appendix chapter, although they are not required for the topics in the main part, additional features of B-splines are documented that find application in a software tool that was developed as a by-product of the work leading to the present thesis. In many applications, a parameterization of a B-spline curve in terms of its arc length can be useful, e.g. in the field of mobile robotics for path planning purposes as the parameter then represents the distance traveled along the path. Such a parameterization will be derived for the simple oneand also for the multi-dimensional case in Appendix A.1. Construction and evaluation of B-spline curves in curve parameter as well as in arc length representation have been implemented in a software libary, see Appendix A.2. It is not only intended to be primarily used in robotics tasks such as path and trajectory planning, but also in more general applications as it is built upon the framework for symbolic computation and numerical optimization CasADi [2].

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4

176

A.1

A B-Spline Curves

Arc Length Parameterization

Arc length parameterization refers to an alternative representation of the B-spline curve q(t) in terms of its arc length l, i.e. q(l). Therein, the original spline definition (2.2.1) can be re-used by utilizing an intermediate parameterization of the spline parameter in terms of the arc length length, i.e. t(l). In the following such a function will be derived, again using constrained B-spline approximation. Consider a B-spline curve q(t) ∈ Rn that is parameterized w.r.t. its native parameter t ∈ [a, b]. Its arc length at a path parameter value l is l(t) =

⎧ 1  2 0t ⎪ ⎪ ⎪ ⎨ 1 + dτd q(τ ) dτ a 1   ⎪ ⎪ 0t d d ⎪ ⎩ dτ q(τ ) dτ q(τ ) dτ a

n=1 n>1

t ∈ [a, b].

(A.1.1)

The total length of the spline curve is thus found by setting t = b in (A.1.1). For the parameterization t(l), the inverse of the above function needs to be found which is non-trivial. However, an approximation tˆ(l), can be calculated as shown in the following Algorithm 4. Algorithm 4 Computation of arc length parameterization as B-spline curve. q(t) ∈ Rn : B-spline curve parameterized in terms of parameter t   τ ← t 1 t 2 . . . t n : collect knots of all B-spline curves remove duplicates in τ  τ : vector of unique knots nτ ← length of τ   l ← l l(τ1 ) l(τ2 ) . . . l(τnτ ) : vector of arc lengths at knots τ ← ( ): vector of intermediate grid points l ← ( ): vector of arc lengths at intermediate points for all knot intervals [τk , τk+1 ] ∈ τ , k = 1, . . . , nτ − 1 do compute N intermediate points τk < τk,1 < τk,2 < · · · < τk,N < τk+1   τ ← τ τk,1 τk,2 . . . τk,N : collect intermediate grid points 



l ← l l(τk,1 ) l(τk,2 ) . . . l(τk,N ) : compute arc lengths at intermediate grid points end for return τ , l, nτ , τ , l

A.1 Arc Length Parameterization

177

Therein, first the knots of all splines are collected in the vector of unique knots τ of length nτ that has only unique, monotonically increasing elements. The resulting function tˆ(l) will interpolate the arc lengths the knots contained in τ . Therefore, the arc length value at these points are computed and stored in the vector l of the same length nτ . Then, on each interval of τ , N intermediate points are computed and stored in the vector τ of length N (nτ − 1). The corresponding arc lengths are stored in the vector l of the same length. These will be used to approximate the evolution of the arc length between the knots. The arc length parameterization as a B-spline curve tˆ(l) with degree p, NCP control points (which are collected in vector dtˆ) and open-uniform knot distribution is finally found by solving the constrained approximation QP problem

τ −1)  2 1 N (n τ i − tˆ li dtˆ 2 i=1 ˆ s.t. t(li ) = τi , ∀ li ∈ l, ∀ τi ∈ τ

minimize

(A.1.2a) (A.1.2b)

that is a special form of (2.2.11) wherein τi , li , τ i and li are the i-th elements of the vectors τ , l, τ and l, respectively. While the objective in (A.1.2a) is used to quadratically approximate the arc length function at the grid points collected in the vector τ , the equality constraints (A.1.2b) represent the arc lengths at the knots collected in τ that are interpolated. In addition, a term that smoothes the resulting function tˆ(l) can be included, cf. (2.2.11). The degree p should be chosen as the highest degree (or higher) that occurs in any element of the original curve q(t) in order to preserve the original degree of continuous differentiability. The number of intermediate points N and the number of control points dtˆ need to be chosen in order to sufficiently capture the arc length evolution.

178

A.2

A B-Spline Curves

B-Spline Software Package

As a side-project to the work leading up to the present thesis, a software tool to construct and evaluate B-spline n-D curves in multiple development environments was developed. Since its internal release, it has been used in various industrial projects and theses. Overview of functionality • Evaluation: recursive (slow, but accurate) or matrix-vector (fast, but inaccurate) mode. • Evaluation: path parameterization. • Construction: simple mode (with all elements known). • Construction: constraint approximation mode. The functionality is available as a native C++ implementation with an easy-to-use MATLAB interface (matrix-vector evaluation/construction), and also partly as an easy-to-use class-based MATLAB-CasADi library (recursive evaluation/construction). Furthermore, evaluation as well as construction by constrained approximation of B-spline surfaces are also implemented in the tool. This functionality, however, is not documented here as it is entirely out of scope of the present thesis. A release as open-source software is planned for 2019.

A.2.1

Open Challenges

Performance Improvement, Code Refactoring In order to increase the performance of the provided library, refactoring it as a native C++ CasADi-based implementation is advisable. This would also increase its versatility and enable its use on more platforms.

A.2 B-Spline Software Package

179

Path Parameter Constraints In certain cases, it is required to prescribe a particular arc length to a curve. This, however, yields a nonlinear programming problem as the arc length is nonlinear w.r.t. the B-spline curve value q(t), cf. (A.1.1). Examples for possible applications are path and trajectory planning in mobile robotics.

Appendix B

Mechatronic Multibody Systems In this chapter, the essential subject of computational modeling of mechatronic systems such as robots as well as the solution of the resulting differential-algebraic equations (DAE) is discussed. For robotic systems, a model of their kinematic and dynamic behavior is required. Model stands for a mathematical representation of inherent physical processes. Deriving a mathematical model representing a system’s dynamics is facilitated by means of well-known analytic, e.g. Lagrange’s formalisms, or synthetic techniques, e.g. Newton-Euler equations, or Bremer’s Projection Equation [17, 18]. Once such a dynamics model has been established, it can be applied for various purposes such as • simulation, i.e. numerical computation of the solution of the differential-algebraic equations of motion (EoM), • system analysis, e.g. investigation of inherent system properties such as stability, • parameter identification and • model-based feed-forward and feed-back control

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020 A. Reiter, Optimal Path and Trajectory Planning for Serial Robots, https://doi.org/10.1007/978-3-658-28594-4

182

B Mechatronic Multibody Systems

A model is derived with its designated purpose in mind. To this end, a certain level of detail has to be carefully chosen that appropriately balances computational cost with physical accuracy by considering only relevant effects. As a consequence, practical models suffer from inherent inaccuracies limiting their applicability. Reducing the level of detail of a once-derived model is enabled by means of model reduction techniques. The EoM are given as differential equations, or possibly differential-algebraic equations. To this end, the question about their solution, i.e. finding a time-dependent function that solves the EoM, arises which is treated in the following section. Example B.1: AWARO (modeling the dynamics of a two-legged walking machine)

Figure B.1: Schematics of Autonomous Walking Robot (AWARO).

An example for model reduction is presented in [84] where the dynamics of a two-legged walking robot from Figure B.1 is considered. Therein, at first, a rather detailed model of the mechanical system is derived. It incorporates not only the torso and the limbs represented as rigid bodies, but also actuator units that include structural gear elasticity. This model with as many as 34 degrees of freedom (DOF) is intended for use in (offline) parameter identification and for (online) inverse dynamics-based computation of feed-forward control torques in order to increase joint trajectory tracking accuracy. However, the resulting model is not suited for realtime tasks such as numerical simulation required for pre-computing the near future step-sequence in an iterative manner due to its high computational cost. Therefore, system reduction measures are taken such as omitting gear elasticity, yielding 20 DOF.

B.1 Solution of the Equations of Motion

183

Additionally assuming ideal joint position tracking, justified by the application of the mentioned feed-forward control, and thus avoiding joint dynamics, only the 6 position and orientation torso coordinates are left. Furthermore, dropping also angular momentum results in a mere 3 DOF, i.e. the system is modeled as an inverted three-dimensional pendulum. This simple model is then used for real-time iterative gait pattern generation.

B.1

Solution of the Equations of Motion

The EoM of a mechatronic system are denoted M(q)¨s + h(q, s˙ ) = Q(q, u), subjected to q˙ = H(q)˙s,

(B.1.1a) (B.1.1b)

wherein q denotes the vector of minimal (position) coordinates, s˙ is the vector of non-holonomic velocities, M is the generalized inertia matrix, h is a vector of generalized forces containing nonlinear terms due to Coriolis and friction terms and Q denotes generalized forces due to system inputs u such as external forces and torques.

B.1.1

Constraints

A system such as (B.1.1) can be subjected to kinematic equality constraints on position level, i.e. geometric constraints, or on velocity level. Note, that velocity-level constraints exist that do not necessarily have a correspondence on position-level, i.e. non-holonomic constraints. B.1.1.1

Geometric Constraints

Geometric, i.e. holonomic, constraints are collected in the vector g(q) = 0 which is in general nonlinear w.r.t. the system coordinates q.

(B.1.2)

184 B.1.1.2

B Mechatronic Multibody Systems Velocity Constraints

A consequence of the position-level requirement (B.1.2) is the corresponding velocity-level constraint ˙ ˙ = g(q, q)

d d g(q) = g(q) q˙ = 0 dt dq 

(B.1.3)

  Jg (q)

that is obtained straightforwardly by time-derivation. Therein, Jg is called the holonomic velocity constraint Jacobian. In contrast, non-holonomic velocity-level constraints ˙ g(q, s˙ ) =

d ˙ g(q, s˙ ) s˙ = 0 d˙s   

(B.1.4)

Jg˙ (q)

in general do not have a direct correspondence on position-level. However, a combination multiple non-holonomic velocity-level constraints may yield a holonomic velocity-level constraint and thus has a position-level correspondence. B.1.1.3

Constrained Equations of Motion

In order to satisfy above constraints, constraint forces λ are introduced into the equations of motion, resulting in the system of DAE M(q)¨s + h(q, s˙ ) − A (q)λ = Q(q, u) subjected to q˙ = H(q)˙s ⎡ ⎣ 

⎤

Jg (q)H(q) ⎦ s˙ = 0. Jg˙ (q)  A(q)

(B.1.5a) (B.1.5b) (B.1.5c)



Therein, the holonomic and non-holonomic velocity constraints are collected in (B.1.5c) and denoted in Pfaffian form A(q)˙s = 0. Note that in (B.1.5) position-level constraints (B.1.2) are not included, i.e. are not required to be satisfied.

B.1 Solution of the Equations of Motion

185

Note that this scheme is not restricted to minimal coordinate formulation. In fact, a choice of non-minimal, i.e. redundant, coordinates can be implemented together with a set of additional geometric constraints. Holonomic velocity coordinates are considered a special case with H(q) = I, ˙ Consequently non-holonomic constraints are also omitted, i.e. i.e. s˙ = q. Jg˙ = O.

B.1.2

Solution of System of DAEs

With the time derivative of (B.1.5c) d ˙ ˙ s + A(q)¨s = 0, (A(q)˙s) = A(q, q)˙ dt

(B.1.6)

(B.1.5) can be re-written as ⎡ ⎣

⎤⎛

M(q) −A(q) ⎦⎝ ¨s λ A(q) O

⎞

⎛

⎠

=⎝

Q(q, u) − h(q, s˙ ) ˙ ˙ s −A(q, q)˙

⎞ ⎠

subjected to q˙ = H(q)˙s.

(B.1.7a) (B.1.7b)

The goal of the next step is to solve (B.1.7) for the accelerations ¨s. These of course can be found by simply solving the above (n + m) × (n + m) system of linear equations.

B.1.3

Divide & Conquer: Constraint Forces and Accelerations

However, by adding an intermediate step to compute the constraint forces, the computational effort is changed to the solutions of an m × m system and two n × n systems. With \ being the operator that indicates solving a system of linear equations (rather than left-multiplying the inverse of the matrix), re-writing (B.1.5a) yields M(q)¨s + h(q, s˙ ) − A (q)λ = Q(q, u) ¨s − M(q)\A (q)λ = M(q)\(Q(q, u) − h(q, s˙ )) A(q)¨s − A(q)M(q)\A (q)λ = A(q)M(q)\(Q(q, u) − h(q, s˙ ))

186

B Mechatronic Multibody Systems

and by substituting (B.1.6), the constraint forces λ are found to be '

( 



˙ ˙ s . λ = − A(q)M(q)\A (q) \ A(q)M(q)\(Q(q, u) − h(q, s˙ )) + A(q, q)˙ (B.1.8) Now that the generalized constraint forces λ are known, (B.1.5a) is solved for the accelerations ¨s, i.e. M(q)¨s + h(q, s˙ ) − A (q)λ = Q(q, u) 



¨s = M(q)\ Q(q, u) − h(q, s˙ ) + A (q)λ .

(B.1.9)

Note that in (B.1.8) and (B.1.9), three systems of linear equations with the matrix M = M > 0 need to be solved. Therein, the opportunity to further reduce the computational burden by applying a re-usable decomposition of M arises, e.g. Cholesky factorization.

B.1.4

Solution by Time Integration

State-Space Representation By re-writing the system of second-order DAEs (B.1.5) as system of firstorder DAEs, i.e. state-space representation with the state ⎛

x=

⎝

⎞

q⎠ , s˙

(B.1.10)

the solution is finally obtained by time integration ⎛

x˙ =

⎝

q˙ ¨s

⎞

⎛

⎠

⎝

=

H(q)˙s ¨s

⎞ ⎠

(B.1.11)

with the velocity transformation (B.1.5b). Numerical integration introduces drift effects due to accumulation of numerical artifacts resulting from round-off errors and accuracy limitations of the used number format. Thereby, the included position and velocity constraints are no longer satisfied. The Baumgarte stabilization technique

B.1 Solution of the Equations of Motion

187

[9] is a well-known method to mitigate such issues. Therein, the right-hand side of the second line in (B.1.7) is augmented by additional terms, i.e. ˙ ˙ ˙ q˙ → −A(q, ˙ q˙ + Pg(q) + DA(q)q˙ −A(q, q) q)

(B.1.12)

in the holonomic case and ˙ ˙ ˙ s → −A(q, ˙ s + DA(q)˙s −A(q, q)˙ q)˙

(B.1.13)

in the non-holonomic case. These additional terms act in a similar manner as a PD controller and drive the position-level and velocity-level constraint residues g(q) (only in holonomic case) and A(q)˙s, respectively, towards zero. Therein, the stabilization matrices P, D > 0 are used. An overview of other integration stabilization techniques is given in [58].

B.1.5

Conditional Constraints

Models, represented by the DAE formulation (B.1.5), can be operated in multiple stages in which the active set of constraints may vary. Conditions under which certain constraints are considered active may depend on the state x of the system, i.e. position q and velocity s˙ , or external quantities such as system inputs u. Constraint Redundancy Resolution In practice, multiple constraint sets may become active, depending on their respective activation conditions. In the active set of constraints, constraints may be redundant, i.e. they are included in more the one currently active constraint subset. Without further treatment, this would yield a constraint Jacobian A with linearly dependent rows. This issue is resolved in step (B.1.8) that solves for the constraint forces λ by means of a singular value ' ( ˜ decomposition of the (square) matrix A(q) = A(q)M(q)\A (q) wherein redundant constraints are omitted by thresholding small singular values. Note, that conditions may also depend on the constraint forces λ, as they are an algebraic function of the state x and the input u, cf. (B.1.8).

188

B.1.6

B Mechatronic Multibody Systems

Solution Algorithm

The above solution strategy for solving the equations of motion of constrained mechatronic multibody systems is summarized in Algorithm 5. Therein, a single dynamic system is considered, that is denoted in possibly redundant position coordinates q and non-holonomic velocity coordinates s˙ . It is subjected to conditional, possibly redundant, non-holonomic constraints. The solution of the EoM is sought to be obtained in simulation, i.e. using numerical time integration methods on a time grid t0 < t1 < t2 < . . . that is either fixed (fixed step-size integrator), or variable (variable step-size integrator). The goal of the provided algorithm is to compute the system’s ˙ cf. (B.1.11), depending on the system state x(tk ) and state derivative x, the input u(tk ) at time step tk . The result is then used in time integration in order to obtain the state (B.1.10) at the following time step tk+1 .

B.2

Code Generation Utility

In order to enable efficient numerical simulation of dynamic models, the Maple package SimCode C has been developed that facilitates automated code generation for differential-algebraic functions for use in MathWorks MATLAB/Simulink. Key features of SimCode C are • support for second-order ordinary differential equations, e.g. equations of motion of mechatronic systems – redundant and minimal coordinate formulation – non-holonomic velocity coordinates – resolution of conditional, redundant algebraic constraints – constraint stabilization using Baumgarte’s method • support for algebraic functions depending on state, accelerations, or constraint forces • multiple parameter vectors • multiple inputs/outputs

B.2 Code Generation Utility

189

Algorithm 5 Computation of state derivative x˙ (and constraint forces λ). inputs: x, u q, s˙ ← x compute M(q), H(q) L ← Cholesky decomposition of M(q) A(x): determine currently active set of constraints compute h(q, s˙ ) g(q) ← ( ): vector of active holonomic position-level constraints Jg (q) ← [ ]: constraint Jacobian of active holonomic velocity-level constraints ˙ ← [ ]: time derivative of constraint Jacobian of active holonomic J˙ g (q,q) velocity-level constraints for all holonomic constraints i ∈ A(x) do ⎛ ⎞ g g ← ⎝ ⎠: collect active position-level constraints gi ⎡ ⎤ ⎡ ⎤ ˙ ⎦ Jg (q) ⎦ ˙ J˙ g (q,q) ⎣ ⎣ ˙ ← ˙ , Jg (q,q) : collect active velocityJg (q) ← Jgi (q) ˙ Jgi (q,q) level constraints end for Jg˙ (q) ← [ ]: constraint Jacobian of non-holonomic velocity-level constraints J˙ g˙ (q, s˙ ) ← [ ]: time derivative of constraint Jacobian of active nonholonomic velocity-level constraints for all non-holonomic constraints⎡i ∈ A(x)⎤do ⎤ ⎡ ˙ J (q) ˙ g ˙ g˙ (q, s˙ ) ← ⎣ Jg˙ (q, s˙ ) ⎦: collect active velocity-level ⎦, J Jg˙ (q) ← ⎣ ˙ Jg˙ i (q) Jg˙ i (q, s˙ ) constraints end for ⎡ ⎡ ⎤ ⎤ ˙ ⎦ Jg (q)⎦ ˙ J˙ g (q,q) ⎣ ⎣ A(q) ← , A(q) ← ˙ : assemble constraint Jacobian Jg˙ (q) Jg˙ (q, s˙ ) compute constraint forces λ  SVD; use Cholesky decomposition L compute acceleration ¨s ⎛cf. (B.1.9); use Cholesky decomposition L ⎞ ⎛ ⎞ ˙ H(q)˙ s q ⎠ return x, ˙ λ update state derivative x˙ ← ⎝ ⎠ = ⎝ ¨s ¨s

190

B Mechatronic Multibody Systems

• multiple (independent) dynamic systems/algebraic functions with common code generation for increased evaluation efficiency • code generation targets – C MEX Function for MATLAB – C MEX S-Function for Simulink – MATLAB S-Function for Simulink – MATLAB MEX S-Function for Simulink – Simulink template model – initialization file with dummy parameters of correct dimensions – make file for building above C functions and generating a Simulink test model as well an initialization file While this tool does not take the elaborate task of creating a dynamics model off of the user, it greatly the reduces burden of efficiently implementing it for use in numeric simulation and control. The generated C programming language code makes use of linear algebra functions of the free and open-source GNU Scientific Library (GSL) [49] and the Basic Linear Algebra Subprograms (BLAS) low-level routines wrapped therein. Therein, static memory allocation is used in order to enable use on some real-time systems which disallow memory allocation at run-time. SimCode C is intended to act as a full replacement of the deprecated code generation software [76]. A release as open-source software is planned for 2019.

B.2.1

Current Limitations and Open Challenges

Conservation of Momentum The above solution strategy does not take into account a relevant conservation quantity of mechanical systems: momentum. In general, at a switching point between different sets of active constraints, the total generalized momentum may not be continuous.

B.2 Code Generation Utility

191

By injecting an impulsive generalized force into the system at such a transition point, this issue is mitigated. Note, that an impulsive force results in a discontinuity of the system velocities s˙ . The paper [85] presents a solution strategy the introduces conservation of momentum into the numerical solution of multi-body systems. As a use case, a variable topology mechanism is considered whose joints become locked during motion. However, a problem that has not been treated in this thesis are possible limit cycles arising from the presence of conditional constraint and momentum conservation methods. The issue therein is that the change of the velocities s˙ may modify the active set of constraints that requires re-computation of the impulsive constraint forces and thus yet another change of s˙ . Contact Problems Contact problems are of great relevance for the mechanical part of many mechatronic systems, e.g. walking machines, or industrial robots interacting with their environment. Currently, only a very simple form of contact problems can be solved, i.e. contact that is toggled by geometric but not dynamic constraints. Further development will be focused on contact problems that not only allow conditions depending on accelerations but also on constraint forces. Part of this is described in [53] wherein the contact problem is posed as a Linear Complementarity Problem (LCP). LCPs are most efficiently solved by special solvers, e.g. [130], or by virtue of re-writing as a QP problem by any QP solver, e.g. [44].