Method and apparatus for controlling robot arms using elastic distortion simulations

The present disclosure generally relates to the field of robotics and computer animation, more particularly, method and apparatus to solve the inverse kinematics problem to control a kinematic chain such as a robot arm or an animation character's skeleton to reach a target position. The new method simulates a kinematic chain whose links and joints are elastic and can be distorted. The method distorts the kinematic chain to move its end to the target position, calculates distortions, and iteratively adjusts link and joint configurations of the kinematic chain to reduce distortions while keeping its end at the target position until a solution with near zero distortions is found. The resulting link and joint configurations of the simulated kinematic chain then can be used for the actual kinematic chain to reach the same target position.

Skip to: Description  ·  Claims  ·  References Cited  · Patent History  ·  Patent History
Description
TECHNICAL FIELD

The present disclosure generally relates to the field of robotics and computer animation, more particularly, solving inverse kinematics to calculate the link and joint configurations needed to control a kinematic chain such as a robot arm or an animation character's skeleton to reach a target position.

BACKGROUND

This section describes approaches that could be employed, but are not necessarily approaches that have been previously conceived or employed. Hence, unless explicitly specified otherwise, any approaches described in this section are not prior art to the claims in this application, and any approaches described in this section are not admitted to be prior art by inclusion in this section.

In robotics, forward kinematics is to find the position of the end of a robot arm given its link and joint configurations such as joint rotation angles, while inverse kinematics is to find a set of link and joint configurations that move the end of a robot arm to a given target position. Inverse kinematics is also used in computer animation to find the joint rotations needed to move a skeleton to a desired posture. In general, inverse kinematics is harder than forward kinematics, especially when there are large numbers of links and joints and additional factors such as obstacles.

Some robot arms have analytical inverse kinematics solutions which are closed-form formulas that can calculate link and joint configurations given a target position. Analytical solutions are fast, but they may result in robot movements that are not smooth, and in many cases analytical solutions are hard to find or even non-existent. Alternatively, many robotic and animation applications use numerical methods to find approximate solutions for the inverse kinematics problems. Numerical methods such as the Jacobian Inverse Technique are computationally intensive, while other methods such as the Cyclic Coordinate Descent (CCD) method may result in unnatural positions.

This disclosure describes a new numerical method that provides fast and natural inverse kinematics solutions for robot arms and other kinematic chains. The new method simulates a robot arm that has elastic links and joints, moves the end of the robot arm to a target position, calculates distortions caused by that movement, and iteratively reconfigures the links and joints to reduce distortions until a solution that is free of distortions is found.

BRIEF DESCRIPTION OF THE DRAWINGS

To provide a more complete understanding of the present disclosure and advantages thereof, reference is made to the attached drawings, like reference numbers represent like parts, in which:

FIG. 1 illustrates an example robot arm and the kinematic notations that describe its link and joint configurations.

FIGS. 2A, 2B and 2C illustrate different types of elastic distortions of the links and joints of the example robot arm. FIG. 2A illustrates a length distortion. FIG. 2B illustrates a bending distortion. FIG. 2C illustrates a twisting distortion.

FIG. 3 illustrates an embodiment of the elastic distortion algorithm that solves inverse kinematics by simulating elastic distortions.

FIG. 4 illustrates elastic distortion reductions using elastic force simulations.

FIG. 5 shows sample outputs from a computer program that embodies the elastic distortion algorithm.

 DESCRIPTION OF EXAMPLE EMBODIMENTS Overview

A new numerical inverse kinematics method to calculate link and joint configurations of a robot arm to move its end to a target position. The method pretends that the links and joints of the robot arm are infinitely elastic, meaning that the method pretents that they can be distorted, stretched, compressed, bent, or twisted arbitrarily without being broken. The method first moves the end of the robot arm from its initial position to the target position and measures the elastic distortions caused by that movement. The method then iteratively reconfigures the links and joints of the robot arm to reduce distortions while keeping the end of the robot arm at the target position until the total distortion of the whole system is reduced to zero or near zero. In each iteration, the method reconfigures a small set of adjacent links and joints and utilizes different options to find link and joint configurations that reduce the distortions. One option is to use an optimization technique such as Gradient Descent to search for link and joint configurations that minimize the total distortions. Another option is to simulate elastic forces caused by the distortions and apply these forces to move links and joints accordingly to relieve their distortions. The third option is to find link and joint configurations that deviate from the constraints of the robot arm and correct these deviations to eliminate corresponding distortions.

Once a set of link and joint configurations are found for the simulated elastic robot arm to reach a target position with no elastic distortions, the same link and joint configurations can be used for the actual robot arm to reach the same target position.

Because of its imitation of elasticity physics, the method can provide solutions that have smooth and natural robot movements. The method is fast as it involves only a small set of adjacent links and joints in each iteration, unlike other numerical methods which requires optimizing large numbers of parameters of the whole robot arms in each iteration. The method can be generalized to handle obstacles and other external conditions

DETAILED DESCRIPTION

FIG. 1 illustrates an example robot arm and the kinematic notations used in this disclosure to specify its link and joint configurations.

The configuration of a link is specified by the position of its base joint and two orientation vectors. The base joint of a link is the joint closer to the base in the kinematic chain of the robot arm. The first orientation vector of a link is a unit vector that points from the base joint to the other joint of the link, and the second orientation vector is a unit vector that is orthogonal to the first orientation vector. For example, in FIG. 1, the configuration of link L2 is represented by tuple [J2, u2, v2] where J2 is the position of the base joint of link L2, u2 is the first orientation vector pointing from the base joint J2 to the next joint J3, and v2 is the second orientation vector that is orthogonal to u2. Vectors u2 and v2 together represent a cross section of link L2 as it rotates around join J2.

The configuration of a joint is specified by its position and rotation angle, both of which can be derived from the link configurations described above. In particular, the rotation angle of a joint can be derived from the orientation vectors of the adjacent links. For example, in FIG. 1, the position of the first joint J1 is a part of the configuration [J1, u1, v1] of link L1, and the joint rotation angle of joint J1 can be derived from the orientation vector v1 of link L1. Similarly, the position of the second joint J2 is a part of the configuration [J2, u2, v2] of link L2, and its rotation angle is the angle between u1 and u2.

As the robot arm moves, its link and joint configurations vary but are subject to constraints. One set of constraints are the length constraints of the links. For example, as link L1 rotates around joint J1, the position of joint J2 changes, but the distance between joints J1 and J2 remains a constant which is the length l1 of link L1. In general, if li is the length of link Li and |Ji+1−Ji| are the distance between joints Ji+1 and Ji, then the length constraints of the robot arm in FIG. 1 can be written as:
|J2−J1|=l1  Length Constraint 1:
|J3−J2|=l2  Length Constraint 2:
|J4−J3|=l3  Length Constraint 3:
|J5−J4|=l4  Length Constraint 4:
|J6−J5|=l5  Length Constraint 5:

Another set of constrains are the joint constraints that limit the orientations of the links as they rotate around the joint rotation axis. For example, the rotation axis of joint J1 is the upright unit vector z0. As link L1 rotates around joint J1, its second orientation vector v1 varies, but its first orientation vector u1 is subject to the constraint that u1 is fixed and aligns with z0. Similarly, as link L2 rotates around joint J2, its first orientation vector u2 varies, but its second orientation vector v2 is subject to the constraint that v2 aligns with the second orientation vector v1 of link L1. The joint constraints of the robot arm in FIG. 1 can be written as:
u1=z0  Joint Constraint 1:
v2=v1  Joint Constraint 2:
v3=v2  Joint Constraint 3:
v4=u3  Joint Constraint 4:
v5=u4  Joint Constraint 5:
u6=u5  Joint Constraint 6:

The robot arm may have additional joint constraints that limit the rotation angles of some joints. For example, if joint J2 can only rotate from 0 to 90 degree, then there is a corresponding constraint that the angle between u2 and u1 must be in the same range.

Using the about notations, the forward kinematics of a robot arm can be easily calculated from the joint rotation angles. For example, starting the configuration [J1, u1, v1] of the base link L1 and the joint rotation angle r2 of joint J2, the configuration [J2, u2, v2] of the next link L2 can be calculated using simple vector calculations:
J2=J1+l1·u1
v2=v1
u2=R2·u1, where R2 is the rotation matrix by angle r2 around axis v2

To solve the inverse kinematics problem, the new method simulates a robot arm whose links and joints are elastic and can be distorted Links and joints are distorted when their configurations deviate from the constraints of the robot arm. For example, a link is stretched or compressed when it deviates from its length constraint. Similarly, a link is bended or twisted when its orientation deviates from a joint constraint.

FIGS. 2A, 2B, and 2C illustrate different types of elastic distortions and how they can be quantified using a distortion measurement that calculates distortions according to the deviations of the link and joint configurations from their constraints.

FIG. 2A illustrates a length distortion which is caused by a deviation of a length constraint. In FIG. 2A, link L5 is stretched when joint J6 at the end of link L5 is moved to J6′, and the new length |J6′−J5| of link L5 no longer satisfies Length Constraint 5 that |J6′−J5|=l5. The length distortion DL of link L5 can be measured by the deviation of its length from the length constraint using the following formula:
DL=δ/l5=∥J6′−J5|−l5|/l5

FIG. 2B illustrates a bending distortion which is caused by a deviation of the first orientation vector of a link from a joint constraint. In FIG. 2B, link L5 is bended when its first orientation vector u5 deviates from the first orientation vector u6 of link L6 as required by Joint Constraint 6 that u5=u6. The bending distortion in this case can be measured by the deviation angle α between u5 and u6:
DB=α=α cos(u5·u6)

Alternatively, DB5 can be measured by the Manhattan distance between u5 and u6:
DB=∥u5x−u6x|+|u5y−u6y|+|u5z−u6z|

FIG. 2C illustrates a twisting distortion which is caused by a deviation of the second orientation vector of a link from a joint constraint. In FIG. 2C, link L2 is twisted when its second orientation vector v2 deviates from the second orientation v3 of link L3 as required by Joint Constraint 3 that v2=v3. The twisting distortion in this case can be measured by the deviation angle β between v2 and v3:
DT=β=a cos(v2·v3)

Alternatively, DT5 can be measured by the Manhattan distance between v2 and v3:
DT=|v2x−v3x|+|u2y−u3y|+|v2z−u3z|

Using a distortion measurement such as the one described above, the new method solves the inverse kinematics problem of a simulated elastic robot arm by distorting the robot arm to move its end to the desired target location and iteratively reducing distortions by adjusting the link and joint configurations until the robot arm is free of distortions.

FIG. 3 illustrates an example embodiment of the elastic distortion algorithm that solves the inverse kinematics problem of the robot arm in FIG. 1. In step 1, the end link L6 of the robot arm is moved to the desired target position. Since link L5 is elastic, it remains attached to L6, but the relocation of L6 distorts L5.

In step 2, the algorithm iterates from link L5 to L2 and adjusts their configurations to reduce distortions. In particular, for each link Li, step 2A calculates the distortions of link Li and its neighbor links and joints, and step 2B adjusts the configuration of link Li and its neighbor links and joints to reduce their distortions. In general, changing the configuration of a link or joint may affect the configurations of its neighbor links and joints and their distortions. For example, moving the position of joint Ji changes the orientations of neighbor links Li and Li−1 and affects their distortions. By taking the neighbor links and joints into account, the algorithm aims at reducing the total distortion of the system in each iteration.

After step 2 completes adjusting links L5 to L2, step 3 of the algorithm adjusts link L1 to reduce the distortions of link L1 and its neighbor links and joints while ensuring that L1 is properly attached to the base of the robot arm. Step 4 then repeats similar adjustments as in step 2 but in opposite order, from link L2 to L5, to further reduce the distortions of the system. In step 5, the algorithm adjusts the free parameters of link L6 that are not bound by the target configuration to reduce the distortions of link L6 and its neighbor links and joints. The algorithm then repeats steps 2, 3, 4, and 5 until the total distortion of the system is close to zero within a margin of error. The resulting link and joint configurations of the simulated elastic robot arm then can be used for the actual robot arm to reach similar target position. If the algorithm cannot reduce the total distortion to near zero after certain maximum number of iterations, the algorithm returns an error indicating that the target cannot be reached.

When adjusting the links in steps 2 to 5, the algorithm can use different options to find new link and joint configurations that reduce distortions. The first option is to use an optimization method such as Gradient Descent to find configurations that minimizes the objective function which is the sum of the distortions of a link and its neighbor links and joints. For example, for each link Li, Gradient Descent can be used to find a configuration [Ji, ui, vi] that minimizes the sum of the distortions of link Li and its neighbor links and joints. Unlike optimizations used in other numerical inverse kinematics methods which involve large numbers of parameters of the entire robot arm in each iteration, optimizations in the new method involves only a small set of neighbor links and joints in each iteration, and thus it is more efficient and converges faster. In addition, if the distortion measurement is a piece-wise linear function of the configuration parameters, e.g. when the Manhattan distances are used to measure joint constraint deviations, then the Piece-wise Linear Programming method can be used to efficiently minimize distortions.

The second option to reduce distortions is to simulate elastic forces caused by the distortions and apply these forces to adjust the links and joints accordingly to relieve their distortions. FIGS. 2A, 2B and 2C illustrate elastic forces caused by different types of elastic distortions. In FIG. 2A, the length distortion (stretching) of link L5 results in elastic force FL that pulls J5 closer to J6′. The magnitude of elastic force FL is proportional to the length distortion DL. In FIG. 2B, the bending distortion of link L5 results in elastic force FB that tends to rotate L5 to realign its orientation vector u5 with u6 to restore Joint Constraint 6. The magnitude of FB is proportional to the bending distortion DB. In FIG. 2C, the twisting distortion to link L2 results in a pair of elastic forces FT and −FT that tend to twist L2 to realign its orientation vector v2 with v3 to restore Joint Constraint 3. The magnitudes of FT and −FT are proportional to the twisting distortion DT.

Since elastic forces tend to counter the elastic distortions that cause them, the new method simulates the effects of elastic forces to move distorted links and joints to new configurations with smaller distortions. FIG. 4 illustrates the simulated elastic forces on link L5 after link L6 is moved from its initial configuration [J6, u6, v6] to target configuration [J6′, u6′, v6′]. Since the distance between J6′ and J5 is longer than the original length of link L5, linked L5 is stretched, resulting in elastic force FL that pulls the base joint J5 of link L5 toward J6′. At the same time, the first orientation vector of L5 which points from J5 to J6′ is no longer aligned with u6′ as required by Joint Constraint 6. This bending distortion results in another elastic force FB that tends to rotate link L5 to realign it first orientation vector with u6′. The sum of FL and FB is elastic force FE. The algorithm simulates the effect of elastic force FE that moves the base joint J5 of link L5 along the direction of FE to new position J5′ as showed in FIG. 4. Similarly, elastic forces can be simulated to twist link L5's second orientation vector v5 toward v5′ to realign it with the new orientation vector u4′ according to Joint Constraint 5 that v5′=u4′. The new configuration [J5′, u5′, v5′] of link L5 results in smaller distortions for link L5 and its neighbor links and joints.

The third option to reduce distortions is to correct the constraint deviations that cause them, if such corrections result in a net reduction in total distortion. For example, after end link L6 is moved to the target position, the total distortion of links L5 and its neighbor links and joints may be reduced by the following corrections:

Set u5′=u6′ to correct the violation of Joint Constraint 6

Set J5′=J6′−u5′. l5 to correct the violation of Length Constraint 5

Set v5′=u4′ to correct the violation of Joint Constraint 5

This method can quickly find candidate reconfigurations with few calculations, but it may not always result in a net reduction in total distortion, since changing a configuration parameter to correct one constraint deviation may cause another deviation of a different constraint that results in a net increase in total distortion. In iterations when this is the case, the algorithm can fall back to the elastic force simulation option or the Gradient Descent option to reduce the total distortion of the system.

FIG. 5 shows the outputs of a computer program that embodies the elastic distortion algorithm to solve the inverse kinematics of a robot arm with six links. Iteration 0 shows the initial configurations of the robot arm and the target position for the end link. Iteration 1 shows the distorted robot arm after its end link is moved to the target position. Iteration 5 show the robot arm with reduced distortions after 5 iterations. Finally, the output at iteration 44 shows that the robot arm has no distortion while its end link is at the target position.

The embodiment described above can be extended to include external constraints such as obstacles. In particular, the elastic distortion algorithm can simulate distortions caused by collisions of an elastic robot arm with obstacles and adjust the link and joint configurations to reduce these distortions until a solution that is free of distortions is found.

Claims

1. A method of solving the inverse kinematics problem of a kinematic chain with multiple links and joints, comprising:

determining constraints on configurations of the links and joints of the kinematic chain;
pretending that the kinematic chain is elastic, and the links and joints can be distorted so that the configurations of the links and joints can deviate from the constraints;
defining a distortion measurement function which quantifies distortions as a function of deviations of the configurations of the links and joints from the constraints;
running an algorithm that comprises of distorting the pretended elastic kinematic chain to put an end of the kinematic chain in a target position and/or orientation, calculating distortions using the distortion measurement function, and iteratively adjusting the configurations of the links and joints to reduce the distortions while keeping the end of the kinematic chain in the target position and/or orientation until the total distortion of the kinematic chain is zero or smaller than a predetermined threshold, or a maximum number of iterations is reached; and
returning the configurations of the links and joints with total distortion being zero or smaller than the predetermined threshold, if found, as an inverse kinematics solution for the kinematic chain.

2. The method of claim 1, wherein the configurations of the links and joints include positions and orientation vectors of the links and joints.

3. The method of claim 1, wherein the constraints of include constraints on the positions and orientation vectors of the links and joints.

4. The method of claim 1, wherein in each iteration the algorithm adjusts the configurations of a set of neighbor links and joints to reduce the total distortion.

5. The method of claim 1, wherein the algorithm adjusts the configurations of the links and joints to reduce the distortions by using an optimization method such as Gradient Descent to find configurations that minimize the distortions.

6. The method of claim 5, wherein the distortion measurement function is a piece-wise linear function of the configurations of the links and joints, and the optimization method is a Piece-wise Linear Programing method.

7. The method of claim 1, wherein the algorithm adjusts the configurations of the links and joints to reduce the distortions by calculating elastic forces caused by the distortions and applying the calculated elastic forces to change the configurations of the links and joints.

8. The method of claim 1, wherein the algorithm adjusts the configurations of the links and joints to reduce the distortions by correcting the deviations of the configurations of the links and joints from the constraints.

9. The method of claim 1, wherein the constraints include constraints imposed by obstacles and other external conditions and the distortion measurement function includes calculating distortions caused by deviations from the constraints imposed by obstacles and other external conditions.

10. An apparatus of solving the inverse kinematics problem of a kinematic chain with multiple links and joints, comprising a computer running a computer program that:

receives as inputs a set of constraints on configurations of the links and joints of the kinematic chain;
simulates that the links and joints of the kinematic chain can be distorted so that the configurations of the links and joints can deviate from the constraints;
defines a distortion measurement function which quantifies distortions as a function of deviations of the configurations of the links and joints from the constraints;
executes an algorithm that comprises of distorting the simulated kinematic chain to put an end of the kinematic chain in a target position and/or orientation, calculating distortions using the distortion measurement function, and iteratively adjusting the configurations of the links and joints to reduce the distortions while keeping the end of the kinematic chain in the target position and/or orientation until the total distortion of the kinematic chain is zero or smaller than a predetermined threshold, or a maximum number of iterations is reached; and
returns the configurations of the links and joints with total distortion being zero or smaller than the predetermined threshold, if found, as an inverse kinematics solution for the kinematic chain.

11. The apparatus of claim 10, wherein the configurations of the links and joints include positions and orientation vectors of the links and joints.

12. The apparatus of claim 10, wherein the constraints of include constraints on the positions and orientation vectors of the links and joints.

13. The apparatus of claim 10, wherein in each iteration the algorithm adjusts the configurations of a set of neighbor links and joints to reduce the total distortion.

14. The apparatus of claim 10, wherein the algorithm adjusts the configurations of the links and joints to reduce the distortions by using an optimization method such as Gradient Descent to find configurations that minimize the distortions.

15. The apparatus of claim 14, wherein the distortion measurement function is a piece-wise linear function of the configurations of the links and joints, and the optimization method is a Piece-wise Linear Programing method.

16. The apparatus of claim 10, wherein the algorithm adjusts the configurations of the links and joints to reduce the distortions by calculating elastic forces caused by the distortions and applying the calculated elastic forces to change the configurations of the links and joints.

17. The apparatus of claim 10, wherein the algorithm adjusts the configurations of the links and joints to reduce the distortions by correcting the deviations of the configurations of the links and joints from the constraints.

18. The apparatus of claim 10, wherein the constraints include constraints imposed by obstacles and other external conditions and the distortion measurement function includes calculating distortions caused by deviations from the constraints imposed by obstacles and other external conditions.

Referenced Cited
U.S. Patent Documents
10786910 September 29, 2020 Asada
20110264111 October 27, 2011 Nowlin
20180015620 January 18, 2018 Nakaya
20200298403 September 24, 2020 Nilsson
Foreign Patent Documents
109176532 September 2020 CN
WO-2015030650 March 2015 WO
Other references
  • Wang, L.-C.T. and Chen, C.C., “A Combined Optimization Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators”, IEEE Transactions On Robotics and Automation, Aug. 1991, vol. 7, Issue: 4, IEEE, USA.
  • Aristidou, A. and Lasenby, J., “FABRIK: A fast, iterative solver for the Inverse Kinematics problem”, Graphical Models, May 2011, 73 (2011) 243-260, Elsevier, Netherlands.
Patent History
Patent number: 11745343
Type: Grant
Filed: Nov 18, 2020
Date of Patent: Sep 5, 2023
Patent Publication Number: 20220152822
Inventors: Darrion Vinh Nguyen (Milpitas, CA), Hao-Nhien Qui Vu (Fountain Valley, CA)
Primary Examiner: Jonathan L Sample
Assistant Examiner: Byron Xavier Kasper
Application Number: 16/952,070
Classifications
Current U.S. Class: Stereotaxic Device (606/130)
International Classification: B25J 9/16 (20060101);