INVERSE KINEMATICS SOLUTION SYSTEM FOR USE WITH ROBOTS

Abstract

An inverse kinematics solution system for use with a robot, which is used for obtaining a joint angle value corresponding to a target pose value on the basis of an inputted target pose value and degree of freedom of a robot and which comprises: a parameters initialization module, an inverse kinematics scheduler, a Jacobian calculating unit, a pose updating unit and a parameters selector. The system is implemented by means of hardware and may quickly obtain motion parameters, which are used for controlling a robot, while reducing power consumption.

Claims

1. An inverse kinematics solution system for a robot, which is used for obtaining a joint angle value corresponding to a target pose value based on the input target pose value and degree-of-freedom of a robot, the system comprising: a parameter initialization module configured to obtain an initial parameter value, the initial parameter value comprising an initial joint angle value, an initial pose value and an initial error value of the initial pose value relative to the target pose value; an inverse kinematics scheduler configured to determine, based on the target pose value, whether to select a current joint angle value as a joint angle value corresponding to the target pose value or transmit the current joint angle value to a Jacobian calculation unit, wherein the current joint angle value is the initial joint angle value from the parameter initialization module or a joint angle value from a parameter selector; the Jacobian calculation unit configured to obtain a Jacobian matrix and a Jacobian transposed matrix based on the received joint angle value; pose update units configured to obtain a plurality of new joint angle values based on the Jacobian matrix and the Jacobian transposed matrix; and the parameter selector configured to select a joint angle value from the plurality of new joint angle values and transmit said joint angle value to the inverse kinematics scheduler.

2. The system according to claim 1, characterized in that the parameter initialization module comprises: a random signal generator, configured to generate the initial joint angle value; a triangular square calculator and a matrix multiplier, configured to obtain the initial pose value; and an adder and a triangular square calculator, configured to obtain the initial error value.

3. The system according to claim 1, characterized in that the inverse kinematics scheduler determines whether to select the current joint angle value as a joint angle value corresponding to the target pose value or transmit the current joint angle value to the Jacobian calculation unit by comparing an error corresponding to the current joint angle value with a predetermined threshold, wherein the error corresponding to the current joint angle value refers to an error between a pose value corresponding to the current joint angle value and the target pose value.

4. The system according to claim 1, characterized in that the Jacobian calculation unit comprises a triangular square calculator and a matrix multiplier, configured to obtain the Jacobian matrix and the Jacobian transposed matrix.

5. The system according to claim 1, characterized in that each of the pose update units comprises: a parameter calculator configured to calculate a forward parameter; a variable update module configured to calculate an increment of a joint angle variable; a register configured to store the increment of the joint angle variable; a pose calculation module configured to calculate a new pose value according to the increment of the joint angle variable; an error calculator configured to calculate an error between the new pose value and the target pose value; and a controller configured to control the execution of the variable update module, the register, the pose calculation module and the error calculator.

6. The system according to claim 5, characterized in that the parameter calculator calculates the forward parameter by use of a matrix multiplier.

7. The system according to claim 6, characterized in that the matrix multiplier is a 4*4 matrix multiplier.

8. The system according to claim 5, characterized in that the pose calculation module calculates a new pose by use of an adder and a triangular square calculator.

9. The system according to claim 1, characterized in that the parameter selector comprises: a comparator configured to compare a plurality of joint angle values; and a data selector configured to select a joint angle value.

10. The system according to claim 1, characterized in that there are at least two pose update units configured to execute the parallel calculation.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The following drawings are merely schematically illustrative and explanatory of the present invention and are not intended to limit the scope of the present invention, in which:

[0023] FIG. 1 shows a schematic diagram of a multi-degree-of-freedom robot;

[0024] FIG. 2 shows a flowchart of an inverse kinematics solution process based on Jacobian transposition according to an embodiment of the present invention;

[0025] FIG. 3 shows a block diagram of an inverse kinematics solution system for a multi-degree-of-freedom robot according to an embodiment of the present invention;

[0026] FIG. 4 shows a block diagram of a pose update unit according to an embodiment of the present invention;

[0027] FIG. 5 shows a schematic diagram of a triangular square calculator and a 4*4 matrix multiplier according to an embodiment of the present invention; and

[0028] FIG. 6 shows an example of Jacobian matrix calculation according to an embodiment of the present invention.

DETAILED DESCRIPTION OF THE PRESENT INVENTION

[0029] To make the objectives, technical solutions, design methods and advantages of the present invention clearer, the present invention will be further described below in detail by specific embodiments with reference to the drawings. It should be understood that the specific embodiments described herein are merely for explaining the present invention rather than limiting the present invention.

[0030] The system of the present invention can be applied to the control of the mechanical arms of a multi-degree-of-freedom robot. The mechanical arm of the robot shown in FIG. 1 has 8 joints (i.e., 8 degrees of freedom). The motion of the robot can be controlled by controlling the rotation of each joint. The inverse kinematics solution for the mechanical arm means that an angle of each joint of the robot corresponding to a target pose is solved based on the target pose P.sub.L(x.sub.o, y.sub.o, z.sub.o) of the robot's gripper. The association is generally used to establish, on the basis of the kinematics analysis on the mechanical arm, a relation equation between the target pose of the robot's gripper and the change in the angle of each joint, and the inverse kinematics solution is performed by means of a Jacobian matrix. The establishment of the relation equation and the detailed solution process of the Jacobian matrix belong to the prior art and will not be described here. The descriptions herein will focus on the system for realizing inverse kinematics solution for a robot by use of ASIC hardware.

[0031] The present invention is aimed at obtaining a joint angle corresponding to each joint, (.sub.1, .sub.2, . . . .sub.8) (by taking 8 degrees of freedom as an example), by the system of the present invention when the target pose P.sub.L (x.sub.o, y.sub.o, z.sub.o) of the robot is given. By inputting the obtained joint angles into the robot, the motion of the robot can be flexibly controlled. The control system in FIG. 1 may adopt the system of the present invention.

[0032] To facilitate the understanding of the hardware implementation of the present invention, FIG. 2 shows an inverse kinematics solution process based on Jacobian transposition according to the present invention.

[0033] Step 1: A target pose P.sub.L(x.sub.o, y.sub.o, z.sub.o) is an input, where x.sub.0, y.sub.0, z.sub.0 represent coordinate values in three directions.

[0034] Step 2: A set of initial values of a joint angle variable, .sub.init, is randomly generated, and the joint angle variable is assumed as =.sub.init; an initial pose P.sub.init is calculated, and a pose variable is assumed as P=P.sub.init; and, an initial error e.sub.init (i.e., an error between the initial pose and the target pose) is calculated, and an error variable is assumed as error=e.sub.init.

[0035] Step 3: The error variable error is compared with a predetermined threshold to determine whether the precision requirement is satisfied, that is, error<Threshold; if the precision requirement is satisfied, the joint angle variable is an output; or otherwise, the following steps will be executed continuously.

[0036] Step 4: A Jacobian matrix J and a Jacobian transposed matrix J.sup.T are calculated according to the joint angle variable .

[0037] Step 5: Forward parameters .sub.1, .sub.2, . . . , .sub.k are calculated in parallel according to the J, J.sup.T and error, and a corresponding joint increment is solved.

[0038] Step 6: New poses P.sub.1, P.sub.2, . . . , P.sub.k and errors e.sub.1, e.sub.2, . . . , e.sub.k between the new poses and the target pose P.sub.L are calculated according to the and .

[0039] Step 7: The smallest error and its corresponding joint angle variable are selected from e.sub.1, e.sub.2, . . . , e.sub.k, and the error variable error is made equal to the smallest error.

[0040] Step 8: It is determined whether the error satisfies the precision requirement, that is, error<Threshold ; if the error satisfies the precision requirement, the joint angle variable is output; or otherwise, the process returns to the step 4 and will be continuously executed.

[0041] FIG. 3 shows a block diagram of a multi-degree-of-freedom inverse kinematics solution system according to an embodiment of the present invention. The system includes a parameter initialization module, an inverse kinematics scheduler, a Jacobian calculation unit, pose update units and a parameter selector. The input of the system is a target pose P.sub.L(x.sub.o, y.sub.o, z.sub.o) and the degree-of-freedom N, where the degree-of-freedom N may be any value. The output is respective joint angle values corresponding to the target pose.

[0042] The parameter initialization module is configured to randomly generate initial parameters, for example, initializing the joint angle variable .sub.init, calculating the initial poses P.sub.init and e.sub.init, and the like. The parameter initialization module may be implemented by a random signal generator.

[0043] The inverse kinematics scheduler is configured to coordinate the scheduling and control of operational tasks in the whole system, for example, determining the size of the current error and the predetermined error threshold, controlling the number of iterations, and the like. The inverse kinematics scheduler may be implemented by a finite state machine, including a comparator, a data selector or the like.

[0044] The Jacobian calculation unit (JCU) is configured to calculate a Jacobian matrix corresponding to the joint angle variable and a transposed matrix thereof. The Jacobian calculation unit may be implemented by a triangular square calculator and a 4*4 matrix multiplier.

[0045] The pose update unit (PUU) is configured to calculate a new pose of the mechanical arm of the robot, the joint angle variables, and the corresponding error. A plurality of pose update units (e.g., 32) may be employed for parallel calculation. Each of the pose update units contains a parameter calculator and a forward kinematics calculator.

[0046] The parameter selector is configured to select the smallest error from a plurality of errors and return the smallest error to the inverse kinematics scheduler. The parameter selector may be implemented by a comparator or a sequencer.

[0047] Additionally, the system is further provided with an input/output interface. Each module contains a memory or a register for storing the intermediate calculation results or the parameters received from other modules. The parameters are transmitted among modules through a bus.

[0048] Specifically, the operation process executed by the system according to the present invention is as follows. The target pose P.sub.L(x.sub.o, y.sub.o, z.sub.o) and the degree-of-freedom N (in this embodiment, N=8) are received from the outside of the system. The parameter initialization module randomly initializes the joint angle variable and calculates the initial pose. The Jacobian calculation unit calculates a Jacobian matrix corresponding to the joint angle variable and a transposed matrix thereof. The inverse kinematics scheduler controls, according to the degree-of-freedom N, the number of iterations of the triangular square or Jacobian matrix calculation. The pose update units calculate new poses, joint angle variables and corresponding errors in parallel. Meanwhile, according to whether the error error is less than the threshold Threshold , the inverse kinematics scheduler controls the number of iterations of the pose update units. The parameter selector selects the smallest error from a plurality of errors, and returns the smallest error to the inverse kinematics scheduler. During this process, when the inverse kinematics scheduler determines that the error error is less than the threshold Threshold, the iteration is stopped and the system is controlled to output joint angles, (.sub.1, .sub.2, . . . .sub.8).

[0049] To further understand the present invention, FIG. 4 shows a block diagram of the pose update unit in FIG. 3. Each of the pose update units (PUUs) includes a parameter calculator which may be implemented by a multiplier and an adder; and, a forward kinematics calculator (FKU). The forward kinematics calculator contains a controller, a variable update module, a register, a pose calculation module, and an error calculator.

[0050] The pose update units receive the Jacobian matrix J and the transposed matrix J.sup.T from the Jacobian calculation unit in FIG. 3, the error error, the current pose P, and the target pose P.sub.L, internally generate different forward parameters .sub.1, .sub.2, . . . , .sub.k in parallel and calculate new poses in parallel. The output is k sets of corresponding joint angle variables .sub.1, .sub.2, . . . , .sub.k and errors e.sub.1, e.sub.2, . . . , e.sub.k. The parameter calculator is configured to calculate forward parameters .sub.1, .sub.2, . . . , .sub.k forward kinematics calculator. The forward kinematics calculator calculates the new joint angle variable and the new pose. The variable update module of the forward kinematics calculator calculates an increment of the joint angle variable and stores the increment into the register. The pose calculation module calculates a new pose according to the increment and transfers the new pose to the error calculator. The error calculator calculates an error between the new pose and the target pose. The controller is configured to control the order of execution of the variable update module, the register, the pose calculation module, and the error calculation. The forward kinematics calculator adopts a pipeline operation. The controller controls the variable update module to calculate the increment and then stores the increment into the register, and meanwhile controls the pose calculation module to calculate the new pose and controls the variable update module to continuously receive the forward parameter .sub.k for calculation of the increment . The controller plays a scheduling role in the whole pipeline.

[0051] FIG. 5 shows a block diagram of the Jacobian calculation unit according to an embodiment of the present invention. The Jacobian calculation unit includes a triangular square calculator and a 4*4 matrix multiplier.

[0052] FIG. 6 shows an example of the Jacobian matrix calculation by the Jacobian calculation unit in FIG. 5. In a first step, n 4*4 matrices T[n] are constructed according to joint angles .sub.1, .sub.2, . . . , .sub.k, . . . , .sub.n, and trigonometric functions of matrix elements are calculated by the triangular square calculator to obtain TR[n]. In a second step, the 4*4 matrix multiplier receives matrices TR[k] and TR[k-1] and performs a matrix multiplication operation to obtain TRL[n]. In a third step, TRL[n] is subtracted from TRL[k], and then multiplied by TRL[k] through the matrix multiplier to obtain a final Jacobian matrix J[n]. The calculation process does not mean that the calculation of n trigonometric functions or the matrix multiplication must be performed simultaneously. It is also possible that the Jacobian matrix is serially calculated by a pipelining technology.

[0053] In conclusion, the specific process of the multi-degree-of-freedom inverse kinematics solution system according to the present invention is as follows.

[0054] Step 1: The initialization module randomly generates a set of initial values of a joint angle variable, .sub.init, by the random signal generator, calculates an initial pose P.sub.init by use of the triangular square calculator and the 4*4 matrix multiplier, calculates an initial error error by use of an adder and the triangular square calculator, and transmits the parameters to the inverse kinematics scheduler.

[0055] Step 2: The inverse kinematics scheduler receives the parameter error and then determines, by use of a comparator, whether the error satisfies the precision requirement, that is, error<Threshold; if the error satisfies the precision requirement, a data selector selects an output circuit to output the joint angle variable ; or otherwise, the data selector selects a calculation circuit to transmit the joint angle variable to the Jacobian calculation unit for continuous execution.

[0056] Step 3: The Jacobian calculation unit receives the joint angle variable , and calculates a Jacobian matrix J and a Jacobian transposed matrix J.sup.T by use of the triangular square calculator and the 4*4 matrix multiplier. The inverse kinematics scheduler controls the number of iterations of the Jacobian matrix calculation according to the input degree-of-freedom, and transmits the obtained Jacobian matrix J and Jacobian transposed matrix J.sup.T to each of the parallel pose update units (PUUs) at the end of the calculation.

[0057] Step 4: The pose update units calculate parameters .sub.1, .sub.2, . . . , .sub.k according to the J, J.sup.T and error by use of the 4*4 matrix multiplier and transfers the parameters to the internal forward kinematics calculator (FKU).

[0058] Step 5: The variable update module in the forward kinematics calculator receives the error and J.sup.T, calculates a joint increment by use of the 4*4 matrix multiplier and stores the joint increment into the register.

[0059] Step 6: The pose calculation module receives the in the register, and calculates a new pose P by use of an adder and the triangular square calculator.

[0060] Step 7: The error calculator calculates errors e.sub.1, e.sub.2, . . . , e.sub.k between the new poses and the target pose by use of an adder and the triangular square calculator; and, the whole pose calculation units outputs k sets of new joint angle variables .sub.1, .sub.2, . . . , .sub.k and errors e.sub.1, e.sub.2, . . . , e.sub.k.

[0061] Step 8: The parameter selector selects the smallest error error from e.sub.1, e.sub.2, . . . , e.sub.k by a comparator, selects a corresponding join angle variable by a data selector, and transmits the smallest error and the corresponding joint angle variable to the inverse kinematics scheduler.

[0062] Step 9: The inverse kinematics scheduler determines, by a comparator, whether the error satisfies the precision requirement, that is, error<Threshold; if the error satisfies the precision requirement, a data selector selects an output circuit to output the joint angle variable ; or otherwise, the data selector selects a calculation circuit to transmit the joint angle variable to the Jacobian calculation unit, and step 3 is repeated for continuous execution.

[0063] It should be understood by those skilled in the art that the comparator, the determiner, the triangular square calculator and the like involved herein may be commercially-available standard devices, and the 4*4 matrix multiplier may also be a matrix multiplier of other dimensionalities.

[0064] Although various embodiments of the present invention have been described above, the foregoing descriptions are merely exemplary but not exhaustive, and are not limited to the disclosed embodiments. Numerous modifications and alterations will be apparent to a person of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The terms used herein are selected to best explain the principles of the embodiments, practical applications or technical improvements in the market, or to enable those skilled in the art to understand the embodiments disclosed herein.