Unified Closed Form Inverse Kinematics for the ... - Semantic Scholar

ROBOTIK 2012

Unified Closed Form Inverse Kinematics for the KUKA youBot Shashank Sharma, Gerhard K. Kraetzschmar Bonn-Rhein-Sieg University of Applied Sciences, Sankt Augustin, Germany Christian Scheurer, Rainer Bischoff KUKA Laboratories GmbH, Augsburg, Germany

Abstract Mobile manipulators are of high interest to industry because of the increased flexibility and effectiveness they offer. The combination and coordination of the mobility provided by a mobile platform and of the manipulation capabilities provided by a robot arm leads to complex analytical problems for research. These problems can be studied very well on the KUKA youBot, a mobile manipulator designed for education and research applications. Issues still open in research include solving the inverse kinematics problem for the unified kinematics of the mobile manipulator, including handling the kinematic redundancy introduced by the holonomic platform of the KUKA youBot. As the KUKA youBot arm has only 5 degrees of freedom, a unified platform and manipulator system is needed to compensate for the missing degree of freedom. We present the KUKA youBot as an 8 degree of freedom serial kinematic chain, suggest appropriate redundancy parameters, and solve the inverse kinematics for the 8 degrees of freedom. This enables us to perform manipulation tasks more efficiently. We discuss implementation issues, present example applications and some preliminary experimental evaluation along with discussion about redundancies.

1

Introduction

Mobile manipulation, the seamless integration and synchronization of mobility and manipulation, is considered a key technology for professional service robotics as well as for future flexible manufacturing scenarios. However, present technology in this field is not yet sufficiently mature as stated by [1], [2], [3], [4], [5], [6]. Mobile manipulation is still being considered a field of basic research, and open research problems exist in abundance [1], [2], [3], [4]. Mobile manipulators are redundant for tasks described in six DOF1 because of the extra mobility provided by the base. Mobile robot systems developed so far do not use this redundancy judiciously during the planning and execution step and therefore, tend to treat the mobile platform and the manipulator independently as two different systems. Handling of redundancy at planning level and during control is one of the most significant and challenging part in designing a mobile manipulation system [1]. Our research is based on the KUKA youBot mobile manipulator [2], Figure 1. It aims at questions as how to compute platform positions directly from the Cartesian 6D target position for the tool center point of the manipulator using a unified analytical IK solution, how to deal with infinite null-space solutions and how to make use of these redundancies in real robot applications. 1 Degree

of Freedom Goal Regions 3 Inverse kinematics 2 Workspace

139

Figure 1: KUKA youBot - an open source enabled omnidirectional mobile manipulator for research and education

1.1

Motivation & Problem Formulation

The manipulation planning approach called Inverse Kinematics based Bidirectional RRT (IKBiRRT) described in [8] and illustrated in Figure 2 uses Cartesian WGRs2 to treat different possible grasps for the object which depend on the object’s shape and the gripper used. Redundancy introduced by a mobile platform is proposed to be solved within an IK3 solver including some heuristic strategy for base positioning [5]. The planner is probabilistic complete and makes it possible to consider redundant solutions as planning time goes to infinity. However, the efficiency of the planner highly depends on the ability of the IK solver

to generate all possible solutions and that too quickly, but such an IK solver is missing for the KUKA youBot. Since, the WGR is defined in Cartesian space and depicts all potential grasps, a sampled value of these Cartesian goals has to be mapped into the joint space of the robot. As a mobile manipulator is generally redundant, we propose a unified, closed-form 6D IK solver for the KUKA youBot considering also the base positioning without any heuristics. Therefore, we need to define appropriate redundancy parameters that help us to realize this closed form IK solution.

IKBiRRT

Mapping of Cartesian space to joint space and dealing with redundancies

IK Solver

Start

nodes

O b s t a c l e

Goal 2 Goal 1 WGR Cartesian Space

Path

WGR

Joint Space

Figure 2: IKBiRRT planner: Samples of WGR described in Cartesian space being mapped to joint space using IK solver [8]. Notice, the redundancy when same Cartesian 6D goal is mapped to different goals in joint space, is generally due to kinematic redundancy, in our case mainly due to platform movement with the KUKA youBot The contributions of this paper are the identification of redundancy parameters for doing mobile manipulation with the KUKA youBot and the usage of these redundancies to define a unified closed form IK solution for the KUKA youBot. Furthermore, for a given 6D goal pose, valid ranges for redundancies are computed. Positioning of the base is then based on the chosen value of the redundancies and so is in-built in the IK solver. Planning for the KUKA youBot as a unified robot system can accordingly be performed using the IKBiRRT approach. Additionally, null space movement for the KUKA youBot can also be achieved. The real challenge in implementing such a unified closed form IK solver for the KUKA youBot is the high dependency on the robot’s mechanical design. Therefore, most of the industrial robotic manipulators are based on the already established and well studied design of the PUMA 560 [11] robotic manipulator. Due to the presence of the last three axes of the arm intersecting at one common point and forming a wrist, Pieper’s approach [10] can be applied to solve the IK. This approach greatly simplifies the system of non-linear equations by decoupling the translational and rotational components of the tool center point while solving the IK. For robotic arms not adhering to this design

140

tailor-made IK solvers need to be built. For the 5DOF KUKA youBot arm, such a solution cannot be used because of the missing wrist joint. Secondly, since the arm is a 5DOF manipulator for tasks described in Cartesian 6D, the missing degree of freedom needs to be somehow compensated by the mobile base.

2

Related Work

Inverse kinematics can be solved by numerical or analytical approaches [9], [11], [14]. The preference for the former or latter depends on the application it is used in. Numerical methods are mainly used for robotic systems like mobile manipulators where analytical instructions are not provided yet and for highly redundant systems. Whereas, analytical closed form IK solvers dominate in deliberative planning like trajectory generation and path planning [8], [11]. There exist several approaches to overcome missing analytical IK solutions in the path planning community, e.g. in manipulation tasks when positioning the platform. In [13] base positions are sampled based on a robot specific probability distribution called inverse reachability map. [5] presents a two-stage approach for Cartesian path planning where a so called manipulability map representing implicit and discretized IK information is computed prior to computing a constrained Cartesian trajectory. As described, most existing approaches to mobile manipulation are based on this two stage approach requiring precomputation, discretization, good heuristics and probability distributions thereby, introducing inefficiency and suboptimality into global planning algorithms. So we think, that the flexibility of mobile manipulators in manipulation tasks can be further increased when solutions to the base positioning problem is built into the IK solver of the mobile robot system.

3

Solution Approach

Figure 3: Description of redundancy parameter: (a) ρ1 , (b) ρ2 , (c) ρ3

3.1

IK Solver

Our analytical closed form IK solver described in detail in [12] and shown as a component in Figure 4 consists of the following steps to determine unique base and joint values. We first begin with defining two internal parameters, namely β and θ ( Section 3.1.1) from the given 6 dimensional goal pose. We then calculate the value of the rotation of the platform while ρ1 is assumed to be zero. Next, we compute the second, third and fourth joint value of the arm based on redundancy parameters ρ2 and ρ3 . After that, we compute the position of the platform in the two dimensional x, y plane and fix the fifth joint of the arm. In the end, the base orientation is corrected and the first joint value for the arm is determined using the redundancy parameter ρ1 .

Z

Z

We propose to describe the redundancies for the KUKA youBot as shown in Figure 3 where ρ1 resolves the redundancy between the rotation of the platform and the rotation of the first manipulator joint, ρ2 resolves the redundancy between linear platform movement and the arm reach which has to lie in certain intervals irrespective of the rotation of the first joint of the arm and ρ3 deciding whether the elbow (joint 3) of the arm is pointing upwards or downwards.

Flange

Y

Flange

r33 β

r31

X

World

θ

X

Y

r32

Flange

Figure 5: The coordinate system shown in black in the left end of the picture is the world coordinate frame. At the right side is the flange/end effector coordinate system when it has reached the goal. The red vector in the left world coordinate system depicts the Z direction of the flange coordinate system. The green vectors are the projection of this vector in the respective planes. r31 , r32 and r33 are the components of the world coordinate system in which the Z of the flange coordinate is described. These values are extracted from the goal rotation matrix. θ is the angle that the platform turns about its center. Considering the angle θ1 of the arm to be zero, the angle θ can be extracted from the Figure 5. This angle is described as the angle that the projection of the Z vector of the flange makes with the X axis of the world coordinate system when projected onto its X − Y plane. This angle can be expressed as:

x

ρ1

y

IK Solver

ρ2 ρ3 (Elbow)

θ = arctan2(r32 , r31 )

θ θ1 θ2 θ3 θ4 θ5

3.1.2

Calculation of θ2 , θ3 , θ4

We now calculate the above three mentioned angles as per the procedure below. Consider the part of the arm comprising of links L2 , L3 and L4 as shown in Figure 6.

L4

Figure 4: Input is the Cartesian 6D pose of the goal and the redundancy parameters ρ1 , ρ2 and ρ3 . Output is the platform position and orientation: x, y, θ and arm joint angles θ1 to θ5 respectively L3

Calculation of β and θ

Φ

As shown in Figure 5, β is the angle that the +Z direction of the flange of the manipulator makes with the X − Z plane of the base given that the robot is in the goal state. It is the angle that the projection of the Z vector of the flange makes with the X axis of the world coordinate system in the plane marked by the blue colour in Figure 5. The red coloured unit vector can be described as a linear combination of the unit vectors r31 , r32 and r33 . These unit vector components can be extracted from the goal pose. The third column of the rotation matrix of the goal describes exactly the components. Therefore, β can be calculated as from Figure 5 as: β = arctan2(r33 ,

p

(r31 )2 + (r32 )2 )

(1)

141

θ4

β

θ3 L2

3.1.1

(2)

Z'

Redundancy Parameters

6D Goal pose

β

θ2 X'

Figure 6: KUKA youBot arm representation for solving the joints 2, 3 and 4 The Z’ in Figure 6 refers to the vertical distance between the goal and the axis 2 of the youBot arm. Z W is the vertical position of the goal in frame of world coordinate and Z2W is the vertical distance of the axis 2 from the world coordinate system. The value of Z2W is supposed to be constant and can be calculated by the kinematic design of the KUKA youBot.

then X 0 = ρ2

(3)

X 0 is defined as the redundancy parameter earlier defined by ρ2 as shown in Equation 3. This value can vary in a given interval and is defined by the kinematics and joint limits of the youBot arm. Also from Figure 6: ⇒ Z 00 = L2 sin(θ2 ) − L3 sin(θ2 + θ3 ) Z 00 = Z 0 − L4 sin(β)

(5)

⇒ X 00 = L2 cos(θ2 ) − L3 cos(θ2 + θ3 )

(6)

X 00 = X 0 − L4 cos(β)

(7)

and,

(17)

k2 = rsin(γ)

(18)

Equation 11 and Equation 12 can now be rewritten as, Z 00 = sin(θ2 − γ) r

(4)

where,

k1 = rcos(γ)

(19)

X 00 = cos(θ2 − γ) r From Equation 19 and Equation 20 we get,

(20)

θ2 = arctan2(Z 00 , X 00 ) + γ from Equation 16:

where, θ2 = arctan2(Z 00 , X 00 ) + arctan2(k2 , k1 )

(21)

Squaring and adding both sides of the Equation 4 and Equation 6 and simplifying using trigonometric identities we get:   −Z 002 − X 002 + L22 + L23 (8) cos(θ3 ) = 2L2 L3

Now we will calculate θ4 . In the Figure 6 we we see that,

Also,

3.1.3 p sin(θ3 ) = ± 1 − cos2 (θ3 )

(9)

θ3 = arctan2(sin(θ3 ), cos(θ3 ))

(10)

⇒ It is important to note that now we get two values of sin(θ3 ) from Equation 9. These two values can used to calculate two values of angle θ3 as per Equation 10. These two values denote the elbow position of the arm represented by the redundancy parameter ρ3 as shown in Figure 3. Now, we rewrite Equation 4 and Equation 6 in the form below: Z 00 = k1 sin(θ2 ) − k2 cos(θ2 )

β = φ + (π − θ4 ) ⇒ θ4 = θ2 + θ3 − β

(22)

Calculation of Base Positions x and y

The platform position x and y is calculated as follows: Suppose we calculate the TCP4 from the unified FK5 model with the values of the arm joints calculated as before and rest to be zero. We name the x position of this TCP as Xf and y as Yf , then the x and y positions of the platform are the difference between the xgoal (Xg )from Xf and ygoal (Yg ) from Yf respectively as can be seen in Figure 7.

Z

(11) Z

X 00 = k1 cos(θ2 ) + k2 sin(θ2 )

Y

(12)

X

Goal position Z

Y

{W}

where,

Δy

X Y

(Xf, Yf)

k1 = L2 − L3 cos(θ3 )

(13)

k2 = L3 sin(θ3 )

(14)

Δx

(Xg, Yg)

X

we perform change of variables by rewriting the constant k1 and k2 as below: If, q r = + k12 + k22 (15)

Figure 7: Calculation of ∆x and ∆y for the base placement of the youBot while solving for the IK.

and

and γ = arctan2(k2 , k1 )

4 Tool

(16)

Center Point Kinematics

5 Forward

142

⇒ ∆x = x = Xg − Xf

(23)

∆y = y = Yg − Yf

(24)

3.1.4

Calculation of θ5

Therefore, if ρ1 is varied we need to calculate new values of platform position and orientation. Now, Goal pose

X Y

Y

θ5

Xa = Xp + L ∗ cosθinitial

Z and,

End effector

Ya = Yp + L ∗ sinθinitial

X

Figure 8: The solid coordinate system shows the goal pose and the dashed coordinate system is the current end effector frame. The rotation of θ5 in the given direction as pointed by the arrow with respect to the the Z axis would position the end effector exactly at the desired pose.

where, Xa is x position of the first arm joint axis and Xp Equation 23 is x position of the platform frame, both in the world frame. L is the distance between Xa and Xp and θinitial (Equation 2) is the orientation of the platform in world frame, when ρ1 is zero. Similarly, Ya and Yp (Equation 24) are the y position of the arm and platform respectively. Now, if redundancy parameter ρ1 is varied then the position of the platform is described by the following set of equations:

We now perform FK on the calculated arm joint values, the base position and orientation. We call this end effector frame, represented by the dotted coordinate frame in Figure 8. We see at this configurations there is only a rotational offset about the Z direction of the end effector to compensate for the desired goal frame. This can be written in equation form as: G3,3 = E3,3 ∗ Rz (θ5 )

Yp = Ya − (L ∗ sin(θinitial − ρ1 ))

(32)

θf inal = θinitial − ρ1

(33)

where, θf inal is the final orientation of the platform as ρ1 is varied. From Equation 8 we see that the value of cos(θ3 ) lies between −1 and +1. Therefore, the value of ρ2 can lie in an interval so that the below mentioned set of inequality holds true for specific values of X 00 ,  −1 ≤

g1,2 g2,2 g3,2

−Z 002 − X 002 + L22 + L23 2L2 L3

 ≤ +1



g1,3 g2,3  g3,3

 cos(θ5 ) −sin(θ5 ) 0 = sin(θ5 ) cos(θ5 ) 0 0 0 1

since L2 , L3 and Z 00 are constants. We can relate the X 00 to ρ2 from Equation 7 and Equation 3. ⇒ X 00 = ρ2 − L4 cos β (34)



(27)

4

On solving, we get:

3.1.5

(31)

(25)

where, G and E are the goal and end effector rotation matrices. Rz is the rotation matrix due to rotation around Z axis. ⇒ T E3,3 ∗ G3,3 = Rz (θ5 ) (26) This can be rewritten as:    e1,1 e2,1 e3,1 g1,1 e1,2 e2,2 e3,2  ∗ g2,1 e1,3 e2,3 e3,3 g3,1

Xp = Xa − (L ∗ cos(θinitial − ρ1 ))

cos(θ5 ) = e1,1 g1,1 + e2,1 g2,1 + e3,1 g3,1

(28)

sin(θ5 ) = e1,2 g1,1 + e2,2 g2,1 + e3,2 g3,1

(29)

⇒ θ5 = arctan 2(sin(θ5 ), cos(θ5 ))

(30)

Dealing with Redundancies ρ1 and ρ2

To compensate for ρ1 as shown in Figure 3 we need to turn the base at the axis of rotation similar to that of arm joint 1.

143

Results

The Figure 9 shows the results of plotting the joint values when the redundancies ρ1 or ρ2 are varied and clearly show that the redundancies are independent of each other because there is no discontinuity in the curves and the trajectories of the joints are smooth. Hence, enabling us to develop joint controllers using which we are able to show the null space motion of the KUKA youBot as a mobile manipulator system both in simulation as well as on the real robot. Our solution to the IK is also able to generate valid ranges of these redundancies depending on the given 6D goal pose.

Joint Values vs Redundancy Parameter P1 For a Certain Configuration 3

1

Joint Values

[3] Khatib, O.; Brock O.: Elastic strips: A framework for motion generation in human environments, The International Journal of Robotics Research, Vol. 21, No 12, 2002.

x vs p1 in meters y vs p1 in meters theta vs p1 in radians joint1 vs p1 in radians joint2 vs p1 in radians joint3 vs p1 in radians joint4 vs p1 in radians joint5 vs p1 in radians

2

(a)

0

[4] Brock, O.; Kemp, C.: Guest editorial: special issue on autonomous mobile manipulation, Autonomous Robots, vol. 28, 1: pp.1-3, 2010

-1

-2

-3

-3

-2

-1

0

1

2

3

P1 Joint Values vs Redundancy Parameter P2 For a Certain Configuration 2

1

Joint Values

[5] Zacharias, F.; Borst, C.; Beetz, M.; Hirzinger, G.: Positioning mobile manipulators to perform constrained linear trajectories, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems: pp.2578-2584, September 22-26, 2008, Acropolis Convention Center, Nice, France

x vs p2 in meters y vs p2 in meters theta vs p2 in radians joint1 vs p2 in radians joint2 vs p2 in radians joint3 vs p2 in radians joint4 vs p2 in radians joint5 vs p2 in radians

1.5

(b)

0.5

0

-0.5

-1

0.2

0.25

0.3

0.35

0.4

0.45

P2

Figure 9: Change in joint values when modifying individual redundancies: (a) for ρ1 , (b) for ρ2

5

Conclusion

Planning and control for a unified mobile manipulator system comprising of a mobile base and an arm is a challenging topic, computing an analytical closed form IK for such redundant system was an unsolved problem. We reviewed current state of the art approaches in mobile manipulation and pointed out their disadvantages. Our approach overcomes these deficits by considering a unified serial kinematic chain for the KUKA youBot. We propose redundancy parameters that enable us to develop an analytical closed form IK solution for the KUKA youBot. Using these redundancies one can extract base positions from the computed IK solution. Our IK solver can vary these redundancy parameters to compute and iterate over all possible solutions in the null space of the mobile manipulator. This will enable us to use the IKBiRRT approach for planning mobile manipulation tasks more deliberatively and efficiently. Acknowledgement The EC, funded parts of the developments through FP7 - BRICS (ICT-231940)

References [1] Pin, F.G.; Culioli, J.C: Optimal Positioning of Combined Mobile Platform-Manipulator Systems for Material Handling Tasks, Journal of Intelligent & Robotic Systems, vol. 6, Numbers 2-3: pp. 165-182, DOI: 10.1007/BF00248014, The Netherlands, 1992 [2] Bischoff, R.; Prassler, E.; Huggenberger, U.: KUKA youBot - A Mobile Manipulator for Research and Education, Proc. of the IEEE International Conference on Robotic and Automation (ICRA-2011), IEEE Press, Shanghai, China, 2011

144

[6] Jain, A.; Kemp, C.: Pulling open doors and drawers: Coordinating an omni-directional base and a compliant arm with Equilibrium Point control, IEEE International Conference on Robotics and Automation, 3-7 May :pp.1807-1814, Anchorage, AK, 2010 [7] Yamamoto, Y.; Yun, X.: Unified Analysis on Mobility and Manipulability of Mobile Manipulators, Proc. of the IEEE International Conference of Robotics and Automation (ICRA-1999), IEEE Press, 1999. vol. 2: pp. 1200-1206, Detroit, MI, USA [8] Berenson, D.; Srinivasa, S.; Ferguson, D.; Romea, A.; Kuffner, J.: Manipulation planning with workspace goal regions, Proc. of the IEEE International Conference of Robotics and Automation (ICRA-2009), IEEE Press, 1999. [9] Khatib, O.; Siciliano, B., editors: Springer Handbook of Robotics, Springer, Berlin, Heidelberg, Germany, 2008. [10] Pieper, D.: The kinematics of manipulators under computer control, Technical Report, Ph.D. Dissertation, Stanford University, Dept. Mech. Engg (TR AIM-72,101), 1968. [11] Craig, J.: Introduction to Robotics Mechanics and Control, Third edition, Chapter 3, Manipulator Kinematics, Pearson Prentice Hall, New Jersey, USA, 2005. [12] Sharma, S.; Scheurer, C.; Bischoff, R.; Kraetzschmar, G.: Unified Inverse Kinematics Approach with the KUKA youBot for Mobile Manipulation Technical Report: University of Applied Sciences, Bonn Rhein Sieg, Sankt Augstin, Germany, 2012 [13] Diankov, R.; et al.: BiSpace Planning: Concurrent Multi-Space Exploration, Proceedings of Robotics: Science and Systems IV, Zurich, Switzerland, 2008 [14] Baker, D.; Wampler, C.: On the Inverse Kinematics of Redundant Manipulators, International Journal of Robotics Research, vol.7, No.2, 1988