LSPB Trajectory Planning: Design for the Modular Robot Arm Applications Li Xianhua, Tan Shili, Feng Xiaowei, Rong Hailiang School of Mechatronics and Automation Shanghai University Shanghai, China
[email protected]
Abstract —In this paper, we report a household service robot with multi-sensor and modular arms. According to the DH convention, the coordinate system of robot arm is established. The methods of solving direct and inverse kinematics are introduced. Because both acceleration and speed of the arm modular are limited, so LSPB trajectory planning method is designed for our robot arm. Simulations and experimental results on our modular robot arm are presented.
the controller. When the module executes a movement, the position is monitored in real time, and the required data is transmitted by sensors back to internal logic, so we can get the data of the module constantly. The module interface is based on standard PC hardware with a standard operating system, so we can easily integrate many commercially available add-on peripherals.
Keywords- LSPB; modular arm; inverse kinematics
I.
I NTRODUCTION
The leader of the PC revolution Bill Gates presented a paper named “A robot in every home” in Scientific American” in January, 2007 , in that paper he predicted that the next hot filed would be robotics and the robot would become become one member of every home in future[1]. Now the development direction of household service robot is intelligence, light weight and modularization. The first Reconfigurable modular Manipulator System (RMMS) was established in Carnegie Mellon University in the 1980s [2]. With financial support from the national 863 program of China (Household Service Multi-robot System), our laboratory has developed a household service robot which is with two six-DOF modular arms and multi-sensor as shown in Fig. 1. Our robot is 175cm high and 95kg weight. It has 22 DOF which consists of two in undercarriage, two in waist, twelve in two arms, and six in head. The platform is equipped with voice recognize system, telephone and mobile phone message control system and wireless network control system. The multi-sensor includes a laser-scanner, a trinocular vision sensor, a ceiling camera and an electronic compass. It can autonomously navigate and move in the relative unstructured environments of household. Every modular arm is 783cm long and 11kg weight only, this reduces the weight of the whole robot. With sufficient battery power, it can work about more than half an hour. Every modular arm consists of seven modules: six for joints and one for gripper, so two arms have fourteen modules. Arm controller communicates with these modules via CAN bus. Every module is integrated with a brushless DC servo motor, complete performance and control electronics, a brake and a harmonic drive gear. Both the control electronic circuit board and motor are powered by +24V. The module has a hollow shaft, so it is very convenient to laying the electrical wire and the signal wire. The brushless DC servo motor is controlled by the internal logic, which receives the required parameters from
Figure 1. The diagram of household service robot
II.
DIRECT AND INVERSE KINEMATICS ANALYSIS
The direct kinematics equation establishes the functional relationship between the joint variables and the end-effector position and orientation. The inverse kinematics problem consists of the determination of the joint variables corresponding to a given end-effector position and orientation. To compute the direct kinematics function, it is possible to resort to a DH parameters table for the arm structure, which can be identified from the base to the end of the arm. The last three joints of our robot arm form a typical spherical wrist structure, but configuration of the first three joints is different from the PUMA robot [3]. The coordinate system of arm is shown in Fig. 2 and DH parameters are specified in Table I .
978-1-4244-4994-1/09/$25.00 ©2009 IEEE
z 6 y6
x6 z 5
d 6
y4 z 4
y5
x4
x5
d 4
z 3 y2 z 2
y3
x2
x3
d 2 z 1 y0
z 0
y1
x x1 0
Figure 2. Coordinate frame of one arm
TABLE I.
t f
DH PARAMETERS FOR THE ARM
Joint
θ i
α i
ai
d i
Joint range
1
θ 1
-90
0
0
-180~+180
2
θ 2
90
0
328
-180~+180
3
θ 3
-90
0
0
-120~+120
4
θ 4
90
0
276.5
-180~+180
5
θ 5
-90
0
0
-120~+120
6
θ 6
0
0
178.5
-180~+180
We can easily solve the direct kinematics of arm according to data listed in Table I, so once the joint angles of arm have been given, the position and orientation of arm's end-effector are computed. To solve the inverse kinematics, geometric and algebraic solution methods are employed. For the first three joint angles, we use geometric method, and the last three, we use algebraic method combined with the values of the first three angles [4]. For this kind of arm, there exist eight solutions when the values of position and orientation of the end-effector are specified. We use the "minimum route" rule to choose one from the eight solutions: firstly we choose one solution from the two solutions of the first joint; and then choose one from the two solutions of the second and fourth joints separately using the same method; finally we get only one solution for the arm. III.
three main parts: the first part is a quadratic polynomial, second part is a linear function, and the last part is also a quadratic polynomial. LSPB trajectory switches between maximum, minimum, and zero acceleration to achieve time-optimal motion while respecting speed limits, and it allows a direct verification of whether the resulting velocities and accelerations can be supported by the physical mechanical manipulator [5-8]. As usually this method needs only the initial and joint angles, traveling time and either angular acceleration or angular velocity, but in practice the user is offered the option to specify the velocity and acceleration percentage with respect to the maximum allowable velocity and acceleration; this choice is aimed at avoiding occurrences when the specification of a much too short motion duration would involve much too large values of velocities and/or accelerations, beyond those achievable by the manipulator[3]. When the final position, velocity and acceleration are specified, the traveling time is certain. The condition of LSPB trajectory planning method is: initial speed is zero, both the initial and final acceleration are constant. There are two possibilities: first is the arm can reach its max velocity, a trapezoidal velocity profile is assigned, and the traveling time is computed in equation (1); second is it can not reach its max speed, a triangle velocity profile is assigned, and the traveling time is computed in equation (2).
LINEAR SEGMENTS WITH PARABOLIC BLENDS TRAJECTORY (LSPB)
The LSPB trajectory is such that the velocity is “ramp up” to its specific value initially and then “ramped down” at the goal position. To achieve this, the trajectory will divided into
=
t f =
2 ⋅ v / a + ( Δθ ⋅ a − v 2 ) /(v ⋅ a)
(1)
4 ⋅ Δθ / a
(2)
Where t f is traveling time, a is acceleration, v is velocity, and
Δθ
is traveling distance.
Once t f is computed, t c the time of arm reaching its max velocity is computed in equation (3), and the following sequence of polynomials is generated as in (4): t c
=
t f / 2 − (t f
2
⋅ a − 4(θ f − θ i )) /(4 ⋅ a )
θ i + 0.5 ⋅ a ⋅ t 2 θ (t ) = θ i + a ⋅ t c ⋅ (t − 0.5 ⋅ t c ) θ − 0.5 ⋅ a ⋅ (t − t ) 2 f f
(3) 0 ≤ t ≤ t c
t c (t f
<
t ≤ (t f
− t c ) <
− t c )
(4)
t ≤ t f
We use the method above to calculate traveling time for every joint, choose the max long time from the six results as the arm traveling time, and then use (4) to calculate every joint's sequence points. Because we assign the same time to each joint, we ensure that every joint can arrive the target at the same time. For LSPB trajectory planning, there are two advantages as bellows: (1) Not necessary to specify the arm’s traveling time; (2) Before running the algorithm, the kinematic limits (the max value of velocity and acceleration) are specified. When given the final position, velocity and acceleration, the trajectory is ascertained according to equation (1)-(4), and a plot of the LSPB trajectory is shown in Fig.3.
experiment using the sixth joint. Using LSPB trajectory planning, the module moves from -90 to 90, acceleration 50 and velocity 40. The simulation curves are shown in Fig. 5, and the experiment results are shown in Fig. 6. From the picture, we can see that both position and velocity curves are smooth and the whole trend of the acceleration is consistent with the simulation curves.
θ
vel acc ) e e r g e d ( Y
0
tc
time(sec)
tf
Figure 3. Linear Segment with Parabolic Blends Trajectory
IV.
SIMULATION AND EXPERIMENT
The modular arm is different from industrial manipulator. Industrial manipulators generally controlled by dedicated controllers. As upgrades become costly and interfacing becomes complex due to hardware and software conflicts, the flexibility of the robotic manipulators is reduced [9]. Every module has the same interface, so it is convenient to combine modules together as a robot arm. For our modular arm, an Intel Pentium Core 2 industrial computer is used as the central controller, the control software is implemented using VC++ programming language. Because the module is highly integrated, the speed is not as fast as industrial manipulators, the numeric values of kinematic constraints are listed in Table 2.
Figure 4. initial and final position of the arm 150
100
TABLE II.
VALUES OF THE KINEMATIC LIMITS OF THE JOINTS
50
0
Joint 1 2 3 4 5 6
Kinematic constraints Velocity (deg/s) Acceleration (deg/s2) 30 130 30 140 30 140 35 145 35 145 70 200
Aiming at grapping a bottle of water, first the service robot gets the position of the bottle by its trinocular vision sensor, changes the coordinates to the arm coordinate system, computes the inverse kinematics and chooses the only one solution using the method above. The arm moves from the initial to the final joint configuration using LSPB trajectory planning method in given final position, acceleration and velocity. In this experiment, the final position is given by trinocular vision sensor, acceleration is 50, and velocity is 70. Considering the reality of Windows Operation System and the number of the modules, the controller interpolates the joint position every 50ms. The initial and the final position of arm are shown as Fig 4. Using the trajectory planning above, the trajectory is relatively smooth, and the arm can finish its job in the accuracy of ±5mm. As we can get both the position and velocity from the module in real time, we do another
-50
-100
-150 0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Figure 5. simulation results of position, speed and acceleration 150
100
50
0
-50
-100
-150 0
0.5
1
1.5
2
2.5 3 time(sec)
3.5
4
4.5
5
Figure 6. experiment results of position, speed and acceleration
V.
CONCLUSIONS AND FUTURE WORK
In this work, the kinematic model of a module arm is established, both the direct and inverse kinematics are solved, and LSPB trajectory planning is designed for arm motion under the condition of giving acceleration, velocity and final position. Simulation method is employed to compare the experiment result. In the experiment, the arm gets the position of the bottle by the trinocular vision sensor, moves to the position using the LSPB trajectory planning method in the interpolating time 50ms and successfully grasps the object. So the correction of kinematic algorithms and the LSPB trajectory planning method are proved. Future work includes: designing a jerk bounded trajectory planning method because the LSPB method generates discontinuous acceleration and torques which causes the joint motion to be always delayed with respect the reference trajectory, and adding a force sensor to enhance the humanrobot interaction ability. ACKNOWLEDGEMENTS The authors are grateful for the financial support from the State High-Tech Development Plan (863 program) of China under contract No. 2007AA041604.
R EFERENCES [1]
Bill Gates “A robot in every home” January,pp.58-65
[2]
D. Schmitz, P. Khosla and T. Kanade, “The CMU reconfigurable modular manipulator system”, Proceeding of the international Symposium and Exposition on Robots, pp. 473-488, 1988.
[3]
Bruno Siciliano, Robotics, Springer-Verlag, London, 2009.
[4]
Wuxin Huang, Shili Tan, Xianhua Li, “Inverse Kinematics of Compliant Manipulator Based on the Immune Genetic Algorithm”, Fourth International Conference on Natural Computation , Jinan,Shandong,China,2008, October, pp.390-394.
[5]
Atef A. Ata and Mohamed Y. Sa’adah, “Soft Motion Trajectory for Planar Redundant Manipulator”, 9th International Conference on Control, Automation, Robotics and Vision, Singapore, Singapore. December 8, 2006.
[6]
Sonja Macfarlane and Elizabeth A. Croft, “Jerk-Bounded Manipulator Trajectory Planning: Design for Real-Time Applications”, IEEE Transactions on Robotics and Automation, Institute of Electrical and Electronics Engineers Inc, ,vol 19(1),pp. 42-52, February 2003.
[7]
A. Gasparetto, V. Zanotto, “A new method for smooth trajectory planning of robot manipulators”, Mechanism and Machine Theory, Elsevier, pp.455-471, June 2006.
[8]
D. Costantinescu, E.A. Croft, Smooth and time optimal trajectory planning for industrial manipulators along specified paths, Journal of Robotic Systems.pp.233-249, 17(5) , 2000.
[9]
Farooq m, Wang Daobo, “Implementation of a new PC based controller for a PUMA robt”, Journal of Zhejiang University SCIENCE A, JZUS, pp.1962-1970, August, 2007.
Scientific American , 2006
