Manipulator joint-limit handling algorithm

• US 9,205,555 B2
• Filed: 03/22/2011
• Issued: 12/08/2015
• Est. Priority Date: 03/22/2011
• Status: Active Grant

• Current Assignee: Harris Corporation (L3Harris Technologies, Inc.)
• Original Assignee: Harris Corporation (L3Harris Technologies, Inc.)
• Inventors: Bosscher, Paul M., Summer, Matthew D.
• Primary Examiner(s): Holwerda, Stephen
• Application Number:
  US13/053,430
• Publication Number:
  US 20120245736A1
• Time in Patent Office: 1,722 Days
• Field of Search: 700/262, 700/263
• US Class Current: 1/1
• CPC Class Codes: B25J 9/1607 Calculation of inertia, jacobian matrixes and inverses

Abstract

A desired movement command (203) for a robotic device (100) having n joints (112) operating in an m degrees of freedom task space is analyzed to determine if it would cause any of the joint angular limits to be violated. In the case where a non-zero number L (241) of the joints (112) have angular limits that are violated, a revised movement command (254) is then constructed using J_mod (251), which includes all columns in a Jacobian matrix J (211) except for those columns corresponding to L actively limited joints.

Claims

1. A system for controlling a robotic device, the robotic device comprising joints moving an end-effector that operates in an m degrees of freedom task space, the robotic device being driven by n actuators, the system in communications with actuator driver circuitry to control the actuators, the system comprising:

a processing unit in communications with the actuator driver circuitry; and

a memory in communications with the processing unit, the memory comprising program code executable by the processing unit to cause the processing unit to perform the following steps;

determining if a desired movement command for the robotic device would cause at least a first joint of the n actuated joints to violate a corresponding joint travel limit;

constructing a modified Jacobian matrix J_mod by removing a matrix column directly from a Jacobian matrix J that is associated with the first joint which would violate the corresponding joint travel limit if the desired movement command is executed, where the Jacobian matrix J is an m×

n matrix mathematically expressing current positions of the joints;

constructing a revised movement command for the robotic device by solving the following equation;

{dot over (q)}_mod=J_mod(J_mod^TW²J_mod)<sup>−

1</sup>J_mod^TW²{dot over (x)}_cmd

wherein {dot over (q)}_mod is an (n−

L)×

1 joint velocity command for joints that are not currently being limited, J_mod is an m×

(n−

L) matrix, J_mod^T is the transpose of J_mod, W is an m×

m matrix comprising weighting factors to weight respective movements of the n actuated joints, and {dot over (x)}_cmd is the desired movement command for the end-effector of the robotic device, where the revised movement command prevents the first joint from violating the corresponding joint travel limit, is not identical to the desired movement command and if submitted to the actuator driver circuitry causes movement of at least a portion of the n actuated joints; and

utilizing the revised movement command in place of the desired movement command to control the actuator driver circuitry to cause corresponding movement of the robotic device.

2. The system of claim 1, wherein the revised movement command is constructed by solving the following equation:

{dot over (q)}_new=re({dot over (q)}_mod),

wherein {dot over (q)}_new is an n×

1 new joint velocity command, and re( ) is a function that inserts a respective predefined value for each of the L actively limited joint into {dot over (q)}_mod to generate {dot over (q)}_new.

3. The system of claim 2, wherein the program code further causes the processing unit to determine if {dot over (q)}_new would cause any of a plurality of joint travel limits of the robotic device to be violated.

4. The system of claim 1, wherein the program code further causes the processing unit to generate the m×

(n−

L) matrix J_mod by;

determining an m×

n Jacobian matrix J for the robotic device utilizing the current state of the robotic device, the current state of the robotic device comprising current displacements of each of the n joints; and

utilizing the Jacobian matrix J to generate the matrix J_mod, wherein J_mod comprises all columns in J except for those columns corresponding to the L actively limited joints.