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 control of a robot, the system comprising an interface, a parameter initialization module, an inverse kinematics scheduler, a Jacobian calculation unit, and a plurality of pose update units executed in parallel and a parameter selector, wherein: the interface is configured to receive a target pose value of the robot, and to send a joint angle value corresponding to the target pose value to the robot to control the motion of the robot; the parameter initialization module is configured to randomly generate an initial joint angle value, and to calculate an initial pose value based on the generated initial joint angle value and an initial error value of the initial pose value relative to the target pose value, and to send the initial joint angle value, the initial pose value and the initial error value to the inverse kinematics scheduler; the inverse kinematics scheduler is configured to compare the received error value to a predetermined threshold to determine whether to select the current joint angle value as the joint angle value corresponding to the target pose value or transmit the current joint angle value to the 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 the parameter selector; the Jacobian calculation unit is configured to obtain a Jacobian matrix and a Jacobian transposed matrix based on the received joint angle value, and to send the Jacobian matrix and the Jacobian transposed matrix to each of the plurality of pose update units; the plurality of pose update units is configured to generate different forward parameters in parallel and calculate new pose values in parallel based on the Jacobian matrix and the Jacobian transposed matrix, and to output each new joint angle value corresponding to each new pose value and each error value of each new pose value relative to the target pose value to the parameter selector; and the parameter selector is configured to select the smallest error value of the error values from the plurality of parallel pose update units and its corresponding joint angle value, and transmit the selected error value and joint angle value to the inverse kinematics scheduler; and wherein each of the pose update units comprises: a parameter calculator configured to calculate a forward parameter based on the Jacobian matrix, the Jacobian transposed matrix and the current error value; a variable update module configured to calculate an increment of a joint angle variable based on the forward parameter, the Jacobian transposed matrix and the current error value; 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 and the current joint angle value; an error calculator configured to calculate an error value 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, and output a new joint angle value corresponding to the new pose value and the error value to the parameter selector.

2. The system according to claim 1, characterized in that the parameter initialization module comprises: a random signal generator, configured to randomly generate the initial joint angle value; a triangular square calculator and a matrix multiplier, configured to calculate the initial pose value; and an adder and a triangular square calculator, configured to obtain the initial error value between the initial pose value and the target pose 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 based on the current joint angle value.

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

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

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

8. The system according to claim 1, characterized in that the parameter selector comprises: a comparator configured to select the smallest error value of the error values from the plurality of parallel pose update units; and a data selector configured to select the joint angle value corresponding to the smallest error value, and transmit the selected error value and joint angle value to the inverse kinematics scheduler.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) 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:

(2) FIG. 1 shows a schematic diagram of a multi-degree-of-freedom robot;

(3) FIG. 2 shows a flowchart of an inverse kinematics solution process based on Jacobian transposition according to an embodiment of the present invention;

(4) 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;

(5) FIG. 4 shows a block diagram of a pose update unit according to an embodiment of the present invention;

(6) 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

(7) FIG. 6 shows an example of Jacobian matrix calculation according to an embodiment of the present invention.

DETAILED DESCRIPTION OF THE PRESENT INVENTION

(8) 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.

(9) 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.

(10) 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.

(11) 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.

(12) 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.

(13) 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.

(14) 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.

(15) Step 4: A Jacobian matrix J and a Jacobian transposed matrix J.sup.T are calculated according to the joint angle variable θ.

(16) 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.

(17) 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 θ.

(18) 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.

(19) 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.

(20) 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.

(21) 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.

(22) 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.

(23) 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.

(24) 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.

(25) 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.

(26) 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.

(27) 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).

(28) 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.

(29) 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.

(30) 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.

(31) 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.

(32) In conclusion, the specific process of the multi-degree-of-freedom inverse kinematics solution system according to the present invention is as follows.

(33) 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.

(34) 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.

(35) 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.

(36) 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).

(37) 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.

(38) 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.

(39) 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.

(40) 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.

(41) 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.

(42) 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.

(43) 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.