Normal form approach in the motion planning of space robots: a case study

Abstract

We examine applicability of normal forms of non-holonomic robotic systems to the problem of motion planning. A case study is analyzed of a planar, free-floating space robot consisting of a mobile base equipped with an on-board manipulator. It is assumed that during the robot’s motion its conserved angular momentum is zero. The motion planning problem is first solved at velocity level, and then torques at the joints are found as a solution of an inverse dynamics problem. A novelty of this paper lies in using the chained normal form of the robot’s dynamics and corresponding feedback transformations for motion planning at the velocity level. Two basic cases are studied, depending on the position of mounting point of the on-board manipulator. Comprehensive computational results are presented, and compared with the results provided by the Endogenous Configuration Space Approach. Advantages and limitations of applying normal forms for robot motion planning are discussed.

1 Introduction

The idea of establishing equivalences between diverse geometric objects, specifically differential systems, inspired by a series of seminal papers by Cartan [5], has been known in mathematics for quite a long time. An inherent aspect of these equivalences has been the concept of normal forms, to mention only those named after Engel et al. [5], or Pfaff and Darboux [3]. Equivalence between tensor fields exploited within the Lie Transform Method results in transparent proofs of the Morse Theorem or the Poincaré Lemma, extendable to the infinite dimensions [1]. A correspondence between Pfaffian differential systems and control systems (vector field distributions) manifests itself in the introduction of feedback equivalence of control systems, and corresponding normal forms [6]. Perhaps the most classical result on normal forms of linear control systems under feedback is that of Brunovsky and his celebrated canonical forms [2]. Recently, a counterpart of the Brunovsky result for mechanical control systems and mechanical feedback equivalence has been discovered [11]. It should be recognized that the whole paradigm of linearization of nonlinear control systems by either static or dynamic feedback, along with the concept of differentially flat systems, rely on establishing a feedback equivalence of a nonlinear system to a Brunovsky canonical form. Another example of a canonical form of control systems is the chained form system [10].

The significance of normal forms in control theory stems from two sources. First, the normal forms facilitate to understand the structure and behavior of a control system. This type of knowledge can be extracted from the Brunovsky canonical form that tells us that a system behaves just like a number of independent, parallel chains of integrators. A similar role is played by normal forms of space robots discovered recently in [14]; additionally these last normal forms have been exploited in order to characterize a singular behavior of the robot. Second, and more important, is the applicability of normal forms for the design of control algorithms. Assuming that both the normal form and the feedback transformation producing the normal form are known explicitly, we are in a position to transform by feedback a control problem addressed in an original system to the normal form, solve the problem in the normal form, and then transfer the solution by the inverse feedback back to the original system. This latter avenue will be followed in this paper. In what follows, the application of normal forms to control will be referred to as the Normal Form Approach (NFA).

The objective of this paper is to examine the applicability of the NFA to motion planning of non-holonomic robotic systems. As a sort of the benchmark problem we have chosen the motion planning of free–floating space robots subject to the conservation of angular momentum. Primarily, we use this problem as a means for extending the knowledge on the NFA. For our case study we have chosen a space robot designed in the Space Research Centre (SRC) of the Polish Academy of Science as a laboratory platform for experimental verification of control algorithms for servicing and debris cleaning operations in Space [16]. The design of control algorithms for this robot presents a theoretical challenge and likely will find a practical implementation. Specifically, we study the dynamics of the SRC space manipulator composed of a floating base (a spacecraft) and a 2DOF planar on-board manipulator with revolute joints. The robot is driven exclusively by torques exerted at these joints. It has been assumed that the mounting point of the manipulator on the base can be arbitrary, not necessarily coinciding with its center of mass, compare with [12]. For the SRC manipulator, in a conference paper [24] we have established the feedback equivalence to the chained form system, and computed explicitly the relevant feedback transformations.

In this paper the motion planning problem of the space robot has been decomposed into the planning of joint velocities of the on-board manipulator (velocity level) and the planning of torques exerted at the joints (torque level). Our main contribution consists in using the normal form to designing a motion planning algorithm at the velocity level. Because the feedback transformations to the normal form are known explicitly, the motion planning at velocity level provides a closed form solution. Given the joint velocities, the torque level motion planning has been reduced to the standard inverse dynamics problem for the on-board manipulator. The efficiency of the NFA to motion planning has been demonstrated by numeric computations, whose results have been compared with a solution of the motion planning problem provided by the Endogenous Configuration Space Approach (ECSA), an iterative method of motion planning for non-holonomic robotic systems, based on the concept of robot’s Jacobian [21]. From the results of computations one can learn that in space applications the NFA distinguishes by its accuracy and efficiency, however, may encounter difficulties concerned with computability, complexity, and local existence of the feedback transformations. It turns out that some of these difficulties can be alleviated by a proper choice of the mounting point of the on-board manipulator on the spacecraft.

In order to put the content of this paper in a broader context, it should be recalled that (the kinematics or dynamics of) a non-holonomic robotic system can be represented by means of a driftless control system or a control affine system. The motion planning for such systems constitutes a preliminary step of synthesis of a control algorithm, that amounts to finding a control able to drive the system from its initial to a terminal state, in a prescribed time. Given this control, we can also compute a trajectory joining these two states that serves as a reference trajectory. The motion planning is a typical control problem amenable to a variety of existing methods of control theory; the monograph [8] may serve as the “bible” of this topic. Usually, these methods resort to intense iterative numeric computations; the ECSA is such an example. In contrast to iterative methods, the NFA offers a closed form motion planning algorithm. The second step of synthesis of a control algorithm, following the motion planning, consists in the tracking control of the reference trajectory. To this objective, in [22] we have used a predicitive control algorithm to track a trajectory of a wheeled mobile robot, that has been planned using the ECSA. In [17] a model predictive control algorithm has been applied to tracking control a free-floating space robot. Desirable performance qualities characterize the finite-time tracking algorithm for mobile manipulators, based on the terminal sliding mode manifold technique, described in [4]. As is well known, both the predictive control as well as the sliding mode control exhibit some robustness against model uncertainty and external disturbance. It is worth recalling that the trajectory tracking problem in chained form systems, like our normal form, can be solved by means of the backstepping technique [7]. Another paradigm of trajectory tracking control is the practical stability. In [9] a practical asymptotic stability of a trajectory tracking algorithm has been achieved by exploiting the technique of transverse functions. Three future perspectives of trajectory tracking control have been opened recently by a combination of finite-time and practical stability. A prescribed performance of the tracking error can be achieved by joint neural network and backstepping techniques [18]. A prescribed performance and disturbance estimation can be realized by an event-triggered, fuzzy-adaptive control fostered in [20]. Finally, a design of a fuzzy-adaptive, fault-tolerant control algorithm, able to cope with faults of actuators, has been presented in [19].

The composition of this paper is the following. We begin with Sect. 2 focused on the definition of feedback equivalence. Section 3 introduces the SRC space manipulator, its Lagrangian, and its control system representation. A fundamental theorem on the normal form and the related feedback transformations are provided in Sect. 4. In Sect. 5 results of numerical computations have been collected, culminating in an analysis of advantages and limitations of the NFA presented in Sect. 6. Section 7 concludes the paper. For completeness, in Appendix we have included a proof of Theorem 1.

2 Normal forms

In this section we recall the concepts of feedback equivalence and normal forms of control systems, as well as design a normal form-based motion planning algorithm. Let two control-affine systems

$$\begin{aligned}&\varSigma : \dot{q}=f(q)+G(q)u=f(q)+\sum _{i=1}^mg_i(q)u_i,\nonumber \\&\tilde{\varSigma } : \dot{\tilde{q}}=\tilde{f}(\tilde{q})+\tilde{G}(\tilde{q})\tilde{u}=\tilde{f}(\tilde{q})+\sum _{i=1}^m\tilde{g}_i(\tilde{q})\tilde{u}_i, \end{aligned}$$

(1)

be given, where $q,\tilde{q}\in {\mathbb {R}}^n$ and $u,\tilde{u}\in {\mathbb {R}}^m$, and all vector fields defining these systems are smooth ($C^\infty$). Systems $\sigma$ and $\tilde{\sigma }$ are referred to as feedback equivalent if there exist a smooth diffeomorphism $\tilde{q}=\varphi (q)$ and a control affine feedback $u=\alpha (q)+\beta (q)\tilde{u}$ defined by a smooth function $\alpha (q)$ and a smooth invertible matrix $\beta (q)$, such that

$$\begin{aligned}&\frac{\partial \varphi (q)}{\partial q}(f(q)+G(q)\alpha (q)) =\tilde{f}(\varphi (q)),\;\nonumber \\ {}&\frac{\partial \varphi (q)}{\partial q}G(q)\beta (q)=\tilde{G}(\varphi (q)). \end{aligned}$$

(2)

When the feedback transformations, specifically diffeomorphism $\varphi (q)$, are defined only locally, the feedback equivalence is called local. A control affine system $\tilde{\varSigma }=\varSigma _0$, of simple form, that is feedback equivalent to a given system bears the name of normal form of this system. The “simplicity” of the normal form can hardly be defined mathematically, however, usually it can be decided credibly after inspection of the system equations. Obviously, from the control point of view a crucial requirement is availability of control algorithms for the system in the normal form.

The role of the NFA in motion planning can be summarized in the following way. Suppose that we have established a normal form $\varSigma _0$ of a control system $\varSigma$, along with the feedback transformations $(\varphi (q),\alpha (q),\beta (q))$. Let a motion planning problem be addressed in system $\varSigma$, consisting of finding a control u(t) such that the system’s trajectory at a certain time $T>0$ reaches a prescribed point $q_\mathrm{d}$ in the state space. Knowing the state diffeomorphism $\varphi$ we compute $\tilde{q}_\mathrm{d}=\varphi (q_\mathrm{d})$, and re-formulate the problem in the normal form system: find a control $\tilde{u}(t)$ such that $\tilde{q}(T)=\tilde{q}_\mathrm{d}$. Next, using a motion planning algorithm available for the normal form, we compute a control $\tilde{u}_\mathrm{d}(t)$ and the corresponding trajectory $\tilde{q}_\mathrm{d}(t)$ defining a solution of the problem. Finally, using the inverse feedback transformations, we find the trajectory $q_\mathrm{d}(t)=\varphi ^{-1}(\tilde{q}_\mathrm{d}(t))$ and the control $u_\mathrm{d}(t)=\alpha (q_\mathrm{d}(t))+\beta (q_\mathrm{d}(t))\tilde{u}_\mathrm{d}(t)$ solving the motion planning problem in the original system. If the feedback transformations and their inverse are given explicitly, in the closed form, it may be assumed that the computation of control can be accomplished with arbitrary accuracy and efficiency. For the Reader’s convenience, main features of the NFA have been displayed schematically in Fig. 1 (MPA = motion planning algorithm, F = feedback).

Fig. 1

Top: motion planning problem, bottom: motion planning using NFA

3 Space robot

As an illustration of application of the NFA we shall solve a motion planning problem for the space robot shown in Fig. 2, called the SRC space manipulator.

Fig. 2

SRC space manipulator: view

The SRC space manipulator has been designed in the Space Research Center of the Polish Academy of Science as a prototype device for experiments with interception of various objects in Space, including removing space debris. This is a free-floating robot, composed of a planar base and a planar, 2DOF on-board manipulator with revolute joints. Technical characteristics of the robot can be found in [16]. A schematic picture of the SRC space manipulator is shown in Fig. 3.

Fig. 3

SRC space manipulator: schematic

Various aspects of mathematical modeling and control of the SRC space manipulator have been considered in our previous publications [14, 23], under assumption that the on-board manipulator is attached to the base at its center of mass (i.e. in Fig. 3 both $a,b=0$.). In particular, in [23] two normal forms of the SRC space manipulator were discovered, that in [14] have been extended to robots carrying on board more than 2 DOF manipulators. In a recent paper [24] a normal form (the chained form system) of the SRC space manipulator has been established for the case of arbitrary a and b. This last result will be taken as a guideline in this paper.

Fig. 4

Motion planning: $a=b=0$, feedback (11)

Fig. 5

Motion planning: $a=b=0$, feedback (12)

Fig. 6

Motion planning: $a=b=0.1$, feedback (14)

Fig. 7

Motion planning: $a=b=0.2$, feedback (14)

Fig. 8

Motion planning: $a=b=0.3$, feedback (14)

Fig. 9

Motion planning: $a=b=0$, ECSA

Fig. 10

Motion planning: $a=b=0.1$, ECSA

Fig. 11

Motion planning: $a=b=0.2$, ECSA

Fig. 12

Motion planning: $a=b=0.3$, ECSA

3.1 Lagrangian

In coordinates $q=(\bar{x},\bar{y},\phi ,\theta _1,\theta _2)\in {\mathbb {R}}^2\times {\mathbb {T}}^3$ denoting, respectively, the position of the center of mass of the robot and the orientation of its base with respect to the external frame (X, Y), and joint positions of the on-board manipulator, the Lagrangian of the SRC space manipulator can be expressed, see [24], as

$$\begin{aligned} L(q,\dot{q})&=\frac{1}{2}m_\mathrm{c}(\dot{\bar{x}}^2+\dot{\bar{y}}^2) \nonumber \\&\quad +\frac{1}{2}\mathrm {I}_{P}\dot{\phi }^2+\frac{1}{2}\mathrm {A}(\dot{\phi }+\dot{\theta }_{1})^2+ \frac{1}{2}\mathrm {B}(\dot{\phi }+\dot{\theta }_{12})^2\nonumber \\&\quad +\mathrm {C}(\dot{\phi }+\dot{\theta }_1)(\dot{\phi }+\dot{\theta }_{12})c_2 \nonumber \\&\quad +\mathrm {D}\dot{\phi }(\dot{\phi }+\dot{\theta }_1)(ac_1+bs_1) \nonumber \\&\quad +\mathrm {E}\dot{\phi }(\dot{\phi }+\dot{\theta }_{12})(ac_{12}+bs_{12}). \end{aligned}$$

(3)

The notations used in (3) have the following meaning: $m_i$, $l_i$, $d_i$, $i=1,2,$ denote the mass, the length and the position of the center of mass of the ith link, a, b are coordinates of the mounting point with respect to the body frame $(X_b,Y_b)$, $\theta _{12} = \theta _1 + \theta _2$, and $s_i$, $c_i$, denote, respectively, $\sin (\theta _i)$ and $\cos (\theta _i)$. Also, we define

$$\begin{aligned}&m_\mathrm{c}=M+m_1+m_2,\; \\&\mathrm {A}=\frac{m_1m_2(l_1-d_1)^2+M (m_1 d_1^2+m_2l_1^2)}{m_c},\\&\mathrm {B}=\frac{(M+m_1)m_2d_2^2}{m_\mathrm{c}},\; \\&\mathrm {C}=\frac{m_1m_2(l_1-d_1)d_2+Mm_2l_1d_2}{m_\mathrm{c}},\;\\&\mathrm {D}=\frac{M(m_1d_1+m_2l_1)}{m_\mathrm{c}},\; \\&\mathrm {E}=\frac{Mm_2d_2}{m_\mathrm{c}},\; \\&\mathrm {I}_{\mathrm{P}}=\mathrm {I}+\frac{M(m_1+m_2)(a^2+b^2)}{m_\mathrm{c}}, \end{aligned}$$

where symbols M and $\mathrm {I}$ refer to the mass and the moment of inertia of the base. Numerical values of parameters of the SRC space manipulator will be specified in the section devoted to computations.

Because the robot is free-floating, the linear and angular momenta are conserved during its motion, resulting in the following equations of motion

$$\begin{aligned}&m_\mathrm{c}\dot{\bar{x}}=\mathrm {p}_1,\quad m_\mathrm{c}\dot{\bar{y}}=\mathrm {p}_2 \end{aligned}$$

(4)

$$\begin{aligned}&F\dot{\phi }+G\dot{\theta }_1+H\dot{\theta }_2=\mathrm {p} \end{aligned}$$

(5)

$$\begin{aligned}&\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial L}{\partial \dot{\theta }_i}-\frac{\partial L}{\partial \theta _i}=\tau _i,\quad i=1,2. \end{aligned}$$

(6)

Constants $\mathrm {p}_1$, $\mathrm {p}_2$ and $\mathrm {p}$ in Eqs. (4) and (5) represent the conserved linear and angular momenta. Equation (6) refers to the dynamics of the on-board manipulator whose joints are actuated by torques $\tau _1$ and $\tau _2$. It is clearly seen from (4) that the center of mass of the robot moves uniformly along a straight line, in a way completely independent of the motions described by the remaining two equations. For this reason, we shall focus on Eq. (5), and solve the motion planning problem assuming joint velocities as controls. Eventually, the actuating torques will be determined from equation (6).

3.2 Control system representation

For $p=0$ (e.g. when at the initial time instant the base and the joints do not move), condition (5) of the angular momentum conservation results in a driftless control system

$$\begin{aligned} \dot{q}=g_1(q)u_1+g_2(q)u_2, \end{aligned}$$

(7)

where $q=(\phi ,\theta _1,\theta _2)\in {\mathbb {T}}^3$, defined by a pair of control vector fields $g_1(q)=(-\frac{G}{F},1,0)^\top$ and $g_2(q)=(-\frac{H}{F},0,1)^\top$. These vector fields are determined by three functions

$$\begin{aligned} F(\theta _1,\theta _2)&=\mathrm {I}_P+\mathrm {A}+\mathrm {B}+2\mathrm {C}c_2+2\mathrm {D}(ac_1+bs_1)\nonumber \\&\quad +2\mathrm {E}\,(ac_{12}+bs_{12}),\nonumber \\ G(\theta _1,\theta _2)&=\mathrm {A}+\mathrm {B}+2\mathrm {C}c_2+\mathrm {D}(ac_1+bs_1)\nonumber \\&\quad +\mathrm {E}\,(ac_{12}+bs_{12}), \nonumber \\ H(\theta _1,\theta _2)&=\mathrm {B}+\mathrm {C}c_2+\mathrm {E}(ac_{12}+bs_{12}) \end{aligned}$$

(8)

computed in [24]. In the same reference it has been shown that if the function

$$\begin{aligned} \sigma (q)&=(\mathrm {B}\mathrm {D}-\mathrm {C}\mathrm {E})(-as_1+bc_1)\nonumber \\&\quad +(\mathrm {C}\mathrm {D}-\mathrm {A}\mathrm {E})(-as_{12}+bc_{12})\nonumber \\&\quad +(\mathrm {D}\mathrm {E}(a^2+b^2)-\mathrm {C}\mathrm {I}_P)s_2 \end{aligned}$$

(9)

is nonzero, system (7) is controllable.

4 Chained form system

In reference [24] we have established that the driftless system (7) is feedback equivalent to the chained form system (a normal form)

$$\begin{aligned} \varSigma _0 : \dot{z}_1=v_1,\;\dot{z}_2=v_2,\;\dot{z}_3=z_2v_1. \end{aligned}$$

(10)

This is elucidated by the following

Theorem 1

• 1. If $a=b=0$ then system (7) is feedback equivalent to the chained form system (10) on condition that $\sin \theta _2\ne 0$. The feedback transformation establishing this equivalence has either the form

  $$\begin{aligned}&z_1=\phi +\theta _1,\;z_2=-2\mathrm {C}c_2,\nonumber \\&z_3=(\mathrm {I}+\mathrm {A}+\mathrm {B})\phi +(\mathrm {A}+\mathrm {B})\theta _1+\mathrm {B}\theta _2+\mathrm {C}s_2,\nonumber \\&v_1=\frac{\mathrm {I}}{F}u_1-\frac{H}{F}u_2,\;v_2=2\mathrm {C}s_2u_2, \end{aligned}$$

  (11)

  or the form

  $$\begin{aligned} z_1&=\theta _1,\;z_2=-\mathrm {sign}(s_2)\frac{G}{F},\nonumber \\ z_3&=\mathrm {sign}(s_2)\phi -\frac{1}{4}\pi +\frac{1}{2}\theta _2\nonumber \\&\quad +\frac{1}{2}\frac{U-2B}{\sqrt{U^2-4C^2}}\arcsin \frac{2\mathrm {C}+Uc_2}{U+2\mathrm {C}c_2}\nonumber \\ v_1&=u_1,\;v_2=\frac{2\mathrm {CI}|s_2|}{(U+2\mathrm {C}c_2)^2}u_2. \end{aligned}$$

  (12)

  Hereabout, $\mathrm {sign}(\cdot )$ denotes the sign function, and $|\cdot |$ is the modulus. Functions F, G and H need to be taken as

  $$\begin{aligned}&F(\theta _2)=\mathrm {I}+\mathrm {A}+\mathrm {B}+2\mathrm {C}c_2,\\&G(\theta _2)=\mathrm {A}+\mathrm {B}+2\mathrm {C}c_2,\\&H(\theta _2)=\mathrm {B}+\mathrm {C}c_2, \end{aligned}$$

  and $U=\mathrm {I}+\mathrm {A}+\mathrm {B}$.

• 2. If $a\ne 0$ or $b\ne 0$ then system (7) is feedback equivalent to the chained form system (10) provided that function $\sigma (q)$ defined by (9) and function

  $$\begin{aligned} P(q)=-\mathrm {C}s_2+\mathrm {E}(-as_{12}+bc_{12}) \end{aligned}$$

  (13)

  are nonzero. The feedback transformation establishing this equivalence is given by

  $$\begin{aligned} z_1&=h_1(q)=\theta _1,\nonumber \\ z_3&=h_2(q)=-\mathrm {sign}(P)\phi -\frac{1}{2}\arcsin \frac{F-U}{2f}\nonumber \\&\quad +\frac{Z}{2\sqrt{d}}\arcsin \frac{FU-d}{2fF},\nonumber \\ z_2&=h_3(q)=\mathrm {sign}(P)\frac{G}{F}+\frac{\partial h_2(q)}{\partial \theta _1} \nonumber \\ v_1&=u_1,\;v_2=\frac{\partial h_3(q)}{\partial \theta _1}u_1+\mathrm {sign}(P)\frac{2\sigma }{F^2}u_2, \end{aligned}$$

  (14)

  where functions F and G are defined by (8), and

  $$\begin{aligned}&U=\mathrm {I}_P+\mathrm {A}+\mathrm {B}+2\mathrm {D}(ac_1+bs_1),\;Z=U-2\mathrm {B},\nonumber \\&f=\sqrt{\mathrm {C}^2+2\mathrm {C}\mathrm {E}(ac_1+bs_1)+\mathrm {E}^2(a^2+b^2)},\nonumber \\&d=U^2-4f^2. \end{aligned}$$

  (15)

A proof of this theorem has been provided originally in op. cit., but, to make this paper self-contained, it will be reproduced in Appendix. It should be noticed that the feedback (11) has been tuned specifically to the form of system (7) with $a=b=0$, whereas feedback (12) comes out after a substitution of $a=b=0$ into the general feedback transformations (14).

5 Numerical computations

In this section we demonstrate an application of the normal form (10) to an example motion planning problem for the SRC space manipulator (7). The motion planning problem consists in finding a control that will carry out the system from the initial configuration $q_0=(45{^\circ },120{^\circ },80{^\circ })^\top$ to the final configuration $q_d=(0{^\circ },180{^\circ },140{^\circ })^\top$ within time $T = 10s$. The following numerical values of the robot’s geometric and mechanical parameters have been borrowed from [15]:

$$\begin{aligned} l_1&= 0.619m,\; l_2 = 0.6m,\\ d_1&= 0.313m,\; d_2 = 0.287m,\\ M&= 12.9kg,\;I = 0.208kgm^2,\\ m_1&= 4.5kg,\; m_2 = 1.5kg. \end{aligned}$$

Solution of the problem for $a=b=0$ will be found first using the feedback transformations (11) and (12), and then for several cases of $a\ne$ and $b\ne 0$ by means of the feedback transformations (14). For comparison, for nonzero a and b, a solution of this motion planning problem provided by the Endogenous Configuration Space Approach will also be delivered.

For the Normal Form Approach, the initial and final configurations have been first transferred to the normal form in accordance with the procedure described in section 2, yielding $z_0$ and $z_d$. Then, the motion planning problem in the chained form system has been solved by adopting polynomial control functions, compare [13, 25],

$$\begin{aligned} v_1(t)= & {} a_1 + b_1t + c_1t^2 + d_1t^3\quad \text {and} \\ v_2(t)= & {} a_2 + b_2t + c_2t^2, \end{aligned}$$

subject to a request that the initial and final values of controls (in the normal form as well in the original system) need to be zero. The coefficients of control functions have been computed symbolically as

$$\begin{aligned}&a_1=0,\; \\&b_1 = \frac{4(22w_1w_2 - 35w_3 + 35w_1z_{20})}{3w_2T^2},\\&c_1 = -\frac{4(19w_1w_2 - 35w_3 + 35w_1z_{20})}{w_2T^3},\; \\&d_1 = \frac{140(w_1w_2 - 2w_3 + 2w_1z_{20})}{3w_2T^4}, \end{aligned}$$

and

$$\begin{aligned} a_2=0,\;b_2=\frac{6w_2}{T^2},\;c_2=-\frac{6w_2}{T^3}, \end{aligned}$$

where

$$\begin{aligned} w_1 = z_{d1} - z_{01},\;w_2 = z_{d2} - z_{02},\; w_3 = z_{d3} - z_{03}, \end{aligned}$$

and $T=10$.

For $a=b=0$, results of solving the problem using the NFA with feedback (11) are displayed in Fig. 4, while for the feedback transformation (12) – they are shown in Fig. 5. The next Figs. 6, 7 and 8 show solutions of this problem, respectively, for $a=b=0.1$, $a=b=0.2$, and $a=b=0.3$, obtained by the NFA employing the feedback transformation (14). These figures display trajectories of the base orientation and the joint positions, trajectories of the normal form variables z, velocity controls in system (5), and torques driving the system (6). Correspondingly, Figs. 9, 10, 11 and 12 present solutions for these same parameters a and b, provided by the ECSA based on the Jacobian pseudoinverse, and making use of trigonometric control functions with constant terms and the first order harmonics. Domains of existence of the feedback (14) for various a and b are presented in Figs. 13 and 14.

6 Discussion

While conducting computations we have made a number of observations that reveal advantages and limitations of the NFA. A fundamental advantage is that the normal form approach provides a closed form solution to the motion planning problem. This is accomplished by replacing a complicated control problem in the original system by a much simpler problem addressed in the normal form, and using direct and inverse feedback transformations, see Fig. 1. Basically, if the feedback transformations are available in the closed form, the computation of control signals should be as accurate and efficient as our computing machinery allows. In our case, a moderately optimized program code has resulted in the computation time of the velocity control u(t) about 0.2s for the case of $a=b=0$, and in average of 1.3s for $a,b\ne 0$. When the torques are to be found, an extra 0.15s needs to be added for zero a and b, and 0.4s otherwise. This additional time is devoted to solving the inverse dynamics problem in system (6), accompanied by the interpolation of some signals. Computations referring to the ECSA have taken about 1.5s, plus extra 0.5s to determine the torques. Our computations have been done on a PC equipped with i7-6700HQ CPU 2.60GHz and 16 GB RAM. Note that the computation time for the ECSA, as an iterative method, will grow up when increasing the required accuracy of results, whereas the available accuracy of the NFA is practically unlimited.

Fig. 13

Domains of existence of feedback (14): $a=b=0.1$ and $a=b=0.2$

Fig. 14

Domains of existence of feedback (14): $a=b=0.3$ and $a=1.0$, $b=-1.0$

On the other hand, the following limitations of the NFA need to be recognized:

• 1. Computability of feedback transformations: Typically, conditions for the existence of feedback transformations that establish the equivalence between control systems (1) are not constructive. If they are satisfied, the feedback transformations need to be found by solving the equivalence Eq. (2) for $(\varphi (q),\alpha (q),\beta (q))$, given the vector fields $f,\tilde{f}$, $g_i,\tilde{g}_i$. These are PDEs whose explicit solution may be hard or just impossible to obtain explicitly. This is not our case, but this difficulty is faced when studying normal forms of the extended SCR space manipulator derived in [14].

• 2. Complexity of feedback transformations: Even if the solution of the equivalence equations is available in the closed form, it may be complex, like our formula (14). A closer look at these transformations prompts two observations. Firstly, formulas for the partial derivatives $\frac{\partial h_2}{\partial \theta _1}$ and $\frac{\partial h_3}{\partial \theta _1}$ look quite complicated, so we have restrained from writing them explicitly and left them to be processed by computer. Secondly, the inversion of the diffeomorphism $z=h(\phi ,\theta _1,\theta _2)$ necessarily invokes numerical computations. To be more specific, given z, it is easy to determine $\theta _1=z_1$. Next, given $\theta _1$, and using the fact that $h_3$ does not depend on $\phi$, we use the equation $z_2=h_3(q)$ in order to find $\theta _2$. This is allowed, at least locally, by the Implicit Function Theorem since $\frac{\partial h_3}{\partial \theta _2}\ne 0$ whenever $\sigma (q)\ne 0$. Therefore, $\theta _2(z_1,z_2)$ can be computed numerically, e.g. using the Newton algorithm

  $$\begin{aligned} \frac{\mathrm{d}\theta _2(s)}{\mathrm{d}s}=-\gamma \left( \frac{\partial h_3(z_1,\theta _2)}{\partial \theta _2}\right) ^{-1}(h_3(z_1,\theta _2)-z_2), \end{aligned}$$

  and passing to the limit $\theta _2=\lim _{s\rightarrow +\infty } \theta _2(s)$. Having found $\theta _1$ and $\theta _2$, we finish by analytically computing $\phi (z_1,z_2.z_3)$ from the identity $z_3=h_2(\phi ,z_1,z_2)$. We have been following exactly this way in our computations. Noticeably, within the NFA we have just a single iterative numerical process, whereas the ECSA involves multidimensional numerical computations.

• 3. Local definiteness of feedback transformations: In the feedback equivalence the diffeomorphism $\varphi (q)$ is usually defined only locally, on a certain neighborhood of a specific point, so from the control point of view establishing the size of its domain of existence is of vital importance. This has been done for the feedback transformations introduced in Theorem 1. As long as the manipulator is mounted at the center of mass of the base, the feedback transformation (11) exists if and only if $\sin \theta _2\ne 0$. The domain of existence of this feedback is quite large and has a simple shape. It is easily observed that, since for $a=b=0$ the zero loci of functions $\sigma (q)$ and P(q) coincide, the domains of existence of (11) and (12) are the same. The situation complicates if the mounting point of the on-board manipulator is different than the center of mass. The feedback transformation (14) is well defined on condition that both the functions $\sigma (q)$ and P(q) are nonzero. This makes the domains of existence of (14) smaller and of complicated shapes that restricts applicability of the NFA. In order to vizualize the domains of existence of feedback transformations (14) as depending on the position of the mounting point we present a number of plots for $a=b=0.1$, $a=b=0.2$, $a=b=0.3$ and $a=1.0$, $b=-1.0$. It follows that applicability of the NFA depends strongly on the choice of the mounting point; the approach seems fairly efficient in the three former cases, but readily impossible in the last case. It turns out that, if the NFA is to be used for the control of a space robot, it makes sense to take into account in the process of space robot design the plots like Figs. 13 and 14, and mount the on-board manipulator appropriately.

7 Conclusion

The objective of this paper has been to examine the applicability of the Normal Form Approach to motion planning of robots. A motion planning problem for a space manipulator has been chosen as a sort of benchmark problem. This problem has two features: on the one hand, refers to a practically meaningful example of a control system, and on the other, involves rather complex transformations producing the normal form. A motion planning algorithm for this space robot has been designed using the chained system normal form of the robot’s dynamics. Thanks to explicitly known feedback transformations a major part of the solution of the motion planning problem has been obtained in the closed form. Performance of the NFA-based algorithm has been examined, and compared with the ECSA-based planning representing the class of iterative methods. Potential advantages and disadvantages of the NFA in motion planning have been recognized.

Appendix: proof of theorem 1

We shall confine to demonstrating item 2 that refers to the case of nonzero a and b. To this objective, relying on the result of Murray and Sastry [10], pp. 369–370, we associate with system (7) the distributions

$$\begin{aligned} \varDelta _0&=\mathrm{span}\{g_1,g_2,g_{12}\},\; \nonumber \\ \varDelta _1&=\mathrm{span}\{g_2,g_{12}\},\;\varDelta _2=span\{g_2\}. \end{aligned}$$

(16)

It is easily checked that outside posture singularities $\sigma (q)=0$, see (9), these distributions are involutive and of dimension 3, 2 and 1. This allows us to introduce coordinate functions $h_1$, $h_2$ and $h_3$ satisfying the following conditions

$$\begin{aligned} dh_1\varDelta _1=0,\;dh_1g_1=1,\;dh_2\varDelta _2=0,\;h_3=dh_2g_1. \end{aligned}$$

(17)

Now, using the form of vector fields $g_1$, $g_2$ and $g_{12}$, we obtain

$$\begin{aligned} h_1=\theta _1,\; -H\frac{\partial h_2}{\partial \phi }+F\frac{\partial h_2}{\partial \theta _2}=0. \end{aligned}$$

(18)

The partial differential equations defining $h_2$ can be solved by means of the method of characteristics. Following this method we need to find a first integral of vector field $X=(-H,0,F)^\top$ that for fixed $\theta _1$ satisfies the differential equation

$$\begin{aligned} y&-d\phi =\frac{H}{F}d\theta _2 \nonumber \\&=\frac{\mathrm {B}+\mathrm {C}c_2+\mathrm {E}(ac_{12}+bs_{12})}{\mathrm {I}_P+\mathrm {A}+\mathrm {B}+2\mathrm {C}c_2+2\mathrm {D}(ac_1+bs_1)+2\mathrm {E}(ac_{12}+bs_{12})}d\theta _2. \end{aligned}$$

(19)

Referring to notations (15) we observe that $F=Z+2H$, Z being independent of $\theta _2$, so the right hand side of (19) can be written as

$$\begin{aligned} \frac{F-Z}{4F}\frac{\mathrm{d}F}{P}, \end{aligned}$$

(20)

where $P=-\mathrm {C}s_2+\mathrm {E}(-as_{12}+bc_{12})$, see (13). The next observation is that

$$\begin{aligned} P^2+\frac{(F-U)^2}{4}=f^2, \end{aligned}$$

(21)

f depending only on $\theta _1$ or, equivalently,

$$\begin{aligned} P=\mathrm {sign}(P)\sqrt{f^2-\frac{(F-U)^2}{4}}. \end{aligned}$$

(22)

Finally, all these observations result in the differential equation

$$\begin{aligned} -\mathrm {sign}(P)\mathrm{d}\phi&=\mathrm {sign}(s_2) \frac{1}{2}\frac{(F-Z)\mathrm{d}F}{F\sqrt{4f^2-(F-U)^2}} \nonumber \\&=\frac{1}{2}(I_1-ZI_2) \end{aligned}$$

(23)

whose solution depends on two elementary integrals

$$\begin{aligned}&I_1=\int \frac{\mathrm{d}F}{\sqrt{4f^2-(F-U)^2}},\quad \text {and}\quad \nonumber \\&I_2=\int \frac{\mathrm{d}F}{F\sqrt{4f^2-(F-U)^2}}. \end{aligned}$$

(24)

In computation of these integrals we use the fact that $d=U^2-4f^2>0$. In this way, in accordance with the method of characteristics, we obtain the coordinate function $h_2(q)$. Having found $h_2$, we set $h_3=dh_2g_1$ that, together with $h_1=\theta _1$, determines the coordinate change. In order to find the feedback, we compute

$$\begin{aligned} \dot{z}_1&=\dot{\theta }_1=u_1=v_1,\nonumber \\ \dot{z}_2&=\frac{\partial h_3}{\partial \phi }\dot{\phi }+ \frac{\partial h_3}{\partial \theta _1}u_1+\frac{\partial h_3}{\partial \theta _2}u_2=\frac{\partial h_3}{\partial \theta _1}u_1 \nonumber \\&\quad + \mathrm {sign}(P)\frac{\partial }{\partial \theta _2}\left( \frac{G}{F}\right) u_2+ \frac{\partial ^2h_2}{\partial \theta _2\partial \theta _1}u_2 \nonumber \\&=\frac{\partial h_3}{\partial \theta _1}u_1+\mathrm {sign}(P) \left( \frac{\partial }{\partial \theta _2}\left( \frac{G}{F}\right) -\frac{\partial }{\partial \theta _1}\left( \frac{H}{F}\right) \right) u_2\nonumber \\&\quad =\frac{\partial h_3}{\partial \theta _1}u_1+\mathrm {sign}(P)\frac{2\sigma }{F^2}u_2=v_2,\nonumber \\ \dot{z}_3&=\frac{\partial h_2}{\partial \phi }\dot{\phi }+\frac{\partial h_2}{\partial \theta _1}u_1+\frac{\partial h_2}{\partial \theta _2}u_2\nonumber \\&\quad =-\mathrm {sign}(P)\left( -\frac{G}{F}u_1-\frac{H}{F}u_2\right) \nonumber \\&\quad +\frac{\partial h_2}{\partial \theta _1}u_1 -\mathrm {sign}(P)\frac{H}{F}u_2\nonumber \\&\quad =\left( \mathrm {sign}(P)\frac{G}{F}+\frac{\partial h_2}{\partial \theta _1}\right) u_1=z_2v_1. \end{aligned}$$

(25)

Obviously, having substituted into (14) $a=b=0$, we get (12).