SAMI 2016 • IEEE 14th International Symposium on Applied Machine Intelligence and Informatics • January 21-23, 2016 • Herl’any, Slovakia
Solution of the closed-loop inverse kinematics algorithm using the Crank-Nicolson method Dániel András Drexler∗ ∗ Budapest
University of Technology and Economics, Department of Control Engineering and Information Technology Magyar tudósok krt 2. H-1117 Budapest, Hungary Email:
[email protected]
Abstract—The closed-loop inverse kinematics algorithm is a numerical method used to approximate the solution of the inverse kinematics problem of robot manipulators based on the explicit Euler integration, that is a simple numerical integration technique. It is known from the theory of numerical techniques that the Crank-Nicolson method gives results with better convergence properties than the explicit Euler method, however it needs the application of the implicit Euler method, that needs the solution of the differential inverse kinematics problem, which is not known in advance in the robotics problem. An iterative method is proposed to calculate the implicit Euler solution, and the results are used to carry out the numerical integration using the Crank-Nicolson method. Experimental results show that the application of the Crank-Nicolson method yields better convergence for the differential inverse positioning problem of planar nonredundant and redundant manipulators than the application of the explicit Euler method. Both the case of reaching a single end effector point and path tracking are analyzed and the results are compared to the results acquired using the explicit and implicit Euler methods separately; the convergence is faster if the goal is to reach a desired end effector configuration, while the tracking error is smaller in the case of path tracking if the numerical integration is done in the proposed way. Index Terms—differential inverse kinematics, numerical integration, explicit Euler, implicit Euler
I. I NTRODUCTION The differential inverse kinematics algorithm is a numerical technique used to solve the inverse kinematics problem of robot manipulators (i.e. finding the required joint path of the robot for the desired end effector path of the robot [1], [2], [3]). The algorithm is based on solving simultaneously a system of linear equations and a system of differential equations [2], [4], [5]. The system of equations is being solved by using the inverse (or pseudoinverse for redundant arms) of the task Jacobian of the manipulator, resulting in the local change of joint variables (the joint velocities), and the system of differential equations is solved using explicit Euler integration (i.e. adding the joint velocities multiplied by the sampling time to the joint variables). The closed-loop inverse kinematics (CLIK) algorithm [6], [7] modifies the desired end effector velocity by adding an error term that is derived from the difference of the desired and the actual end effector pose (after finding a suitable representation for rotations if the orientation of the robot is also considered) multiplied by a gain factor with appropriate dimensions. Thus the error term is added to the desired velocity after multiplication by a gain factor, and this task velocity
978-1-4673-8740-8/16/$31.00 ©2016 IEEE
– 351 –
is mapped to joint velocities using the inverse of the task Jacobian, as being discussed in Section II. The convergence of tracking can be increased by increasing this gain factor, however this gain parameter can not be increased arbitrarily due to stability issues [6]. Note that by the convergence of the tracking we mean the convergence of the solution of the CLIK algorithm to the exact solution of the inverse kinematics problem, i.e. the joint variables that result exactly in the desired end effector position, orientation or both (depending on the task). The convergence of tracking may be increased further without modifying the gain factor using a numerical integration scheme different than the explicit Euler method. The application of the Crank-Nicolson method [8] (discussed in Section III) is investigated in order to gain better convergence properties. However, the application of the Crank-Nicolson method requires the joint velocities acquired using the implicit Euler method, that needs the solution of the (differential) inverse kinematics problem, that is not known in advance. In order to acquire the implicit solution, an iterative scheme is proposed to calculate the implicit Euler solution of the differential equation in Section IV that converges to the implicit Euler solution. This approximation is used to carry out the numerical integration using the Crank-Nicolson method. The proposed technique is used to solve the inverse positioning problem of planar arms in Section V. The planar inverse positioning examples are taken only because they are simple and they can be used effectively to demonstrate the results. Note that this does not mean that the proposed method is limited to the inverse positioning problem of planar manipulators only, the method is given for general task Jacobian and task velocity in Sections II and IV. In the experiments, two problems are investigated: in the first case the end effector of the robot has to reach a desired end effector position, in the second case, the end effector of the robot has to track a linear path. Both cases are investigated on a nonredundant and a redundant arm. The proposed technique is compared to the application of the explicit and implicit Euler techniques separately in the experiments in Section V. According to the experiments, the application of the CrankNicolson method has better convergence properties in all the cases than the explicit and implicit Euler methods as discussed in Section VI.
D. A. Drexler • Solution of the Closed-Loop Inverse Kinematics Algorithm Using the Crank-Nicolson Method
II. C LOSED - LOOP INVERSE KINEMATICS ALGORITHM Suppose that we have a manipulator with n joints, also called an n degrees of freedom (n-dof) manipulator. The joint variables (joint angles for rotational and displacements for prismatic joints) are stored in the n-dimensional joint variable ⊤ vector denoted by θ = (θ1 , θ2 , . . . , θn ) . Let the position and orientation of the end effector of the manipulator be described by the transformation g(θ), also called the end effector pose. The analytical Jacobian of the manipulator is the Jacobian of g(θ), i.e. J a = ∂g(θ)/(∂θ). The ith column of the analytical Jacobian describes the partial velocities caused by the motion of the ith joint. The first three rows of the Jacobian contain the linear velocity generators, and the last three rows contain the angular velocity generators. The analytical Jacobian contains the linear and angular velocity generators for the general spatial case. However, in practice, it may happen that only the positioning problem is addressed, or only the orientation is examined. It may also happen, that we are interested only in planar motion. The problem we address thus defines the task space. If the task space is known, we can project the Jacobian to the task space to get the task Jacobian, and the desired velocities and tracking errors to get the task vector. The inverse kinematics scheme is then applied using the task Jacobian and the task vector. For example, in [9], [10] the spatial positioning problem is addressed, so the task Jacobian is the first three rows of the analytical Jacobian, and the task vector is the linear combination of the desired linear velocity vector and the positioning error, while in [11] the spatial orientation is considered, so the task Jacobian is the last three rows of the analytical Jacobian, and the task vector is the linear combination of the desired angular velocity and the orientation error given in a vectorangle representation. Let J(θ[k]) be the task Jacobian in the joint configuration θ[k] defined in the kth step (the kTs time instant, with Ts being the sampling time). Let ∆td [k] be the desired task space velocity in the kth step, and e[θ[k], k] be the task space error in the kth step. The task space velocity in the kth step that needs to be mapped into joint motion is t[θ[k], k] = ∆td [k] + αe[θ[k], k].
(1)
The CLIK algorithm maps this vector to the joint velocity vector using the pseudoinverse of the task Jacobian, and calculates the joint variables in the following time step using explicit Euler integration as #
θ[k + 1] = θ[k] + Ts J (θ[k])t[θ[k], k],
(2)
where J # denotes the Moore-Penrose pseudoinverse of the Jacobian that is required instead of the simple matrix inverse if the manipulator is redundant (i.e. it has more degrees of freedom than the dimension of the task space, thus the task Jacobian is not square). III. N UMERICAL INTEGRATION SCHEMES The Crank-Nicolson method was originally proposed to solve heat-conduction type partial differential equations in [8].
– 352 –
In the Crank-Nicolson method, integration with respect to time is done by calculating the average of the explicit Euler and implicit Euler results. Let the value of the unknown function be denoted by f [k] in the kth step (time instant kTs ), and the difference of the function in the kth step by ∆f [k]. If f [k] and ∆f [k] is known, then the value of the function in the (k + 1)th step can be approximated as f [k + 1] = f [k] + Ts ∆f [k],
(3)
that is called the explicit Euler (EE) method. The implicit Euler (IE) method determines the function value at the (k +1)th step as f [k + 1] = f [k] + Ts ∆f [k + 1] (4) that demands the knowledge of ∆f in the (k + 1)th step, thus this is an implicit method. The Crank-Nicolson (CN) method is the average of (3) and (4), i.e. f [k + 1] = f [k] + Ts (0.5∆f [k] + 0.5∆f [k + 1]) .
(5)
When this method is applied to heat-conduction type partial differential equations, the method is numerically stable and it has second-order convergence in time. IV. C RANK -N ICOLSON METHOD APPLIED IN THE CLIK SCHEME
In the robotics problem, the function in the kth step is the θ[k] joint variable, and the difference ∆θ[k] is calculated using the task vector t[k] and the task Jacobian J(θ[k]). The difference ∆θ[k+1] can only be calculated if θ[k+1] is known in advance, however it is not the case in practice, because the goal of the CLIK algorithm is to calculate θ[k + 1]. An iteration is proposed in order to calculate the difference ∆θ[k + 1] used in the implicit Euler scheme. This difference will be denoted by ∆θIE [k] := ∆θ[k + 1]. First, the difference is initialized using the explicit Euler solution as ∆θ(0) [k] = Ts J # (θ[k])t[θ[k], k] = ∆θEE [k].
(6)
After the initialization, the joint variables in the (k + 1)th step are calculated using the explicit Euler method method, and the difference is recalculated as ˜ + 1] θ[k
=
∆θ(i+1) [k]
=
θ[k] + Ts ∆θ(i) [k] (7) # ˜ ˜ Ts J (θ[k + 1]) t[θ[k + 1], k + 1]. (8)
Note that in (7) the joint variable vector θ[k] is unchanged ˜ + 1] is throughout the iteration, since the approximation θ[k recalculated in every iteration with the newest estimation of the difference. If this iteration converges, i.e. lim ∆θ(i+1) [k] − ∆θ(i) [k] = 0 (9) i→∞
then it has a fixed-point, that is the difference in the implicit Euler solution, i.e. lim ∆θ(i) [k] = ∆θIE [k].
i→∞
(10)
SAMI 2016 • IEEE 14th International Symposium on Applied Machine Intelligence and Informatics • January 21-23, 2016 • Herl’any, Slovakia
∆θCN [k] = 0.5∆θEE [k] + 0.5∆θIE [k].
0
1
−0.4 −0.6
(11)
−0.8
and the solution of the differential inverse kinematics problem at the (k + 1)th step is [k].
(12)
0.5
1
1.5 Time [sec]
2
2.5
3
EE IE CN
−1.6 θ
V. E XPERIMENTAL RESULTS
0
−1.4
2
θ[k + 1] = θ[k] + Ts ∆θ
CN
EE IE CN
−0.2 θ
Experiments showed that this iteration converges, and the convergence is fast, i.e. 4-5 iterations is enough to reach the fixed-point. If the implicit Euler solution is known, then the Crank-Nicolson solution can be calculated as
−1.8 −2 −2.2
0.5
1
1.5 Time [sec]
2
2.5
0 −0.1
0
0.5
1
1.5 Time [sec]
2
2.5
The resulting joint paths after the run of the CLIK algorithm with the three different methods are in Figure 1. The implicit Euler solution had the slowest, and the Crank-Nicolson solution had the fastest convergence. The end effector error (the x and y projections of the error, and the error norm) is in Figure 2. The joint velocities are in Figure 3.
– 353 –
3
y−error
0 EE IE CN
−0.5 −1
0
0.5
1
1.5 Time [sec]
2
2.5
3
error norm
1 EE IE CN
0.5 0
0
0.5
1
1.5 Time [sec]
2
2.5
3
Fig. 2. The error of the end effector position of the 2-dof arm reaching the pd desired point
0 EE IE CN
⊤
∆θ
1
−0.5
−1
−1.5
0
0.5
1
1.5 Time [sec]
2
2.5
3
2
0 ∆ θ2
Let the initial joint configuration of the arm be θ = (0, π/2) . The problem we consider is the inverse positioning problem of the planar arm. Since the arm moves in the x − y plane, the desired position and velocity will be defined by two coordinates only, and the task Jacobian is the first two rows of the analytical Jacobian. 1) Case I, fix point: In the first case, the task is to reach the point ⊤ pd = 1 0 . (14)
3
EE IE CN
0.1
A. Inverse positioning problem of 2-dof planar manipulator Let the joint axes of the planar arm point at the z-axis of the ⊤ fixed spatial coordinate frame, i.e. ω1 = ω2 = (0, 0, 1) . Let two arbitrarily chosen points on the two joint axes be q1 = ⊤ ⊤ (0, 0, 0) , q2 = (0, l1 , 0) given in the home configuration (i.e. when θ1 = θ2 = 0), and the end effector position in the ⊤ home configuration be p(0) = (0, l1 + l2 , 0) . Here, the l1 and l2 parameters are the lengths of the links of the manipulator, in the examples l1 = l2 = 1. Using these descriptions, one can create the kinematic model of the robot using the techniques e.g. from [1], [12], [13], [11]. The initial end effector pose in homogeneous coordinates is I p(0) g(0) = . (13) 0 1
0
Fig. 1. The joint paths of the 2-dof planar arm reaching the pd desired point
x−error
The proposed algorithm is tested on the inverse positioning problem of a nonredundant and a redundant planar manipulator. The solutions using the explicit Euler (EE), implicit Euler (IE) and Crank-Nicolson (CN) methods are generated and compared. The sampling time during the experiments was Ts = 0.1 sec, the number of iterations of the CLIK scheme was N = 30, the number of iterations to find the implicit solution was M = 5, and the gain parameter used in the CLIK algorithm was α = 5.
EE IE CN
−2
−4
Fig. 3. point
0
0.5
1
1.5 Time [sec]
2
2.5
3
The joint velocity paths of the 2-dof arm reaching the pd desired
D. A. Drexler • Solution of the Closed-Loop Inverse Kinematics Algorithm Using the Crank-Nicolson Method
0.4
1.5
0.3
θ1
∆ θ1
1 EE IE CN
0.5
0
0
0.5
1
1.5 Time [sec]
2
2.5
0
3
1
1.5 Time [sec]
2
2.5
3
2
∆θ
θ2
0.5
−0.1
−1.8 −2 −2.2
0
0.5
1
1.5 Time [sec]
2
2.5
−0.4
3
EE IE CN
0 −0.1 −0.2
0
0.5
1
1.5 Time [sec]
2
2.5
3
0
0.5
1
1.5 Time [sec]
2
2.5
3
1.5 Time [sec]
2
0.01
EE IE CN
0 −0.01
EE IE CN
0.04 0.02 0
−0.2
EE IE CN
−0.3
Fig. 4. The joint paths of the 2-dof arm tracking the desired path (pd [k], ∆pd [k]), k = 1, 2, . . . , N.
x−error
0
0 EE IE CN
−1.6
y−error
EE IE CN
0.1
−1.4
error norm
0.2
0
0.5
1
2.5
3
Fig. 5. The tracking error of the 2-dof arm tracking the desired path (pd [k], ∆pd [k]), k = 1, 2, . . . , N.
2) Case II, path tracking: The task is to track the path ⊤ ⊤ 1 1 pd [k] = − k/N 0 (15) ⊤ −1 0 ∆p[k] = (16)
with k = 1, 2, . . . , N . The resulting joint paths after the run of the CLIK algorithm with the three different methods are in Figure 4. The tracking error (the x and y projections of the error, and the error norm) is in Figure 5. The joint velocities are in Figure 6. Figure 5 shows that the tracking error was significantly lower with the CN method. B. Inverse positioning problem of redundant 5-dof planar manipulator Let the joint axes of the planar arm point at the zaxis of the fixed spatial coordinate frame, i.e. ωi = ⊤ (0, 0, 1) with i = 1, 2, . . . , 5. Let the five arbitrarily chosen
– 354 –
0
0.5
1
1.5 Time [sec]
2
2.5
3
Fig. 6. The joint velocity paths of the 2-dof arm tracking the desired path (pd [k], ∆pd [k]), k = 1, 2, . . . , N.
⊤
points on the five joint axes be q1 = (0, 0, 0) , q2 = ⊤ ⊤ ⊤ (0, l1 , 0) , q3 = (0, l1 + l2 , 0) , q4 = (0, l1 + l2 + l3 , 0) , ⊤ q5 = (0, l1 + l2 + l3 + l4 , 0) given in the home configuration (i.e. when θi = 0, i = 1, 2, . . . , 5), and the end effector position in the home configuration be p(0) = ⊤ (0, l1 + l2 + l3 + l4 + l5 , 0) . Here, the li = 1 parameter values are used with i = 1, 2, . . . , 5. Using these descriptions, one can create the kinematic model of the robot. The end effector pose in the home configuration is defined as in (13). The initial ⊤ configuration of the robot is θ = (0, π/2, 0, 0, 0) . Since the task is inverse positioning in the plane, the desired end effector position and velocity is described by two coordinates only, and the task Jacobian is the first two rows of the analytical Jacobian. 1) Case I, fix point: The task is to reach the point ⊤ pd = 3 1 . (17) The resulting joint paths after the run of the CLIK algorithm with the three different methods are in Figure 7. The implicit Euler solution had the slowest, and the Crank-Nicolson solution had the fastest convergence in this case as well. The end effector error (the x and y projections of the error, and the error norm) is in Figure 8. The joint velocities are in Figure 9. 2) Case II, path tracking: The task is to track the path ⊤ ⊤ 4 1 pd [k] = − k/N 0 (18) ⊤ −1 0 (19) ∆p[k] =
with k = 1, 2, . . . , N . The resulting joint paths after the run of the CLIK algorithm with the three different methods are in Figure 10. The tracking error (the x and y projections of the error, and the error norm) is in Figure 11. The joint velocities are in Figure 12. Figure 11 shows that the tracking error was significantly lower with the CN method in the redundant case as well.
SAMI 2016 • IEEE 14th International Symposium on Applied Machine Intelligence and Informatics • January 21-23, 2016 • Herl’any, Slovakia
0
1.5 1
0
0.5
1
1.5 Time [sec]
2
2.5
x−error
θ1
EE IE CN
0.5 0
3
−0.4 −0.6 −0.8 −1
EE IE CN
−1.5
0
0.5
1
1.5 Time [sec]
2
2.5
3
θ3
EE IE CN
1
1.5 Time [sec]
2
2.5
−0.2
0
0.5
1
1.5 Time [sec]
2
2.5
EE IE CN
0
−0.1
3
0
0.5
1
1.5 Time [sec]
2
2.5
3
1 EE IE CN
0.8 error norm
EE IE CN
−0.2
3
−0.05
0
θ4
0.5
0.05
0
−0.4
0
0.1
y−error
θ2
−1
−2
EE IE CN
−0.2
0.6 0.4 0.2
−0.4
0
0.5
1
1.5 Time [sec]
2
2.5
3
0
0
0.5
1
θ5
0
2
2.5
3
EE IE CN
−0.2
−0.4
1.5 Time [sec]
0
0.5
1
1.5 Time [sec]
2
2.5
Fig. 8. The error of the end effector position of the 5-dof arm reaching the pd desired point
3
6 ∆ θ1
Fig. 7. The joint paths of the 5-dof planar arm reaching the pd desired point
EE IE CN
4 2 0
VI. C ONCLUSION
0
0.5
1
1.5 Time [sec]
2
2.5
3
−1 0.5
1
1.5 Time [sec]
2
2.5
3
∆ θ3
EE IE CN
−0.5 −1 −1.5
0
0.5
1
1.5 Time [sec]
2
2.5
3
0 EE IE CN
−0.5 −1 −1.5
– 355 –
0
0
R EFERENCES
0
0.5
1
1.5 Time [sec]
2
2.5
3
0 ∆ θ5
[1] R. M. Murray, S. S. Sastry, and Z. Li, A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [2] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics - Modelling, Planning and Control. Springer, 2009. [3] J. Selig, Geometric Fundamentals of Robotics (Second Edition). Springer, 2005. [4] Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for robot manipulator control,” Journal of Dynamic Systems, Measurement, and Control, vol. 108, no. 3, pp. 163–171, 1986. [5] I. Charles W. Wampler, “Manipulator inverse kinematics solutions based on vector formulations and damped least-squares methods,” IEEE Transactions on Systems, Man, and Cybernetics, vol. SMC-16, no. 1, January/February 1986.
EE IE CN
−0.5
−1.5
∆ θ4
Experimental results showed that the numerical solution of the inverse kinematics problem converges to the real solution faster if the Crank-Nicolson method is applied instead of the explicit Euler method, thus this method can be successfully applied in robotics. The main drawback of the Crank-Nicolson method is that it needs an implicit solution that requires the knowledge of the joint variables in the future, that is not available. An iterative approach was proposed to approximate the implicit solution without the future joint variable values. The experiments showed that this iterative approach converges, and can be applied in practical cases.
∆ θ2
0
EE IE CN
−0.5 −1 −1.5
Fig. 9. point
0
0.5
1
1.5 Time [sec]
2
2.5
3
The joint velocity paths of the 5-dof arm reaching the pd desired
D. A. Drexler • Solution of the Closed-Loop Inverse Kinematics Algorithm Using the Crank-Nicolson Method
∆ θ1
1 θ1
EE IE CN
0.5 0
EE IE CN
1
1.5
0
0.5
1
1.5 Time [sec]
2
2.5
3
0.5
0
0
0.5
1
EE IE CN
−1.5
1.5 Time [sec]
2
2.5
3 EE IE CN
0
∆θ
2
θ
2
−1
−2
0
0.5
1
1.5 Time [sec]
2
2.5
0.5
1
1.5 Time [sec]
2
2.5
0.5
1
1.5 Time [sec]
2
2.5
3 EE IE CN
−0.1
−0.2
0
0.5
1
−0.2
1.5 Time [sec]
2
1.5 Time [sec]
2
2.5
1
1.5 Time [sec]
2
2.5
3 4
0.5
EE IE CN
5
0
θ
∆θ
0
−0.1
−0.2
−0.2
0
0.5
1
1.5 Time [sec]
2
2.5
0
0.5
1
2.5
0
3
3 EE IE CN
∆θ
5
−0.4
3 EE IE CN
0 −0.4
3 EE IE CN
3
0
∆θ
3
θ
4
0
0
−0.2
0
θ
−0.2
EE IE CN
0
−0.4
−0.1
3
−0.1
−0.2
Fig. 10. The joint paths of the 5-dof arm tracking the desired path (pd [k], ∆pd [k]), k = 1, 2, . . . , N.
0.5
1
1.5 Time [sec]
2
2.5
3
Fig. 12. The joint velocity paths of the 5-dof arm tracking the desired path (pd [k], ∆pd [k]), k = 1, 2, . . . , N.
EE IE CN
0
0
x−error
−0.05 −0.1 −0.15 −0.2
0
0.5
1
1.5 Time [sec]
2
2.5
3
−3
5
x 10
y−error
0 EE IE CN
−5 −10 −15
0
0.5
1
1.5 Time [sec]
2
2.5
3
0.025
error norm
0.02 0.015 EE IE CN
0.01 0.005 0
0
0.5
1
1.5 Time [sec]
2
2.5
3
Fig. 11. The tracking error of the 5-dof arm tracking the desired path (pd [k], ∆pd [k]), k = 1, 2, . . . , N.
– 356 –
[6] P. Falco and C. Natale, “On the stability of closed-loop inverse kinematics algorithms for redundant robots,” IEEE Transactions on Robotics, vol. 27, pp. 780 –784, 2011. [7] L. Sciavacco and B. Siciliano, “A solution algorithm to the inverse kinematic problem for redundant manipulators,” IEEE Transactions on Robotics and Automation, vol. 4, no. 4, pp. 403–410, 1988. [8] J. Crank and P. Nicolson, “A practical method for numerical evaluation of solutions of partial differential equations of the heat-conduction type,” Advances in Computational Mathematics, vol. 6, no. 1, pp. 207–226, 1996. [Online]. Available: http://dx.doi.org/10.1007/BF02127704 [9] D. A. Drexler and I. Harmati, “Regularization of the inverse positioning problem of revolute joint manipulators,” in Proceedings of the Workshop on the Advances of Information Technology: WAIT 2015, May 2015, pp. 83 – 95. [10] D. A. Drexler, “Regularization of the differential inverse positioning problem of an elbow manipulator,” in Proceedings of the IEEE International Symposium on Intelligent Systems and Informatics, Subotica, Serbia, September 2015. [11] D. A. Drexler and I. Harmati, “Regularization of the differential inverse orientation problem of serial revolute joint manipulators,” in Proceedings of the IEEE International Symposium on Applied Computational Intelligence and Informatics, Timisoara, Romania, May 2014. [12] ——, “Joint constrained differential inverse kinematics algorithm for serial manipulators,” Periodica Polytechnica: Electrical Engineering, vol. 66, no. 4, 2012. [13] ——, “Regularized Jacobian for the differential inverse positioning problem of serial revolute joint manipulators,” in Proceedings of the IEEE International Symposium on Intelligent Systems and Informatics, Subotica, Serbia, September 2013.
