# US 2021/0197371 A1 · Redundant robotic arm control method, redundant robotic arm controller, and computer readable storage medium

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2021/0197371 A1
- Document ID: US20210197371A1
- Status: Published application
- Kind: A1
- Application number: 17/134,565
- Filed: 2020-12-28
- Published: 2021-07-01
- Earliest claimed: 2019-12-30
- Assignees: Ubtech Robotics Corp Ltd
- Applicants: Ubtech Robotics Corp Ltd
- Inventors: Liqun Huang; Xiaoyu Ren; Yuesong Wang; Youjun Xiong
- Web page: https://patentgrep.com/patent/US20210197371A1

## CPC

- B25J 9/1607: {Calculation of inertia, jacobian matrixes and inverses}
- B25J 9/023: {Cartesian coordinate type}
- B25J 9/1643: {redundant control}
- B25J 9/1651: {acceleration, rate control}
- B25J 13/085: {Force or torque sensors (B25J13/082, B25J13/084 take precedence)}
- G05B 2219/39529: Force, torque sensor in wrist, end effector
- G05B 2219/40367: Redundant manipulator
- G05B 2219/40599: Force, torque sensor integrated in joint
- B25J: MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES ({manipulators specially adapted for use in surgery A61B34/70; manipulators used in cleaning hollow articles B08B9/04}; manipulators associated with rolling mills B21B39/20; manipulators associated with forging machines B21J13/10; {manipulators associated with picking-up and placing mechanisms B23P19/007}; means for holding wheels or parts thereof B60B30/00; {vehicles with ground-engaging propulsion means, e.g. walking members B62D57/02, B62D57/032; devices for picking-up and depositing articles or materials between conveyors B65G47/90, B65G47/91; manipulators with gripping or holding means for transferring packages B65H67/065}; cranes B66C; {manipulators used in the protection or supervision of pipe-line installations F17D5/00; walking equipment adapted for nuclear steam-generators F22B37/006}; manipulators specially adapted for, or associated with, nuclear reactors G21C; {apparatus used for handling wafers during manufacture or treatment of semiconductor H10P72/50})
- G05B: CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS (systems for controlling or regulating non-electric variables G05D; systems for regulating electric or magnetic variables G05F; control devices or systems insofar as characterised by mechanical features only G05G)

## Abstract

The present disclosure provides a redundant robotic arm control method, a redundant robotic arm, and a computer readable storage medium. The method includes: obtaining an external force acting on an end of the robotic arm and an external torque acting on each joint; calculating a first joint speed of each joint based on a degree of influence of the joint on the end in each motion dimension and the external force acting on the end; determining a zero space speed of each joint corresponding to a current position of the end based on a link torque of an external force acting on a link with respect to the joint; calculating a total joint speed based on the first joint speed and the zero space speed; and controlling the robotic arm to the move according to the total joint speed.

## Claims

### Claim 1

A computer-implemented control method for a controller electrically connected to a redundant robotic arm, comprising steps of:
obtaining an external force acting on an end of the redundant robotic arm and an external torque acting on each joint;
calculating a first joint speed of each joint based on a degree of influence of the joint on the end in each motion dimension and the external force acting on the end, wherein the first joint speed is a speed component of the external force acting on the end corresponding to the joint;
determining a zero space speed of each joint corresponding to a current position of the end based on a link torque of an external force acting on a link with respect to the joint, wherein the zero space speed is a speed of the joint when the end remains unchanged in position while the external force is acting on the link;
calculating a total joint speed based on the first joint speed and the zero space speed; and
controlling the redundant robotic arm to the move according to the total joint speed.

### Claim 2 (depends on 1)

The method of claim 1, wherein the step of calculating the first joint speed of each joint based on the degree of influence of the joint on the end in each motion dimension and the external force acting on the end comprises:
obtaining Cartesian parameters of the end of the robotic arm, wherein the Cartesian parameters comprise an inertia parameter, a damping parameter, and a stiffness parameter of each dimension of the end;
calculating a speed deviation of the end based on the Cartesian parameters and the external force acting on the end, wherein the speed deviation is a deviation between a current speed of the end and an expected speed of a blocked task;
calculating a target speed of the end based on the speed deviation of the end; and
calculating the first joint speed of each joint based on the target speed of the end.

### Claim 3 (depends on 2)

The method of claim 2, wherein the relationship of an acceleration deviation {umlaut over ({tilde over (x)})} of the end, the speed deviation {dot over ({tilde over (x)})}of the end, a displacement deviation {tilde over (x)} of the end with respect to the external force acting on the end Fext is:
Fext=Me{dot over ({tilde over (x)})}+Be{dot over ({tilde over (x)})}+Ke{tilde over (x)};
where, Fext is a matrix of a component of the external force acting on the end in each motion dimension;
the speed deviation of the end is calculated based on the Cartesian parameters and the external force acting on the end through an equation of:
x
~
.
=
s
M
e

s
2
+
B
e

s
+
K
e

F
ext
;
where, Me is a matrix composed of the inertia parameter of each dimension of the end, Be is a matrix composed of the damping parameter of each dimension of the end, and Ke is a matrix composed of the stiffness parameter of each dimension of the end, s is a Laplace transform.

### Claim 4 (depends on 1)

The method of claim 1, wherein the step of determining the zero space speed of each joint corresponding to the current position of the end based on the link torque of the external force acting on the link with respect to the joint comprises:
calculating a first torque acting on each joint generated by the external force acting on the link;
calculating a speed deviation of each joint based on the first torque of the joint, wherein the speed deviation is a difference between an actual speed of the joint and an expected speed of the joint;
calculating a speed control weight of each joint based on a weight coefficient of the joint and the speed deviation of the joint;
calculating a zero space basis vector of the robotic arm based on a first matrix composed of the degree of influence of the joint on the end in each motion dimension and the external force acting on the end, wherein each element of the zero space basis vector represents the speed of a joint; and
calculating the zero space speed based on the speed control weight and the zero space basis vector.

### Claim 5 (depends on 4)

The method of claim 4, wherein the first torque acting on each joint generated by the external force acting on the link is calculated through equations of:
τext=τlink+JTFext; and
τlink=τext−JTFext;
where, τlink is a matrix composed of a torque acting on the joint generated by the external force acting on the link, τext is a total amount of external torques on the joint, JT is the transpose of the first matrix composed of the degree of influence of the joint on the end in each motion dimension and the external force acting on the end, Fext is a matrix of a component of the external force acting on the end in each motion dimension.

### Claim 6 (depends on 5)

The method of claim 5, wherein the speed deviation of each joint is calculated based on the first torque of the joint through equations of:
τ
link
=
M
J

q
~
¨
+
B
J

q
~
.
+
K
J

q
~
;
and
q
~
.
=
s
M
J

s
2
+
B
J

s
+
K
J

τ
link
;
where, {tilde over (q)} is a matrix composed of a displacement deviation of the joint, {dot over ({tilde over (q)})} is a matrix composed of the velocity deviation of the joint, {umlaut over ({tilde over (q)})} is a matrix composed of an angular velocity deviation of the joint, MJ is a matrix composed of an inertia parameter of the joint, BJ is a matrix composed of the damping parameter of the joint, KJ is a matrix composed of the stiffness parameter of the joint, and s is a Laplace transform.

### Claim 7 (depends on 6)

The method of claim 6, wherein the robotic arm is preset with an optimized objective function of:
g

(
q
.
)
=
1
2

(
q
.
null
-
q
~
.
)
T

W

(
q
.
null
-
q
~
.
)
;
the speed control weight of each joint is calculated based on the weight coefficient of the joint and the speed deviation of the joint through an equation of:
(
∂
g

(
q
.
)
∂
q
.
)
T
=
W

(
q
.
null
-
q
~
.
)
=
WZ


λ
-
q
~
.
=
0
;
where, {dot over (q)}null is the zero space speed.

### Claim 8

A controller for a redundant robotic arm, comprising:
a processor;
a memory coupled to the processor; and
one or more computer programs stored in the memory and executable on the processor;
wherein, the one or more computer programs comprise:
instructions for obtaining an external force acting on an end of the redundant robotic arm and an external torque acting on each joint;
instructions for calculating a first joint speed of each joint based on a degree of influence of the joint on the end in each motion dimension and the external force acting on the end, wherein the first joint speed is a speed component of the external force acting on the end corresponding to the joint;
instructions for determining a zero space speed of each joint corresponding to a current position of the end based on a link torque of an external force acting on a link with respect to the joint, wherein the zero space speed is a speed of the joint when the end remains unchanged in position while the external force is acting on the link;
instructions for calculating a total joint speed based on the first joint speed and the zero space speed; and
instructions for controlling the redundant robotic arm to the move according to the total joint speed.

### Claim 9 (depends on 8)

The controller of claim 8, wherein the instructions for calculating the first joint speed of each joint based on the degree of influence of the joint on the end in each motion dimension and the external force acting on the end comprise:
instructions for obtaining Cartesian parameters of the end of the robotic arm, wherein the Cartesian parameters comprise an inertia parameter, a damping parameter, and a stiffness parameter of each dimension of the end;
instructions for calculating a speed deviation of the end based on the Cartesian parameters and the external force acting on the end, wherein the speed deviation is a deviation between a current speed of the end and an expected speed of a blocked task;
instructions for calculating a target speed of the end based on the speed deviation of the end; and
instructions for calculating the first joint speed of each joint based on the target speed of the end.

### Claim 10 (depends on 9)

The controller of claim 9, wherein the relationship of an acceleration deviation {umlaut over ({tilde over (x)})} of the end, the speed deviation {dot over ({tilde over (x)})} of the end, a displacement deviation {tilde over (x)} of the end with respect to the external force acting on the end Fext is:
Fext=Me{umlaut over ({tilde over (x)})}+Be{dot over ({tilde over (x)})}+Ke{tilde over (x)};
where, Fext is a matrix of a component of the external force acting on the end in each motion dimension;
the speed deviation of the end is calculated based on the Cartesian parameters and the external force acting on the end through an equation of:
x
~
.
=
s
M
e

s
2
+
B
e

s
+
K
e

F
ext
;
where, Me is a matrix composed of the inertia parameter of each dimension of the end, Be is a matrix composed of the damping parameter of each dimension of the end, and Ke is a matrix composed of the stiffness parameter of each dimension of the end, s is a Laplace transform.

### Claim 11 (depends on 9)

The controller of claim 9, wherein the instructions for determining the zero space speed of each joint corresponding to the current position of the end based on the link torque of the external force acting on the link with respect to the joint comprise:
instructions for calculating a first torque acting on each joint generated by the external force acting on the link;
instructions for calculating a speed deviation of each joint based on the first torque of the joint, wherein the speed deviation is a difference between an actual speed of the joint and an expected speed of the joint;
instructions for calculating a speed control weight of each joint based on a weight coefficient of the joint and the speed deviation of the joint;
instructions for calculating a zero space basis vector of the robotic arm based on a first matrix composed of the degree of influence of the joint on the end in each motion dimension and the external force acting on the end, wherein each element of the zero space basis vector represents the speed of a joint; and
instructions for calculating the zero space speed based on the speed control weight and the zero space basis vector.

### Claim 12 (depends on 11)

The controller of claim 11, wherein the first torque acting on each joint generated by the external force acting on the link is calculated through equations of:
τext=τlink+JTFext; and
τlink=τext−JTFext;
where, τlink is a matrix composed of a torque acting on the joint generated by the external force acting on the link, τext is a total amount of external torques on the joint, JT is the transpose of the first matrix composed of the degree of influence of the joint on the end in each motion dimension and the external force acting on the end, Fext is a matrix of a component of the external force acting on the end in each motion dimension.

### Claim 13 (depends on 12)

The controller of claim 12, wherein the speed deviation of each joint is calculated based on the first torque of the joint through equations of:
τ
link
=
M
J

q
~
¨
+
B
J

q
~
.
+
K
J

q
~
;
and
q
~
.
=
s
M
J

s
2
+
B
J

s
+
K
J

τ
link
;
where, {tilde over (q)} is a matrix composed of a displacement deviation of the joint, {dot over ({tilde over (q)})} is a matrix composed of the velocity deviation of the joint, {umlaut over ({tilde over (q)})} is a matrix composed of an angular velocity deviation of the joint, MJ is a matrix composed of an inertia parameter of the joint, BJ is a matrix composed of the damping parameter of the joint, KJ is a matrix composed of the stiffness parameter of the joint, and s is a Laplace transform.

### Claim 14 (depends on 13)

The controller of claim 13, wherein the robotic arm is preset with an optimized objective function of:
g

(
q
.
)
=
1
2

(
q
.
null
-
q
~
.
)
T

W

(
q
.
null
-
q
~
.
)
;
the speed control weight of each joint is calculated based on the weight coefficient of the joint and the speed deviation of the joint through an equation of:
(
∂
g

(
q
.
)
∂
q
.
)
T
=
W

(
q
.
null
-
q
~
.
)
=
WZ


λ
-
q
~
.
;
where, {dot over (q)}null is the zero space speed.

### Claim 15

A non-transitory computer readable storage medium for storing one or more computer programs, wherein the one or more computer programs comprise:
instructions for obtaining an external force acting on an end of a redundant robotic arm and an external torque acting on each joint;
instructions for calculating a first joint speed of each joint based on a degree of influence of the joint on the end in each motion dimension and the external force acting on the end, wherein the first joint speed is a speed component of the external force acting on the end corresponding to the joint;
instructions for determining a zero space speed of each joint corresponding to a current position of the end based on a link torque of an external force acting on a link with respect to the joint, wherein the zero space speed is a speed of the joint when the end remains unchanged in position while the external force is acting on the link;
instructions for calculating a total joint speed based on the first joint speed and the zero space speed; and
instructions for controlling the redundant robotic arm to the move according to the total joint speed.

### Claim 16 (depends on 15)

The storage medium of claim 15, wherein the instructions for calculating the first joint speed of each joint based on the degree of influence of the joint on the end in each motion dimension and the external force acting on the end comprise:
instructions for obtaining Cartesian parameters of the end of the robotic arm, wherein the Cartesian parameters comprise an inertia parameter, a damping parameter, and a stiffness parameter of each dimension of the end;
instructions for calculating a speed deviation of the end based on the Cartesian parameters and the external force acting on the end, wherein the speed deviation is a deviation between a current speed of the end and an expected speed of a blocked task;
instructions for calculating a target speed of the end based on the speed deviation of the end; and
instructions for calculating the first joint speed of each joint based on the target speed of the end.

### Claim 17 (depends on 16)

The storage medium of claim 16, wherein the relationship of an acceleration deviation {umlaut over ({tilde over (x)})} of the end, the speed deviation {dot over ({tilde over (x)})} of the end, a displacement deviation {tilde over (x)} of the end with respect to the external force acting on the end Fext is:
Fext=Me{umlaut over ({tilde over (x)})}+Be{dot over ({tilde over (x)})}+Ke{tilde over (x)};
where, Fext is a matrix of a component of the external force acting on the end in each motion dimension;
the speed deviation of the end is calculated based on the Cartesian parameters and the external force acting on the end through an equation of:
x
~
.
=
s
M
e

s
2
+
B
e

s
+
K
e

F
ext
;
where, Me is a matrix composed of the inertia parameter of each dimension of the end, Be is a matrix composed of the damping parameter of each dimension of the end, and Ke is a matrix composed of the stiffness parameter of each dimension of the end, s is a Laplace transform.

### Claim 18 (depends on 16)

The storage medium of claim 16, wherein the instructions for determining the zero space speed of each joint corresponding to the current position of the end based on the link torque of the external force acting on the link with respect to the joint comprise:
instructions for calculating a first torque acting on each joint generated by the external force acting on the link;
instructions for calculating a speed deviation of each joint based on the first torque of the joint, wherein the speed deviation is a difference between an actual speed of the joint and an expected speed of the joint;
instructions for calculating a speed control weight of each joint based on a weight coefficient of the joint and the speed deviation of the joint;
instructions for calculating a zero space basis vector of the robotic arm based on a first matrix composed of the degree of influence of the joint on the end in each motion dimension and the external force acting on the end, wherein each element of the zero space basis vector represents the speed of a joint; and
instructions for calculating the zero space speed based on the speed control weight and the zero space basis vector.

### Claim 19 (depends on 18)

The storage medium of claim 18, wherein the first torque acting on each joint generated by the external force acting on the link is calculated through equations of:
τext=τlink+JTFext; and
τlink=τext−JTFext;
where, τlink is a matrix composed of a torque acting on the joint generated by the external force acting on the link, τext is a total amount of external torques on the joint, JT is the transpose of the first matrix composed of the degree of influence of the joint on the end in each motion dimension and the external force acting on the end, Fext is a matrix of a component of the external force acting on the end in each motion dimension.

### Claim 20 (depends on 19)

The storage medium of claim 19, wherein the speed deviation of each joint is calculated based on the first torque of the joint through equations of:
τ
link
=
M
J

q
~
¨
+
B
J

q
~
.
+
K
J

q
~
;
and
q
~
.
=
s
M
J

s
2
+
B
J

s
+
K
J

τ
link
;
where, {tilde over (q)} is a matrix composed of a displacement deviation of the joint, {dot over ({tilde over (q)})} is a matrix composed of the velocity deviation of the joint, {umlaut over ({tilde over (q)})} is a matrix composed of an angular velocity deviation of the joint, MJ is a matrix composed of an inertia parameter of the joint, BJ is a matrix composed of the damping parameter of the joint, KJ is a matrix composed of the stiffness parameter of the joint, and s is a Laplace transform.
