**Obstacle Avoidance for Redundant Manipulators as Control Problem**

Leon Žlajpah and Tadej PetriĀ *Jožef Stefan Institute, Ljubljana Slovenia* 

### **1. Introduction**

202 Serial and Parallel Robot Manipulators – Kinematics, Dynamics, Control and Optimization

This work is supported by The Scientific and Technological Research Council of Turkey

Chen, N.X.; Song, S.M. (1994). Direct position analysis of the 4-6 Stewart Platform*, ASME J. of Mechanical Design,* Vol. 116, No. 1, (March 1994), pp. (61-66), ISSN 1050-0472

Kassem, A.M.; Yousef, A. M. (2009). Servo DC Motor Position Control Based on Sliding Mode Approach, *5th Saudi Technical Conference and Exhibition STCEX 2009*, January 2009 Kim, D.; Chung, W. (1999). Analytic Singularity Equation and Analysis of Six-DOF Parallel

*Automation*, Vol. 15, No. 4, (August 1999), pp. (612-622), ISSN 1042-296X Kizir, S. ; Bingül, Z. ; Oysu, C. ; Küçük, S. (2011). Development and Control of a High

Küçük, S.; Bingül, Z. (2008). *Robot Dinamiği ve Kontrolü*, Birsen press, ISBN 978-975-511-516-

Liao, Q.; Seneviratne, L.D.; Earles, S.W.E. (1993). Forward kinematic analysis for the general

Merlet, J.P. (1992). Direct kinematics and assembly modes of parallel manipulators, *Int. J. of Robotics Research*, Vol. 11, No. 2, (April 1992), pp. (150-162), ISSN 0278-3649 Merlet, J.P. (Ed(s).). (2006). *Parallel Robots*, Springer, ISBN-10 1-4020-4133-0, Netherlands Nauna, P.; Waldron, K.J.; Murthy, V. (1990). Direct kinematic solution of a Stewart Platform,

Reckdahl, K.J. (1996). Dynamics and control of mechanical systems containing closed

Sefrioui, J.; Gosselin., C.M. (1993). Singularity analysis and representation of planar parallel

Stewart, D. (1965). A Platform with Six Degrees of Freedom, *Proceedings of the Institute of* 

Utkin, V.I., (1993). Sliding mode control design principles and applications to electric drives, *IEEE Transactions on Industrial Electronics*, Vol. 40, No. 1, pp. (23–36), ISSN 1042-296X

Wikipedia, 06.04.2011, Available from: http://en.wikipedia.org/wiki/Stewart\_platform

manipulators, *Robot. Autom. Syst.*, Vol. 10, No. 4, pp. (209-224)

*Mechanical Engineering*, Vol. 180, Part 1, No. 5, pp. (371-386)

*Systems IROS'93*, ISBN 0-7803-0823-9, Yokohama, Japan, July 1993

Mathworks Inc., 06.04.2011, Available from: http://www.mathworks.com/

Niesing, B. (2001). Medical Engineering, *Fraunhofer Magazine*, Vol. 2

kinematic chains, *Phd Thesis*, Stanford University

Construction, *Int. J. of Robotics Research*, Vol. 5, No. 2, (June 1986), pp. (157-182),

Manipulators Using Local Structurization Method, *IEEE Transactions on Robotics and* 

Precision Stewart Platform, *International Journal of Technological Sciences,* Vol. 3, No.

4-6 Stewart Platform, *IEEE/RSJ International Conference on Intelligent Robots and* 

*IEEE Trans. Robotics Automat.,* Vol. 6, No. 4, (August 1990), pp. (438-444), ISSN 1042-

Dspace Inc., 06.04.2011, Available from: http://www.dspaceinc.com/en/inc/home.cfm Hunt, K.H. (1983). Structural kinematics of in-parallel-actuated robot-arms, *ASME J. Mech., Trans. Automat. Des.*, Vol. 105, No. 4, (December 1983), pp. (705–712), ISSN 0738-0666 Fitcher, E.F. (1986). A Stewart Platform-Based Manipulator: General Theory and Practical

Bonev, I. (2003). The True Origins of Parallel Robots, 06.04.2011, Available from:

http://www.parallemic.org/Reviews/Review007.html

**6. Acknowledgment** 

**7. References** 

(TUBITAK) under the Grant No. 107M148.

ISSN 0278-3649

1, pp. (51-59)

0, İstanbul

296X

One of the goals of robotics research is to provide control algorithms that allow robotic manipulators to move in an environment with objects. The contacts with these objects may be part of the task, e.g. in the assembly operations, or they may be undesired events. If the task involves some contacts with the environment it is necessary to control the resulting forces. For that purpose, different control approaches have been proposed like hybrid position/force control (Raibert & Craig, 1981) or impedance control (Hogan, 1985), which have also been applied to redundant manipulators (Khatib, 1987; Park et al., 1996; Woernle, 1993; Yoshikawa, 1987). However, in most cases the contact is supposed to occur between the end-effector or the handling object and the object in the workspace. Except in some special cases all other contacts along the body of the robot manipulator are not desired and have to be avoided. In the case when the contacts are not desired, the main issue is how to accomplish the assigned task without any risk of collisions with the workspace objects. A natural strategy to avoid obstacles would be to move the manipulator away from the obstacle into a configuration where the manipulator is not in contact with the obstacle. Without changing the motion of the end-effector, the reconfiguration of the manipulator into a collision-free configuration can be made only if the manipulator has redundant degrees-of-freedom (DOF). The flexibility depends on the degree-of-redundancy, i.e., on the number of redundant DOF. A high degree-of-redundancy is important, especially when the manipulator is working in an environment with many potential collisions with obstacles.

Generally, the obstacle-avoidance (or collision-avoidance) problem can be solved with two classes of strategies: global (planning) and local (control). The global ones, like high-level path planning, guarantee to find a collision-free path from the initial point to the goal point, if such a path exists. They often operate in the configuration space into which the manipulator and all the obstacles are mapped and a collision-free path is found in the unoccupied portion of the configuration space (Lozano-Perez, 1983). However, these algorithms are very computationally demanding and the calculation times are significantly longer than the typical response time of a manipulator. This computational complexity limits their use for practical obstacle avoidance just to simple cases. Furthermore, as global methods do not usually rely on any sensor feedback information, they are only suitable for static and well-defined environments. On the other hand, local strategies treat obstacle avoidance as a control problem. Their aim is not to replace the higher-level, global, collision-free path planning but to make use of the capabilities of low-level control, e.g., they can use the sensor information

Manipulators as Control Problem 3

Obstacle Avoidance for Redundant Manipulators as Control Problem 205

obstacles in the neighborhood of the manipulator. We propose an algorithm that considers all

Most tasks performed by a redundant manipulator are broken down into several subtasks with different priorities. Usually, the task with the highest priority, referred to as the main task, is associated with the positioning of the end-effector in the task space, and other subtasks are associated with the obstacle avoidance and other additional tasks (if the degree-of-redundancy is high enough). However, in some cases it is necessary (e.g., for safety reasons) that the end-effector motion is not the primary task. As, in general, task-priority algorithms do not always allow simple transitions or the changing of priority levels between the tasks (Sciavicco & Siciliano, 2005), we propose a novel formulation of the primary and secondary tasks, so that the desired movement of the end-effector is in fact a secondary task. The primary task is now the obstacle avoidance and it is only active if we approach a pre-defined threshold, i.e., the critical distance to one of the obstacles. While far from the threshold, our algorithm allows undisturbed control of the secondary task. If we approach the threshold, the primary task smoothly takes over and only allows joint control in the null-space of the primary task. A similar approach can also be found in Sugiura et al. (2007), where they used a blending coefficient for blending the end-effector motion with the obstacle-avoidance motion, and in Mansard et al. (2009) where they proposed a generic solution to build a smooth

Next, we propose strategies that are also considering the dynamics. In this case, it is reasonable to define the obstacle avoidance at the force level, i.e., the forces are supposed to generate the motion that is necessary to avoid the obstacle. We discuss three approaches regarding the sensors used to detect the obstacles: no sensors, tactile sensors and proximity sensors or vision. First of all, we want to investigate what happens if the manipulator touches an obstacle, especially how to control the contact forces and how to avoid the obstacle after the contact. Therefore, we propose a strategy that utilizes the self-motion caused by the contact forces to avoid an obstacle after the collision. The main advantage of this strategy is that it can be applied to the systems without any contact or force sensors. However, a prerequisite for this strategy to be effective is that the manipulator is backdrivable. As an alternative for stiff systems (having high-ratio gears, high friction, etc.), we propose using tactile sensors. Finally, we deal with proximity sensors and we propose a virtual forces strategy, where a virtual force component in a direction away from the obstacle is assigned to each point on the body of the manipulator, which is close to an obstacle. Like other classical methods for obstacle avoidance this one prevents any part of the manipulator touching an obstacle. Also here we address the problem of multiple obstacles in the workspace, which have to be simultaneously avoided. The computational efficiency of all the proposed algorithms (at the kinematic level and considering the dynamics) allows the real-time application in an unstructured or time-varying environment. The efficiency of the proposed control algorithms is illustrated by simulations of highly redundant planar manipulator moving in an unstructured and time-varying

The robotic systems under study are serial manipulators. We consider redundant systems, i.e., the dimension of the joint space *n* exceeds the dimension of the task space *m*. The difference between *n* and *m* will be denoted as the degree of redundancy *r*, *r* = *n* − *m*. Note that in this definition the redundancy is not only a characteristics of the manipulator itself but

the obstacles in the neighborhood of the robot.

control law for any kind of unilateral constraints.

environment and by experiments on a real robot manipulator.

**2. Background**

to change the path if the obstacle appears in the workspace or it moves. Hence, they are suitable when the obstacle position is not known in advance and must be detected in real-time during the task execution. A significant advantage of local methods is that they are less computationally demanding and more flexible. These characteristics make local methods good candidates for on-line collision avoidance, especially in unstructured environments. However, the drawback is that they may cause globally suboptimal behavior or may even get stuck when a collision-free path cannot be found from the current configuration.

With the problem of the collision avoidance of redundant manipulators, there have been different approaches to the local methods proposed by many researchers in the past (Brock et al., 2002; Colbaugh et al., 1989; Glass et al., 1995; Guo & Hsia, 1993; Khatib, 1986; Kim & Khosla, 1992; Maciejewski & Klein, 1985; McLean & Cameron, 1996; Seraji & Bon, 1999; Volpe & Khosla, 1990). The approach proposed by Maciejewski and Klein (Maciejewski & Klein, 1985) is to assign to the critical point an avoiding task space vector, with which the point is directed away from the obstacle. Colbaugh, Glass and Seraji (Colbaugh et al., 1989; Glass et al., 1995) used configuration control and defined the constraints representing the obstacle-avoidance. The next approach is based on potential functions, where a repulsive potential is assigned to the obstacles and an attractive potential to the goal position (Khatib, 1986; Kim & Khosla, 1992; McLean & Cameron, 1996; Volpe & Khosla, 1990). The fourth approach uses the optimization of an objective function maximizing the distance between the manipulator and the obstacles (Guo & Hsia, 1993). Most of the proposed methods solve the obstacle-avoidance problem at the kinematic level. Velocity null-space control is an appropriate way to control the internal motion of a redundant manipulator. Some of the control strategies are acceleration based or torque based, considering also the manipulator dynamics (Brock et al., 2002; Khatib, 1986; Newman, 1989; Xie et al., 1998). However, it is established that certain acceleration-based control schemes exhibit instabilities (O'Neil, 2002). An alternative approach is the augmented Jacobian, as introduced in Egeland (1987), where the secondary task is added to the primary task so as to obtain a square Jacobian matrix that can be inverted. The main drawback of this technique are the so-called algorithmic singularities. They occur when the secondary task causes a conflict with the primary task. Khatib investigated in depth the use of the second-order inverse kinematic, either at the torque or acceleration level, starting from Khatib (1987) to recent task-prioritised humanoid applications (Mansard et al., 2009; Sentis et al., 2010).

Here, we want to present some approaches to on-line obstacle-avoidance for the redundant manipulators at the kinematic level and some approaches, which are considering also the dynamics of the manipulator.

Like in most of the local strategies that solve the obstacle-avoidance problem at the kinematic level (Colbaugh et al., 1989; Glass et al., 1995; Guo & Hsia, 1993; Kim & Khosla, 1992; Maciejewski & Klein, 1985; Seraji & Bon, 1999), the aim of the proposed strategies is to assign each point on the body of the manipulator, which is close to the obstacle, a motion component in a direction away from the obstacle. The emphasis is given to the definition of the avoiding motion. Usually, the avoiding motion is defined in the Cartesian space. As obstacle avoidance is typically a one-dimensional problem, we use a one-dimensional operational space for each critical point. Consequently, some singularity problems can be avoided when not enough "redundancy" is available locally. Additionally, we propose an approximative calculation of the motion that is faster that the exact one. Another important issue addressed in this paper is how the obstacle avoidance is performed when there are more simultaneously active 2 Will-be-set-by-IN-TECH

to change the path if the obstacle appears in the workspace or it moves. Hence, they are suitable when the obstacle position is not known in advance and must be detected in real-time during the task execution. A significant advantage of local methods is that they are less computationally demanding and more flexible. These characteristics make local methods good candidates for on-line collision avoidance, especially in unstructured environments. However, the drawback is that they may cause globally suboptimal behavior or may even

With the problem of the collision avoidance of redundant manipulators, there have been different approaches to the local methods proposed by many researchers in the past (Brock et al., 2002; Colbaugh et al., 1989; Glass et al., 1995; Guo & Hsia, 1993; Khatib, 1986; Kim & Khosla, 1992; Maciejewski & Klein, 1985; McLean & Cameron, 1996; Seraji & Bon, 1999; Volpe & Khosla, 1990). The approach proposed by Maciejewski and Klein (Maciejewski & Klein, 1985) is to assign to the critical point an avoiding task space vector, with which the point is directed away from the obstacle. Colbaugh, Glass and Seraji (Colbaugh et al., 1989; Glass et al., 1995) used configuration control and defined the constraints representing the obstacle-avoidance. The next approach is based on potential functions, where a repulsive potential is assigned to the obstacles and an attractive potential to the goal position (Khatib, 1986; Kim & Khosla, 1992; McLean & Cameron, 1996; Volpe & Khosla, 1990). The fourth approach uses the optimization of an objective function maximizing the distance between the manipulator and the obstacles (Guo & Hsia, 1993). Most of the proposed methods solve the obstacle-avoidance problem at the kinematic level. Velocity null-space control is an appropriate way to control the internal motion of a redundant manipulator. Some of the control strategies are acceleration based or torque based, considering also the manipulator dynamics (Brock et al., 2002; Khatib, 1986; Newman, 1989; Xie et al., 1998). However, it is established that certain acceleration-based control schemes exhibit instabilities (O'Neil, 2002). An alternative approach is the augmented Jacobian, as introduced in Egeland (1987), where the secondary task is added to the primary task so as to obtain a square Jacobian matrix that can be inverted. The main drawback of this technique are the so-called algorithmic singularities. They occur when the secondary task causes a conflict with the primary task. Khatib investigated in depth the use of the second-order inverse kinematic, either at the torque or acceleration level, starting from Khatib (1987) to recent task-prioritised humanoid

Here, we want to present some approaches to on-line obstacle-avoidance for the redundant manipulators at the kinematic level and some approaches, which are considering also the

Like in most of the local strategies that solve the obstacle-avoidance problem at the kinematic level (Colbaugh et al., 1989; Glass et al., 1995; Guo & Hsia, 1993; Kim & Khosla, 1992; Maciejewski & Klein, 1985; Seraji & Bon, 1999), the aim of the proposed strategies is to assign each point on the body of the manipulator, which is close to the obstacle, a motion component in a direction away from the obstacle. The emphasis is given to the definition of the avoiding motion. Usually, the avoiding motion is defined in the Cartesian space. As obstacle avoidance is typically a one-dimensional problem, we use a one-dimensional operational space for each critical point. Consequently, some singularity problems can be avoided when not enough "redundancy" is available locally. Additionally, we propose an approximative calculation of the motion that is faster that the exact one. Another important issue addressed in this paper is how the obstacle avoidance is performed when there are more simultaneously active

get stuck when a collision-free path cannot be found from the current configuration.

applications (Mansard et al., 2009; Sentis et al., 2010).

dynamics of the manipulator.

obstacles in the neighborhood of the manipulator. We propose an algorithm that considers all the obstacles in the neighborhood of the robot.

Most tasks performed by a redundant manipulator are broken down into several subtasks with different priorities. Usually, the task with the highest priority, referred to as the main task, is associated with the positioning of the end-effector in the task space, and other subtasks are associated with the obstacle avoidance and other additional tasks (if the degree-of-redundancy is high enough). However, in some cases it is necessary (e.g., for safety reasons) that the end-effector motion is not the primary task. As, in general, task-priority algorithms do not always allow simple transitions or the changing of priority levels between the tasks (Sciavicco & Siciliano, 2005), we propose a novel formulation of the primary and secondary tasks, so that the desired movement of the end-effector is in fact a secondary task. The primary task is now the obstacle avoidance and it is only active if we approach a pre-defined threshold, i.e., the critical distance to one of the obstacles. While far from the threshold, our algorithm allows undisturbed control of the secondary task. If we approach the threshold, the primary task smoothly takes over and only allows joint control in the null-space of the primary task. A similar approach can also be found in Sugiura et al. (2007), where they used a blending coefficient for blending the end-effector motion with the obstacle-avoidance motion, and in Mansard et al. (2009) where they proposed a generic solution to build a smooth control law for any kind of unilateral constraints.

Next, we propose strategies that are also considering the dynamics. In this case, it is reasonable to define the obstacle avoidance at the force level, i.e., the forces are supposed to generate the motion that is necessary to avoid the obstacle. We discuss three approaches regarding the sensors used to detect the obstacles: no sensors, tactile sensors and proximity sensors or vision. First of all, we want to investigate what happens if the manipulator touches an obstacle, especially how to control the contact forces and how to avoid the obstacle after the contact. Therefore, we propose a strategy that utilizes the self-motion caused by the contact forces to avoid an obstacle after the collision. The main advantage of this strategy is that it can be applied to the systems without any contact or force sensors. However, a prerequisite for this strategy to be effective is that the manipulator is backdrivable. As an alternative for stiff systems (having high-ratio gears, high friction, etc.), we propose using tactile sensors. Finally, we deal with proximity sensors and we propose a virtual forces strategy, where a virtual force component in a direction away from the obstacle is assigned to each point on the body of the manipulator, which is close to an obstacle. Like other classical methods for obstacle avoidance this one prevents any part of the manipulator touching an obstacle. Also here we address the problem of multiple obstacles in the workspace, which have to be simultaneously avoided.

The computational efficiency of all the proposed algorithms (at the kinematic level and considering the dynamics) allows the real-time application in an unstructured or time-varying environment. The efficiency of the proposed control algorithms is illustrated by simulations of highly redundant planar manipulator moving in an unstructured and time-varying environment and by experiments on a real robot manipulator.

#### **2. Background**

The robotic systems under study are serial manipulators. We consider redundant systems, i.e., the dimension of the joint space *n* exceeds the dimension of the task space *m*. The difference between *n* and *m* will be denoted as the degree of redundancy *r*, *r* = *n* − *m*. Note that in this definition the redundancy is not only a characteristics of the manipulator itself but also of the task. This means that a nonredundant manipulator may also become a redundant manipulator for a certain task.

#### **2.1 Modelling**

Let the configuration of the manipulator be represented by the *n*-dimensional vector q of joint positions, and the end-effector position (and orientation) by the *m*-dimensional vector x of the task positions (and orientations). Then, the kinematics can be described by the following equations

$$\dot{x} = f(\mathbf{q}) \qquad \dot{\mathbf{q}} = \mathbf{J}^{\#} \dot{x} + \mathbf{N} \dot{q} \qquad \qquad \ddot{\mathbf{q}} = \mathbf{J}^{\#} (\ddot{x} - \dot{\mathbf{J}} \dot{\mathbf{q}}) + \mathbf{N} \ddot{q} \tag{1}$$

where f is an *m*-dimensional vector function representing the manipulator's forward kinematics, **<sup>J</sup>** is the *<sup>m</sup>* <sup>×</sup> *<sup>n</sup>* manipulator's Jacobian matrix, **<sup>J</sup>**# is the generalized inverse of the Jacobian matrix **J** and **N** is a matrix representing the projection into the null space of **J**, **<sup>N</sup>** = (**<sup>I</sup>** <sup>−</sup> **<sup>J</sup>**#**J**).

For the redundant manipulators the static relationship between the *m*-dimensional generalized force in the task space F , and the corresponding *n*-dimensional generalized joint space force τ is

$$\boldsymbol{\tau} = \mathbf{J}^T \mathbf{F} + \mathbf{N}^T \boldsymbol{\tau} \tag{2}$$

Manipulators as Control Problem 5

Obstacle Avoidance for Redundant Manipulators as Control Problem 207

where ¨x*<sup>c</sup>* and φ represent the task space and the null-space control law, respectively. The task

where e, e = x*<sup>d</sup>* − x, is the tracking error, ¨x*<sup>d</sup>* is the desired task space acceleration, and **K***<sup>v</sup>* and **K***<sup>p</sup>* are constant gain matrices. The selection of **K***<sup>v</sup>* and **K***<sup>p</sup>* can be based on the desired task space impedance. To perform the additional subtask, the vector φ is given in the form

where ˙ϕ is the desired null space velocity and **K***<sup>n</sup>* is an *n*×*n* diagonal gain matrix. The velocity ϕ˙ is defined by the subtask. E.g., let *p* be a function representing the desired performance

The obstacle-avoidance problem is usually defined as how to control the manipulator to track the desired end-effector trajectory while simultaneously ensuring that no part of the manipulator collides with any obstacle in the workspace of the manipulator. To avoid any obstacles the manipulator has to move away from the obstacles into the configuration where the distance to the obstacles is larger (see Fig. 1). Without changing the motion of the end-effector, the reconfiguration of the manipulator into a collision-free configuration can be done only if the manipulator has redundant degrees-of-freedom (DOF). Note that the flexibility or the *degree-of-redundancy* of the manipulator does not depend only on the number of redundant DOF but also on the "location" of the redundant DOF. Namely, it is possible that the redundant manipulator cannot avoid an obstacle, because it is in a configuration where the avoiding motion in the desired direction is not possible. A high degree-of-redundancy is important, especially when the manipulator is working in an environment with many

A good strategy for obstacle avoidance is to identify the points on the robotic arm that are near obstacles and then assign to them a motion component that moves those points away from the obstacle, as shown in Fig. 1. The motion of the robot is perturbed only if at least one part of the robot is in the critical neighborhood of an obstacle, i.e., the distance is less than a prescribed minimal distance. We denote the obstacles in the critical neighborhood as the *active obstacles* and the corresponding closest points on the body of a manipulator as the *critical points*. Usually, it is assumed that the motion of the end effector is not disturbed by any obstacle. Otherwise, the task execution has to be interrupted and the higher-level path planning has to recalculate the desired motion of the end-effector. If the path-tracking accuracy is not important control algorithms which move the end-effector around obstacles

As the obstacle avoidance is supposed to be done on-line, it is not necessary to know the exact position of the obstacles in advance. Of course, to allow the manipulator to work in an unstructured and/or dynamic environment, some sensors have to be used to determine the position of the obstacles or measure the distance between the obstacles and the body of the manipulator. There are different types of sensor systems that can be used to detect objects in the neighborhood of the manipulator. They can be tactile or proximity sensors. The tactile sensors, like artificial skin, can detect the obstacle only if they touch it. On the other hand, the proximity sensors can sense the presence of an obstacle in the neighborhood. We have

x¨ *<sup>c</sup>* = x¨ *<sup>d</sup>* + **K***v*e˙ + **K***p*e (8)

<sup>φ</sup> <sup>=</sup> **<sup>N</sup>**ϕ¨ <sup>+</sup> **<sup>N</sup>**˙ <sup>ϕ</sup>˙ <sup>+</sup> **<sup>K</sup>***n*e˙*n*, ˙e*<sup>n</sup>* <sup>=</sup> **<sup>N</sup>**(ϕ˙ <sup>−</sup> <sup>q</sup>˙) (9)

space control ¨x*<sup>c</sup>* can be selected as

criterion. To optimize *p* we can select ˙ϕ as (6).

**3. Obstacle-avoidance strategy**

potential collisions with obstacles.

on-line, can be used.

where **N***<sup>T</sup>* is a matrix representing the projection into the null space of **J***<sup>T</sup>*#. Assuming the manipulator consists of rigid bodies the joint space equations of motion can be written in the form

$$
\tau = \mathbf{H}\ddot{q} + h + g - \tau\_{\mathbb{F}} \tag{3}
$$

where τ is an *n*-dimensional vector of control torques, **H** is an *n* × *n* inertia matrix, h is an *n*-dimensional vector of Coriolis and centrifugal forces, g is an *n*-dimensional vector of gravity forces, and the vector τ*<sup>F</sup>* represents torques due to external forces acting on the manipulator.

#### **2.2 Kinematic control**

For velocity control the following kinematic controller can be used

$$
\dot{q} = \mathbf{J}^{\#} \dot{x}\_{\mathbb{C}} + \mathbf{N} \dot{\varphi} \tag{4}
$$

where ˙x*<sup>c</sup>* and ˙ϕ represent the task space control law and the arbitrary joint velocities, respectively. The task space control ˙x*<sup>c</sup>* can be selected as

$$
\dot{x}\_{\ell} = \dot{x}\_{\ell} + \mathbf{K}\_{\mathcal{P}} e \tag{5}
$$

where e, e = x*<sup>d</sup>* − x, is the tracking error, ˙x*<sup>e</sup>* is the desired task space velocity, and **K***<sup>p</sup>* is a constant gain matrix. To perform the additional subtask, the velocity ˙ϕ is used. Let *p* be a function representing the desired performance criterion. Then, to optimize *p* we can select ˙ϕ as

$$
\dot{\varphi} = \mathbf{K}\_p \nabla p \tag{6}
$$

Here, ∇*p* is the gradient of *p* and **K***<sup>p</sup>* is a gain.

#### **2.3 Torque control**

To decouple the task space and null-space motion we propose using a controller given in the form

$$\tau\_{\mathbb{C}} = \mathbf{H}(\mathbf{J}^{\#}(\ddot{x}\_{\mathbb{C}} - \dot{\mathbf{J}}\dot{\mathbf{q}}) + \mathbf{N}(\boldsymbol{\phi} - \dot{\mathbf{N}}\dot{\boldsymbol{q}})) + \boldsymbol{h} + \boldsymbol{g} \tag{7}$$

where ¨x*<sup>c</sup>* and φ represent the task space and the null-space control law, respectively. The task space control ¨x*<sup>c</sup>* can be selected as

$$
\ddot{x}\_{\mathcal{L}} = \ddot{x}\_d + \mathbf{K}\_{\mathcal{V}} \dot{e} + \mathbf{K}\_{\mathcal{V}} e \tag{8}
$$

where e, e = x*<sup>d</sup>* − x, is the tracking error, ¨x*<sup>d</sup>* is the desired task space acceleration, and **K***<sup>v</sup>* and **K***<sup>p</sup>* are constant gain matrices. The selection of **K***<sup>v</sup>* and **K***<sup>p</sup>* can be based on the desired task space impedance. To perform the additional subtask, the vector φ is given in the form

$$
\phi = \mathbf{N}\ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_{\text{ll}}\dot{e}\_{\text{n}\prime} \qquad \dot{e}\_{\text{n}} = \mathbf{N}(\dot{\varphi} - \dot{\mathbf{q}}) \tag{9}
$$

where ˙ϕ is the desired null space velocity and **K***<sup>n</sup>* is an *n*×*n* diagonal gain matrix. The velocity ϕ˙ is defined by the subtask. E.g., let *p* be a function representing the desired performance criterion. To optimize *p* we can select ˙ϕ as (6).

#### **3. Obstacle-avoidance strategy**

4 Will-be-set-by-IN-TECH

also of the task. This means that a nonredundant manipulator may also become a redundant

Let the configuration of the manipulator be represented by the *n*-dimensional vector q of joint positions, and the end-effector position (and orientation) by the *m*-dimensional vector x of the task positions (and orientations). Then, the kinematics can be described by the following

where f is an *m*-dimensional vector function representing the manipulator's forward kinematics, **<sup>J</sup>** is the *<sup>m</sup>* <sup>×</sup> *<sup>n</sup>* manipulator's Jacobian matrix, **<sup>J</sup>**# is the generalized inverse of the Jacobian matrix **J** and **N** is a matrix representing the projection into the null space of **J**,

For the redundant manipulators the static relationship between the *m*-dimensional generalized force in the task space F , and the corresponding *n*-dimensional generalized joint

Assuming the manipulator consists of rigid bodies the joint space equations of motion can be

where τ is an *n*-dimensional vector of control torques, **H** is an *n* × *n* inertia matrix, h is an *n*-dimensional vector of Coriolis and centrifugal forces, g is an *n*-dimensional vector of gravity forces, and the vector τ*<sup>F</sup>* represents torques due to external forces acting on the manipulator.

where ˙x*<sup>c</sup>* and ˙ϕ represent the task space control law and the arbitrary joint velocities,

where e, e = x*<sup>d</sup>* − x, is the tracking error, ˙x*<sup>e</sup>* is the desired task space velocity, and **K***<sup>p</sup>* is a constant gain matrix. To perform the additional subtask, the velocity ˙ϕ is used. Let *p* be a function representing the desired performance criterion. Then, to optimize *p* we can select ˙ϕ

To decouple the task space and null-space motion we propose using a controller given in the

q˙ = **J**

where **N***<sup>T</sup>* is a matrix representing the projection into the null space of **J***<sup>T</sup>*#.

For velocity control the following kinematic controller can be used

respectively. The task space control ˙x*<sup>c</sup>* can be selected as

τ*<sup>c</sup>* = **H**(**J**

#(x¨ *<sup>c</sup>* <sup>−</sup> ˙

Here, ∇*p* is the gradient of *p* and **K***<sup>p</sup>* is a gain.

#x˙ + **N**q˙ q¨ = **J**

#(x¨ <sup>−</sup> ˙

τ = **J***T*F + **N***T*τ (2)

τ = **H**q¨ + h + g − τ*<sup>F</sup>* (3)

#x˙ *<sup>c</sup>* + **N**ϕ˙ (4)

x˙ *<sup>c</sup>* = x˙ *<sup>e</sup>* + **K***p*e (5)

ϕ˙ = **K***p*∇*p* (6)

**<sup>J</sup>**q˙) + **<sup>N</sup>**(<sup>φ</sup> <sup>−</sup> **<sup>N</sup>**˙ <sup>q</sup>˙)) + <sup>h</sup> <sup>+</sup> <sup>g</sup> (7)

**J**q˙) + **N**q¨ (1)

manipulator for a certain task.

x = f(q) q˙ = **J**

**2.1 Modelling**

equations

**<sup>N</sup>** = (**<sup>I</sup>** <sup>−</sup> **<sup>J</sup>**#**J**).

space force τ is

written in the form

**2.2 Kinematic control**

as

form

**2.3 Torque control**

The obstacle-avoidance problem is usually defined as how to control the manipulator to track the desired end-effector trajectory while simultaneously ensuring that no part of the manipulator collides with any obstacle in the workspace of the manipulator. To avoid any obstacles the manipulator has to move away from the obstacles into the configuration where the distance to the obstacles is larger (see Fig. 1). Without changing the motion of the end-effector, the reconfiguration of the manipulator into a collision-free configuration can be done only if the manipulator has redundant degrees-of-freedom (DOF). Note that the flexibility or the *degree-of-redundancy* of the manipulator does not depend only on the number of redundant DOF but also on the "location" of the redundant DOF. Namely, it is possible that the redundant manipulator cannot avoid an obstacle, because it is in a configuration where the avoiding motion in the desired direction is not possible. A high degree-of-redundancy is important, especially when the manipulator is working in an environment with many potential collisions with obstacles.

A good strategy for obstacle avoidance is to identify the points on the robotic arm that are near obstacles and then assign to them a motion component that moves those points away from the obstacle, as shown in Fig. 1. The motion of the robot is perturbed only if at least one part of the robot is in the critical neighborhood of an obstacle, i.e., the distance is less than a prescribed minimal distance. We denote the obstacles in the critical neighborhood as the *active obstacles* and the corresponding closest points on the body of a manipulator as the *critical points*. Usually, it is assumed that the motion of the end effector is not disturbed by any obstacle. Otherwise, the task execution has to be interrupted and the higher-level path planning has to recalculate the desired motion of the end-effector. If the path-tracking accuracy is not important control algorithms which move the end-effector around obstacles on-line, can be used.

As the obstacle avoidance is supposed to be done on-line, it is not necessary to know the exact position of the obstacles in advance. Of course, to allow the manipulator to work in an unstructured and/or dynamic environment, some sensors have to be used to determine the position of the obstacles or measure the distance between the obstacles and the body of the manipulator. There are different types of sensor systems that can be used to detect objects in the neighborhood of the manipulator. They can be tactile or proximity sensors. The tactile sensors, like artificial skin, can detect the obstacle only if they touch it. On the other hand, the proximity sensors can sense the presence of an obstacle in the neighborhood. We have

Manipulators as Control Problem 7

Obstacle Avoidance for Redundant Manipulators as Control Problem 209

where ˙x*<sup>d</sup>* is the desired task space velocity, **K** is an *m* x *m* positive-definite matrix and e is the

where x*<sup>d</sup>* is the desired task space position. The second term in (12), i.e., the homogeneous solution ˙q*h*, represents the part of the joint velocity causing the motion of the point *Ao*. The term **J***o***J**#x˙ *<sup>e</sup>* is the velocity in *Ao* due to the end-effector motion. The matrix **J***o***N** is used to transform the desired critical point velocity from the operational space of the critical point into the joint space. Note that the above solution guarantees that we achieve exactly the desired

The matrix **J***o***N** combines the kinematics of the critical point *Ao* and the null-space matrix of the whole manipulator. Hence, its properties define the flexibility of the system for avoiding the obstacles. We want to point out that the properties of the matrix **J***o***N** do not depend only on the position of the point *Ao* but also on the definition of the operational space associated with the critical point. Usually, it is assumed that all critical points belong to the Cartesian space. Hence, the velocity ˙x*<sup>o</sup>* is a 3-dimensional vector and the dimension of the matrix **J***o***N** is 3 × *n*. This also implies that 3 degrees-of-redundancy are needed to move one point from an obstacle. Consequently, it seems that a manipulator with 2 degrees-of-redundancy is not capable of avoiding obstacles, and of course, this is not true. For example, consider a planar 3 DOF manipulator that is supposed to move along a line, as shown in Fig. 2. As this is a planar case, the task space is 2-dimensional (e.g. *x* and *y*) and the manipulator has 1 degree-of-redundancy. Defining the velocity ˙x*<sup>o</sup>* in the same space as the end-effector velocity, i.e., as a 2-dimensional vector, reveals the matrix **J***o***N** to have the dimension 2 × 3. Furthermore, due to 1 degree-of-redundancy the components of the velocity vector ˙x*<sup>o</sup>* are not independent. Hence, the rank of **J***o***N** is 1, and the pseudoinverse (**J***o***N**)# does not exist. As the obstacle-avoidance strategy only requires motion in the direction of the line connecting the critical point with the closest point on the obstacle, this is a one-dimensional constraint and only one degree-of-redundancy is needed to avoid the obstacle, generally. Therefore, we propose using a reduced operational space for the obstacle avoidance and define the Jacobian

Let d*<sup>o</sup>* be the vector connecting the closest points on the obstacle and the manipulator (see Fig. 1) and let the operational space in *Ao* be defined as one-dimensional space in the direction of d*o*. Then, the Jacobian, which relates the joint space velocities ˙q and the velocity in the

**<sup>J</sup>***do* <sup>=</sup> <sup>n</sup>*<sup>T</sup>*

where **J***<sup>o</sup>* is the Jacobian defined in the Cartesian space and n*<sup>o</sup>* is the unit vector in the direction

**<sup>J</sup>***do* **<sup>J</sup>**#x˙ *<sup>e</sup>* become scalars. Consequently, the computation of (**J***do***N**)# is faster. For example,

**<sup>J</sup>***do***<sup>N</sup>** (**J***do***N**)*<sup>T</sup>*

our example in Fig. 2, the pseudoinverse (**J***do***N**)# exists and the manipulator can perform the

when calculating the Moore-Penrose pseudoinverse of (**J***do***N**) defined as

primary task and simultaneously avoid the obstacle, as shown in Fig. 2.

(**J***do***N**)# = (**J***do***N**)*<sup>T</sup>*

we do not have to invert any matrix because the term (**J***do***NJ***<sup>T</sup>*

�*do*� . Now, the dimension of the matrix **<sup>J</sup>***do* is 1 <sup>×</sup> *<sup>n</sup>*, and the velocities ˙x*<sup>o</sup>* and

−<sup>1</sup>

= **NJ***<sup>T</sup> do*

x˙ *<sup>o</sup>* only if the degree of redundancy of the manipulator is sufficient.

**4.2 Exact solution using a reduced operational space**

e = x*<sup>d</sup>* − x, (14)

*<sup>o</sup>* **J***<sup>o</sup>* (15)

(**J***do***NJ***<sup>T</sup> do*

*do*

)−<sup>1</sup> (16)

) is a scalar. Going back to

task position error, defined as

**J***<sup>o</sup>* as follows.

of d*o*, n*<sup>o</sup>* = *<sup>d</sup><sup>o</sup>*

direction of d*o*, can be calculated as

Fig. 1. Manipulator motion in presence of some obstacles

compared the capabilities of a manipulator equipped with both types of sensors. Actually, we have also investigated if and how the manipulator can avoid obstacles without any sensors for the detection of obstacles. As in the case of tactile sensors and when no sensors are used the manipulator has to "touch" the obstacles, we allow a collision with an obstacle. However, after the collision the manipulator should move away from the obstacle and the collision forces should be kept as low as possible.

#### **4. Obstacle avoidance using kinematic control**

The proposed velocity strategy considers the obstacle-avoidance problem at the kinematic level. Let ˙x*<sup>e</sup>* be the desired velocity of the end-effector, and *Ao* be the critical point in the neighborhood of an obstacle (see Fig. 1). To avoid a possible collision, one possibility is to assign to *Ao* such a velocity that it moves away from the obstacle, as proposed in Maciejewski & Klein (1985). Hence, the motion of the end-effector and the critical point can be described by the equations

$$\mathbf{J}\dot{\mathbf{q}} = \dot{\mathbf{x}}\_{\ell} \qquad \qquad \qquad \mathbf{J}\_{\ell}\dot{\mathbf{q}} = \dot{\mathbf{x}}\_{\ell} \tag{10}$$

where **J***<sup>o</sup>* is a Jacobian matrix associated with the point point *Ao*. There are some possibilities to find a common solution for both equations.

#### **4.1 Exact solution**

Let ˙x*<sup>c</sup>* in (4) equal ˙x*e*. Then, combining (4) and (10) yields

$$
\varphi = \left( \mathbf{J}\_o \mathbf{N} \right)^\# \left( \dot{\mathbf{x}}\_o - \mathbf{J}\_o \mathbf{J}^\# \dot{\mathbf{x}}\_e \right) \tag{11}
$$

Now, using this ˙ϕ in (4) gives the final solution for ˙q in the form

$$\dot{q} = \mathbf{J}^{\#} \dot{x}\_{\mathcal{C}} + (\mathbf{J}\_o \mathbf{N})^{\#} (\dot{x}\_o - \mathbf{J}\_o \mathbf{J}^{\#} \dot{x}\_{\mathcal{C}}) \tag{12}$$

because **N** is both hermitian and idempotent (Maciejewski & Klein, 1985; Nakamura et al., 1987). The meaning of the terms in the above equation can be easily explained. The first term **J**#x˙ *<sup>c</sup>* guarantees the joint motion necessary for the desired end-effector velocity. Also, ˙x*<sup>c</sup>* is used in (12) instead of ˙x*<sup>e</sup>* to indicate that a task space controller can be used to compensate for any task space tracking errors, e.q.

$$
\dot{x}\_{\mathcal{L}} = \dot{x}\_d + \mathbf{K}e.\tag{13}
$$

6 Will-be-set-by-IN-TECH

compared the capabilities of a manipulator equipped with both types of sensors. Actually, we have also investigated if and how the manipulator can avoid obstacles without any sensors for the detection of obstacles. As in the case of tactile sensors and when no sensors are used the manipulator has to "touch" the obstacles, we allow a collision with an obstacle. However, after the collision the manipulator should move away from the obstacle and the collision forces

The proposed velocity strategy considers the obstacle-avoidance problem at the kinematic level. Let ˙x*<sup>e</sup>* be the desired velocity of the end-effector, and *Ao* be the critical point in the neighborhood of an obstacle (see Fig. 1). To avoid a possible collision, one possibility is to assign to *Ao* such a velocity that it moves away from the obstacle, as proposed in Maciejewski & Klein (1985). Hence, the motion of the end-effector and the critical point can be described

where **J***<sup>o</sup>* is a Jacobian matrix associated with the point point *Ao*. There are some possibilities

<sup>ϕ</sup>˙ = (**J***o***N**)#(x˙ *<sup>o</sup>* <sup>−</sup> **<sup>J</sup>***o***<sup>J</sup>**

#x˙ *<sup>c</sup>* + (**J***o***N**)#(x˙ *<sup>o</sup>* <sup>−</sup> **<sup>J</sup>***o***<sup>J</sup>**

because **N** is both hermitian and idempotent (Maciejewski & Klein, 1985; Nakamura et al., 1987). The meaning of the terms in the above equation can be easily explained. The first term **J**#x˙ *<sup>c</sup>* guarantees the joint motion necessary for the desired end-effector velocity. Also, ˙x*<sup>c</sup>* is used in (12) instead of ˙x*<sup>e</sup>* to indicate that a task space controller can be used to compensate for

**J**q˙ = x˙ *<sup>e</sup>* **J***o*q˙ = x˙ *<sup>o</sup>* (10)

#x˙ *<sup>e</sup>*) (11)

#x˙ *<sup>e</sup>*) (12)

x˙ *<sup>c</sup>* = x˙ *<sup>d</sup>* + **K**e. (13)

Fig. 1. Manipulator motion in presence of some obstacles

**4. Obstacle avoidance using kinematic control**

to find a common solution for both equations.

any task space tracking errors, e.q.

Let ˙x*<sup>c</sup>* in (4) equal ˙x*e*. Then, combining (4) and (10) yields

Now, using this ˙ϕ in (4) gives the final solution for ˙q in the form

q˙ = **J**

should be kept as low as possible.

by the equations

**4.1 Exact solution**

where ˙x*<sup>d</sup>* is the desired task space velocity, **K** is an *m* x *m* positive-definite matrix and e is the task position error, defined as

$$\mathbf{e} = \mathbf{x}\_d - \mathbf{x}\_\prime \tag{14}$$

where x*<sup>d</sup>* is the desired task space position. The second term in (12), i.e., the homogeneous solution ˙q*h*, represents the part of the joint velocity causing the motion of the point *Ao*. The term **J***o***J**#x˙ *<sup>e</sup>* is the velocity in *Ao* due to the end-effector motion. The matrix **J***o***N** is used to transform the desired critical point velocity from the operational space of the critical point into the joint space. Note that the above solution guarantees that we achieve exactly the desired x˙ *<sup>o</sup>* only if the degree of redundancy of the manipulator is sufficient.

#### **4.2 Exact solution using a reduced operational space**

The matrix **J***o***N** combines the kinematics of the critical point *Ao* and the null-space matrix of the whole manipulator. Hence, its properties define the flexibility of the system for avoiding the obstacles. We want to point out that the properties of the matrix **J***o***N** do not depend only on the position of the point *Ao* but also on the definition of the operational space associated with the critical point. Usually, it is assumed that all critical points belong to the Cartesian space. Hence, the velocity ˙x*<sup>o</sup>* is a 3-dimensional vector and the dimension of the matrix **J***o***N** is 3 × *n*. This also implies that 3 degrees-of-redundancy are needed to move one point from an obstacle. Consequently, it seems that a manipulator with 2 degrees-of-redundancy is not capable of avoiding obstacles, and of course, this is not true. For example, consider a planar 3 DOF manipulator that is supposed to move along a line, as shown in Fig. 2. As this is a planar case, the task space is 2-dimensional (e.g. *x* and *y*) and the manipulator has 1 degree-of-redundancy. Defining the velocity ˙x*<sup>o</sup>* in the same space as the end-effector velocity, i.e., as a 2-dimensional vector, reveals the matrix **J***o***N** to have the dimension 2 × 3. Furthermore, due to 1 degree-of-redundancy the components of the velocity vector ˙x*<sup>o</sup>* are not independent. Hence, the rank of **J***o***N** is 1, and the pseudoinverse (**J***o***N**)# does not exist.

As the obstacle-avoidance strategy only requires motion in the direction of the line connecting the critical point with the closest point on the obstacle, this is a one-dimensional constraint and only one degree-of-redundancy is needed to avoid the obstacle, generally. Therefore, we propose using a reduced operational space for the obstacle avoidance and define the Jacobian **J***<sup>o</sup>* as follows.

Let d*<sup>o</sup>* be the vector connecting the closest points on the obstacle and the manipulator (see Fig. 1) and let the operational space in *Ao* be defined as one-dimensional space in the direction of d*o*. Then, the Jacobian, which relates the joint space velocities ˙q and the velocity in the direction of d*o*, can be calculated as

$$\mathbf{J}\_{d\_0} = \mathbf{n}\_0^T \mathbf{J}\_o \tag{15}$$

where **J***<sup>o</sup>* is the Jacobian defined in the Cartesian space and n*<sup>o</sup>* is the unit vector in the direction of d*o*, n*<sup>o</sup>* = *<sup>d</sup><sup>o</sup>* �*do*� . Now, the dimension of the matrix **<sup>J</sup>***do* is 1 <sup>×</sup> *<sup>n</sup>*, and the velocities ˙x*<sup>o</sup>* and **<sup>J</sup>***do* **<sup>J</sup>**#x˙ *<sup>e</sup>* become scalars. Consequently, the computation of (**J***do***N**)# is faster. For example, when calculating the Moore-Penrose pseudoinverse of (**J***do***N**) defined as

$$(\mathbf{J}\_{d\_o}\mathbf{N})^\# = (\mathbf{J}\_{d\_o}\mathbf{N})^T \left(\mathbf{J}\_{d\_o}\mathbf{N} \left(\mathbf{J}\_{d\_o}\mathbf{N}\right)^T\right)^{-1} = \mathbf{N}\mathbf{J}\_{d\_o}^T (\mathbf{J}\_{d\_o}\mathbf{N}\mathbf{J}\_{d\_o}^T)^{-1} \tag{16}$$

we do not have to invert any matrix because the term (**J***do***NJ***<sup>T</sup> do* ) is a scalar. Going back to our example in Fig. 2, the pseudoinverse (**J***do***N**)# exists and the manipulator can perform the primary task and simultaneously avoid the obstacle, as shown in Fig. 2.

Fig. 2. Planar 3DOF manipulator: tracking of a line and obstacle avoidance using the Jacobian **J***do*

#### **4.3 Selection of avoiding velocity**

The efficiency of the obstacle-avoidance algorithm also depends on the selection of the desired critical point velocity *x*˙*o*. We propose changing *x*˙*<sup>o</sup>* with respect to the obstacle distance

$$
\dot{\mathfrak{X}}\_0 = \mathfrak{a}\_{\overline{\upsilon}} \mathfrak{v}\_0 \tag{17}
$$

Manipulators as Control Problem 9

Obstacle Avoidance for Redundant Manipulators as Control Problem 211

Fig. 3. The obstacle avoidance gain *α<sup>v</sup>* and the homogenous term gain *α<sup>h</sup>* versus the distance

decreases when approaching from *di* to *dm*, if the obstacle is not moving, of course. With such

The control law (19) can be used for a single obstacle. When more than one obstacle is active at one time then the worst-case obstacle (nearest) has to be used, which results in discontinuous velocities and may cause oscillations in some cases. Namely, when switching between active obstacles the particular homogenous solutions are not equal and a discontinuity in the joint velocities occurs. To improve the behavior we propose to use a weighted sum of the

#x˙ *<sup>c</sup>* +

where *no* is the number of active obstacles, and *wi*, *αh*,*<sup>i</sup>* and ˙q*h*,*<sup>i</sup>* are the weighting factor, the gain and the homogenous solution for the *i*-th active obstacle, respectively. The weighting

> *wi* <sup>=</sup> *di* − �d*o*,*i*� ∑*no*

Although the actual velocities in the critical points differ from the desired ones, using ˙q*EX* improves the behavior of the system and when one point is much closer to the obstacle than another, then its weight approaches 1 and the velocity in that particular point is close to the

As an illustration we present a simulation of a planar manipulator with 4 revolute joints. The primary task is to move along a straight line from point *P*<sup>1</sup> to point *P*2, and the motion is obstructed by an obstacle. The task trajectory has a trapezoid velocity profile with an acceleration of 4*ms*−<sup>2</sup> and a max. velocity of 0.4*ms*−1. We chose the the critical distance *dm* = 0.2*m*. The simulation results using the exact velocity controller EX (19) are presented in

Another possible solution for ˙ϕ is to calculate joint velocities that satisfy the secondary goal

without compensating for the contribution of the end-effector motion and then substitute ˙ϕ

This approach avoids the singularity problem of (**J***do***N**) (Chiaverini, 1997). The formulation (24) does not guarantee that we will achieve exactly the desired *x*˙*<sup>o</sup>* even if the degree of

#x˙ *<sup>c</sup>* + **NJ**#

*do*

*x*˙*<sup>o</sup>* is not equal to *x*˙*o*, in general.

ϕ˙ = **J** # *do*

q˙*AP* = **J**

*do*

*no* ∑ *i*=1

*wiαh*,*i*q˙*h*,*<sup>i</sup>* (21)

*<sup>i</sup>*=1(*di* − �d*o*,*i*�) (22)

*x*˙*<sup>o</sup>* (23)

*x*˙*<sup>o</sup>* (24)

a selection of *α<sup>v</sup>* and *α<sup>h</sup>* smooth velocities can be obtained.

q˙*EX* = **J**

homogenous solution of all the active obstacles

factors *wi* are calculated as

**4.4 Approximate solution**

into (4), which yields

redundancy is sufficient because **<sup>J</sup>***do***NJ**#

desired value.

Fig. 4(a).

as

to the obstacle

where *vo* is the nominal velocity and *α<sup>v</sup>* is the obstacle-avoidance gain defined as

$$\mathfrak{a}\_{\mathcal{D}} = \begin{cases} \left(\frac{d\_m}{\|\mathbf{d}\|}\right)^2 - 1 & \text{for} \quad \|\mathbf{d}\_\bullet\| < d\_m\\ 0 & \text{for} \quad \|\mathbf{d}\_\bullet\| \ge d\_m \end{cases} \tag{18}$$

where *dm* is the critical distance to the obstacle (see Fig. 3). If the obstacle is too close (�d*o*� ≤ *db*) the main task should be aborted. The distance *db* is subjected to the dynamic properties of the manipulator and can also be a function of the relative velocity d˙ *<sup>o</sup>*. To assure smooth transitions it is important that the magnitude of ˙x*<sup>o</sup>* at *dm* is zero. Special attention has to be given to the selection of the nominal velocity *vo*. Large values of *vo* would cause unnecessarily high velocities and consequently the manipulator would move far from the obstacle. Such motion may cause problems if there are more obstacles in the neighborhood of the manipulator. Namely, the manipulator may bounce between the obstacles. On the other hand, too small value of *vo* would not move the critical point of the manipulator away from the obstacle.

For a smoother motion, was is proposed in Maciejewski & Klein (1985) to change the amount of the homogenous solution to be included in the total solution

$$\dot{q}\_{EX} = \mathbf{J}^{\#} \dot{x}\_{\mathcal{C}} + \boldsymbol{\alpha}\_{h} (\mathbf{J}\_{d\_o} \mathbf{N})^{\#} (\dot{x}\_o - \mathbf{J}\_{d\_o} \mathbf{J}^{\#} \dot{x}\_{\mathcal{C}}) \tag{19}$$

We have selected *α<sup>h</sup>* as

$$\mathfrak{a}\_{h} = \begin{cases} 1 & \text{for } \qquad ||\mathbf{d}\_{0}|| \le d\_{m} \\ \frac{1}{2} (1 - \cos(\pi \frac{||\mathbf{d}\_{0}|| - d\_{m}}{d\_{l} - d\_{m}})) & \text{for } \quad d\_{m} < ||\mathbf{d}\_{0}|| < d\_{l} \\ 0 & \text{for } \qquad d\_{l} \le ||\mathbf{d}\_{0}|| \end{cases} \tag{20}$$

where *di* is the distance where the obstacle influences the motion. From Fig. 3 it can be seen that in the region between *db* and *dm* the complete homogenous solution is included in the motion specification and the avoidance velocity is inversely related to the distance. Between *dm* and *di* the avoidance velocity is zero and only a part of the homogenous solution is included. As the homogenous solution compensates for the motion in the critical point due to the end-effector motion, the relative velocity between the obstacle and the critical point 8 Will-be-set-by-IN-TECH

Fig. 2. Planar 3DOF manipulator: tracking of a line and obstacle avoidance using the

where *vo* is the nominal velocity and *α<sup>v</sup>* is the obstacle-avoidance gain defined as

properties of the manipulator and can also be a function of the relative velocity d˙

*α<sup>v</sup>* =

of the homogenous solution to be included in the total solution

q˙*EX* = **J**

<sup>2</sup> (<sup>1</sup> <sup>−</sup> cos(*<sup>π</sup>* �*do*�−*dm*

*α<sup>h</sup>* =

⎧ ⎪⎨

1

⎪⎩

⎧ ⎨ ⎩

� *dm* �*do*� �2

The efficiency of the obstacle-avoidance algorithm also depends on the selection of the desired critical point velocity *x*˙*o*. We propose changing *x*˙*<sup>o</sup>* with respect to the obstacle distance

− 1 for �d*o*� < *dm*

0 for �d*o*� ≥ *dm*

where *dm* is the critical distance to the obstacle (see Fig. 3). If the obstacle is too close (�d*o*� ≤ *db*) the main task should be aborted. The distance *db* is subjected to the dynamic

smooth transitions it is important that the magnitude of ˙x*<sup>o</sup>* at *dm* is zero. Special attention has to be given to the selection of the nominal velocity *vo*. Large values of *vo* would cause unnecessarily high velocities and consequently the manipulator would move far from the obstacle. Such motion may cause problems if there are more obstacles in the neighborhood of the manipulator. Namely, the manipulator may bounce between the obstacles. On the other hand, too small value of *vo* would not move the critical point of the manipulator away from

For a smoother motion, was is proposed in Maciejewski & Klein (1985) to change the amount

#x˙ *<sup>c</sup>* <sup>+</sup> *<sup>α</sup>h*(**J***do***N**)#(x˙ *<sup>o</sup>* <sup>−</sup> **<sup>J</sup>***do* **<sup>J</sup>**

1 for �d*o*� ≤ *dm*

0 for *di* ≤ �d*o*�

where *di* is the distance where the obstacle influences the motion. From Fig. 3 it can be seen that in the region between *db* and *dm* the complete homogenous solution is included in the motion specification and the avoidance velocity is inversely related to the distance. Between *dm* and *di* the avoidance velocity is zero and only a part of the homogenous solution is included. As the homogenous solution compensates for the motion in the critical point due to the end-effector motion, the relative velocity between the obstacle and the critical point

*di*−*dm* )) for *dm* <sup>&</sup>lt; �d*o*� <sup>&</sup>lt; *di*

*x*˙*<sup>o</sup>* = *αvvo* (17)

(18)

(20)

*<sup>o</sup>*. To assure

#x˙ *<sup>e</sup>*) (19)

Jacobian **J***do*

the obstacle.

We have selected *α<sup>h</sup>* as

**4.3 Selection of avoiding velocity**

Fig. 3. The obstacle avoidance gain *α<sup>v</sup>* and the homogenous term gain *α<sup>h</sup>* versus the distance to the obstacle

decreases when approaching from *di* to *dm*, if the obstacle is not moving, of course. With such a selection of *α<sup>v</sup>* and *α<sup>h</sup>* smooth velocities can be obtained.

The control law (19) can be used for a single obstacle. When more than one obstacle is active at one time then the worst-case obstacle (nearest) has to be used, which results in discontinuous velocities and may cause oscillations in some cases. Namely, when switching between active obstacles the particular homogenous solutions are not equal and a discontinuity in the joint velocities occurs. To improve the behavior we propose to use a weighted sum of the homogenous solution of all the active obstacles

$$\dot{q}\_{EX} = \mathbf{J}^{\#} \dot{x}\_{\mathcal{L}} + \sum\_{i=1}^{n\_o} w\_i \alpha\_{h,i} \dot{q}\_{h,i} \tag{21}$$

where *no* is the number of active obstacles, and *wi*, *αh*,*<sup>i</sup>* and ˙q*h*,*<sup>i</sup>* are the weighting factor, the gain and the homogenous solution for the *i*-th active obstacle, respectively. The weighting factors *wi* are calculated as

$$w\_{l} = \frac{d\_{l} - ||\mathbf{d}\_{0,i}||}{\sum\_{i=1}^{n\_{\boldsymbol{\theta}}} (d\_{l} - ||\mathbf{d}\_{0,i}||)} \tag{22}$$

Although the actual velocities in the critical points differ from the desired ones, using ˙q*EX* improves the behavior of the system and when one point is much closer to the obstacle than another, then its weight approaches 1 and the velocity in that particular point is close to the desired value.

As an illustration we present a simulation of a planar manipulator with 4 revolute joints. The primary task is to move along a straight line from point *P*<sup>1</sup> to point *P*2, and the motion is obstructed by an obstacle. The task trajectory has a trapezoid velocity profile with an acceleration of 4*ms*−<sup>2</sup> and a max. velocity of 0.4*ms*−1. We chose the the critical distance *dm* = 0.2*m*. The simulation results using the exact velocity controller EX (19) are presented in Fig. 4(a).

#### **4.4 Approximate solution**

Another possible solution for ˙ϕ is to calculate joint velocities that satisfy the secondary goal as

$$
\dot{\varphi} = \mathbf{J}\_{d\_\diamond}^\# \dot{\mathbf{x}}\_\diamond \tag{23}
$$

without compensating for the contribution of the end-effector motion and then substitute ˙ϕ into (4), which yields

$$
\dot{\mathbf{q}}\_{AP} = \mathbf{J}^{\#} \dot{\mathbf{x}}\_{\mathcal{L}} + \mathbf{N} \mathbf{J}\_{d\_o}^{\#} \dot{\mathbf{x}}\_o \tag{24}
$$

This approach avoids the singularity problem of (**J***do***N**) (Chiaverini, 1997). The formulation (24) does not guarantee that we will achieve exactly the desired *x*˙*<sup>o</sup>* even if the degree of redundancy is sufficient because **<sup>J</sup>***do***NJ**# *do x*˙*<sup>o</sup>* is not equal to *x*˙*o*, in general.

Manipulators as Control Problem 11

Obstacle Avoidance for Redundant Manipulators as Control Problem 213

0 0.2 0.4 0.6 0.8 1

Fig. 5. Robot motion in the presence of some obstacle (left figure). Obstacle-avoidance proximity gain to the power of *n* (middle figure) and the planar 4DOF manipulator tracking

line while avoiding the obstacle (right figure).

*<sup>a</sup>* is given as

where the matrix **N**�

task *Ta* is not active *λ*(x*a*) = 0.

a new definition of the velocity ˙q. The velocity ˙q is now defined as

q˙ = **J** # *<sup>a</sup>*x˙ *<sup>a</sup>* + **N**�

**N**�

end-effector path recalculation or higher-level path planning is needed.

the task *Tb*. Using the reduced operational space yields

where v<sup>0</sup> is a nominal velocity and *λ* (d0) is defined as

⎧ ⎨ ⎩ � *dm* ||d0|| �*<sup>n</sup>*

*λ*(d0) =

Next, let the avoiding velocity ˙x*d*<sup>0</sup> be defined as

n=10

In many cases it would be of benefit to have the possibility to change the priority of particular subtasks. Using formulation (27) this cannot be done in a smooth way. Therefore, we propose

*<sup>a</sup>* = **I** − *λ*(x*a*)**J**

where *λ*(x*a*) is a scalar measure of how "active" is the primary task *Ta*, scaling the vector x*<sup>a</sup>* to the interval [0, 1]. When the primary task *Ta* is active the *λ*(x*a*) = 1 and the value when the

The proposed algorithm allows a smooth transition in both ways, i.e., between observing the task *Ta* and the task *Tb* in null-space of the task *Ta* or just the unconstrained movement of the task *Tb*. It can be used for different robotic tasks. When applied to obstacle avoidance, task *Ta* is the obstacle avoidance and the end-effector motion is the task *Tb*. Before, we have assumed that the end-effector motion is not disturbed by an obstacle. Now, it is assumed that the motion of the end effector can be disturbed by any obstacle (see Fig. 5). If such a situation occurs, usually the task execution has to be interrupted and higher-level path planning has to be employed to recalculate the desired motion of the end effector. However, if the end-effector path tracking is not essential, we can use the proposed control (28). Consequently, no

Let the primary task *Ta* be the motion in the direction d<sup>0</sup> and the motion of the end-effector be

*a***J** #

(λ(d0))*n*

0 0.5 1

n=1 n=2

P1

(33)

P2

*<sup>b</sup>*x˙ *<sup>b</sup>*, (28)

#**J**, (29)

**J***<sup>a</sup>* = **J***o*, (30)

**J***<sup>b</sup>* = **J**. (31)

x˙ *<sup>d</sup>*<sup>0</sup> = *λ* (d0) v0, (32)

, *n* = 1, 2, 3... ||d0|| ≥ *dm* 1 ||d0|| < *dm*

d0

Fig. 4. Planar 4DOF manipulator tracking a line while avoiding obstacles using velocity control (the dotted line indicates the critical distance)

To avoid the obstacle the goal velocity in *Ao* is represented by the vector ˙x*o*. Using the original method (12) the velocity in *Ao* is exactly ˙x*o*. The joint velocities ˙q*EX* assure that the component of the velocity at point *Ao* (i.e., **J***o*q˙*EX*) in the direction of ˙x*<sup>o</sup>* is as required. The approximate solution ˙q*AP* gives, in most cases, a smaller magnitude of the velocity in the direction of ˙x*<sup>o</sup>* (see **J***o*q˙*AP*). Therefore, the manipulator moves closer to the obstacle when ˙q*AP* is used. This is not so critical, because the minimal distance also depends on the nominal velocity *vo*, which can be increased to achieve larger minimal distances. Additionally, the approximate solution possesses certain advantages when many active obstacles have to be considered. The joint velocities can be calculated as

$$\dot{q}\_{AP} = \mathbf{J}^{\#} \dot{x}\_{\mathcal{C}} + \mathbf{N} \sum\_{i=1}^{n\_o} \mathbf{J}\_{d\_o, i}^{\#} \dot{x}\_{o, i} \tag{25}$$

where *no* is the number of active obstacles, and therefore, the matrix **N** has to be calculated only once. Of course, pseudoinverses **J**# *<sup>o</sup>*,*<sup>i</sup>* have to be calculated for each active obstacle. For the same system and task as before, the simulation results using the approximate velocity controller AP (24) are presented in Fig. 4(b). We can see that in the case of the AP controller, the links are coming closer to the obstacle as in the case of the EX controller. However, when changing the desired critical point velocity *x*˙*o*, i.e., using a higher order in (18), the minimal distance can be increased.

#### **4.5 Obstacle avoidance as a primary task**

For a redundant system multiple tasks can be arranged in priority. Let us consider two tasks, *Ta* and *Tb*

$$x\_{\boldsymbol{a}} = f\_{\boldsymbol{a}}(\boldsymbol{q}) \qquad \qquad \qquad \boldsymbol{x}\_{\boldsymbol{b}} = \mathbf{f}\_{\boldsymbol{b}}(\boldsymbol{q}) \tag{26}$$

For each of the tasks, the corresponding Jacobian matrices can be defined as **J***<sup>a</sup>* and **J***b*, and their corresponding null-space projections as **N***<sup>a</sup>* and **N***b*. Assuming that task *Ta* is the primary task, Eq. (4) can be rewritten as

$$
\dot{q} = \mathbf{J}\_a^\# \dot{x}\_a + \mathbf{N}\_a \mathbf{J}\_b^\# \dot{x}\_b \tag{27}
$$

Fig. 5. Robot motion in the presence of some obstacle (left figure). Obstacle-avoidance proximity gain to the power of *n* (middle figure) and the planar 4DOF manipulator tracking line while avoiding the obstacle (right figure).

In many cases it would be of benefit to have the possibility to change the priority of particular subtasks. Using formulation (27) this cannot be done in a smooth way. Therefore, we propose a new definition of the velocity ˙q. The velocity ˙q is now defined as

$$
\dot{q} = \mathbf{J}\_a^\# \dot{x}\_a + \mathbf{N}\_a' \mathbf{J}\_b^\# \dot{x}\_{b\prime} \tag{28}
$$

where the matrix **N**� *<sup>a</sup>* is given as

10 Will-be-set-by-IN-TECH

a) EX controller

b) AP controller

To avoid the obstacle the goal velocity in *Ao* is represented by the vector ˙x*o*. Using the original method (12) the velocity in *Ao* is exactly ˙x*o*. The joint velocities ˙q*EX* assure that the component of the velocity at point *Ao* (i.e., **J***o*q˙*EX*) in the direction of ˙x*<sup>o</sup>* is as required. The approximate solution ˙q*AP* gives, in most cases, a smaller magnitude of the velocity in the direction of ˙x*<sup>o</sup>* (see **J***o*q˙*AP*). Therefore, the manipulator moves closer to the obstacle when ˙q*AP* is used. This is not so critical, because the minimal distance also depends on the nominal velocity *vo*, which can be increased to achieve larger minimal distances. Additionally, the approximate solution possesses certain advantages when many active obstacles have to be considered. The joint

#x˙ *<sup>c</sup>* + **N**

where *no* is the number of active obstacles, and therefore, the matrix **N** has to be calculated

the same system and task as before, the simulation results using the approximate velocity controller AP (24) are presented in Fig. 4(b). We can see that in the case of the AP controller, the links are coming closer to the obstacle as in the case of the EX controller. However, when changing the desired critical point velocity *x*˙*o*, i.e., using a higher order in (18), the minimal

For a redundant system multiple tasks can be arranged in priority. Let us consider two tasks,

For each of the tasks, the corresponding Jacobian matrices can be defined as **J***<sup>a</sup>* and **J***b*, and their corresponding null-space projections as **N***<sup>a</sup>* and **N***b*. Assuming that task *Ta* is the primary task,

#

q˙ = **J** # *<sup>a</sup>*x˙ *<sup>a</sup>* + **N***a***J**

*no* ∑ *i*=1 **J** #

Fig. 4. Planar 4DOF manipulator tracking a line while avoiding obstacles using velocity

P2

P2

<sup>0</sup> <sup>2</sup> <sup>4</sup> <sup>6</sup> <sup>0</sup>

t

<sup>0</sup> <sup>2</sup> <sup>4</sup> <sup>6</sup> <sup>0</sup>

t

*do* ,*ix*˙*o*,*<sup>i</sup>* (25)

*<sup>b</sup>*x˙ *<sup>b</sup>* (27)

*<sup>o</sup>*,*<sup>i</sup>* have to be calculated for each active obstacle. For

x*<sup>a</sup>* = f*a*(q) x*<sup>b</sup>* = f*b*(q) (26)

0.05 0.1 0.15 0.2 0.25 0.3

0.05 0.1 0.15 0.2 0.25 0.3

minimal distance [m]

minimal distance [m]

P1

P1

q˙*AP* = **J**

control (the dotted line indicates the critical distance)

velocities can be calculated as

distance can be increased.

Eq. (4) can be rewritten as

*Ta* and *Tb*

only once. Of course, pseudoinverses **J**#

**4.5 Obstacle avoidance as a primary task**

$$\mathbf{N}\_a^\prime = \mathbf{I} - \lambda(x\_a)\mathbf{J}^\#\mathbf{J},\tag{29}$$

where *λ*(x*a*) is a scalar measure of how "active" is the primary task *Ta*, scaling the vector x*<sup>a</sup>* to the interval [0, 1]. When the primary task *Ta* is active the *λ*(x*a*) = 1 and the value when the task *Ta* is not active *λ*(x*a*) = 0.

The proposed algorithm allows a smooth transition in both ways, i.e., between observing the task *Ta* and the task *Tb* in null-space of the task *Ta* or just the unconstrained movement of the task *Tb*. It can be used for different robotic tasks. When applied to obstacle avoidance, task *Ta* is the obstacle avoidance and the end-effector motion is the task *Tb*. Before, we have assumed that the end-effector motion is not disturbed by an obstacle. Now, it is assumed that the motion of the end effector can be disturbed by any obstacle (see Fig. 5). If such a situation occurs, usually the task execution has to be interrupted and higher-level path planning has to be employed to recalculate the desired motion of the end effector. However, if the end-effector path tracking is not essential, we can use the proposed control (28). Consequently, no end-effector path recalculation or higher-level path planning is needed.

Let the primary task *Ta* be the motion in the direction d<sup>0</sup> and the motion of the end-effector be the task *Tb*. Using the reduced operational space yields

$$\mathbf{J}\_{\mathbf{d}} = \mathbf{J}\_{o\prime} \tag{30}$$

$$\mathbf{J}\_b = \mathbf{J}.\tag{31}$$

Next, let the avoiding velocity ˙x*d*<sup>0</sup> be defined as

$$
\dot{x}\_{d\_0} = \lambda \left( d\_0 \right) v\_{0\prime} \tag{32}
$$

where v<sup>0</sup> is a nominal velocity and *λ* (d0) is defined as

$$\lambda(\mathbf{d}\_0) = \left\{ \left( \frac{d\_m}{||\mathbf{d}\_0||} \right)^n \right\} \; n = 1, 2, 3... \quad ||\mathbf{d}\_0|| \ge d\_m \tag{33}$$

where *n* = 1, 2, 3... and *dm* is the critical distance to the obstacle. Then eq. (28) can be rewritten in the form

$$
\dot{\mathbf{q}} = \mathbf{J}\_o^\dagger \boldsymbol{\lambda} \left(\mathbf{d}\_0\right) \mathbf{v}\_0 + \mathbf{N}\_0' \mathbf{J}^\# \dot{\mathbf{x}}\_c. \tag{34}
$$

Manipulators as Control Problem 13

Obstacle Avoidance for Redundant Manipulators as Control Problem 215

to measure the contact forces and the controller includes a force control loop. But when the contacts between the manipulator and an obstacle can take place anywhere on the body of the manipulator, it is questionable whether the forces arising from such contacts can be measured. Assuming that the contact forces are not measurable and no other sensors to detect the obstacles are present, the contact forces have to be considered in the controller as disturbances. Now the problem is, how to generate a motion that would move a part of the manipulator away from the obstacle. To solve this problem we propose to follow a very basic principle: *an action causes a reaction*. In other words, if a manipulator acts with a force on an obstacle then the obstacle acts with a force in the opposite direction on the manipulator and our idea is to take advantage of this reaction force to move the manipulator away from the obstacle. To make such a motion possible, the control must not force the manipulator to oppose the reaction force (e.g. by preserving the configuration). This means that the system should be compliant to these external forces. A prerequisite for such an approach is that the manipulator is *backdrivable*, meaning that any force at the manipulator is immediately felt at the motors, so

To decouple the task space and the null-space motion we propose to use the controller (7). Actually, from the obstacle-avoidance point of view, any controller for the redundant manipulators could be used provided that the controller outputs are the joint torques. However, this is not enough to decouple the task space and null-space motion. Namely, torques applied through the null-space of **J***<sup>T</sup>*#, i.e., **N***T*τ , can affect the end-effector acceleration, depending on the choice of the generalized inverse. It turns out for redundant

is the unique generalized inverse that decouples the task space and the null-space motion, i.e., assures that the task space acceleration is not affected by any torques applied through the null

**J***T*, and that the end-effector forces do not produce any accelerations in the null-space

**<sup>J</sup>**(e¨ <sup>+</sup> **<sup>K</sup>***v*e˙ <sup>+</sup> **<sup>K</sup>***p*e) + **<sup>N</sup>**¯ (−q¨ <sup>+</sup> <sup>ϕ</sup>¨ <sup>+</sup> **<sup>N</sup>**¯˙ <sup>ϕ</sup>˙ <sup>+</sup> **<sup>K</sup>***n*e˙*<sup>n</sup>* <sup>−</sup> **<sup>N</sup>**¯˙ <sup>q</sup>˙) = <sup>−</sup>**H**−1τ*<sup>F</sup>* (39)

where τ*<sup>F</sup>* represents the influence of all external forces caused by the contacts with obstacles

*ao* ∑ *i*=1 **J***T o*,*i*

τ*<sup>F</sup>* =

**J** defined as

**J** = **H**−1**J***T*(**JH**−1**J***T*)−<sup>1</sup> (36)

**<sup>J</sup>**q˙) + **<sup>N</sup>**¯ (<sup>φ</sup> <sup>−</sup> **<sup>N</sup>**¯˙ <sup>q</sup>˙)) + <sup>h</sup> <sup>+</sup> <sup>g</sup> (37)

**<sup>J</sup>**q˙) + **<sup>N</sup>**¯ (ϕ¨ <sup>+</sup> **<sup>N</sup>**¯˙ <sup>ϕ</sup>˙ <sup>+</sup> **<sup>K</sup>***n*e˙*<sup>n</sup>* <sup>−</sup> **<sup>N</sup>**¯˙ <sup>q</sup>˙)) + <sup>h</sup> <sup>+</sup> <sup>g</sup> (38)

F*o*,*<sup>i</sup>* (40)

<sup>e</sup>¨ <sup>+</sup> **<sup>K</sup>***v*e˙ <sup>+</sup> **<sup>K</sup>***p*<sup>e</sup> <sup>=</sup> <sup>−</sup>**JH**−1τ*<sup>F</sup>* (41)

the manipulator reacts rapidly, drawing back from the source of the force.

systems that only the so-called "dynamically consistent" generalized inverse ¯

¯

**<sup>J</sup>**(x¨ *<sup>c</sup>* <sup>−</sup> ˙

**<sup>J</sup>**(x¨ *<sup>d</sup>* <sup>+</sup> **<sup>K</sup>***v*e˙ <sup>+</sup> **<sup>K</sup>***p*<sup>e</sup> <sup>−</sup> ˙

Using the inertia weighted generalized inverse in Eq. (7) yields

Combining (3) and (37), and considering Eqs. (8) and (9) yields

τ = **H**(¯

space of ¯

of **J** (Featherstone & Khatib, 1997).

**<sup>H</sup>**q¨ <sup>+</sup> <sup>h</sup> <sup>+</sup> <sup>g</sup> <sup>−</sup> <sup>τ</sup>*<sup>F</sup>* <sup>=</sup> **<sup>H</sup>**(¯

¯

Premultiplying (39) with **J** yields

which simplifies to

Here, ˙x*<sup>c</sup>* is the task controller for the end-effector tracking and **N**� <sup>0</sup> is given by

$$\mathbf{N}'\_0 = \mathbf{I} - \lambda \left(\mathbf{d}\_0\right) \mathbf{J}\_o^\dagger \mathbf{J}\_o. \tag{35}$$

Formulation (34) allows unconstrained joint movement, while *λ*(d0) is close to zero (*λ*(d0) ≈ 0). Thus, the robot can track the desired task space path, while it is away from the obstacle. On the other hand, when the robot is close to the obstacle (*λ*(d0) ≈ 1), the null space in (35) takes the form **N**� <sup>0</sup> = **N**0, and only allows movement in the null space of the primary task, i.e., the obstacle-avoidance task. In this case, we can still move the end effector, but the tracking error can increase due to the obstacle-avoiding motion.

#### **4.6 Singular configurations**

An important issue in the control of redundant manipulators is singular configurations where the associated Jacobian matrices lose the rank. Usually, only the configuration of the whole manipulator is of interest, but in obstacle avoidance we have also to consider the singularities of two manipulator substructures defined by the critical point *Ao*: (a) the part of the manipulator between the base and the point *Ao* and (b) the part between *Ao* and the end-effector. Although it can be assumed that the end-effector Jacobian **J**# is not singular along the desired end-effector path (otherwise the primary task can not be achieved), this is not always true for the matrix **J***o***N**. Namely, when part (a) is in the singular configuration then **J***<sup>o</sup>* is not of full rank and when part (b) is in the singular configuration then **J***<sup>o</sup>* retains the rank but **J***do***N** becomes singular. Hence, when approaching either singular configuration the values of ˙q*<sup>h</sup>* become unacceptably large. As the manipulator is supposed to move in an unstructured environment it is practically impossible to know when **J***o***N** will become singular. Therefore, a very important advantage of the proposed Jacobian **J***do* compared to **J***<sup>o</sup>* is that the system has significantly fewer singularities when **J***do* is used.

#### **5. Obstacle avoidance using forces**

Impedance control approaches for obstacle-avoidance were first introduced by Hogan (Hogan, 1985). These approaches make use of the additive property of impedances to supplement an impedance controller with additional disturbance forces to avoid the obstacles. The disturbance forces are generated from the artificial potential field. Lee demonstrated this approach with his reference adaptive impedance controller and gave promising results for a simulated 2-DOF robot (Lee et al., 1997).

The advantage of these approaches is that the dynamic behavior of the manipulator as it interacts with obstacles is adjustable through the gains in the obstacle-induced disturbing force. However, this approach is tightly coupled with the control scheme and requires the use of a compliance controller, which may not be desirable, depending on the task.

#### **5.1 Obstacle avoidance without any sensors**

First we want to investigate what happens if the manipulator collides with an obstacle. We are especially interested in how to control the manipulator so that it avoids the obstacle after the collision and how to minimize the contact forces. When the task itself includes contacts with the environment (e.g. assembly), the manipulator is equipped with an appropriate sensor 12 Will-be-set-by-IN-TECH

where *n* = 1, 2, 3... and *dm* is the critical distance to the obstacle. Then eq. (28) can be rewritten

*<sup>o</sup>λ* (d0) v<sup>0</sup> + **N**�

<sup>0</sup> = **I** − *λ* (d0)**J**

Formulation (34) allows unconstrained joint movement, while *λ*(d0) is close to zero (*λ*(d0) ≈ 0). Thus, the robot can track the desired task space path, while it is away from the obstacle. On the other hand, when the robot is close to the obstacle (*λ*(d0) ≈ 1), the null space in (35)

the obstacle-avoidance task. In this case, we can still move the end effector, but the tracking

An important issue in the control of redundant manipulators is singular configurations where the associated Jacobian matrices lose the rank. Usually, only the configuration of the whole manipulator is of interest, but in obstacle avoidance we have also to consider the singularities of two manipulator substructures defined by the critical point *Ao*: (a) the part of the manipulator between the base and the point *Ao* and (b) the part between *Ao* and the end-effector. Although it can be assumed that the end-effector Jacobian **J**# is not singular along the desired end-effector path (otherwise the primary task can not be achieved), this is not always true for the matrix **J***o***N**. Namely, when part (a) is in the singular configuration then **J***<sup>o</sup>* is not of full rank and when part (b) is in the singular configuration then **J***<sup>o</sup>* retains the rank but **J***do***N** becomes singular. Hence, when approaching either singular configuration the values of ˙q*<sup>h</sup>* become unacceptably large. As the manipulator is supposed to move in an unstructured environment it is practically impossible to know when **J***o***N** will become singular. Therefore, a very important advantage of the proposed Jacobian **J***do* compared to **J***<sup>o</sup>* is that the system has

Impedance control approaches for obstacle-avoidance were first introduced by Hogan (Hogan, 1985). These approaches make use of the additive property of impedances to supplement an impedance controller with additional disturbance forces to avoid the obstacles. The disturbance forces are generated from the artificial potential field. Lee demonstrated this approach with his reference adaptive impedance controller and gave promising results for a

The advantage of these approaches is that the dynamic behavior of the manipulator as it interacts with obstacles is adjustable through the gains in the obstacle-induced disturbing force. However, this approach is tightly coupled with the control scheme and requires the use

First we want to investigate what happens if the manipulator collides with an obstacle. We are especially interested in how to control the manipulator so that it avoids the obstacle after the collision and how to minimize the contact forces. When the task itself includes contacts with the environment (e.g. assembly), the manipulator is equipped with an appropriate sensor

of a compliance controller, which may not be desirable, depending on the task.

0**J**

†

<sup>0</sup> = **N**0, and only allows movement in the null space of the primary task, i.e.,

#x˙ *<sup>c</sup>*. (34)

*<sup>o</sup>* **J***o*. (35)

<sup>0</sup> is given by

q˙ = **J** †

**N**�

Here, ˙x*<sup>c</sup>* is the task controller for the end-effector tracking and **N**�

error can increase due to the obstacle-avoiding motion.

significantly fewer singularities when **J***do* is used.

**5. Obstacle avoidance using forces**

simulated 2-DOF robot (Lee et al., 1997).

**5.1 Obstacle avoidance without any sensors**

in the form

takes the form **N**�

**4.6 Singular configurations**

to measure the contact forces and the controller includes a force control loop. But when the contacts between the manipulator and an obstacle can take place anywhere on the body of the manipulator, it is questionable whether the forces arising from such contacts can be measured. Assuming that the contact forces are not measurable and no other sensors to detect the obstacles are present, the contact forces have to be considered in the controller as disturbances. Now the problem is, how to generate a motion that would move a part of the manipulator away from the obstacle. To solve this problem we propose to follow a very basic principle: *an action causes a reaction*. In other words, if a manipulator acts with a force on an obstacle then the obstacle acts with a force in the opposite direction on the manipulator and our idea is to take advantage of this reaction force to move the manipulator away from the obstacle. To make such a motion possible, the control must not force the manipulator to oppose the reaction force (e.g. by preserving the configuration). This means that the system should be compliant to these external forces. A prerequisite for such an approach is that the manipulator is *backdrivable*, meaning that any force at the manipulator is immediately felt at the motors, so the manipulator reacts rapidly, drawing back from the source of the force.

To decouple the task space and the null-space motion we propose to use the controller (7). Actually, from the obstacle-avoidance point of view, any controller for the redundant manipulators could be used provided that the controller outputs are the joint torques. However, this is not enough to decouple the task space and null-space motion. Namely, torques applied through the null-space of **J***<sup>T</sup>*#, i.e., **N***T*τ , can affect the end-effector acceleration, depending on the choice of the generalized inverse. It turns out for redundant systems that only the so-called "dynamically consistent" generalized inverse ¯ **J** defined as

$$\mathbf{j} = \mathbf{H}^{-1} \mathbf{j}^T (\mathbf{J} \mathbf{H}^{-1} \mathbf{J}^T)^{-1} \tag{36}$$

is the unique generalized inverse that decouples the task space and the null-space motion, i.e., assures that the task space acceleration is not affected by any torques applied through the null space of ¯ **J***T*, and that the end-effector forces do not produce any accelerations in the null-space of **J** (Featherstone & Khatib, 1997).

Using the inertia weighted generalized inverse in Eq. (7) yields

$$\boldsymbol{\tau} = \mathbf{H}(\bar{\mathbf{J}}(\ddot{x}\_{\varepsilon} - \dot{\mathbf{J}}\dot{q}) + \bar{\mathbf{N}}(\phi - \mathbf{\hat{N}}\dot{q})) + \boldsymbol{h} + \boldsymbol{g} \tag{37}$$

Combining (3) and (37), and considering Eqs. (8) and (9) yields

$$\mathbf{H}\ddot{q} + h + g - \tau\_{\mathrm{F}} = \mathbf{H}(\mathbf{J}(\ddot{x}\_{d} + \mathbf{K}\_{\mathrm{v}}\dot{e} + \mathbf{K}\_{\mathrm{p}}e - \mathbf{J}\dot{q}) + \mathbf{\tilde{N}}(\ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_{\mathrm{n}}\dot{e}\_{\mathrm{n}} - \dot{\mathbf{N}}\dot{q})) + h + g \tag{38}$$

which simplifies to

$$\mathbf{J}(\ddot{e} + \mathbf{K}\_{\nu}\dot{e} + \mathbf{K}\_{\mathcal{P}}e) + \mathbf{\tilde{N}}(-\ddot{q} + \ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_{\mathcal{U}}\dot{e}\_{\mathcal{U}} - \dot{\mathbf{N}}\dot{q}) = -\mathbf{H}^{-1}\tau\_{\mathcal{F}}\tag{39}$$

where τ*<sup>F</sup>* represents the influence of all external forces caused by the contacts with obstacles

$$\tau\_{\mathbf{F}} = \sum\_{i=1}^{a\_o} \mathbf{J}\_{o,i}^T \mathbf{F}\_{o,i} \tag{40}$$

Premultiplying (39) with **J** yields

$$
\ddot{e} + \mathbf{K}\_{\upsilon}\dot{e} + \mathbf{K}\_{\mathcal{V}}e = -\mathbf{J}\mathbf{H}^{-1}\boldsymbol{\tau}\_{\mathcal{F}}\tag{41}
$$

Note that the contact forces affect the motion in the task space, which results in the tracking error of the end-effector. Of course, if the obstacle avoidance is successful the contact forces vanish and the task position error converges to zero. The proper choice of **K***<sup>v</sup>* and **K***<sup>p</sup>* assures the asymptotical stability of the homogeneous part of the system, ¨e + **K***v*e˙ + **K***p*e = 0. A detailed analysis of the influence of the external forces is given in Žlajpah & Nemec (2003). Next we analyse the behavior in null-space. Premultiplying (39) with **N**¯ yields

$$-\dot{\mathbf{N}}\mathbf{H}^{-1}\boldsymbol{\tau}\_{\mathbf{F}} = \dot{\mathbf{N}}(-\ddot{\mathbf{q}} + \ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_{\text{ll}}\dot{\mathbf{e}}\_{\text{ll}} - \dot{\mathbf{N}}\dot{\mathbf{q}}) \tag{42}$$

Manipulators as Control Problem 15

Obstacle Avoidance for Redundant Manipulators as Control Problem 217

P2

Fig. 6. Planar 4DOF manipulator tracking a line while avoiding obstacles without using any

P2

Fig. 7. Planar 4DOF manipulator tracking a line while avoiding obstacles without using any

the compliancy in the null space. Actually, the impact forces are the main problem with this type of control. Namely, the magnitude of the impact forces cannot be controlled with the controller, the controller can decrease the contact forces after the impact. The magnitude of the impact forces depends primarily on the kinetic energy of the bodies that collide and the stiffness of the contact. Therefore, this approach to obstacle avoidance is limited to cases where the manipulator is moving slowly or the manipulator body (or obstacles) is covered with soft material, which reduces the impact forces. Note that the impact forces (the magnitude and the time of occurrence) are not the same in these examples due to the different close-loop

As already mentioned, the obstacle-avoidance approach based on contact forces without any contact sensors cannot be applied to manipulators that are not backdrivable, like manipulators with high-ratio gears. The alternative strategy is a sensor based motion control, where a kind of tactile sensors mounted on a manipulator detect the contact with an obstacle. The main advantage of using the tactile sensor information in control is that the obstacle avoidance becomes "active", meaning the avoiding motion is initiated by the controller. So, it can be

With tactile sensors the collisions can be detected. Our approach is to identify the points on the manipulator that are in contact with obstacles and to assign to them additional virtual force components that move the points away from the obstacle (see Fig. 8). We propose that these forces are not included into the close-loop controller, but they are applied as the virtual

τ*<sup>o</sup>* = **J***<sup>T</sup>*

applied to any manipulator, irrespective of the backdrivability of the manipulator.

Suppose that F*<sup>o</sup>* is acting at point *Ao* somewhere on the link *i* (see Fig. 8). The static relation between the force F*<sup>o</sup>* and the corresponding joint torques τ*<sup>o</sup>* is

50

50


100


100

<sup>0</sup> <sup>2</sup> <sup>4</sup> <sup>6</sup> <sup>0</sup>

t

<sup>0</sup> <sup>2</sup> <sup>4</sup> <sup>6</sup> <sup>0</sup>

t

*<sup>o</sup>* F*<sup>o</sup>* (44)

P1

P1

sensors to detect the obstacles (CF); stiff in null space

dynamics of the system.

**5.2 Obstacle avoidance with tactile sensors**

external forces to the manipulator.

sensors to detect the obstacles (CF); compliant in null space

Rearranging Eq. (42) and using the relations ¨e*<sup>n</sup>* <sup>=</sup> **<sup>N</sup>**¯ (ϕ¨ <sup>−</sup> <sup>q</sup>¨) + **<sup>N</sup>**¯˙ (ϕ˙ <sup>−</sup> <sup>q</sup>˙) and **NH**¯ <sup>−</sup><sup>1</sup> <sup>=</sup> **H**−1**N**¯ *<sup>T</sup>*, we obtain

$$\bar{\mathbf{N}}(\ddot{\mathbf{e}}\_{\text{ll}} + \mathbf{K}\_{\text{ll}}\dot{\mathbf{e}}\_{\text{ll}}) = -\bar{\mathbf{N}}\mathbf{H}^{-1}\bar{\mathbf{N}}^{T}\boldsymbol{\tau}\_{\text{F}} = -\mathbf{H}\_{\text{ll}}^{\ddagger}\boldsymbol{\tau}\_{\text{F}} \tag{43}$$

where **H***<sup>n</sup>* is the *null-space effective inertia matrix* describing the inertial properties of the system in the null-space

$$\mathbf{H}\_n = \bar{\mathbf{N}}^T \mathbf{H} \bar{\mathbf{N}}$$

and **H**‡ *<sup>n</sup>* is the generalized inverse of **H***n*, defined as

$$\mathbf{H}\_n^\ddagger = \mathbf{N} \mathbf{H}^{-1} \mathbf{N}^T$$

The selection of the null space dynamic properties is subjected to the subtask that the manipulator should perform. Actually, in most cases it is required that the null space velocity tracks a given desired null space velocity ˙ϕ. For good null space velocity tracking, the gain matrix **K***<sup>n</sup>* should be high, which means that the system is stiff in the null space. On the other hand, when the manipulator collides with an obstacle, the self-motion is initiated externally by the contact force. As we have already mentioned, in this case the system should be compliant in the null-space. Therefore, the gain matrix **K***<sup>n</sup>* should be low. As both requirements for **K***<sup>n</sup>* are in conflict, a compromise has to made when selecting **K***<sup>n</sup>* or the on-line adaptation of **K***<sup>n</sup>* has to be used. The problem with the second possibility is that we have to be able to detect the current state, i.e., if the manipulator is in contact with an object. If this is possible, then it is easy to set low gains when the collision occurs and high gains when the manipulator is "free". If we cannot detect the contact and the probability of collisions is high, then it is rational to select a low **K***n*, but we must be aware that too low values of **K***<sup>n</sup>* may cause instability in the null-space. The lower bounds for **K***<sup>n</sup>* can be obtained with a Lyapunov analysis (Žlajpah & Nemec, 2003).

As an illustration we use the same example as before, where the manipulator is supposed to have 4 revolute joints. The primary task is to move along a straight line from point *P*<sup>1</sup> to point *P*2, and the motion is obstructed by an obstacle. The task trajectory has a trapezoid velocity profile with an acceleration of 4*ms*−<sup>2</sup> and a max. velocity of 0.4*ms*−1. The control algorithm is given by Eq. (37) (CF). The task space controller parameters (Eq. 8) are **K***<sup>p</sup>* = 1000**I**s−<sup>2</sup> and **K***<sup>v</sup>* = 80**I**s−1, which are tuned to ensure good tracking of the task trajectory (stiff task space behavior). The null space controller is a modified version of the controller (9)) φ = −**K***n*q˙, i.e., the desired null-space velocity is set to zero. We have compared two sets of null-space gains: (i) Low **K***n*, which ensures compliant null-space behavior, and (ii) Medium **K***n*, which makes the null-space more stiff. The simulation results are presented in Figs. 6 and 7.

Although the manipulator has finished the desired task in both cases, we can see that making the null-space more compliant results in decreased contact forces. As expected, the impact force does not depend on the stiffness in the null space and cannot be decreased by increasing 14 Will-be-set-by-IN-TECH

Note that the contact forces affect the motion in the task space, which results in the tracking error of the end-effector. Of course, if the obstacle avoidance is successful the contact forces vanish and the task position error converges to zero. The proper choice of **K***<sup>v</sup>* and **K***<sup>p</sup>* assures the asymptotical stability of the homogeneous part of the system, ¨e + **K***v*e˙ + **K***p*e = 0. A detailed analysis of the influence of the external forces is given in Žlajpah & Nemec (2003).

Rearranging Eq. (42) and using the relations ¨e*<sup>n</sup>* <sup>=</sup> **<sup>N</sup>**¯ (ϕ¨ <sup>−</sup> <sup>q</sup>¨) + **<sup>N</sup>**¯˙ (ϕ˙ <sup>−</sup> <sup>q</sup>˙) and **NH**¯ <sup>−</sup><sup>1</sup> <sup>=</sup>

where **H***<sup>n</sup>* is the *null-space effective inertia matrix* describing the inertial properties of the system

**H***<sup>n</sup>* = **N**¯ *<sup>T</sup>***HN**¯

*<sup>n</sup>* = **NH**¯ <sup>−</sup>1**N**¯ *<sup>T</sup>*

The selection of the null space dynamic properties is subjected to the subtask that the manipulator should perform. Actually, in most cases it is required that the null space velocity tracks a given desired null space velocity ˙ϕ. For good null space velocity tracking, the gain matrix **K***<sup>n</sup>* should be high, which means that the system is stiff in the null space. On the other hand, when the manipulator collides with an obstacle, the self-motion is initiated externally by the contact force. As we have already mentioned, in this case the system should be compliant in the null-space. Therefore, the gain matrix **K***<sup>n</sup>* should be low. As both requirements for **K***<sup>n</sup>* are in conflict, a compromise has to made when selecting **K***<sup>n</sup>* or the on-line adaptation of **K***<sup>n</sup>* has to be used. The problem with the second possibility is that we have to be able to detect the current state, i.e., if the manipulator is in contact with an object. If this is possible, then it is easy to set low gains when the collision occurs and high gains when the manipulator is "free". If we cannot detect the contact and the probability of collisions is high, then it is rational to select a low **K***n*, but we must be aware that too low values of **K***<sup>n</sup>* may cause instability in the null-space. The lower bounds for **K***<sup>n</sup>* can be obtained with a Lyapunov analysis (Žlajpah &

As an illustration we use the same example as before, where the manipulator is supposed to have 4 revolute joints. The primary task is to move along a straight line from point *P*<sup>1</sup> to point *P*2, and the motion is obstructed by an obstacle. The task trajectory has a trapezoid velocity profile with an acceleration of 4*ms*−<sup>2</sup> and a max. velocity of 0.4*ms*−1. The control algorithm is given by Eq. (37) (CF). The task space controller parameters (Eq. 8) are **K***<sup>p</sup>* = 1000**I**s−<sup>2</sup> and **K***<sup>v</sup>* = 80**I**s−1, which are tuned to ensure good tracking of the task trajectory (stiff task space behavior). The null space controller is a modified version of the controller (9)) φ = −**K***n*q˙, i.e., the desired null-space velocity is set to zero. We have compared two sets of null-space gains: (i) Low **K***n*, which ensures compliant null-space behavior, and (ii) Medium **K***n*, which makes

Although the manipulator has finished the desired task in both cases, we can see that making the null-space more compliant results in decreased contact forces. As expected, the impact force does not depend on the stiffness in the null space and cannot be decreased by increasing

the null-space more stiff. The simulation results are presented in Figs. 6 and 7.

**<sup>N</sup>**¯ (e¨*<sup>n</sup>* <sup>+</sup> **<sup>K</sup>***n*e˙*n*) = <sup>−</sup>**NH**¯ <sup>−</sup>1**N**¯ *<sup>T</sup>*τ*<sup>F</sup>* <sup>=</sup> <sup>−</sup>**H**‡

<sup>−</sup> **NH**¯ <sup>−</sup>1τ*<sup>F</sup>* <sup>=</sup> **<sup>N</sup>**¯ (−q¨ <sup>+</sup> <sup>ϕ</sup>¨ <sup>+</sup> **<sup>N</sup>**¯˙ <sup>ϕ</sup>˙ <sup>+</sup> **<sup>K</sup>***n*e˙*<sup>n</sup>* <sup>−</sup> **<sup>N</sup>**¯˙ <sup>q</sup>˙) (42)

*<sup>n</sup>*τ*<sup>F</sup>* (43)

Next we analyse the behavior in null-space. Premultiplying (39) with **N**¯ yields

**H**‡

*<sup>n</sup>* is the generalized inverse of **H***n*, defined as

**H**−1**N**¯ *<sup>T</sup>*, we obtain

in the null-space

Nemec, 2003).

and **H**‡

Fig. 6. Planar 4DOF manipulator tracking a line while avoiding obstacles without using any sensors to detect the obstacles (CF); compliant in null space

Fig. 7. Planar 4DOF manipulator tracking a line while avoiding obstacles without using any sensors to detect the obstacles (CF); stiff in null space

the compliancy in the null space. Actually, the impact forces are the main problem with this type of control. Namely, the magnitude of the impact forces cannot be controlled with the controller, the controller can decrease the contact forces after the impact. The magnitude of the impact forces depends primarily on the kinetic energy of the bodies that collide and the stiffness of the contact. Therefore, this approach to obstacle avoidance is limited to cases where the manipulator is moving slowly or the manipulator body (or obstacles) is covered with soft material, which reduces the impact forces. Note that the impact forces (the magnitude and the time of occurrence) are not the same in these examples due to the different close-loop dynamics of the system.

#### **5.2 Obstacle avoidance with tactile sensors**

As already mentioned, the obstacle-avoidance approach based on contact forces without any contact sensors cannot be applied to manipulators that are not backdrivable, like manipulators with high-ratio gears. The alternative strategy is a sensor based motion control, where a kind of tactile sensors mounted on a manipulator detect the contact with an obstacle. The main advantage of using the tactile sensor information in control is that the obstacle avoidance becomes "active", meaning the avoiding motion is initiated by the controller. So, it can be applied to any manipulator, irrespective of the backdrivability of the manipulator.

With tactile sensors the collisions can be detected. Our approach is to identify the points on the manipulator that are in contact with obstacles and to assign to them additional virtual force components that move the points away from the obstacle (see Fig. 8). We propose that these forces are not included into the close-loop controller, but they are applied as the virtual external forces to the manipulator.

Suppose that F*<sup>o</sup>* is acting at point *Ao* somewhere on the link *i* (see Fig. 8).

The static relation between the force F*<sup>o</sup>* and the corresponding joint torques τ*<sup>o</sup>* is

$$
\boldsymbol{\tau}\_{\boldsymbol{0}} = \mathbf{J}\_{\boldsymbol{0}}^{\mathrm{T}} \mathbf{F}\_{\boldsymbol{0}} \tag{44}
$$

Manipulators as Control Problem 17

Obstacle Avoidance for Redundant Manipulators as Control Problem 219

where *Ti* and *Td* are the constants for the increase of the gain value and the delayed action, and *ts* and *te* represent the time when the contact occurs and the time when the contact is lost, respectively. With the appropriate selection of *Ti* and *Td* a robust behavior can be obtained.

The next issue is how the obstacle avoidance is performed when there are more simultaneously active obstacles in the neighborhood of the manipulator. Using the proposed obstacle-avoidance formulation the problem of many simultaneously active obstacles can be solved very efficiently. The additivity of the torques makes it possible to avoid multiple

> *ao* ∑ *i*=1 **J***T o*,*i*

t

F*o*,*<sup>i</sup>* (51)

F*o*,*<sup>i</sup>* (52)

**<sup>J</sup>**q˙) + **<sup>N</sup>**¯ (ϕ¨ <sup>+</sup> **<sup>N</sup>**¯˙ <sup>ϕ</sup>˙ <sup>+</sup> **<sup>K</sup>***n*e˙*<sup>n</sup>* <sup>−</sup> **<sup>N</sup>**¯˙ <sup>q</sup>˙)) + <sup>h</sup> <sup>+</sup> <sup>g</sup>

(53)

*<sup>o</sup>*,*<sup>i</sup>* for each critical point and not

αt

1

0 t s t e

Fig. 9. The obstacle-avoidance gain *α<sup>t</sup>* versus the duration of the contact

obstacles by using the sum of the torques due to the relevant virtual forces,

it is necessary to calculate only the transpose of the Jacobian **J***<sup>T</sup>*

computationally, e.g. in Eq. (52) the term **N**¯ *<sup>T</sup>* is calculated only once.

the behavior of the system with contact sensors is described by

and considering this in Eq. (47) yields

the generalized inverse ¯

**<sup>H</sup>**q¨ <sup>+</sup> <sup>h</sup> <sup>+</sup> <sup>g</sup> <sup>−</sup> <sup>τ</sup>*<sup>F</sup>* <sup>−</sup> **<sup>N</sup>**¯ *<sup>T</sup>*τ*<sup>o</sup>* <sup>=</sup> **<sup>H</sup>**(¯

¯

τ*<sup>o</sup>* =

<sup>τ</sup>*o*,*<sup>N</sup>* <sup>=</sup> **<sup>N</sup>**¯ *<sup>T</sup> ao*

∑ *i*=1 **J***T o*,*i*

**J***o*,*i*, as is the case with velocity-based strategies. The redundancy of the

where *ao* is the number of active obstacles. It is clear that when more than one obstacle is active

manipulator is considered in the term **N**¯ *<sup>T</sup>*, which does not depend on the location of particular critical points *Ao*,*i*. Hence, there is no limitation on *ao* regarding the degree-of-redundancy. In the case when *ao* is greater than the degree-of-redundancy, the manipulator is pushed into a configuration where the virtual forces compensate each other, i.e., τ*<sup>o</sup>* = 0. Actually, such situations can occur even when *ao* is less than the degree-of-redundancy, e.g. when one link is under the influence of more than one obstacle. The force formulation has its advantages

For the close-loop control the controller (37) is used again. Augmenting (38) with virtual forces

Actually, the term **N**¯ *<sup>T</sup>*τ*<sup>o</sup>* is also part of the controller and should be on the right-hand side of Eq. (53), but we put it on the left-hand side to emphasize its role. Eq. (53) simplifies to

**<sup>J</sup>**(e¨ <sup>+</sup> **<sup>K</sup>***v*e˙ <sup>+</sup> **<sup>K</sup>***p*e) + **<sup>N</sup>**¯ (−q¨ <sup>+</sup> <sup>ϕ</sup>¨ <sup>+</sup> **<sup>N</sup>**¯˙ <sup>ϕ</sup>˙ <sup>+</sup> **<sup>K</sup>***n*e˙*<sup>n</sup>* <sup>−</sup> **<sup>N</sup>**¯˙ <sup>q</sup>˙) = <sup>−</sup>**H**−1(**N**¯ *<sup>T</sup>*τ*<sup>o</sup>* <sup>+</sup> <sup>τ</sup>*F*) (54)

**<sup>J</sup>**(x¨ *<sup>d</sup>* <sup>+</sup> **<sup>K</sup>***v*e˙ <sup>+</sup> **<sup>K</sup>***p*<sup>e</sup> <sup>−</sup> ˙

Fig. 8. Manipulator motion in the presence of some obstacles: links in contact are moved away from obstacles by additional virtual forces

where **J***<sup>o</sup>* is a Jacobian matrix associated with the point *Ao*. Applying τ*<sup>o</sup>* to the system yields the equation of motion in the form

$$
\tau = \mathbf{H}\ddot{q} + h + g - \tau\_{\mathbb{F}} - \tau\_0 \tag{45}
$$

which is the same as Eq. (3), except that the torques due to the virtual forces forces τ*<sup>F</sup>* are added. In general, by applying the torques τ*<sup>o</sup>* as defined in Eq. (44), the motion of the end-effector and the self-motion of the manipulator are influenced. Note that one of the goals of obstacle avoidance is to disturb the end-effector motion as little as possible. As we are adding virtual forces, it is not necessary that the applied virtual forces correspond to the "real" forces. Therefore, only torques that do not influence the end-effector motion are proposed to be added to generate the obstacle-avoidance motion. In general, the force F*<sup>o</sup>* can be substituted by a force acting in the task space

$$F\_{\alpha\varepsilon} = \mathbf{J}^{T\#} \mathbf{J}\_o^T F\_o \tag{46}$$

and by joint torques acting in the null-space of **J***T*#

$$\boldsymbol{\tau}\_{oN} = \mathbf{N}^T \mathbf{J}\_o^T \mathbf{F}\_o = \mathbf{N}^T \boldsymbol{\tau}\_o \tag{47}$$

Substituting τ*oN* for τ*<sup>o</sup>* in Eq. (45) yields

$$
\tau = \mathbf{H}\ddot{q} + h + g - \tau\_{\overline{F}} - \bar{\mathbf{N}}^T \tau\_0 \tag{48}
$$

The efficiency of the obstacle-avoidance algorithm depends on the selection of the desired force F*o*. In our approach, the virtual force F*<sup>o</sup>* depends on the location of the critical point *Ao*, i.e., the locations of the tactile sensors that detect the contact. As the positions of the sensors are known in advance, it is easy to get the corresponding Jacobian matrix **J***<sup>o</sup>* for each sensor. Additionally, each sensor also has a predefined avoiding direction n*o*. We define the virtual forces F*<sup>o</sup>* as

$$F\_0 = \alpha\_t f\_0 \mathbf{n}\_0 \tag{49}$$

where *α<sup>t</sup>* is the obstacle-avoidance gain. We propose that *α<sup>t</sup>* depends on the duration of the contact and that the achievedvalue is preserved for some time after the contact between the manipulator and the obstacle is lost (see Fig. 9)

$$\mathfrak{a}\_{t} = \begin{cases} 0 & \text{for} \qquad t < t\_{\mathrm{s}} \\ e^{T\_{i}(t-t\_{i})} & \text{for} \quad t\_{\mathrm{s}} \le t < t\_{\mathrm{c}} \\ e^{T\_{i}(t\_{\mathrm{t}}-t\_{\mathrm{s}})}e^{-T\_{\mathrm{d}}(t-t\_{\mathrm{t}})} & \text{for} \quad t\_{\mathrm{c}} \le t \end{cases} \tag{50}$$

16 Will-be-set-by-IN-TECH

Obstacle

*F***o**

A**o**

*x***e** .

τ = **H**q¨ + h + g − τ*<sup>F</sup>* − τ*<sup>o</sup>* (45)

*<sup>o</sup>* F*<sup>o</sup>* (46)

*<sup>o</sup>* <sup>F</sup>*<sup>o</sup>* = **<sup>N</sup>***T*τ*<sup>o</sup>* (47)

<sup>τ</sup> <sup>=</sup> **<sup>H</sup>**q¨ <sup>+</sup> <sup>h</sup> <sup>+</sup> <sup>g</sup> <sup>−</sup> <sup>τ</sup>*<sup>F</sup>* <sup>−</sup> **<sup>N</sup>**¯ *<sup>T</sup>*τ*<sup>o</sup>* (48)

F*<sup>o</sup>* = *α<sup>t</sup> fo*n*<sup>o</sup>* (49)

(50)

Obstacle

Fig. 8. Manipulator motion in the presence of some obstacles: links in contact are moved

where **J***<sup>o</sup>* is a Jacobian matrix associated with the point *Ao*. Applying τ*<sup>o</sup>* to the system yields

which is the same as Eq. (3), except that the torques due to the virtual forces forces τ*<sup>F</sup>* are added. In general, by applying the torques τ*<sup>o</sup>* as defined in Eq. (44), the motion of the end-effector and the self-motion of the manipulator are influenced. Note that one of the goals of obstacle avoidance is to disturb the end-effector motion as little as possible. As we are adding virtual forces, it is not necessary that the applied virtual forces correspond to the "real" forces. Therefore, only torques that do not influence the end-effector motion are proposed to be added to generate the obstacle-avoidance motion. In general, the force F*<sup>o</sup>* can

F*oe* = **J***T*#**J***<sup>T</sup>*

The efficiency of the obstacle-avoidance algorithm depends on the selection of the desired force F*o*. In our approach, the virtual force F*<sup>o</sup>* depends on the location of the critical point *Ao*, i.e., the locations of the tactile sensors that detect the contact. As the positions of the sensors are known in advance, it is easy to get the corresponding Jacobian matrix **J***<sup>o</sup>* for each sensor. Additionally, each sensor also has a predefined avoiding direction n*o*. We define the virtual

where *α<sup>t</sup>* is the obstacle-avoidance gain. We propose that *α<sup>t</sup>* depends on the duration of the contact and that the achievedvalue is preserved for some time after the contact between the

> 0 for *t* < *ts <sup>e</sup>Ti*(*t*−*ts*) for *ts* <sup>≤</sup> *<sup>t</sup>* <sup>&</sup>lt; *te <sup>e</sup>Ti*(*te*−*ts*)*e*−*Td* (*t*−*te*) for *te* <sup>≤</sup> *<sup>t</sup>*

τ*oN* = **N***T***J***<sup>T</sup>*

*<sup>d</sup>***<sup>o</sup>** *<sup>F</sup>***o1**

Critical points

**Jo**

away from obstacles by additional virtual forces

be substituted by a force acting in the task space

and by joint torques acting in the null-space of **J***T*#

manipulator and the obstacle is lost (see Fig. 9)

*α<sup>t</sup>* =

⎧ ⎪⎨

⎪⎩

Substituting τ*oN* for τ*<sup>o</sup>* in Eq. (45) yields

forces F*<sup>o</sup>* as

the equation of motion in the form

where *Ti* and *Td* are the constants for the increase of the gain value and the delayed action, and *ts* and *te* represent the time when the contact occurs and the time when the contact is lost, respectively. With the appropriate selection of *Ti* and *Td* a robust behavior can be obtained.

Fig. 9. The obstacle-avoidance gain *α<sup>t</sup>* versus the duration of the contact

The next issue is how the obstacle avoidance is performed when there are more simultaneously active obstacles in the neighborhood of the manipulator. Using the proposed obstacle-avoidance formulation the problem of many simultaneously active obstacles can be solved very efficiently. The additivity of the torques makes it possible to avoid multiple obstacles by using the sum of the torques due to the relevant virtual forces,

$$\tau\_{\mathcal{O}} = \sum\_{i=1}^{a\_{\mathcal{O}}} \mathbf{J}\_{o,i}^T \mathbf{F}\_{o,i} \tag{51}$$

and considering this in Eq. (47) yields

$$\boldsymbol{\tau}\_{o,N} = \bar{\mathbf{N}}^T \sum\_{i=1}^{a\_o} \mathbf{J}\_{o,i}^T \mathbf{F}\_{o,i} \tag{52}$$

where *ao* is the number of active obstacles. It is clear that when more than one obstacle is active it is necessary to calculate only the transpose of the Jacobian **J***<sup>T</sup> <sup>o</sup>*,*<sup>i</sup>* for each critical point and not the generalized inverse ¯ **J***o*,*i*, as is the case with velocity-based strategies. The redundancy of the manipulator is considered in the term **N**¯ *<sup>T</sup>*, which does not depend on the location of particular critical points *Ao*,*i*. Hence, there is no limitation on *ao* regarding the degree-of-redundancy. In the case when *ao* is greater than the degree-of-redundancy, the manipulator is pushed into a configuration where the virtual forces compensate each other, i.e., τ*<sup>o</sup>* = 0. Actually, such situations can occur even when *ao* is less than the degree-of-redundancy, e.g. when one link is under the influence of more than one obstacle. The force formulation has its advantages computationally, e.g. in Eq. (52) the term **N**¯ *<sup>T</sup>* is calculated only once.

For the close-loop control the controller (37) is used again. Augmenting (38) with virtual forces the behavior of the system with contact sensors is described by

$$\mathbf{H}\ddot{\mathbf{q}} + h + g - \tau\_{\mathbf{F}} - \ddot{\mathbf{N}}^T \tau\_0 = \mathbf{H}(\ddot{\mathbf{J}}(\ddot{x}\_d + \mathbf{K}\_\nu \dot{e} + \mathbf{K}\_p e - \dot{\mathbf{J}}\dot{q}) + \ddot{\mathbf{N}}(\ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_\mathbb{R} \dot{e}\_\mathbb{R} - \dot{\mathbf{N}}\dot{q})) + h + g \tag{53}$$

Actually, the term **N**¯ *<sup>T</sup>*τ*<sup>o</sup>* is also part of the controller and should be on the right-hand side of Eq. (53), but we put it on the left-hand side to emphasize its role. Eq. (53) simplifies to

$$\mathbf{J}(\ddot{e} + \mathbf{K}\_{\nu}\dot{e} + \mathbf{K}\_{\bar{\nu}}e) + \mathbf{N}(-\ddot{q} + \ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_{\mathbb{R}}\dot{e}\_{\mathbb{R}} - \dot{\mathbf{N}}\dot{q}) = -\mathbf{H}^{-1}(\mathbf{N}^{T}\boldsymbol{\tau}\_{0} + \boldsymbol{\tau}\_{\mathbb{F}}) \tag{54}$$

Fig. 10. Planar 4DOF manipulator tracking a line while avoiding obstacles using tactile sensors and virtual forces (TF controller)

Premultiplying (54) with **J** yields

$$
\ddot{e} + \mathbf{K}\_{\upsilon}\dot{e} + \mathbf{K}\_{\mathcal{P}}e = -\mathbf{J}\mathbf{H}^{-1}\tau\_{\mathcal{F}}\tag{55}
$$

Manipulators as Control Problem 19

Obstacle Avoidance for Redundant Manipulators as Control Problem 221

The main difference compared to the system with tactile sensors is how the virtual forces F*<sup>o</sup>* are generated. Now, the virtual force F*<sup>o</sup>* depends on the location of the critical point and the closest point on the obstacle. Clearly, the location of all the objects in the workspace of the manipulator has to be known. This information can be provided by a higher control level that has access to sensory data. As sensor systems are not our concern, we will assume that the sensor can detect obstacles in the workspace of the manipulator and that it outputs the direction and the distance to the closest point obstacle, and the position of the critical point on

Let *Ao* be the critical point and d the vector connecting the closest point on the obstacle and *Ao* (see Fig. 8). To avoid a possible collision a virtual force F*<sup>o</sup>* is assigned to *Ao*, defined as

where *fo* is a scalar gain representing the nominal force, n*<sup>o</sup>* is the unit vector in the direction of d, and *α<sup>f</sup>* is the obstacle-avoidance gain. The gain *α<sup>f</sup>* should depend on the distance to the

> 0 d 0db dm

There are two distances characterizing the changes in the value of the gain: the critical distance *dm* and the abort distance *db*. If the distance between the manipulator and the object is greater than *dm* then the motion of the manipulator is not perturbed. When the distance is decreasing, the force should increase smoothly (to assure smooth transitions it is important that the magnitude of F*<sup>o</sup>* at *dm* is zero). However, if the manipulator is too close to the obstacle (less than *db*) the main task should be, for safety reasons, aborted so that the manipulator can avoid the obstacles (such a situation can occur, especially if moving obstacles are present in the workspace). The distance *db* is subjected to the dynamic properties of the manipulator and can also be a function of the relative velocity between the critical point on the manipulator and the obstacle. The gain *α<sup>f</sup>* can be chosen arbitrarily as long as it has the prescribed form. We

− 1 for �d*o*� < *dm*

(61)

0 for �d*o*� ≥ *dm*

Special attention has to be given to the selection of the nominal force *fo*. Large values of *fo* can cause unnecessarily high accelerations and velocities. Consequently, the manipulator could move far from the obstacle. Such a motion may cause problems if there are more obstacles in the neighborhood of the manipulator. Namely, the manipulator may bounce between the obstacles. On the other hand, too small values of *fo* would not move the critical point

αf

Fig. 11. The obstacle-avoidance gain *α<sup>f</sup>* versus the distance to the obstacle

F*<sup>o</sup>* = *α<sup>f</sup> fo*n*<sup>o</sup>* (60)

the manipulator.

obstacle, as shown in Fig. 11.

propose using the following function

sufficiently away from the obstacle.

*α<sup>f</sup>* =

⎧ ⎨ ⎩

� *dm* �*do*� �2

which is the same as Eq. (41), and premultiplying (54) with **N**¯ yields after simplifications

$$\mathbf{N}\ddot{e}\_{\boldsymbol{\eta}} + \mathbf{N}\mathbf{K}\_{\boldsymbol{\eta}}\dot{e}\_{\boldsymbol{\eta}} = -\mathbf{H}\_{\boldsymbol{\eta}}^{\ddagger}(\tau\_{\boldsymbol{\omicup}} + \tau\_{\boldsymbol{\rm F}}) \tag{56}$$

From Eqs. (55) and (56) we can see that the task space and the null-space motion are influenced by the contact forces. However, these forces decrease after the impact because of the avoiding motion.

Next, we present the simulation results of the same task as before for the system where the tactile sensors (TF) were used to detect the obstacle. The close loop controller was the same as for the CF case (ii) approach, which assures stiff null space behavior. The avoidance motion was generated using Eqs. (49) – (50) with the parameters *fo* = 100*N*, *Ti* = 6*s* and *Td* = 8*s*. The results, see Fig. 10, show that with tactile sensors we can decrease the contact forces after the contact, but the impact force cannot be decreased. Unfortunately, the magnitude of the impact forces is not controllable and hence this approach also requires low velocities and a soft contact.

#### **5.3 Obstacle avoidance with proximity sensors**

When proximity sensors are used to detect obstacles, the collisions between the manipulator and obstacles can be avoided. Also here, our approach is to identify the points on the manipulator that are near obstacles and to assign to them force components that move the points away from the obstacle (see Fig. 8). The strategy is similar to those given in (Brock et al., 2002), although it was developed independently.

Suppose that F*<sup>o</sup>* is acting at point *Ao* somewhere on the link *i* (see Fig. 8). Applying τ*<sup>o</sup>* (see Eq. 44) to the system yields the equation of motion in the form

$$
\boldsymbol{\tau} = \mathbf{H}\ddot{\boldsymbol{q}} + \boldsymbol{h} + \mathbf{g} - \boldsymbol{\tau}\_0 \tag{57}
$$

which is the same as Eq. (3), except that the real external forces τ*<sup>F</sup>* are replaced by the virtual forces τ*o*. As we are using virtual forces, only torques that do not influence the end-effector motion are considered

$$
\boldsymbol{\tau}\_{oN} = \mathbf{N}^T \mathbf{J}\_o^T \mathbf{F}\_o = \mathbf{N}^T \boldsymbol{\tau}\_o \tag{58}
$$

Substituting τ*oN* for τ*<sup>o</sup>* in Eq. (57) yields

$$
\tau = \mathbf{H}\ddot{q} + h + g - \bar{\mathbf{N}}^T \tau\_0 \tag{59}
$$

18 Will-be-set-by-IN-TECH

50


100

<sup>0</sup> <sup>2</sup> <sup>4</sup> <sup>6</sup> <sup>0</sup>

t

*<sup>n</sup>*(τ*<sup>o</sup>* + τ*F*) (56)

<sup>e</sup>¨ <sup>+</sup> **<sup>K</sup>***v*e˙ <sup>+</sup> **<sup>K</sup>***p*<sup>e</sup> <sup>=</sup> <sup>−</sup>**JH**−1τ*<sup>F</sup>* (55)

τ = **H**q¨ + h + g − τ*<sup>o</sup>* (57)

<sup>τ</sup> <sup>=</sup> **<sup>H</sup>**q¨ <sup>+</sup> <sup>h</sup> <sup>+</sup> <sup>g</sup> <sup>−</sup> **<sup>N</sup>**¯ *<sup>T</sup>*τ*<sup>o</sup>* (59)

*<sup>o</sup>* <sup>F</sup>*<sup>o</sup>* = **<sup>N</sup>***T*τ*<sup>o</sup>* (58)

P2

Fig. 10. Planar 4DOF manipulator tracking a line while avoiding obstacles using tactile

which is the same as Eq. (41), and premultiplying (54) with **N**¯ yields after simplifications

From Eqs. (55) and (56) we can see that the task space and the null-space motion are influenced by the contact forces. However, these forces decrease after the impact because of the avoiding

Next, we present the simulation results of the same task as before for the system where the tactile sensors (TF) were used to detect the obstacle. The close loop controller was the same as for the CF case (ii) approach, which assures stiff null space behavior. The avoidance motion was generated using Eqs. (49) – (50) with the parameters *fo* = 100*N*, *Ti* = 6*s* and *Td* = 8*s*. The results, see Fig. 10, show that with tactile sensors we can decrease the contact forces after the contact, but the impact force cannot be decreased. Unfortunately, the magnitude of the impact forces is not controllable and hence this approach also requires low velocities and a

When proximity sensors are used to detect obstacles, the collisions between the manipulator and obstacles can be avoided. Also here, our approach is to identify the points on the manipulator that are near obstacles and to assign to them force components that move the points away from the obstacle (see Fig. 8). The strategy is similar to those given in (Brock

Suppose that F*<sup>o</sup>* is acting at point *Ao* somewhere on the link *i* (see Fig. 8). Applying τ*<sup>o</sup>* (see

which is the same as Eq. (3), except that the real external forces τ*<sup>F</sup>* are replaced by the virtual forces τ*o*. As we are using virtual forces, only torques that do not influence the end-effector

τ*oN* = **N***T***J***<sup>T</sup>*

**<sup>N</sup>**¯ <sup>e</sup>¨*<sup>n</sup>* <sup>+</sup> **NK**¯ *<sup>n</sup>*e˙*<sup>n</sup>* <sup>=</sup> <sup>−</sup>**H**‡

P1

sensors and virtual forces (TF controller)

**5.3 Obstacle avoidance with proximity sensors**

et al., 2002), although it was developed independently.

Eq. 44) to the system yields the equation of motion in the form

Premultiplying (54) with **J** yields

motion.

soft contact.

motion are considered

Substituting τ*oN* for τ*<sup>o</sup>* in Eq. (57) yields

The main difference compared to the system with tactile sensors is how the virtual forces F*<sup>o</sup>* are generated. Now, the virtual force F*<sup>o</sup>* depends on the location of the critical point and the closest point on the obstacle. Clearly, the location of all the objects in the workspace of the manipulator has to be known. This information can be provided by a higher control level that has access to sensory data. As sensor systems are not our concern, we will assume that the sensor can detect obstacles in the workspace of the manipulator and that it outputs the direction and the distance to the closest point obstacle, and the position of the critical point on the manipulator.

Let *Ao* be the critical point and d the vector connecting the closest point on the obstacle and *Ao* (see Fig. 8). To avoid a possible collision a virtual force F*<sup>o</sup>* is assigned to *Ao*, defined as

$$F\_o = \mathfrak{a}\_f f\_o \mathfrak{n}\_o \tag{60}$$

where *fo* is a scalar gain representing the nominal force, n*<sup>o</sup>* is the unit vector in the direction of d, and *α<sup>f</sup>* is the obstacle-avoidance gain. The gain *α<sup>f</sup>* should depend on the distance to the obstacle, as shown in Fig. 11.

There are two distances characterizing the changes in the value of the gain: the critical distance *dm* and the abort distance *db*. If the distance between the manipulator and the object is greater than *dm* then the motion of the manipulator is not perturbed. When the distance is decreasing, the force should increase smoothly (to assure smooth transitions it is important that the magnitude of F*<sup>o</sup>* at *dm* is zero). However, if the manipulator is too close to the obstacle (less than *db*) the main task should be, for safety reasons, aborted so that the manipulator can avoid the obstacles (such a situation can occur, especially if moving obstacles are present in the workspace). The distance *db* is subjected to the dynamic properties of the manipulator and can also be a function of the relative velocity between the critical point on the manipulator and the obstacle. The gain *α<sup>f</sup>* can be chosen arbitrarily as long as it has the prescribed form. We propose using the following function

$$\mathfrak{a}\_f = \begin{cases} \left(\frac{d\_w}{\|\mathbf{d}\_o\|}\right)^2 - 1 & \text{for} \quad ||\mathbf{d}\_o|| < d\_m\\ 0 & \text{for} \quad ||\mathbf{d}\_o|| \ge d\_m \end{cases} \tag{61}$$

Special attention has to be given to the selection of the nominal force *fo*. Large values of *fo* can cause unnecessarily high accelerations and velocities. Consequently, the manipulator could move far from the obstacle. Such a motion may cause problems if there are more obstacles in the neighborhood of the manipulator. Namely, the manipulator may bounce between the obstacles. On the other hand, too small values of *fo* would not move the critical point sufficiently away from the obstacle.

When there are more simultaneously active obstacles in the neighborhood of the manipulator, the sum of the torques due to the relevant virtual forces is used,

$$\tau\_0 = \sum\_{i=1}^{a\_o} \mathbf{J}\_{o,i}^T \mathbf{F}\_{o,i} \tag{62}$$

Manipulators as Control Problem 21

Obstacle Avoidance for Redundant Manipulators as Control Problem 223

From Eq. (69) we can see that the obstacle-avoidance motion is not controlled directly by F*o*. The force F*<sup>o</sup>* initiates the motion, but the resulting motion depends mainly on the null-space controller (9). Although the null-space controller has in this case the same structure as in the case of the contact forces approach, the gains **K***<sup>n</sup>* can be selected to meet the requirements of the subtask the manipulator should perform, i.e., in most cases to track the desired null space velocity. Hence, the gain matrix **K***<sup>n</sup>* should be high. Consequently, to perform satisfactory obstacle avoidance the nominal force *fo* must be higher to predominate over the velocity

In the following the simulation results for the simple task are shown. The task space controller parameters are the same as in the previous example. To avoid the obstacles the proximity sensor distance was selected as *dm* = 0.2*m*. The virtual forces were calculated using Eq. (40) with the parameter *fo* = 800*N*. The simulation results, see Fig. 12, clearly show that the

P1

P2

Soft bumper

Froce sensor

obstacle is avoided without a deviation of the end-effector from the assigned task.

Fig. 12. Planar 4DOF manipulator tracking a line while avoiding obstacles using a virtual

The proposed algorithms were tested on the laboratory manipulator (see Fig. 13 and two KUKA LWR robots (see Fig. 14). The laboratory manipulator was specially developed for testing the different control algorithms. To be able to test the algorithms for the redundant systems the manipulator has four revolute DOF acting in a plane. The link lengths of the manipulator are *l* = (0.184, 0.184, 0.184, 0.203)*m* and the link masses are *m* = 0.83, 0.44, 0.18, 0.045)*kg*. The manipulator is a part of the integrated environment for the design of the control algorithms and the testing of these algorithms on a real system (Žlajpah,

Fig. 13. Experimental manipulator with external force sensor

controller when necessary.

forces approach (VF)

2001).

**6. Experimental results**

and considering this in Eq. (58) yields

$$\boldsymbol{\tau}\_{o,N} = \bar{\mathbf{N}}^T \sum\_{i=1}^{a\_o} \mathbf{J}\_{o,i}^T \mathbf{F}\_{o,i} \tag{63}$$

where *ao* is the number of active obstacles.

To decouple the task space and the null-space motion we propose as before the controller (37). Combining (48) and (37), and considering Eqs. (8) and (9) yields

$$\mathbf{H}\ddot{\mathbf{q}} + h + g - \ddot{\mathbf{N}}^T \boldsymbol{\tau}\_0 = \mathbf{H}(\ddot{\mathbf{J}}(\ddot{x}\_d + \mathbf{K}\_\nu \dot{e} + \mathbf{K}\_\rho e - \dot{\mathbf{J}}\dot{q}) + \ddot{\mathbf{N}}(\ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_\mu \dot{e}\_\mu - \dot{\mathbf{N}}\dot{q})) + h + g \tag{64}$$

As before, the term **N**¯ *<sup>T</sup>*τ*<sup>o</sup>* is also part of the controller and should be on the right-hand side of Eq. (64), but we put it on the left-hand side to emphasize its role.

One of the reasons for using the above control law is that the null-space velocity controller (9) can be used for other lower-priority tasks. Hence, to optimize *p* we can select ˙ϕ in (9) as

$$
\dot{\varphi} = \mathbf{H}^{-1} k\_p \nabla p \tag{65}
$$

Note that when the weighted generalized inverse of **J** is used, the desired null space velocity has to be multiplied by the inverse of the weighting matrix (in our case **H**−1) to assure the convergence of the optimization of *p* (Nemec, 1997).

Next we analyse the behavior of the close-loop system. Premultiplying Eq. (64) by **H**−<sup>1</sup> yields

$$\ddot{q} - \mathbf{H}^{-1} \ddot{\mathbf{N}}^T \boldsymbol{\tau}\_0 = \ddot{\mathbf{J}} (\ddot{x}\_d + \mathbf{K}\_\upsilon \dot{e} + \mathbf{K}\_p e - \dot{\mathbf{J}} \dot{q}) + \ddot{\mathbf{N}} (\ddot{\varphi} + \dot{\mathbf{N}} \dot{\varphi} + \mathbf{K}\_\iota \dot{e}\_\imath - \dot{\mathbf{N}} \dot{q})$$

and by using (1) the above equation can be rewritten in the form

$$\bar{\mathbf{J}}(\ddot{e} + \mathbf{K}\_{\nu}\dot{e} + \mathbf{K}\_{\mathcal{P}}e) + \bar{\mathbf{N}}(-\ddot{q} + \ddot{\varphi} + \dot{\bar{\mathbf{N}}}\dot{\varphi} + \mathbf{K}\_{\mathbb{R}}\dot{e}\_{\mathbb{R}} - \dot{\bar{\mathbf{N}}}\dot{q}) = -\mathbf{H}^{-1}\bar{\mathbf{N}}^{T}\boldsymbol{\tau}\_{0} \tag{66}$$

The dynamics of the system in the task space and in the null-space can be obtained by premultiplying Eq. (66) by **J** and **N**¯ , respectively. Premultiplying (66) with **J** yields

$$
\ddot{e} + \mathbf{K}\_{\upsilon}\dot{e} + \mathbf{K}\_{p}e = 0\tag{67}
$$

since **J**¯ **J** = **I**, **NH**¯ <sup>−</sup><sup>1</sup> = **H**−1**N**¯ *<sup>T</sup>*, and **JN**¯ = **0**. The proper choice of **K***<sup>v</sup>* and **K***<sup>p</sup>* assures the asymptotical stability of the system. Furthermore, it can be seen that the virtual forces do not affect the motion in the task space. Next, premultiplying (66) by **N**¯ yields

$$\ddot{\mathbf{N}}(-\ddot{\mathbf{q}} + \ddot{\varphi} + \dot{\mathbf{N}}\dot{\varphi} + \mathbf{K}\_n \dot{e}\_n - \dot{\mathbf{N}}\dot{q}) = -\ddot{\mathbf{N}}\mathbf{H}^{-1}\ddot{\mathbf{N}}^T\boldsymbol{\tau}\_0\tag{68}$$

because **N**¯ ¯ **<sup>J</sup>** <sup>=</sup> **<sup>0</sup>**. Rearranging Eq. (68) and using the relation ¨e*<sup>n</sup>* <sup>=</sup> **<sup>N</sup>**¯ (ϕ¨ <sup>−</sup> <sup>q</sup>¨) + **<sup>N</sup>**¯˙ (ϕ˙ <sup>−</sup> <sup>q</sup>˙) we obtain

$$\bar{\mathbf{N}}(\ddot{\varphi} - \ddot{q} + \dot{\mathbf{N}}(\dot{\varphi} - \dot{q}) + \mathbf{K}\_{\text{ll}}\dot{e}\_{\text{ll}}) = -\bar{\mathbf{N}}\mathbf{H}^{-1}\bar{\mathbf{N}}^{T}\boldsymbol{\tau}\_{0} = -\mathbf{H}\_{\text{ll}}^{\ddagger}\boldsymbol{\tau}\_{0} \tag{69}$$

From Eq. (69) we can see that the obstacle-avoidance motion is not controlled directly by F*o*. The force F*<sup>o</sup>* initiates the motion, but the resulting motion depends mainly on the null-space controller (9). Although the null-space controller has in this case the same structure as in the case of the contact forces approach, the gains **K***<sup>n</sup>* can be selected to meet the requirements of the subtask the manipulator should perform, i.e., in most cases to track the desired null space velocity. Hence, the gain matrix **K***<sup>n</sup>* should be high. Consequently, to perform satisfactory obstacle avoidance the nominal force *fo* must be higher to predominate over the velocity controller when necessary.

In the following the simulation results for the simple task are shown. The task space controller parameters are the same as in the previous example. To avoid the obstacles the proximity sensor distance was selected as *dm* = 0.2*m*. The virtual forces were calculated using Eq. (40) with the parameter *fo* = 800*N*. The simulation results, see Fig. 12, clearly show that the obstacle is avoided without a deviation of the end-effector from the assigned task.

Fig. 12. Planar 4DOF manipulator tracking a line while avoiding obstacles using a virtual forces approach (VF)
