Inverse kinematics of redundant robot based on VC++

JIANG Ru-kang (姜如康), HUANG Liang-song (黄梁松), JIANG Xue-mei (姜雪梅)

(College of Information and Electrical Engineering, Shandong University of Science and Technology, Qingdao 266590, China)

Inverse kinematics of redundant robot based on VC++

JIANG Ru-kang (姜如康), HUANG Liang-song (黄梁松), JIANG Xue-mei (姜雪梅)

(College of Information and Electrical Engineering, Shandong University of Science and Technology, Qingdao 266590, China)

This paper uses a 7-degree-of-freedom (7-DOF) manipulator end-effector to research inverse kinematics solution, Three methods are used and compared, including fixing an angle method, the iteration method and the neutral network method. By comparison, the iteration method is much better because of its high accuracy, fast speed and stabilization, and it does not require calculation of the pseudoinverse of the Jacobian. Thus, this control scheme is well suited for real-time implementation, which is essential if the end-effector trajectory is continuously modified based on sensor's feedback. Finally, using VC++ and Microsoft foundation classes (MFC) to achieve the main machine interface. Through verification, the precision meets the requirements of general control system in real-time implementation.

7-degree-of-freedom (7-DOF)； iteration; neural network; Microsoft foundation classes (MFC)； inverse kinematics

0 Introduction

A redundant robot manipulator is a manipulator with more degrees of freedom than that are necessary to locate the end-effector at a desired position and orientation[1-5].

Six-degree-of-freedom (6-DOF) is the minimum number of joints required to reach an arbitrary position and orientation of a manipulator end-effector. However, manipulators with 6-DOF are limited in their ability to follow an arbitrary end-effector path because of singular configurations[2], and 7-DOF manipulator is superior to 6-DOF in flexibility and obstacle avoidance ability.

Because there exists redundant freedom, calculation becomes very complicated. This article solves inverse kinematics with three methods and chooses the best one for programming with VC++.

1 Three methods

This paper proposes three methods to solve the inverse kinematics of 7-DOF manipulator. First of all, we need to analyze the mechanical structure of the manipulactor. The structure and coordinate analysis are shown in Fig.1.

Fig.1 Architecture of the manipulator end-effector

From Fig.1, it can be seen that the manipulator to be researched is of 7 axes, and the first four joints are wrist joints with redundancy and other three joints are elbow joints.

According to Denavit-Hartenberg (D-H) method[6-8], homogeneous transformation of the matrix of the humanoid end-effector is got as

px=c1(d6c2s34+a4c2c34+a3c2c3+d3s2)+s1(a4s34-d6c34+a3s3),

py=s1(d6c2s34+a4c2c34+a3c2c3+d3s2)-c1(a4s34-d6c34+a3s3),

1.1 Fixing an angle method

Due to θ2is fixed, it can be obtained as

where

M=a4c4+d6s4+a3;

N=d6c4-a4s4; F=(pz+c2d3)/s2;

θ1and θ3all have four groups answers.

From Eq.(1), it can be got as

The solution of the θ5to θ7can be obtained by

θ5=atan2((axc1s2-azc2+ays1s2)/s6, (azs2c34-ay(c1s34-s1c2c34)+ax(s1s34+c1c2c34))/s6,

θ7=atan2((-ozs2s34-ox(-s1c34+c1c2s34)+oy(c1c34+s1c2s34))/s6,

(-nzs2s34-ny(c1c34+s1c2s34)-nx(-s1c34+c1c2s34))/(-s6)).

In the above equation Q, M, N, F are introduced to faciliate the calculats. They have no practical significance.

1.2 Iteration method

The iteration method does not require calculation of inverse of the Jacobian[6-10]. An efficient formulation for determining joint velocities for given Cartesian components of linear and angular manipulator velocities is obtained. Thus, if the manipulator trajectory is continuously modified based on sensor's feedback, this control method is well suited for real-time implementation, which is essential[1].

We can obtain the solution of the θ4with the same method above.

Taking the derivative of θ4, then it can be got as

where

k1=a4s34-d6c34+a3s3, k2=a4c34+d6s34+a3c3,

C-1=(CTC)-1CT, D-1=DT(DDT)-1.

In the above equations, k1, k2, Jm, Jn, C and D are introduced to faciliate the calculation.

Through the front equations, the angular velocity of each joint is got as

This method takes the thought of the integral, we solve the angle and take it back to the next step, until it reaches the goal.

The solution of the θ5to θ7are the same with the above, they are algebra solutions.

1.3 Neural network method

The neural network has the ability of self learning, it does not need accurate mathematical model[5], even can avoid a lot of miscellaneous strict mathematical formula. Because of ambiguiuy, fault tolerance, adaptability and self-learning characteristics, and many scholars have been using neural networks to solve complex control of robot.

α=atan2(ny, nx),

The learning includes forward propagation and error back propagation. A network including multiple neurons, "layer", the input layer, hidden layer and output layer. Input layer is to receive input and distribute it to the hidden layer. The hidden layer is responsible for the necessary calculation and outputs, it to the output layer, thus the user can see the final result.

To the input layer:

ok=f(netk),

To the buried layer:

yj=f(netj),

The transfer function is single polarity function:

BP algorithm belongs to δ learning rule, using the iteration method to diminish error.

i=0, 1, …, n, j=1, …, m;

The error signal to the output and buried layer:

The specific steps are as follows:

2 Development with VC++

2.1 Building MFC interface

The input of the manipulator needs seven data, forward kinematics output needs twelve data, inverse kinematics output needs eight data. Fig.2 shows the designed interface of man-machine.

Fig.2 Interface of man-machine

After the dialog box established, the data needs to be inputted to program. There is a brief method, for every display frame associated with a related variable, read in and read out can be briefness.

After the front operation completed, the following function will appear in the “Dialog”. It is obvious that the following code should be written in this function.

void CShejiDlg: : OnBtnIN()

{

// TODO: Add your control notification handler code here

}

2.2 Programming with C++

We choose the iteration method to simulation, the reasons are at the end of the paper.

Flow chart of program is shown in Fig.3.

Fig.3 Program flow chart

First we can solve the θ4, and then insert a loop to achieve the iteration of the θ1, θ2and θ3.

J11,…, J13means the inverse of Jm.

Jn1,…,Jn3means the inverse of Jb.

Due to the limitation of length, list the important program segment.

double A1[4][4]={cos(c1), -sin(c1), 0, 0, sin(c1), cos(c1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};

…

double Q=(px^2+py^2+pz^2-a3^2-a4^2-d6^2-d3^2)/(2*a3);

double

num41=atan2(Q, sqrt(a4*a4+d6*d6-Q*Q))-atan2(a4, d6);

…

c4_1=cos(c41)*cos(c41)*(px*pxx+py*pyy+pz*pzz)/a3;

for(i=0; i

{

double J11=(k2*n1*n2-n3*n4)/(m1*m2);

… double f1=-k1+a3*sin(c30);

double f2=k2-a3*cos(c30);

double Jn1=cos(c10)*cos(c20)*f1+sin(c10)*f2;

…

c1_1=(J11*(pxx-Jn1*c4_1)+J12*(pyy-Jn2*c4_1)+J13*(pzz-Jn3*c4_1))*k2;

…

num1=c10+c1_1*dt;

…

c10=num1;

… }

…

…

3 Conclusion

Among the three methods, the method of fixing an angle is simple, but it weakens the redundant characteristics, and the fixed angle need manual control. Neural network method does not need accurate mathematical model to control the motion of manipulator, but the control precision is low, its control time is long and goes against real-time implementation. The iteration method can reduce the amount of calculation and control time, precision is high, so it is much better. Because the manipulator trajectory is continuously modified, this control method is well suited for real-time implementation. We can get the error which is less than 1% through emulating.

[1] Dubey V R, Euler A J, Babcock M S. Real-time implementation of an optimization scheme for seven-degree-of-freedom redundant manipulators. IEEE Transactions on Robotics and Automation, 1991, 1: 579-588.

[2] Chang P. A closed form solution for inverse kinematics of robot mainpulator with redundancy. IEEE Journal of Robotics and Automation, 1987, 3(5): 393-403.

[3] Nakamura Y, Hanafusa H. Optimal redundancy control of robot manipulators. International Journal of Robotics Research, 1987, 6(1): 32-42.

[4] Suh K, Hollerbach J. Local versus global torque optimization of redundant manipulators. Proc. of IEEE International Conference on Robotics and Automation, 1987, 4: 619-624.

[5] Chang P. A closed form solution for control of manipulatorswith kinematic redundancy. Proc. of IEEE International Conference on Robotics and Automation, 1986, 3: 9-14.

[6] Whitney D E. The mathematics of coordinated control of prosthetic arms and manipulators. Journal of Dynamic Systems, Measurement, and Control, 1972, 94(4): 303-309.

[7] Liegeois A. Automatic supervisory control of the configuration and behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and Cybernetics, 1977, SMC-7(12): 868-871.

[8] Korayem M H, Ghariblu H. Analysis of wheel mobile flexibility manipulator dynamic motions with maximum load carrying capacities. Robotics and Autonomous System, 2004, 48: 63-76.

[9] Korayem M H, Ghariblu H, Basu A. Optimal load of elastic joint mobile manipulators imposing an overturning stability constraint. The International Journal of Advanced Manufacturing Technology, 2005, 26(5-6): 638-644.

[10] Shabana A A. Dynamics of multi-body systems. Third edition. Cambridge: Cambridge University Press, 2005.

date： 2012-08-15

Shandong Province Science and Technology Development Plan (No. 2011SJGZ02); Shandong University of Science and Technology Graduate Innovation Fund (No. YCA120355)

JIANG Ru-kang (jiangrukang@163. com)

CLD number： TP242 Document code： A

1674-8042(2013)01-0063-05

10. 3969/j. issn. 1674-8042.2013.01.014