This paper discusses a method for solving the inverse kinematics problem and the optimal path planning problem for a robot manipulator. A manipulator is a robot with links corresponding to human arms and joints corresponding to human joints, and the tip is called the end-effector. The inverse kinematics problem for manipulators is to find the angle of each joint, given the position and orientation of the end-effector. The path planning problem is to find a path to move the end-effector between two specified positions [1].
When operating the manipulator, one needs to solve the inverse kinematics problem (or the path planning problem, respectively) for the desired end-effector position (or the series of positions, respectively).
Methods of solving inverse kinematics problems for manipulators by reducing the inverse kinematics problem to a system of polynomial equations and using the Gröbner basis has been proposed [2,3,4,5,6]. Solving the inverse kinematics problem using the Gröbner basis computation has an advantage that the global solution of the inverse kinematics problem can be obtained before the end-effector will actually be "moved" by simulation or other means. On the other hand, the Gröbner basis computation has the disadvantage of relatively high computational cost compared to local solution methods such as the Newton method. Furthermore, when solving a path planning problem using the Gröbner basis computation, it is necessary to solve the inverse kinematics problem for each point on the path, which is even more computationally expensive.
The third and fourth authors have also previously proposed methods for solving inverse kinematics problems using Gröbner basis computations [7,8,9]. The authors' contributions in their previous work [9] are as follows. We have proposed a method for solving the inverse kinematics problem and the trajectory planning problem of a 3 Degree-Of-Freedom (DOF) manipulator using the Comprehensive Gröbner System (CGS) [10]. In the proposed method, the inverse kinematic problem has been expressed as a system of polynomial equations with the coordinates of the end-effector as parameters and the CGS is calculated in advance to reduce the cost of Gröbner basis computation for each point on the path. In addition, we have proposed a method for solving the inverse kinematics problems using the CGS-QE method [11], which is a quantifier elimination (QE) method based on CGS computations, to certify the existence of a (real) solution. Furthermore, we have proposed a method to certify the existence of a solution to the whole trajectory planning problem using the CGS-QE method, where the points on the trajectory are represented by parameters.
This paper proposes the following method as an extension of the previous work [9].
1. Extension of paths used in path planning problems: while the previous work has used straight lines, this paper uses smooth curves generated by the cubic spline interpolation passing through given points. Using a curved path allows us to plan paths that avoid obstacles, for example. 2. Optimization of the joint configuration obtained as the solution to the path planning problem: when solving the path planning problem, there can be multiple solutions to the inverse kinematics problem at each point on the path. In this case, the question is which of the solutions for adjacent points can be connected to minimize the sum of the configurations of the entire sequence of the joints. In this paper, we reduce this problem to the shortest path problem of a weighted graph and propose a method to compute the optimal sequence of joint configurations using shortest path algorithms.
The manipulator used in this paper is myCobot 280 [12] from Elephant Robotics, Inc. (hereafter referred to simply as "myCobot"). Although myCobot is a 6-DOF manipulator, we treat it as a 3-DOF manipulator by operating only the three main joints used to move the end-effector while the remaining joints are fixed, due to the computational costs for solving the inverse kinematic problem by using the CGS and the CGS-QE method.
Figure 1 shows the components and the coordinate systems of myCobot. The forward kinematics problem for myCobot is derived using the modified Denavit-Hatenberg convention (hereafter abbreviated as "D-H convention"), which is standard in robotics [1]. Let be the Cartesian coordinate system with the origin at the manipulator's base, and let (𝑥, 𝑦, 𝑧) be the coordinates of the end-effector in . Let 𝑖 be the coordinate system with the origin at the joint 𝑖. Note that Joint 0 represents the base and Joint 8 represents the end-effector.
Let 𝜃 1 , 𝜃 3 , 𝜃 4 be the configuration of the joints 1, 3, 4, respectively, to be operated in this paper, and let 𝑠 𝑖 = sin 𝜃 𝑖 , 𝑐 𝑖 = cos 𝜃 𝑖 for 𝑖 = 1, 3, 4. In the following, for a set of polynomials
The system of polynomial equations 𝑓 1 = ⋯ = 𝑓 𝑚 = 0 is denoted by 𝐹 = 0. In this case, the inverse kinematics problem is to find the solution 𝑐 1 , 𝑠 1 , 𝑐 3 , 𝑠 3 , 𝑐 4 , 𝑠 4 to the system of polynomial equations 𝐹 = 0 with 𝐹 = {𝑓 1 , … , 𝑓 6 }, where
Note that the system of equations 𝐹 = 0 has parameters 𝑥, 𝑦, 𝑧 in the coefficients. With our previously proposed method [9], before solving 𝐹 = 0, we calculate the CGS of ⟨𝑓 1 , … , 𝑓 6 ⟩. Then, for a given end-effector coordinate (𝑥, 𝑦, 𝑧) = (𝛼, 𝛽, 𝛾 ) ∈ ℝ 3 , determine the feasibility of such a configuration using the CGS-QE method. If the configuration is feasible, we solve 𝐹 = 0 numerically after substituting parameters 𝑥, 𝑦, 𝑧 with 𝛼, 𝛽, 𝛾, respectively, to obtain the inverse kinematic solution.
In the path planning problem, a finite number of points through which the path passes are given in advance, and a trajectory is generated on the smooth path passing through these points. In our previous study, a straight line was used as the path, but in this paper, a path is generated using cubic spline interpolation [13].
Let (𝑥 0 , 𝑦 0 , 𝑧 0 ), (𝑥 1 , 𝑦 1 , 𝑧 1 ), (𝑥 2 , 𝑦 2 , 𝑧 2 ), (𝑥 3 , 𝑦 3 , 𝑧 3 ) be four different points given in ℝ 3 . For 𝑗 = 0, 1, 2, calculate a curve 𝐶 𝑗 passing through (𝑥 𝑗 , 𝑦 𝑗 , 𝑧 𝑗 ) and (𝑥 𝑗+1 , 𝑦 𝑗+1 , 𝑧 𝑗+1 ) in the form of a function (𝑋 𝑗 (𝑠), 𝑌 𝑗 (𝑠), 𝑍 𝑗 (𝑠)) with 𝑠 ∈ [𝑗, 𝑗 + 1] as a parameter. In cubic spline interpolation, 𝑋 𝑗 (𝑠), 𝑌 𝑗 (𝑠), 𝑍 𝑗 (𝑠) are cubic polynomials, respectively, satisfying the following conditions.
(𝑋 𝑗 (𝑗), 𝑌 𝑗 (𝑗), 𝑍 𝑗 (𝑗)) = (𝑥 𝑗 , 𝑦 𝑗 , 𝑧 𝑗 ), 𝑗 = 0, 1, 2, (𝑋 𝑗 (𝑗 + 1), 𝑌 𝑗 (𝑗 + 1), 𝑍 𝑗 (𝑗 + 1)) = (𝑥 𝑗+1 , 𝑦 𝑗+1 , 𝑧 𝑗+1 ), 𝑗 = 0, 1, 2, (𝑋 ′ 𝑗 (𝑗 + 1), 𝑌 ′ 𝑗 (𝑗 + 1), 𝑍 ′ 𝑗 (𝑗 + 1)) = (𝑋 ′ 𝑗+1 (𝑗 + 1), 𝑌 ′ 𝑗+1 (𝑗 + 1), 𝑍 ′ 𝑗+1 (𝑗 + 1)), 𝑗 = 0, 1, (𝑋 ″ 𝑗 (𝑗 + 1), 𝑌 ″ 𝑗 (𝑗 + 1), 𝑍 ″ 𝑗 (𝑗 + 1)) = (𝑋 ″ 𝑗+1 (𝑗 + 1), 𝑌 ″ 𝑗+1 (𝑗 + 1), 𝑍 ″ 𝑗+1 (𝑗 + 1)), 𝑗 = 0, 1.
(2) In this paper, in addition to the conditions in eq. ( 2), we find the natural cubic Spline interpolation that satisfies
For the trajectory 𝐶 obtained in the previous section, we solve the inverse kinematics problem using the following procedure. First, using the CGS-QE method, we determine the existence of solutions to the inverse kinematics problem for each curve 𝐶 𝑗 (𝑗 = 0, 1, 2) that constitutes the path to move the end-effector. In eq. ( 1), replace 𝑥, 𝑦, 𝑧 in each equation by 𝑋 𝑗 (𝑠), 𝑌 𝑗 (𝑠), 𝑍 𝑗 (𝑠), respectively, to obtain a system of polynomial equations with 𝑠 as parameter. Let 𝐹 ′ 𝑗 (𝑠) be the result. Next, we apply the CGS-QE method to 𝐹 ′ 𝑗 (𝑠) to determine whether the system of equations 𝐹 ′ 𝑗 (𝑠) = 0 has real solutions within the parameter 𝑠 ∈ [𝑗, 𝑗 + 1]. If 𝐹 ′ 𝑗 (𝑠) = 0 has real solutions within the parameter 𝑠 ∈ [𝑗, 𝑗 + 1] (i.e., within 𝑠 ∈ [0, 3] throughout for 𝑠), we calculate the trajectory of the end-effector's position for time 𝑡. Let 𝑇 = 3𝑢 (where 𝑢 is a positive integer) and, for time 𝑡 = 0, … , 𝑇, let 𝑠 = 𝑓 (𝑡) where 𝑓 is a continuous function of [0, 𝑇 ] → [0, 3]. To ensure that the end-effector has no velocity and acceleration at the beginning and end of the trajectory, we obtain 𝑓 as a polynomial of the smallest degree possible to satisfy 𝑓 ′ (𝑡) = 𝑓 ″ (𝑡) = 0 at 𝑡 = 0 and 𝑇. Then, we obtain 𝑓 (𝑡) = 3 (
Finally, we solve the inverse kinematics problem 𝑡 = 0, … , 𝑇. For 𝑗 = 0, 1, 2 and 𝑡 = 𝑗𝑢, … , (𝑗 + 1)𝑢, the configuration of the joints is obtained by solving the system of polynomial equations
Assuming that the time required to solve the inverse kinematics problem (4) at each value of 𝑡 is constant, then the computation time for trajectory planning is expected to be proportional to 𝑇.
We have implemented the above method and conducted experiments. The implementation of the inverse kinematics computation was performed as follows: the CGS computation was performed using an algorithm by Kapur et al. [15] with the implementation by Nabeshima [16] on the computer algebra system Risa/Asir [17]. The existence of the root of the inverse kinematics problem by the CGS-QE method was verified using Risa/Asir with accompanying Wolfram Mathematica 13.1 [18] for simplifying the expression. We have given the set of test points as shown in Table 1, whose unit of the coordinates is [mm]. The experiments were conducted in the following environment: Intel Xeon Silver 4210 3.2 GHz, RAM 256 GB, Linux Kernel 5.4.0, Risa/Asir Version 20230315, Wolfram Mathematica 13.1.
Table 2 shows the results of the experiments for 𝑇 = 10 and 𝑇 = 50. The comprehensive Gröbner system for eq. ( 1) uses precomputed values. If any part of the generated spline for the given coordinates falls outside the feasible region of myCobot, an error is displayed. The average computation time does Table 4 shows the sum of the joint variations [rad]. From the average values of each result, we see that Method 2 (with the Dijkstra method) reduces the total amount of joint rotation.
Table 5 shows the computation time for each method. From the average computation times, it can be seen that Method 1 is approximately 10 times faster than Method 2. Compared to the overall computation time for route planning shown in Table 2, the computation time for optimal route selection is considered sufficiently short, even for using Method 2 (Dijkstra method).
We have proposed a method for trajectory planning of robot manipulators using computer algebra, where the path is provided by cubic spline interpolation. Furthermore, we have proposed a method to optimize the joint configuration of the manipulator by solving the shortest path problem in a weighted graph. The experimental results have shown that the proposed methods can be used to plan the trajectory of the manipulator and optimize the joint configuration.
Future work includes the following.
1. Regarding path planning using cubic splines, it has been pointed out that some parts of the generated path may deviate from the feasible region. In the future, it is necessary to consider methods that ensure the generated trajectory stays within the feasible region while meeting given constraints, such as avoiding obstacles. In response to this, the authors are currently proposing a path-planning method that uses Bézier curves to generate trajectories that remain within the feasible region [21].
M. Mikawa) GLOBE https://researchmap.jp/aterui (A. Terui); https://mikawalab.org/ (M. Mikawa
This work was partially supported by JKA and its promotion funds from KEIRIN RACE.
The system of equations ( 4) may have several different solutions at each time 𝑡. For time 𝑡 = 0, … , 𝑇, suppose that there exist 𝑘 𝑗,𝑡 solutions to the configuration 𝜃 𝑗 of joint 𝑗 (𝑗 = 1, 3, 4) at 𝑡 such that 𝜃 𝑗,𝑡 at each time 𝑡. In order to move each joint more smoothly, it is desirable to select a solution that minimizes the sum of the configuration changes of the joints on the path. For this purpose, for 𝑗 = 1, 3, 4, we define a weighted graph 𝐺 𝑗 = (𝑉 𝑗 , 𝐸 𝑗 ), where
𝑗,𝑡 | 𝑡 ∈ {0, … , 𝑇 }, 𝑘 ∈ {1, … , 𝑘 𝑗,𝑡 }} is the set of vertices corresponding to the joint configuration at each time 𝑡, and 𝐸 𝑗 = {(𝜃
is the set of edges connecting the vertices at adjacent times. Then, in 𝐺 𝑗 , we reduce the problem of finding the optimal path to the one of finding the shortest path from 𝜃 (𝑘) 𝑗,0 to 𝜃 (𝑘) 𝑗,𝑇 . We have solved the shortest path problem using the following methods.
For 𝑡 = 0, … , 𝑇 − 1, find the sequence of joint configurations {𝜃 (𝑘) 𝑗,𝑡 | 𝑡 = 0, … , 𝑇 } satisfying the minimum distance between adjacent joint configurations such that min{|𝜃
Method 2 In the graph 𝐺 𝑗 , find the shortest path starting at 𝜃 (𝑘) 𝑗,0 (𝑘 = 1, … , 𝑘 𝑗,0 ) using the Dijkstra method [19], then find the shortest path with the minimum length among 𝐺 𝑗 s.
The arithmetic complexity of each method above is estimated as follows. Let 𝑇 be the number of points in the trajectory, and assume that the number of solutions to the inverse kinematics problem at each point is constant at 𝑑. The complexity of Method 1 is 𝑂(𝑇 𝑑). On the other hand, the complexity of Method 2, using a binary heap, is estimated to be 𝑂(𝑇 𝑑 2 log(𝑇 𝑑)).
We have implemented the above method and conducted experiments. The implementation of each procedure was based on an implementation [20] in the Python programming language. The experiments were conducted in the following environment: A virtual machine with RAM 13.2 GB, Ubuntu 22.04.3 LTS, Python 3.10.2 on VMware Workstation 16 Player, on the host environment with Intel Core i7-1165G7, RAM 16 GB, Windows 11 Home.
The test points are composed of a total of 𝑇 = 15 points, obtained by dividing each segment of the cubic spline curve passing through each point in Table 3 into 5 parts. The number of solutions to the inverse kinematics problem at each point in each test segment is 𝑑 = 4.
2. Instead of representing the trajectory on the path, it is desired to express any point on the path using parameters and ensure the solution to the inverse kinematics problem for that point. For linear paths, a method has already been proposed by the authors [9], and this will be extended to curves given by cubic splines. 3. Regarding optimal path planning using shortest path calculations on graphs, we have used an implementation of Dijkstra's algorithm using a binary heap in this paper. However, one method to further improve computational efficiency is to use Dijkstra's algorithm with a Fibonacci heap [22]. For example, using an implementation of Dijkstra's algorithm with a Fibonacci heap can be considered to improve computational efficiency. 4. To extend the proposed method to a 6-DOF manipulator. Although myCobot is operated with 3 Degree-Of-Freedom in this paper, it originally had 6 Degree-Of-Freedom, so it is desirable to implement methods for motion planning and path planning with 6 Degree-Of-Freedom.