Motion Planning Algorithms of Redundant Manipulators Based on Self-motion Manifolds

YAO Yufeng, ZHAO Jianwen, and HUANG Bo

State Key Laboratory of Robotics Technique and System, Harbin Institute of Technology, Harbin 150081, China

1 Introduction

The purpose of the redundant manipulator is assumed to optimize manipulator motion performance with a redundant degree-of-freedom (DOF), which is normally referred to as“motion planning”. The manipulator motion planning encompasses the planning of relative position, velocity and acceleration. Planning of velocity and acceleration should favorably allow for the probability of end-effector satisfying the prescribed pose in real time, as a result, the motion planning on the displacement level turns out to be essential for that of redundant manipulator. Unfortunately,the optimization algorithm for inverse kinematics on displacement level is rarely considered. However, in the inverse arithmetic of gradient projection on velocity level,the gradient of performance function is creatively integrated into a velocity inverse solution that significantly facilitates the search work for inverse solution optimization,which was reasonably adapted extensively. Its expression could be described as follows:

Although the calculating process of gradient projection is simple, and its geometric significance is explicit, it has following drawbacks:

(1) The heavy computation for pseudo-inverse+J makes the real-time computation rather difficult.

(2) The error in solving inverse allows for only an imprecise optimization solution.

(3) The tracing of end-effector displacement could be realized only by integral velocity, thus will inevitably result in error accumulation.

(4) A significant difference between a special solution and an homogeneous solution may occur if the magnification coefficient k of real-value scalar goes to the two extremes, and in the context of velocity resolution method, the element of magnitude order being too small will be disregarded, which will eventually lead to the failure of the gradient projection method(GPM).

To this end, scholars have offered several proposals for selection of k[1], among which the real-time correction scheme for k proposed by WU, et al[2], proves to achieve better result, k is defined as follows:

where ψ is adjustment coefficient, and its value may range within (–0.2, –1.0). For computation within this scope, the inverse solution for redundant manipulator remains stable in normal cases; however, the optimization result will be discounted significantly at the same time.

In recent years, scholars have been dedicated to perfecting the gradient projection all the way, the example proposed in Ref. [3]—the secondary calculation method—is a case in point, in which the GPM is used to secure the imprecise solution first, followed by the secondary calculation in compliance with the feature of system configuration, until finally producing a set of precise optimization solutions. However, this method is problematically incomplete since great fluctuation on joint velocity cannot be avoided even if it is possible to overcome the first three drawbacks of the gradient projection method. Ref. [4] presents a modified GPM, in which the pose change of end-effector generates reference velocity in iterative process, thus guarantees that the end-effector will converge to the desired position and pose based on the precision prescribed by subscribers.

The GPM, in nature, is no more than an optimization method on velocity level, which could be applied to motion planning of velocity level; however, it is not the best for tasks of precise trajectory tracking, obstacle avoidance etc.Many scholars attempted a number of numerical methods to keep the end-effector in high pose precision[5–6]while ensuring that the manipulator performs the additional task in the meantime. Refs. [4–6] apparently discard the drawback of end pose error accumulation existed in the conventional gradient projection though, the gap between tracing precision and computation time could hardly be bridged. Still the methods described in Refs. [4–6] are far from satisfying when high displacement tracking precision is required for the manipulators.

In view of the drawbacks displayed in the mentioned methods, scholars begin to attempt new methods for motion planning from the perspective of self-motion—the peculiar motion to redundant manipulators. In pursuit of algorithm universality, the ordinary spatial redundant robot finds it hard to secure an analytical solution for the self-motion manifold, for this reason, the scholars who engage in inverse solution optimization searching from the perspective of self-motion more focus on controlling method[7–8]or retaining the algorithm on velocity level[9].Subsequently the performance of an additional task on the position level remains weak.

For the redundant manipulators used in engineering projects, the geometric relation among joint axis is simple,this implies that the analytical solution of self-motion manifolds are easily reachable, so this supports the feasibility of performing inverse solution optimization searching based on self-motion manifold. In this paper, the inverse solution optimization searching on the displacement level from the perspective of self-motion manifold will be directly accomplished, with the intention of developing a more efficient motion planning method for additional task at displacement level.

2 Derivation of motion planning algorithm

For those manipulators with simple geometric relations of joint axes, the self-motion manifold could be expressed by the displacement variables of a certain joints. In this paper, these joints were defined as redundant ones, and the column vector which is composed of each individual redundant joint displacement was defined as self-motion variable θs. If r is redundant DOF, then θsis r-dimensional column vector. Thus its forward kinematics could be represented via the self-motion variable θsas

wheremT is the pose matrix of end-effector, θ is the joint angle vector of the manipulator.

Without lost of generality, the movement of end-effector frommT tounder the constraint of additional task could be fulfilled in two steps based on the kinematics equivalent concept: (1) lock redundant joint in a certain displacement, the pose matrixof end-effector is left to the motion of the remaining joints; (2) open redundant joint,the self-motion of the mechanism occurs in, hence to perform the additional task.

Let the self-motion manifold of mechanism inbe, when the manipulator has n arm configuration signs,could be represented by 2ncurves (redundant DOF beyond 1) or curved surface (redundant DOF below 1) in joint configuration space, with every curve or curved surface corresponding to a range of different arm configuration sign combination. When the manipulator is required to complete continuous task tracking, the subscriber could select one continuous curve or curved surface CSMifrom CSMto carry out the additional task in the meantime. CSMicould be expressed as parameter equation of self-motion variable θs:

In a general case, the property index HVof additional task could be denoted by function H ( )θ of joint angle vector θ, based on the above formula, HVcould be expressed as the function of θs:

Inspired by the steepest descent method, it is arrived that θsshould change in the direction of gradient ?HVas HVfor θsin order to obtain the speediest decreasing velocity, when H ( )θ conforms to the minimal performance function law, the difference form θscould be put as follows:

where k is scale factor. We bring HVinto the coefficient in order to keep it in a downward direction corresponding to the index law. When the derivative of HVcorresponding to θsis zero, HVremains to be a constant below threshold value, implying that the mechanism is in an optimal state, with no need for additional task. Thus the geometric significance of Eq. (3) could be stated as follows:to locate the changing direction of θson the curve self-motion manifold, to bring down HV.

When redundant DOF of the manipulator exceeds 1,HVis the multivariate function for θs, the solving formula for θsshould be modified as follows:

where sgn(·) is the symbol function.

The above mentioned algorithm could be summarized as follows:

(1) First to effect the precise position and pose tracking of the end-effector by redundant joints; then adjust the redundant joints by the gradient of redundant joint displacement through the additional task.

(2) In order to perform the additional task by employing the self-motion of the manipulator.

(3) Its geometric significance is to locate the changing direction for θson the self-motion manifold, in order to bring down HVvalue to the minimum in the maximum speed. Eq. (3) can be regarded as the special form of Eq. (2).

The algorithm mentioned above can be illustrated in program chart displayed in Fig. 1, where Ssumis the number of sampling points.

3 Algorithm Application

In order to prove the effectiveness of the motion planning algorithm proposed in section 1, we used the simplest planar 3-DOF redundant manipulator to demonstrate how the collision avoidance simulation task is implemented. Its mechanism diagram is shown in Fig. 2.Initial position of the mechanism stays on the positive half axis of axis y, the rotation angles of the three joints are marked as θ1, θ2, andθ3, respectively. The angles range within ( ? π,π], and the values are positive when rotating clockwise around axis z.

Fig. 1. Program chart for motion planning algorithm

Fig. 2. Mechanism diagram of 3-DOF redundant manipulator

3.1 Self-motion manifold of the mechanism

The manipulator has one redundant DOF when disregarding of the posture of the tip executing apparatus of the robot. It can be seen that there are four arm configuration signs in the mechanism, as indicated in Fig. 3.

Fig. 3. Four kinds of arm configuration

For the sake of simplifying the motion analysis, we take joint 1 as redundancy joint, θ1as the variable of self-motion,when θ1value is specified, we get only two kinds of arm configuration sings in the mechanism. Define arm configuration signs:

Suppose the link lengths of the manipulator are l1, l2, and l3, respectively, and the coordinate of point W is ( Wx , Wy ),then we can conclude the following:

Let’s define

where

3.2 Obstacle avoidance motion planning based on self-motion manifold

Now the case of every link avoiding cycle obstacle is cited to illustrate the specific solution procedure.

3.2.1 Establishment of performance index function for obstacle avoidance

As indicated in Fig. 4, select arm configuration sign e= 1, to establish performance index function for collision avoidance when obstacle locating on each link in clockwise.

Fig. 4. Relative position diagram between each link and the obstacle

Set the distances from point C, the center of obstacle,to each bar are1d, d2, and d3, respectively, set the radius of round obstacle is r, and define:

The conclusion is obvious, when, each link can be free from collision with obstacle. The performance function of obstacle avoidance is thus obtainable in compliance with minimal performance function rule and artificial potential field theory:

where kpis proportional coefficient, λ is threshold value of obstacle avoidance distance.

From the above function definition formula, we find that,when dminis equal to λ, the function obtains the minimal value, also the potential energy minimal point. By this way,the obstacle avoidance problem of mechanism link becomes optimization of minimal potential energy.

3.2.2 Simulation process of obstacle avoidance

Suppose that1l, l2,3l are 100 mm respectively and the initial angle1θ is π/6,2θ is π/6, 3θ is π/2, then the initial coordinate (85, 60) of point W can be derived.Suppose that the diameter of the obstacle is 80 mm and its circle centre coordinate is (85, 60), besides, the desired track of point W is line segment WW′, the coordinate of W' is (146.6, 10), thus the initial state of simulation can be described as Fig. 5.

Fig. 5. Initial configuration of mechanism

Based on Eq. (5), set kp=1 and λ=45 , equally divide line segment WW′ into 10 parts, and then trace the above 11 interpolation points so as to implement the additional obstacle avoidance task by using the algorithm shown in Fig. 1. The configuration of each link in motion is shown in Fig. 6, and it shows that the mechanism complete the main task (tracking the reference point of terminal position)along with the additional task (avoiding obstacle C)simultaneously.

Fig. 6. Configuration of each link in the first simulation

Change of each joint angle in motion as indicated in Fig. 7, shows that the changes of joint angle displacement tend to be mild. Although the inverse solution is worked out on the position level, the joint velocity experiences no sharp fluctuation.

Fig. 7. Change of each joint angle in motion

The minimal distance dminfrom each link to circle center C and the change of performance index HVare shown in Fig. 8. It is noticeable from Fig. 8 thatremains larger than the circle radius, which means collision–free between link and the obstacle. The performance index keeps on declining, meaning that obstacle avoidance algorithm works consistently over time;the performance index is in direct proportion to decline velocity, allowing for fast decline velocity when the performance index grows, and vice versa, which may ensure that with the success of obstacle avoidance,redundancy resources could be released quickly, thus enabling the implementation of other additional tasks.

Fig. 8. Change of the minimal distance and performance index in motion

3.2.3 Algorithm application analysis

Some drawbacks still exist in joint velocity optimization as the above algorithm is produced at the displacement level, yet the velocity fluctuation could be constrained considerably by adjusting the relevant parameters. Let’s illustrate this in greater detail by simulation.

Fig. 9. Configuration of each link in the second simulation

Fig. 10 displays changing pattern of each joint velocity before adjustment. In the initial moment, the link gets quite close to the obstacle, allowing for tremendous effect on the mechanism from the additional task, which explains the great fluctuation of each joint velocity explicitly.

Fig. 10. Changing pattern of each joint velocity before adjustment

In order to reduce the fluctuation of joint angle velocity, we choose to diminish the impact on mechanism motion from the additional task, as a big margin still exists in λ value selection,the threshold value λ of obstacle avoidance distance can be brought down to λ=40, now each joint angle velocity fluctuates slightly, as shown in Fig. 11.

Fig. 11. Changing pattern of each joint velocity after adjustment

4 Algorithm Application Example

In order to further confirm the workability of the said algorithm in engineering application, let’s take the window operation of 8-DOF mobile manipulator as an example.The mobile manipulator has two parts, the mobile platform and mechanical arms[10], its simplified model is demonstrated as shown in Fig. 12. The joint configuration of the mechanical arms is identical to the robot configuration of the traditional PUMA560, the 6 joints are roll-pitch-pitch-roll-pitch-roll.

In most cases the mobile manipulator would conduct its window operation vertical to the wall surface motion,which makes it critical to analyze the window operation of the robot inside the wall vertical face. As shown in Fig. 13,we simplify the mobile platform into the rectangle HIJK,and the main arm and lower arm into line segments PE and EW, the pitch motion of the main arm could be displayed by the line segment PE rotating around the point P; while that of the lower arm by the line segment EW rotating around point E. Line segment PK is consolidated to the mobile platform in order to measure the installation height of the mechanical arm.

Fig. 12. Simplified model of 8-DOF mobile manipulator

Fig. 13. Schematic drawing for primary configuration and tracking trajectory of the mobile manipulator

Point W is set as reference point of the end-effector, and this point is supposed to get through the window ABCD on the wall, and then reach the target pointeW along the curved linewhere WWmis a quarter circular arc with circle center of pointtO and radius of 300 mm; and WmWeis a plumb line segment.

To build a plane coordinate system Oxyat the outer point O of the wall, the initial configuration and the coordinate of every points of the robot are shown in Fig. 13, the initial joint displacement isand

Let the minimal distance from points A,B,C, and D to the line segment WE be D1, D2, D3, and D4, the minimal distance to the line segment EP be D5, D6, D7, and D8,respectively. It is clear from the mechanism movement feature that the line segment HP and the wall surface OA consistently parallel, let the spacing between them be D9,we get the following equationThe problem-solving process foris slightly more complicated, however, the solving method follows the same pattern, let’s take D1as an example to illustrate the simplified solving method.

Let’s take the collision avoidance performance index function in Eq. (5), letλ= 400 mm.

Now let’s conduct the simulation of the mechanical arm performing the window operation using MATLAB software with the motion planning algorithm supplied in Fig. 1, take k= 0.6 in Eq. (5), letmm,thus the configuration changing pattern of fold-line KPEW in the course of point W tracking the curveas shown in Fig. 14. It is clear that no overlapping exists between the spatial region scanned by the fold-line KPEW and the wall surface, implying that the mobile manipulator can successfully carry out the window operation with the said algorithm.

Fig. 14. Diagram of robot’s configuration changing pattern in the course of window operation

5 Comparative Analysis between the Algorithm and GPM

Since the gradient projection method is an inverse solution optimization in velocity level, it can achieve a satisfying joint velocity only if all the individual coefficients are adjusted appropriately. However, in practical application, the adjustment of coefficients in GPM proves rather challenging, which often make algorithm unstable.

Make use of gradient-projection method depicted in Eq. (1), and then adjust ψ, kp, λ according to the magnification factor k proposed in Eq. (2), however, the ideal joint velocity is unreachable. If set ψ=?1 , kp=1,λ= 40,the simulation mentioned in section 3.2.3 is implemented via gradient projection method, where the expected point W moves along line WW′to point W′with uniform speed. As sampling period is 10 ms, the position error curve of end point is shown in Fig. 15, which explains by itself that selection of even smaller sample period can unavoidably result in a larger position accumulated error in the GPM.

Fig. 15. Distance error change of end track and desired value

In terms of time consumption, we employ Dual CPU with dominant frequency of 2.0 GHz under MATLAB software environment for calculation, divide the line segment WW′ into 200 parts for tracing interpolation, and in simulation the subprogram is supposed to generate every operation and ?HVin Fig. 1. In contrast to 0.328 s totally consumed in the GPM, to conduct the whole section tracking with the algorithm proposed in this paper, the time consumed turns out to be merely 0.235 s, a slight predominance over the former, which could be explained by the reduced computation load as a result of the spared effort in computing pseudo inverse in the process of tracking. However, for the latter, the computation is done in two steps, meaning the increased number of variable assignment. The improved GPM proposed in Ref. [4] is also tried to conduct simulation, in which the time consumed being 0.623 s when the position tracing precision is set on 0.1 mm. Further investigation is advisably conducted to compare the time consumed for the three methods in performing additional tasks for other redundant manipulators.

6 Conclusions

In this paper, we propose a novel motion planning algorithm for redundant manipulator, which is developed by carrying out inverse solution optimization searching directly on the displacement level by means of self-motion manifold. The said algorithm overcomes the disadvantage of terminal displacement error accumulation existing in the GPM. The change of joint velocity tends to be more stable when effectively performing the task of end-effector trajectory tracking and the avoidance of robot link collision simultaneously, which makes it more suitable for the redundant manipulator to accomplish the additional task of collision avoidance on the displacement level. The efficiency of this algorithm has been sufficiently tested and proven by the simulation result.

In terms of computation load, the Jacobian matrix and pseudo-inverse could be disregarded for the algorithm in question compared to the modified gradient projection; in contrast to other numerical methods, iteration is unnecessary for this algorithm. As a result, when the self-motion manifold of the robot could be approached in analytical method and does not seem over complex,theoretically the computation load of this algorithm will be drastically reduced in comparison with GPM and other numerical methods, which has be testified partly in section 2.2.4. Therefore, the decision is open to the subscribers for this algorithm depending upon the difficulty involved in self-motion manifold solution.

With respect to coefficient adjustment, it is concluded from Eq. (2) and Eq. (3) that the scale factor in this algorithm will only affect redundant joint velocity, which implies that the adjustment work is much easier than the GPM.

[1] GAO Tongyue, DAI Ju. Research on multi-index amalgamation optimization of redundant robots[J]. Journal of Huazhong University of Science and Technology, 2004, 32(10): 71–73. (in Chinese)

[2] WU Ruimin, LIU Tingrong. New Gradient projection method of kinematically redundant manipulators[J]. Chinese Journal of Mechanical Engineering, 1999, 35(1): 76–79. (in Chinese)

[3] ZU Di, WU Zhenwei, TAN Dalong. Efficient inverse kinematic solution for redundant manipulators[J]. Chinese Journal of Mechanical Engineering, 2005, 41(6): 71–75. (in Chinese)

[4] ZHAO Jianwen. Research on mechanism and motion planning of P2S5 redundant robot with reries/parallel hybrid configuration[D].Harbin: Harbin Institute of Technology, 2007. (in Chinese)

[5] MAYORGA Rend V, CHANDANA Sandeep. A neuro-fuzzy approach for the motion planning of redundant manipulators[C]//2006 International Joint Conference on Neural Networks, Sheraton Vancouver Wall Centre Hotel, Vancouver, BC, Canada, July 16–21,2006: 2 873–2 878.

[6] KOKER R I. Reliability-based approach to the inverse kinematics solution of robots using Elman’s networks[J]. Engineering Applications of Artificial Intelligence, 2005, 18 (3): 685–693.

[7] OZBAY U, SAHIN H T, ZERGEROGLU E. Robust tracking control of kinematically redundant robot manipulators subject to multiple self-motion criteria[J]. Robotica, 2008, 26(6): 711–728.

[8] YE Ping, SUN Hanxu, JIA Qingxuan. Study on kinematics optimization of redundant manipulators[C]//IEEE Conference on Robotics, Automation and Mechatronics, Bangkok, Thailand, Jun 7–9, 2006: 1–6.

[9] MULLER A. Collision avoiding continuation method for the inverse kinematics of redundant manipulators[C]//Proceedings –2004 IEEE International Conference on Robotics and Automation,New Orleans, USA, Apr 26–May 1, New Orleans: Institute of Electrical and Electronics Engineers Inc., 2004: 1 593–1 598.

[10] ZHAO Jianwen, DU Zhijiang, SUN Lining. Self-motion manifolds of a 7-DOF redundant robot arm[J]. Chinese Journal of Mechanical Engineering, 2007, 43(9): 132–137. (in Chinese)