Any correspondence concerning this service should be sent to the repository
administrator:
staff-oatao@listes-diff.inp-toulouse.fr
O
pen
A
rchive
T
OULOUSE
A
rchive
O
uverte (
OATAO
)
OATAO is an open access repository that collects the work of Toulouse researchers and
makes it freely available over the web where possible.
This is an author-deposited version published in:
http://oatao.univ-toulouse.fr/
Eprints ID: 16571
To cite this version:
Evain, Hélène and Rognant, Mathieu and Alazard, Daniel and
Mignot, Jean Nonlinear dynamic inversion for redundant systems using the EKF
formalism. (2016) In: 2016 American Control Conference (ACC), 6 July 2016 - 8 July
2016 (Boston, United States).
Nonlinear Dynamic Inversion for Redundant Systems using the EKF
Formalism
H´el`ene Evain
1, Mathieu Rognant
1, Daniel Alazard
2and Jean Mignot
3Abstract— This paper presents an allocator for over-actuated systems based on the Extended Kalman Filter (EKF). The main advantages of the proposed approach are the greater flexibility in handling the constraints and its real-time capabilities. Based on the literature, theoretical convergence results, which ensure the convergence towards the local optimal values looked for, are presented. Another formulation of the kinematic equations of redundant systems that meet some constraints is also proposed in order to go through and/or avoid singularities. The two formulations are combined and applied to an academic example (a planar redundant manipulator arm).
Keywords: redundant actuators, allocation, non-linear dy-namic inversion, EKF, singularities.
I. INTRODUCTION
When a system is over-actuated, control allocation meth-ods are needed to divide the work over the actuators, in order to generate the required forces or torques on the system. Moreover, depending on the configuration of the actuators, internal singularities can occur (configurations where it be-comes impossible to create a force in a direction even if the maximum capacity of the actuators is not reached). Other issues control allocation must face are the saturations of the actuators and the suitability for real-time implementation. These control allocation problems include the control of multi-DOFs (degrees of freedom) manipulator arms, cable robots and submarines. We propose to focus only on the allocation problem (see Fig. 1), and not on the closed-loop regulation. In this paper, vectors are written in bold small
Controller t Allocator yc System u y Sensor ym
Fig. 1. Block diagram of a typical control loop letters and the matrices are in bold capital letters.
Let’s first write a general controllable nonlinear system (1).
(
˙x = f (x) + G(x)u
y = h(x) (1)
This work was supported by ONERA and CNES.
1H. Evain (Ph.D student) and M. Rognant are with ONERA,
Toulouse, France (emails: helene.evain@onera.fr; mathieu.rognant@onera.fr)
2D. Alazard is with ISAE SUPAERO, Toulouse, France 3J. Mignot is with CNES, Toulouse, France
where x ∈ Rn is the state, u ∈ Rm the control input,
y ∈ Rl the controlled output, f ∈ Rn
, h ∈ Rl differentiable
functions, and G ∈ Rn×m a matrix function. m ≥ l for an
over-actuated (redundant) system.
In a classic nonlinear dynamic inversion, extensively stud-ied for the past thirty by researchers like A. Isidori in [1] for instance, the successive Lie derivatives of y are calculated so that the input appears explicitly, assuming the nonlinear system has a relative degree.
For a system of relative degree one, one can write: ˙
y = J(x) ˙x (2)
with J ∈ Rl×nthe jacobian matrix of h. It then derives :
J(x)G(x)u = ˙y − J(x)f (x) (3) If J(x)G(x) is invertible, then the solution is:
u = (J(x)G(x))−1(−J(x)f (x) + ˙y) (4) where ˙y is imposed to follow specifications. In a general case, J(x)G(x) is a l × m matrix and can be rank-deficient for some values of x. Then its inversion can be carried out using the approach developped in this paper. For simplicity, this paper focuses on the case where f is the null function, G is the identity matrix and m = n ≥ l. It comes down to the pure allocation problem where ˙x = u.
Therefore, this paper focuses on computing ˙x which has to respect (2) with a given ˙y, as well as saturation constraints and avoiding/passing singularities. The literature on the topic is rather extensive, but only methods that can be implemented in real-time are mentionned here. For a comprehensive survey of usual allocation methods (also called kinematic control), see the work of B. Siciliano and L. Sciavicco [2] [3] for example. The most common method is the Moore-Penrose (MP) pseudo-inversion:
˙x = JT(JJT)−1y = J˙ †y˙ (5) However, when the matrix JJT becomes non-invertible, the system is in a singularity and (5) fails to compute ˙x. To avoid this issue, L. Sciavicco and B. Siciliano [4] propose to use the Jacobian transpose which always exists instead of the MP pseudo-inverse. In addition, stability results are given with this formulation for closed-loop systems. Another possiblity is to add a term in the MP formulation so that the inversion becomes always possible (6): it is the Singular Robust Inverse method (SRI) (or Damped Least-Square solution), first introduced by [5] [6].
with W a matrix to tune. The main difficulty is to choose W. Different possibilities exist, including the use of the Singular Value Decomposition [7], or methods to take constraints into account [8]. These methods are therefore more robust to singularities, but create errors and do not ensure the passing of the singularities.
A more general solution of (2) is:
˙x = J†y + (I − J˙ †J)xv (7)
with xvan arbitrary vector. (I − J†J) is a projection into the
null-space of J. It is called the gradient method since xv is
often calculated by optimizing different cost functions, as in [9] which maximizes the dynamic manipulability.
Other methods that do not create errors include the “Extended Jacobian” where constraints are added in the J matrix. J. Bailleuil in [10] for instance worked on possible constraint formulations. However, the main issues are that constraints have to be well modelled because they need to be always verified, and numerical singularities can occur [2]. C.W. Wampler [11] studied in depth the inverse kinematics functions, in particular the reduction of the workspace in domains where no singularities exist and the problem can be inverted (see also [12]). [13] propose a task-priority based method. However, the main issue with these methods is that they reduce the capabilities of the system by restricting the workspace and imposing hard constraints.
Optimization solutions are also provided in the literature. In particular, T. Fossen and T. Johansen [14] give a survey of possible optimizations for controlling underwater vehicles. Quadratic optimization with constraints is presented, with multi-parametric quadratic resolutions methods for instance [14]. Nonlinear optimization also exists, which can be trans-formed into linear programming problems [14]. Another optimization method is the use of the Karush-Kuhn-Tucker conditions [15] after defining an optimization problem. An iterative algorithm is then proposed. These methods give optimal solutions, but require a high load of calculation.
Finally, different methods are nowadays proposed, like the Cyclic Co-ordinate Descent where each actuator is moved independently from the others and the Triangulation [16]. They are fast but cannot handle global constraints. Alterna-tives methods based on Neural Networks are also proposed, as well as hybrid methods that mix numerical and analytical calculations [17].
In this paper, the Extended Kalman Filter (EKF) formalism is used to do nonlinear dynamic inversions for redundant systems. It can take constraints into account in a new way and can be easily implemented in real-time. A comparison with respect to the methods listed before is outlined, an ex-ample of theoretical implementation and convergence proofs are presented. Then, a new formulation of the kinematic equations is proposed, which can better handle the singu-larities. It is well adapted for mechanical systems with some assumptions, and is compatible with the EKF. Finally, the EKF and the singularities-bypassing method are applied on an academic redundant manipulator arm example.
II. PROPOSED ALLOCATION METHOD : THE EXTENDED KALMAN FILTER
A. Advantages of the method and comparisons
To the best knowledge of the authors, using the EKF for allocation has not been done before. This method minimizes the same criterion as a SRI, of the type r = 12(m − Cv)TR−1(m − Cv) +1
2(v − vd)P
−1(v − v
d) with: (i) m
the measurements ( ˙y in the SRI case), (ii) v the estimation of the state variables ( ˙x in the SRI case), (iii) vd the predicted
value of v, (iv) C the jacobian matrix of the measurement equations (J in the SRI case) and (v) P and R weight matrices. In the EKF, the gain matrix P is updated with a weight matrix Q and a prediction model, so as to minimize the covariance of the estimation errors.
The EKF also provides more flexibility than methods where constraints are added in the jacobian matrix [10]. Indeed, as some equations may not be exactly verified, they can be more or less weighted through matrices R and Q. It can be useful if the modelling is not perfect, especially when taking constraints into account, or simplified models of dynamics. Also, the possibility of time-varying these matrices gives a flexibility and can act as a task-priority controller since it enables the system to emphasize on different equations depending on the current configuration of the system.
The formulation is not very far from the optimization problems, nevertheless the solution proposed (the usual al-gorithm to solve the EKF) can be carried out in real-time, it is therefore a powerful tool.
Finally, another interesting point is the convergence results that already exist for EKFs, and which can be applied to help tuning the gains so as to remain optimal and convergent. B. The Extended Kalman Filter
This new allocation method uses the Extended Kalman Filter (EKF) that was initially introduced by [18]. It is commonly used to estimate state variables based on the eval-uation of differents eqeval-uations. It is a local optimal observer for nonlinear systems. The discrete formulation is chosen here to remain consistent with the next section, however continuous implementations can be carried out. Let’s take the first-order approximation of (2):
∆y = J(x)∆x with (
∆y = y(t) − y(t − Ts)
∆x = x(t) − x(t − Ts)
(8)
with Ts a chosen sampling period. The goal of EKF is to
estimate an optimal ∆x for a desired ∆y.
The estimated variables in the EKF can be chosen simply as (9): v =v1 v2 with ( v1= ∆x v2= y (9)
Considering a reference model f1on the internal states
dy-namics as well as equation (8), the Kalman model becomes: (
v1(t + Ts) = f1(v1(t), v2(t)) + v1 v2(t + Ts) = J(x)v1(t) + v2(t) + v2
(10) where the state evolution errors v1 and v2 are neglected in the prediction model and sized by the covariance matrix Q. f1can be any function associated with the studied system. In
the general case, the prediction equations read : ˆv(t + Ts) =
fk(ˆv(t)). The Jacobian matrix associated with this model is
denoted F and is of size (n + l) × (n + l): F =∂fk
∂v (11)
The measurement equation (m is the measurement vector) is: m =m1 m2 = r(v)+mwith ( m1(t) = v2(t) + m1 m2(t) = fm(v1(t)) + m2 (12) m1 is the reference input yc sent by the controller for
instance (Fig. 1). To ensure the observability of the EKF, the second equation of (12) is added, and can be used to model additional constraints (e.g. saturations). Perturbations m1 and m2 are sized by the covariance matrix R. Let us denote C = ∂v∂r.
The EKF theory provides a local optimal estimate ˆv. The parameters that need to be set in this filter are P0the initial
covariance matrix on the estimate errors, R and Q. The equations of evolution of the discrete EKF are not given here, but can be easily found in the literature [18].
The method gives the best least-square solution of the problem posed. Since the equations are deterministic, the solution is a compromise between the different equations that can include constraints on the system. The different weights give the importance of each equation.
C. Convergence results
1) Existing results: The convergence of the allocator is an important point to verify. Some results already exist for allocation methods, as the Jacobian transposed [4], already cited. The convergence of the error dynamics was proven in a closed-loop. Another result by T. Johansen [19] shows the exponential global convergence of a closed-loop which contains an exponentially stable controller and an asymp-toticly optimal allocator. [20] gives a generic form of an input allocation with a first-order dynamics that is internally stable, with different characteristics and with saturation constraints. Therefore, some methods have a stability proof, but not all. Moreover, except for the last one, they are closed-loop results. The method from [20] seems to be less friendly for adding constraints (except saturations). This paper focuses only on the open-loop local convergence of the allocator. Therefore, it gives an interesting result whatever the con-troller used, as well as when there is no concon-troller. The Lyapunov function for the proof is from [21] and can be used to show the global stability of the closed-loop system.
2) Theory: [21] gives the conditions for the EKF to be a local uniform asymptotic observer, and thus for ˆv (resp.
ˆ
v2) to tend to v (resp. y). In our case, these conditions will
guarantee the proposed allocation method to converge. These conditions are :
• F is invertible at each time step.
• The following norms are bounded :|| F ||, || F−1 ||,
|| C ||, || R ||,|| Q ||,||| ∂F/∂v |||,||| ∂C/∂v |||.
• Let’s call k(p, q) = r(p) − r(q) − C(p)(p − q). There exists a scalarm such that
| k(p, q) |≤ m ||| ∂C/∂v ||| | p − q |2 • The system is uniformly observable, or equivalently (F,
C) verify the uniform observability condition given in [21], taken from the article of J.J. Deyst and C.F. Price [22], as long as the estimation errors are small enough. That gives boundaries on the estimation covariance error matrix.
• The last condition ensuring the Lyapunov function V = (ˆv − v)TP−1(ˆv − v) to be decreasing is omitted for
brevity and is detailed in [21].
That gives conditions and a proof of the convergence of the nonlinear dynamic inversion proposed. With this proof, it is possible to size the covariance matrices of the EKF and to ensure convergence when additional constraints (that can depend on the configuration of the system) are added. Never-theless, this formulation does not ensure that the singularities will be passed. As in the SRI formulations, a solution always exists, but for the convergence proof, well-conditionned matrices are needed, that is not always the case with the matrix J. Moreover, there may be a lack of stability in the vicinity of a singularity because our variational formulation of (10) may not be representative enough: if a singularity-avoidance is needed and is not well-modelled, the system cannot search in the null-space of J and then the errors created make the system go outside the stability region. That is why it is proposed to improve the model implemented in the EKF, so as the optimal value towards which the system converges always exists. The second-order Taylor expansion, which will help passing the singularities, is now studied.
III. A SINGULARITY-HANDLING FORMULATION Let’s first express differently (2), by doing a Taylor expansion to the second-order approximation:
∆y = h(x + ∆x) − h(x) = J(x)∆x +12Pn i=1(∆xi)∂ 2h(x) ∂xi∂x∆x + o(∆x) 2 (13) Let’s denote Hi= ∂ 2h ∂xi∂x = ∂J
∂xi and define the matrix H as the concatenation of the columns number i in the matrices Hi (14). Its size is l × n.
H =H1(:, 1) H2(:, 2) . . . Hi(:, i) . . . Hn(:, n)
(14) with Hi(:, i) referring to the ithcolumn of Hi.
Then, (13) can be written as: ∆y = J(x)∆x+1 2H(x)∆ 2x+1 2 n X j=1 (∆xj)H0j∆x+o(∆ 2x) (15) with H0j the matrix Hj whose column j is replaced by a
null vector and ∆2x the vector composed of the components
∆x2 i.
Let’s call M the (l × 2n) matrix defined in (16).
M =J(x) H(x) (16)
In the case of manipulator arms, since the direct kinematic model is composed of sinus and cosinus of the xi, then each
column vector of J is rotated by π/2 in H, and so the vectors J(:, i) and H(:, i) are perpendicular to each other for any i. A vector s is then a singular direction to M if and only if it is perpendicular to all vectors of M (equivalently sTM = 0
1×2n). It means that all vectors of M are in the
same hyperplane, hence that we have a planar manipulator arm. If the system is intrinsically 3-D, that guarantees that M is full rank for any x.
The main assumption in this paper is that the matrix M defined in (16) is always of rank l whatever the values of x. From the robotic example given above, this assumption is verified in many mechanical systems.
Another advantage of using M is that the search of the best solution also takes into account the null-space of J through H and therefore will help pass and/or avoid the singularities. See the work of K. Yamada and I. Jikuya [23] for theoretical research and proofs on the usefulness of this formulation.
This matrix M supposed regular is then used to invert the relation (15) rewritten as (17). ∆y −1 2 n X j=1 (∆xj)H0j∆x = M(x) ∆x 1 2∆ 2x + o(∆2x) (17)
o(∆2x) is assumed to be negligible compared to the other
terms (assumption true if the time step is small enough). Then, our goal is to find the vector
∆x 1 2∆ 2x that verifies (17) and that also verifies the obvious internal relationship.
It should be noticed that the input of the equation (17) is ∆y−12Pn
j=1(∆xj)H 0
j∆x, the first term being the reference
input assumed to be available from the controller, and the second term is considered to be a feed-forward from the previously calculated x and ∆x.
H0
i are in general not null for manipulator arms. The
theoretical study of the null-space of JJT and the associated singularities is then more complicated, however it can be noticed that this formulation enables us to avoid numerical singularities, and still search into the null-space of JJT to
bypass the kinematic singularities.
To our best knowledge, only the article [23] uses the second-order Taylor expansion for control. However, the method proposed in article [23] to find
∆x 1 2∆ 2x is rather costly since it is not analytically solvable and requires an
iterative procedure at each sample time. It is hardly gener-alizable to greater numbers of actuators since the resolution of the equations in the Gr¨obner bases can become of very high order. Using the EKF formalism as presented before enables us to estimate ∆x and ∆2x that verify their intrinsic relationship and verifies equation (17) in only one iteration at each time step. For instance, it is possible to modify the EKF presented before by adding ∆2x as new estimated variables.
The term12Pn
j=1(∆xj)H 0
j∆x can be considered as a known
input perturbation in the model.
The combination of the EKF and this formulation gives a new tool that can efficiently invert the dynamic equation (1), take constraints into account with flexibility, handle the singularities, with convergence results that help tuning the parameters of the EKF.
IV. APPLICATION A. Modelling
To apply the proposed method, let’s consider the simple case of a 2-DOFs manipulator arm moving in a reference plane supposed fixed (ur, vr) (see Fig. 2). The manipulator
has 3 arms linked by 3 revolute joints. The objective is to control the end-effector position (y1, y2). The
parametriza-tion is chosen relative : the angles between the arms are the controlled parameters, called q1, q2 and q3. The length of
the arms are noted l1, l2 and l3. In the (ur, vr) plane, the
Fig. 2. Parametrization of the system studied
dynamics of the system (assumed rigid) has the form (18), taken from the book of W. Khalil and E. Dombre [24].
¨
q = −A−1(q)Cq( ˙q, q) + A−1(q)u (18)
with A the inertia matrix and Cq the quadratic velocity
vector including the Coriolis and the centrifugal forces. The joints are controlled by imposing a torque vector u.
To identify to (1), let us denote x =q ˙q
. The kinematic equations read:
y =y1 y2 = h(q), ˙y = ˙y1 ˙ y2 = J(q) ˙q (19) ∆q is computed by using the EKF and the singularity-handling formulation. Indeed, the estimated variables of the EKF were chosen as (20). The prediction equations and the
measurement equations are (20) and (21). v1= ∆q v2= ∆2q v3= ∆y and ˆ v1(t + Ts) =23vˆ1(t) ˆ v2(t + Ts) =49vˆ2(t) ˆ v3(t + Ts) =12vˆ3(t) +12w(t) (20) with w the reference input ∆yc.
In (20), the internal state dynamics of reference model f1
is (2/3, 4/9, 1/2) and is chosen in the following way: the first two equations of (20) are used to limit the maximal accelerations of the joints. The third equation of (20) was chosen to keep the F matrix (11) invertible as required for the convergence proof, and take into account the reference input w(t). m1=P n j=1(∆qj) H0j 2 ∆q = −Jv1− H 2v2+ v3(t) + m1 m2= 03×1= −v12+ v2+ m2 m3= k = v1+ m3 (21) The first two equations of (21) are imposed by the constraints on the problem. Let’s develop the choice of k:
k = k1I3×3 03×3 N 11×3 01×3
T
(22) with N = I−M†M, the projection on the null-space of M = J(q) H(q). This operation is always defined because of the regularity of M. k1 is defined as:
k1=
|| w || wmax
˙
qsatTs (23)
with wmax the maximal norm of w, ˙qsat the maximal
allowable speed of the joint angles and Ts the sampling
period. (22) is relevant for two aspects : it takes into account the saturation constraint, which depends on the distance to the reference value (via w), thus not creating movement when the system has reached its goal. In addition, projecting the obtained value on the null-space of M avoids creating errors (because these components are associated with the null-space of J) and it helps steering the system out of initial singularities, when the system has no speed. Adding a term in the null-space of M is also interesting because v2 gives
components along H which comprises a part of the null-space of J complementary to the null-null-space of M, therefore expanding the search in the null-space of J.
The convergence results of the EKF were used to tune the parameters R, Q and P0: Q = 4.0.10−6I and P0 =
5.10−5I. As for R, a valid interval for its singular values has been computed. R = diag([10−5, 10−5, 1.23.10−5]). Indeed, a lower confidence appears on the last measurement since the last equation does not model perfectly the saturation constraint. The estimates v are all initialized at 0.
The block diagram summarizing the implementation of the application is given in Fig. 3.
B. Simulations
In the simulation, u = Kw(∆q/Ts− ˙qm) (with Kwa gain
matrix and ˙qm the measured joint velocities). It is assumed
that q and ˙q are measured.
Numerical application : The 3 beams are uniform with the same mass per unit of length (3.39kg/m). l1 = l2 =
l3 = 0.4 m, and for each joint Kw = 3 N m s/rad, ˙qsat=
10 rad/s and Ts= 0.001s.
w is taken as the distance between the required position and the current position of the end-effector, with a saturation calculated from the saturation of ˙qsat and the maximal
singular value of J. The value of the saturation obtained is wsat= 0.015 m.
The system is initially in a singular folded configuration: q1 = 0, q2 = π, q3 = π rad. The final state is another
singular state (y1i, y2i) = (−(l1+ l2+ l3), 0) which is in the
initial singular direction. The motion created by the proposed allocator is given in Fig. 4. After 31s, a new singular state is
Fig. 4. Trajectory of the manipulator arm for the first movement. Solid green : initial state. Solid black : final state. Dashed lines : intermediate states (red, blue and green for arm (1), (2) and (3) respectively). required: (y1f, y2f) = (0, 0). This part of the motion is given
in Fig. 5. The joints motion and end-effector positions versus time are in Fig. 6. Even if beginning in a folded singular
Fig. 5. Trajectory of the manipulator arm for the second movement. Solid lines : final state.
configuration, the limits of the workspace has been reached with an error of less than 0.0003m and the system has gone back to a singular position, hence the allocation method manages the worst cases. The maximal error of the end-effector along the vr-axis is less than 1 cm. The time spent to
escape from a singular position is due to the reconfiguration in the null-space of J that does not create movement for the end-effector. In addition, other singularities were avoided along the trajectory (the (0,0) point for instance).
V. CONCLUSION
The method proposed to allocate over-actuated systems is real-time and has stability proofs. The different parameters
− + yc Saturation 03 03 1 212 w + + + z−1 ˆ v(t + Ts) I3 03 02 ˆ v(t) 1 Ts ∆q −
+ Kw Arm Dynamic Model
u q, ˙q
˙qm
Direct Geometric Model
ym F m1(q) 0 k(q) + − Kalman Gain r(ˆv, q)
Fig. 3. Block diagram of the whole system
Fig. 6. (a) Angles q (b) End-effector position with respect to time.
can be tuned to ensure the convergence, the time response as well as the priority among the different equations. Another formulation for singularity-passing and avoiding has been proposed, which has proven efficient on the given example. The forthcoming work will now focus on a better integration of the constraints in the formulation in particular cases, as well as a study on the sensitivity of the method on parameter uncertainties. In addition, a comparison with other existing techniques will be carried out.
REFERENCES
[1] A. Isidori, Nonlinear Control Systems, 2nd ed., Springer Verlag, London, 1989.
[2] B. Siciliano, “Kinematic Control of Redundant Robot Manipulators”, Journal of Intelligent and Robotic Systems, vol. 3, pp. 201-212, 1990. [3] B. Siciliano and L. Sciavicco, Modeling and Control of Robot
Manipulators, 2nd edition, Springer-Verlag, London, 2000.
[4] L. Sciavicco and B. Siciliano, “Solution Algorithm to the Inverse Kinematic Problem for Redundant Manipulators” IEEE Journal of Robotics and Automation, vol. 4, no. 4, pp. 403-410, 1988. [5] C.W. Wampler, “Manipulator Inverse Kinematic Solutions Based on
Damped Least-Squares Methods”, IEEE Transactions on Systems, Man and Cybernetics, vol. 61, pp. 93-101, 1986.
[6] Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for robot manipulator control”, Transactions of ASME Journal of Dynamic Systems, Measurement and Control, vol. 108, pp. 163-171, 1986.
[7] K.A. Ford and C.D. Hall, “Singular Direction Avoidance Steering for Control Moment Gyros”, Journal of Guidance, Control and Dynamics, vol. 3, no. 4, pp. 648-656, 2000.
[8] S. Huang, Y. Peng, W. Wei and J. Xiang, “Clamping Weighted Least-Norm Method for the Manipulator Kinematic Control: Avoiding Joint Limits”, Proceedings of the 33rd Chinese Control Conference (CCC), pp. 8309-8314, 2014.
[9] T. Yoshikawa, “Manipulability of robotic mechanisms”, International Journal of Robotic Research, vol. 4, no. 2, pp. 3-9, 1985.
[10] J. Baillieul, “Kinematic programming alternatives for redundant ma-nipulators”, Proceedings of the 1985 IEEE International Conference on Robotics and Automation, pp. 722-728, 1985.
[11] C.W. Wampler, “Winding number analysis of invertible workspaces for redundant manipulators”, International Journal of Robotic Research, vol. 7, no. 5, pp. 22-31, 1988.
[12] H. Kurokawa, “Exact Singularity Avoidance Control of the Pyramid Type CMG System”, AIAA Guidance and Control Conference, pp. 170-180, 1994.
[13] Y. Nakamura, H. Hanafusa and T. Yoshikawa, “Task-priority based redundancy control of robot manipulators”, Transactions of ASME Journal of Dynamic Systems, Measurement and Control, vol. 6, no. 2, pp. 3-15, 1987.
[14] T.I. Fossen and T.A. Johansen, “A Survey of Control Allocation Methods for Ships and Underwater Vehicles”, 14th Mediterranean Conference on Control and Automation, pp. 1-6, 2006.
[15] Y. Chen and J. Wang, “Fast and Global Optimal Energy-Efficient Con-trol Allocation With Applications to Over-Actuated Electric Ground Vehicles”, IEEE Transactions on Control Systems Technology, vol. 20, no. 5, pp. 1202-1211, 2012.
[16] W. Song and G. Hu, “A Fast Inverse Kinematics Algorithm for Joint Animation”, International Conference on Advances in Engineering, Procedia Engineering, vol. 24, pp. 350-354, 2011.
[17] H. Ananthanarayanan and R. Ordonez, “Real-time Inverse Kinematics of (2n+1) DOF hyper-redundant manipulator arm via a combined numerical and analytical approach”, Mechanism and Machine Theory, vol. 91, pp. 209-226, 2015.
[18] R.E. Kalman and R.S. Bucy, “New Results in Linear Filtering and Prediction Theory”, Journal of Basic Engineering, pp. 95-108, 1961. [19] T.A. Johansen, “Optimizing Nonlinear Control Allocation”, IEEE
Conference on Decision and Control, pp. 403-410, 2004.
[20] L. Zaccarian, “Dynamic allocation for input redundant systems”, Automatica, vol. 45, pp. 1431-1438, 2009.
[21] Y. Song and J.W. Grizzle, “The Extended Kalman Filter as a Local Asymptotic Observer for Discrete-Time Nonlinear Systems”, Journal of Mathematical Systems, Estimation, and Control, vol. 5, no. 1, pp. 59-78, 1995.
[22] J.J. Deyst and C.F. Price, “Conditions for asymptotic stability of the discrete minimum-variance linear estimator”, IEEE trans. Automat. Control, vol. 13, no. 6, pp. 702-705, 1968
[23] K. Yamada and I. Jikuya, “Directional passability and quadratic steering logic for pyramid-type single gimbal control moment gyros”, Acta Astronautica, vol. 102, pp. 103-123, 2014.
[24] W. Khalil and E. Dombre, Modeling, identification and control of robots, Butterworth-Heinemann, Oxford, 2004.
