Kinematic Control Algorithms for On-Line Obstacle Avoidance for Redundant Manipulators

Transcription

1 Kinematic Control Algorithms for On-Line Obstacle Avoidance for Redundant Manipulators Leon Žlajpah and Bojan Nemec Institute Jožef Stefan, Ljubljana, Slovenia, Abstract The paper deals with kinematic control algorithms for on-line obstacle avoidance which allow a kinematically redundant manipulator to move in an unstructured environment without colliding with obstacles. The presented approach is based on the redundancy resolution at the velocity level. The primary task is determined by the end-effector trajectories and for the obstacle avoidance the internal motion of the manipulator is used. The obstacle avoiding motion is defined in onedimensional operational space and hence, the system has less singularities what makes the implementation easier. Instead of the exact pseudoinverse solution we propose an approximate one which is computationally more efficient and allows also to consider many simultaneously active obstacles without any problems. The fast cycle times of the numerical implementation enable to use the algorithm in real-time control. For illustration some simulation results of highly redundant planar manipulator moving in an unstructured and time-varying environment and experimental results of a four link planar manipulator are given. Introduction One of the goals of the robotic research is to provide control algorithms which allow robots to move in an environment with obstacles. The natural strategy to avoid obstacles would be to move the manipulator away from the obstacle into the configuration where the manipulator is not in the contact with the obstacle. 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). 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. For obstacle avoidance several local strategies have been developed [9, 7, 2, 4,, 6, 5, 4]. The local strategies treat the obstacle avoidance as a control problem. Their aim is not to replace higher lever global collision free path planing but to make the use of the capabilities of the low level control, e.g. they can use sensor information to change the path if the obstacle has moved or its position in not known in advance. Like most of the local strategies that solve the obstacle avoidance problem at the kinematic level [9, 4, 8, 5, 6, ], the approach presented in this paper 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 in the paper is given to the definition of the avoiding motion. Usually, the avoiding motion is defined in the Cartesian space. As the obstacle avoidance is typically one-dimensional problem, we use 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 which is faster that the exact one. Another important issue addressed in the paper is how the obstacle avoidance is performed when there are more simultaneously active obstacles in the neighbourhood of the manipulator. We propose an algorithm which considers all obstacles in the neighbourhood of the robot. 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 four link planar manipulator. In experiments simple vision system has been used to detect obstacles. 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 for a certain task. 2. Kinematics Let the configuration of the manipulator be represented by the n-dimensional vector q of joint positions, and

2 the end-effector position (and orientation) by the m- dimensional vector x of task positions (and orientations). Then, the kinematics can be described by the following equations x = f (q) () q = J # ẋx + N q (2) q = J # (ẍx J q) + N q (3) where f is a m-dimensional vector function representing the manipulator forward kinematics, J is the m n manipulator Jacobian matrix, J # is the generalized inverse of the Jacobian matrix J and N is a matrix representing the projection into the null space of J, N = (I J # J). 2.2 Kinematic control Most tasks performed by a redundant manipulator are broken down into several subtasks with different priority. In our case 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). For velocity control the following kinematic controller can be used q = J # ẋx c + N ϕ (4) where ẋx c and ϕ represent the task space control law and arbitrary joint velocities, respectively. The task space control ẋx c can be selected as ẋx c = ẋx e + K p e (5) where e, e = x d x, is the tracking error, ẋx e is the desired task space velocity, and K p 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 ϕ = k p p (6) Here p is the gradient of p and k p is a gain. 3 Obstacle avoidance strategy The obstacle avoidance strategy is to identify the points on the robotic arm which are near obstacles and then, to assign to them a motion component that moves the points away from the obstacle (see Fig. ). The motion of the robot is perturbed only if at least one part of the robot is in the critical neighbourhood of an obstacle, i.e. the distance is less than a prescribed minimal distance. We denote this obstacles active obstacles and the corresponding closest points on the body of a manipulator as critical points. Furthermore, 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 Critical points Jo Ao. Xo Obstacle do Obstacle Desired motion Task path Figure : Manipulator motion in presence of some obstacles level path planing has to recalculate the desired motion of the end-effector. As the obstacle avoidance is supposed to be done on-line it is not necessary to know the exact position of 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. The proposed velocity strategy considers the obstacle avoidance problem at the kinematic level. Let ẋx e be the desired velocity of the end-effector, and A o be the critical point in the neighbourhood of an obstacle (see Fig. ). To avoid a possible collision one possibility is to assign to A o such a velocity that it moves away from the obstacle as it is proposed in [9]. Hence, the motion of the end-effector and the critical point can be described by equations J q = ẋx e (7) J o q = ẋx o (8) where J o is a Jacobian matrix associated with the point point A o. There are some possibilities to find a common solution for both equations. 3. Exact solution Substituting ẋx e for ẋx c and combining (4) and (8) yields [9, ] ϕ = (J o N) # (ẋx o J o J # ẋx e ) (9) Now, using this in (4) gives the final solution for q in the form q = J # ẋx c + (J o N) # (ẋx o J o J # ẋx e ) () because N is both hermitian and idempotent. The meaning of the terms in the above equation can be easily explained. The first term J # ẋx c guarantees the joint motion necessary for the desired end-effector velocity (ẋx c is used here instead of ẋx e to compensate for any task space tracking errors). The rest, i.e. the homogeneous solution q h, represents the motion of the point A o. The term J o J # ẋx e is. xe

3 the velocity in A o due to the end-effector motion, and 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 to achieve exactly the desired ẋx o only if the degree of redundancy of the manipulator is sufficient. The matrix J o N combines the kinematics of the critical point A o and the null-space matrix of the whole manipulator and hence, its properties define the flexibility of the system to avoid 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 A o 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 o 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-ofredundancy is not capable of obstacle avoidance, and of course, this is not true. For example, consider a planar 3 DOF manipulator which 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 degree-of-redundancy. Defining the velocity ẋx o in the same space as the end-effector velocity, i.e. as a 2- dimensional vector, yields the matrix J o N to have dimension 2 3. Furthermore, due to degree-of-redundancy the components of the velocity vector ẋx o are not independent. Hence, the rank of J o N is, and the pseudoinverse (J o N) # does not exist. As the obstacle avoidance strategy requires only the 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-ofredundancy is needed to avoid the obstacle, generally. Therefore, we propose to define the Jacobian J o as follows. Let d o be the vector connecting the closest points on the obstacle and manipulator (see Fig. ) and let the operational space in A o 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 J do = n T o J o () where J o is the Jacobian defined in the Cartesian space and n o is the unit vector in the direction of d o, n o = d o d o. Now, the dimension of the matrix J do is n, and velocities ẋx o and J do J # ẋx e become scalars. Consequently, the computation of (J do N) # is faster, e.g. when calculating the Moore-Penrose pseudoinverse of (J do N) which is defined as (J do N) # = (J do N) T ( J do N (J do N) T ) = NJ T do (J do NJ T d o ) (2) Figure 2: Planar 3DOF manipulator: tracking of a line and obstacle avoidance using the Jacobian J do we do not have to invert any matrix because the term (J do NJ T d o ) 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. An important issue in control of redundant manipulators are 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 to consider also the singularities of two manipulator substructures defined by the critical point A o : (a) the part of the manipulator between the base and the point A o and (b) the part between A o 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 do N. Namely, when part (a) is in singular configuration then J o is not of full rank and when part (b) is in singular configuration then J o retains the rank but J do N becomes singular. Hence, when approaching either singular configuration the values of q h become unacceptably large. As the manipulator is supposed to move in an unstructured environment it is practically impossible to know when J do N will become singular. Therefore, a very important advantage of the proposed Jacobian J do compared to J o is that the system has significantly less singularities when J do is used. The efficiency of the obstacle avoidance algorithm depends also on the selection of the desired critical point velocity ẋ o. We propose to change ẋ o in respect of the obstacle distance ẋ o = α v v o (3) where v o is the nominal velocity and α v is the obstacle avoidance gain defined as ( 2 dm α v = d o ) do < d m (4) d o d m where d m is the critical distance to the obstacle (see Fig. 3). If the obstacle is to close ( d o d b ) the main task should be aborted. The distance d b is subjected to dynamic properties of the manipulator and can also be a function of relative velocity ḋd o. To assure smooth transitions it is important that the magnitude of ẋx o at d m is zero.

4 PSfrag replacements J o J # ẋx e ẋx e α v α h d b d m d i PSfrag replacements ẋx o J o q AP Figure 3: The obstacle avoidance gain α v and the homogenous term gain α h versus distance to obstacle A special attention has to be given to the selection of the nominal velocity v o. Large values of v o would cause unnecessary high velocities and consequently the manipulator would move far from the obstacle. Such motion may cause problems, if there are more obstacles in the neighbourhood of the manipulator. Namely, the manipulator may bounce between the obstacles. On the other hand, to small value of v o would not move the critical point of the manipulator away from the obstacle. For smoother motion, it is proposed in [9] to change the amount of the homogenous solution to be included in the total solution q EX = J # ẋx c + α h (J do N) # (ẋx o J do J # ẋx e ) (5) We have selected α h as d o d m α h = 2 ( cos(π d o d m d sd d m )) d m < d o < d i d i d o (6) where di is the distance where the obstacle influences the motion. From Fig. 3 it can be seen that in the region between d b and d m the complete homogenous solution is included in the motion specification and the avoidance velocity is inversely related to the distance. Between d m and d i there avoidance velocity is zero and only a part of the homogenous solution is included. As the homogenous solution compensates the motion in the critical point due to the end-effector motion, the relative velocity between the obstacle and the critical point decreases when approaching from d i to d m, if the obstacle is not moving, of course. With such selection of α v and α h smooth velocities can be obtained. The control law (5) can be used for a single obstacle. When more than one obstacle is active at the time then the worst-case obstacle (nearest) has to be used which results in discontinues velocities and may cause oscillations in some cases. Namely, when switching between active obstacles the particular homogenous solutions are not equal and discontinuity in joint velocities occur. To improve the behaviour we propose to use a weighted sum of homogenous solution of all active obstacles q EX = J # ẋx c + n o i= w i α h,i q h,i (7) J o q EX Figure 4: Comparison of avoiding velocities in critical point A o for different approaches where n o is the number of active obstacles, and w i, α h,i and q h,i are weighting factor, gain and homogenous solution for the i-th active obstacle, respectively. Weighting factors w i are calculated as d i d o,i w i = n o i= (d i d o,i ) (8) Although the actual velocities in critical points differ from the desired ones, using q EX improves the behaviour of the system and when one point is much closer to the obstacle then other, then its weight approaches to and the velocity in that particular point is close to the desired one. 3.2 Approximate solution Another possible solution for ϕ is to calculate joint velocities which satisfy the secondary goal as ϕ = J # d o ẋ o (9) without compensating the contribution of the end-effector motion and then substitute ϕ into (4) which yields q AP = J # ẋx c + NJ # d o ẋ o (2) This approach avoids the singularity problem of (J do N) [3]. The formulation (2) does not guarantee to achieve exactly the desired ẋ o even if the degree of redundancy is sufficient because J do NJ # d o ẋ o is not equal ẋ o, in general. For illustration, in Fig. 4 resulting velocities for different strategies are shown (Note that the parameters are equal for all of them). To avoid the obstacle the goal velocity in A o is represented by the vector ẋx o. Using the original method () the velocity in A o is exactly ẋx o. Joint velocities q EX assure that the component of the velocity in point A o (i.e. J o q EX ) in direction of ẋx o is as required. The approximate solution q AP gives in most cases smaller magnitude of the velocity in the direction of ẋx o (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 depends also on the nominal velocity v o which can be increased to achieve higher minimal distances.

5 Additionally, the approximate solution possesses certain advantages when many active obstacles have to be considered. The joint velocities can be calculates as q AP = J # ẋx c + N n o i= J # d o,iẋo,i (2) where n o is the number of active obstacles, and therefore, the matrix N has to be calculated only once. Of course, pseudoinverses J # o,i have to be calculated for each active obstacle. 4 Simulation examples PSfrag replacements The following simulation example illustrates the behaviour of a n-r planar manipulator when the manipulator is moving in an unstructured environment with obstacles, i.e. the positions of obstacles are not known. The simulation has been done in MATLAB/SIMULINK using the Planar Manipulators Toolbox [2]. In the example the manipulator is supposed to have revolute joints. The primary task is to move into a small labyrinth. The end-effector path is in the middle of corridor. To analyse the behaviour in narrow free space, the distance between walls is less then the manipulator link length. We have compared two kinematic controllers based on the velocity strategy. The first controller is the exact velocity controller adapted for multiple obstacles (EX) based on Eq. (7) and the second one is the approximate velocity controller (AP) based on Eq. (2). Both controllers have the same task space controller (5) PSfrag withreplacements gain K p = 5Is. To avoid the obstacles the proximity sensor distances have been selected as d m =.5m, di =.m and db =.5m. The avoiding velocities have been calculated using Eq. (3) with v o = 2 for EX approach and v o = for AP approach. Figs. 5, 6 and 7 show the simulation results. Good results are obtained with the EX approach where all critical points are considered with smoothly changing weights (see. Fig. 5). The AP approach is a simplification of the EX approach. Although the direction of the avoiding velocity is not optimal due to the null-space mapping, the obtained results are satisfactory (see Fig. 6). The minimal distance to the obstacle is approximately the same in both cases due to the larger value of nominal velocity v o in AP approach. minlink do q minlink do q Figure 5: Obstacle avoidance using EX approach Figure 6: Obstacle avoidance using AP approach (a)ex approach (b)ap approach 5 Experimental results The proposed approximate velocity controller has been implemented on the laboratory manipulator which has been developed specially for testing the different control algorithms. To be able to test the algorithms for redundant systems the manipulator has four revolute DOF acting in a plane. The link lengths of the manipulator are l = (.84,.84,.84,.23)m. The manipulator is part of the integrated environment for the design Figure 7: Velocities at critical points for EX and AP approach (t=8.s) of the control algorithms and the testing of these algorithms on a real system [3]. The desired task in these experiments has been the tracking the path x d = [.4.2sin(2πt/8),. +.sin(2πt/4)] T and the motion of the manipulator has been obstructed by three obstacles (see Fig. 8). In the current implementation the vision

6 (a) desired path and configurations PSfrag replacements minlink do q (b) minimal distance to obstacles for each link makes easier to consider more obstacles simultaneously. The efficiency of the proposed algorithms is illustrated by simulations of highly redundant planar manipulator moving in an unstructured and time-varying environment. We have implemented the algorithms on a four link planar manipulator. For object detection a simple vision system has been used. The test have proven that the fast cycle times of the numerical implementation make the proposed algorithms suitable for the real-time control. (c) video picture sampled every.5s Figure 8: 4R planar manipulator tracking a path in an unstructured environment with three obstacles system is using a simple USB WebCam which can recognize the scene and output the position of all obstacles in less that.4s. To avoid the obstacles the proximity sensor distance have been selected as d m =.8m. The rate of the velocity controller has been 2Hz (the necessary joint velocities for the avoiding motion are calculated in less than.5ms). The experimental results are given in Fig Conclusion The presented approach is based on the redundancy resolution at the velocity level. The primary task is determined by the end-effector trajectories and for the obstacle avoidance the internal motion of the manipulator is used. The goal is to assign each point on the body of the manipulator, which is close to the obstacle, a velocity component in a direction that is away from the obstacle. We have shown that it reasonable to define the avoiding velocity in one-dimensional operational space. In this way some singularity problems can be avoided when not enough redundancy is available locally. Additionally, the calculation of pseudoinverse of the Jacobian matrix J o is simpler as it includes scalar division instead of matrix inversion. Using approximate calculation of the avoiding velocities has its advantages computationally and it References [] Y. Chen and M. Vidyasagar. Optimal control of robotic manipulators in the presence of obstacles. J. of Robotic Systems, 7(5):72 74, 99. [2] E. Cheung and V. Lumelsky. Motion planning for robot arm manipulators with proximity sensing. In Proc. Intl. Conf. On Robotics and Automation, pages , Philadelphia, Pennsylvania, 988. [3] S. Chiaverini. Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. IEEE Trans. on Robotics and Automation, 3(3):398 4, 997. [4] R. Colbaugh, H. Seraji, and K.L. Glass. Obstacle avoidance for redundant robots using configuration control. J. of Robotic Systems, 6(6):72 744, 989. [5] K.L. Glass, R. Colbaugh, D. Lim, and H. Seraji. Realtime collision avoidance for redundant manipulators. IEEE Trans. on Robotics and Automation, (3): , 995. [6] Z.Y. Guo and T.C. Hsia. Joint trajectory generation for redundant robots in an environment with obstacles. J. of Robotic Systems, (2):9 25, 993. [7] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. Int. J. of Robotic Research, 5:9 98, 986. [8] J.O. Kim and P. Khosla. Real-time obstacle avoidance using harmonic potential functions. IEEE Trans. on Robotics and Automation, 8(3): , 992. [9] A.A. Maciejewski and C.A. Klein. Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments. Int. J. of Robotic Research, 4(3):9 7, 985. [] Y. Nakamura, H. Hanafusa, and T. Yoshikawa. Taskpriority based redundancy control of robot manipulators. Int. J. of Robotic Research, 6(2):3 5, 987. [] H. Seraji and B. Bon. Real-time collision avoidance for position-controlled manipulators. IEEE Trans. on Robotics and Automation, 5(4):67 677, 999. [2] L. Žlajpah. Simulation of n-r planar manipulators. Simulation Practice and Theory, 6(3):35 32, 998. [3] L. Žlajpah. Integrated environment for modelling, simulation and control design for robotic manipulators. Journal of Intelligent and Robotic Systems, 32(2):29 234, 2. [4] H.P. Xie, R.V. Patel, S. Kalaycioglu, and H. Asmer. Realtime collision avoidance for a redundant manipulator in an unstructured environment. In Proc. Intl. Conf. On Intelligent Robots and Systems IROS 98, pages , Victoria, Canada, 998.