# US 12,005,584 B2 · Pose control method, robot 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 12,005,584 B2
- Document ID: US12005584B2
- Status: Granted
- Kind: B2
- Application number: 17/488,341
- Filed: 2021-09-29
- Published: 2022-06-23
- Granted: 2024-06-11
- Earliest claimed: 2020-12-22
- Assignees: Ubtech Robotics Corp Ltd
- Applicants: Ubtech Robotics Corp Ltd
- Inventors: Hongge Wang; Chunyu Chen; Yizhang Liu; Ligang Ge; Jie Bai; Xingxing Ma; Jiangchen Zhou; Youjun Xiong
- Web page: https://patentgrep.com/patent/US12005584B2

## CPC

- B25J 9/1653: {parameters identification, estimation, stiffness, accuracy, error analysis}
- B62D 57/032: with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid (B62D57/024 takes precedence)
- B25J 9/1607: {Calculation of inertia, jacobian matrixes and inverses}
- B25J 9/1664: {characterised by motion, path, trajectory planning}
- B25J 13/088: {with position, velocity or acceleration sensors}
- 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})
- B62D: MOTOR VEHICLES; TRAILERS (steering, or guiding on a desired track, of agricultural machines or implements A01B69/00; wheels, castors, axles, increasing wheel adhesion B60B; vehicle tyres, tyre inflation or tyre changing B60C; connections between vehicles of a train or the like B60D; vehicles for use on rail and road, amphibious or convertible vehicles B60F; suspension arrangements B60G; heating, cooling, ventilating or other air treating devices B60H; windows, windscreens, non-fixed roofs, doors or similar devices, protective coverings for vehicles not in use B60J; propulsion plant arrangements, auxiliary drives, transmissions, controls, instrumentation or dashboards B60K; electric equipment or propulsion of electrically-propelled vehicles B60L; power supply for electrically-propelled vehicles B60M; passenger accommodation not otherwise provided for B60N; adaptations for load transportation or to carry special loads or objects B60P; arrangement of signalling or lighting devices, the mounting or supporting thereof or circuits therefor, for vehicles in general B60Q; vehicles, vehicle fittings or vehicle parts not otherwise provided for B60R; servicing, cleaning, repairing, supporting, lifting, or manoeuvring, not otherwise provided for, B60S; brake arrangements, brake control systems or parts thereof B60T; air-cushion vehicles B60V; motor-cycles, accessories therefor B62J, B62K; testing of vehicles G01M)

## Abstract

A pose control method for a robot includes: estimating a first set of joint angular velocities of all joints of the robot according to a balance control algorithm; estimating a second set of joint angular velocities of all joints of the robot according to a momentum planning algorithm; estimating a third set of joint angular velocities of all joints of the robot according to a pose return-to-zero algorithm; and performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities.

## Claims

### Claim 1

A computer-implemented pose control method for a robot, comprising:
estimating a first set of joint angular velocities of all joints of the robot according to a balance control algorithm;
estimating a second set of joint angular velocities of all joints of the robot according to a momentum planning algorithm;
estimating a third set of joint angular velocities of all joints of the robot according to a pose return-to-zero algorithm; and
performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities;
wherein estimating the second set of joint angular velocities of all joints of the robot according to the momentum planning algorithm comprises:
obtaining actual joint angular velocities of the robot;
calculating an actual momentum of the robot according to the actual joint angular velocities;
constructing a counter-rotational momentum corresponding to the actual momentum; and
calculating the second set of joint angular velocities according to the counter-rotational momentum; and
wherein the actual momentum of the robot is calculated according to the following equation: mfeedback=Aωfeedback, where A represents a predefined mapping matrix, ωfeedback represents the actual angular velocities, and mfeedback represents the actual momentum of the robot.

### Claim 2 (depends on 1)

The method of claim 1, wherein the counter-rotational momentum is constructed according to the following equation:
m
2
=
[
0
0
0
-
k
x
×
m
feedback
⁡
(
0
)
-
k
y
×
m
feedback
⁡
(
1
)
0
]
,
where mfeedback(0) represents a rotational component of the actual momentum in an x-axis direction, mfeedback(1) represents a rotational component of the actual momentum in a y-axis direction, and m2 represents the counter-rotational momentum.

### Claim 3 (depends on 1)

The method of claim 1, wherein the second set of joint angular velocities is calculated according to the following equation: ω2=A*m2, where A represents the predefined mapping matrix, A* represents a generalized inverse matrix of A, m2 represents the counter-rotational momentum, and ω2 represents the second set of joint angular velocities.

### Claim 4 (depends on 1)

The method of claim 1, wherein estimating the third set of joint angular velocities of all joints of the robot according to the pose return-to-zero algorithm comprises:
obtaining an actual pose of each end of the robot, wherein the ends of the robot comprise ends of arms of the robot or ends of feet of the robot;
calculating a desired speed of each end of the robot according to the actual pose and a predefined initial pose of each end of the robot;
calculating joint angular velocities of the robot according to the desired speed of each end of the robot; and
combining the joint angular velocities of the robot into the third joint angular velocities.

### Claim 5 (depends on 4)

The method of claim 4, wherein the joint angular velocities of the robot are calculated according to the following equation: ω3,i=Jivi, where vi represents the desired speed of an i-th end of the robot, Ji represents a kinematic Jacobian matrix from a robot coordinate system to an i-th end coordinate system, ω3,i represents angular velocities of all joints in an i-th chain.

### Claim 6 (depends on 1)

The method of claim 1, wherein performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities, comprises:
calculating desired joint angular velocities of the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities;
calculating desired joint angles of the robot according to the desired joint angular velocities; and
controlling a pose of the robot according to the desired joint angles.

### Claim 7

A robot comprising:
one or more processors;
a memory; and
one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs comprises:
instructions for estimating a first set of joint angular velocities of all joints of the robot according to a balance control algorithm;
instructions for estimating a second set of joint angular velocities of all joints of the robot according to a momentum planning algorithm;
instructions for estimating a third set of joint angular velocities of all joints of the robot according to a pose return-to-zero algorithm; and
instructions for performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities;
wherein the instructions for estimating the third set of joint angular velocities of all joints of the robot according to the pose return-to-zero algorithm comprise:
instructions for obtaining an actual pose of each end of the robot, wherein the ends of the robot comprise ends of arms of the robot or ends of feet of the robot;
instructions for calculating a desired speed of each end of the robot according to the actual pose and a predefined initial pose of each end of the robot;
instructions for calculating joint angular velocities of the robot according to the desired speed of each end of the robot; and
instructions for combining the joint angular velocities of the robot into the third joint angular velocities; and
wherein the joint angular velocities of the robot are calculated according to the following equation: ω3,i=Jivi, where vi represents the desired speed of an i-th end of the robot, Ji represents a kinematic Jacobian matrix from a robot coordinate system to an i-th end coordinate system, ω3,i represents joint angular velocities of all joints in an i-th chain.

### Claim 8 (depends on 7)

The robot of claim 7, wherein the instructions for estimating the second set of joint angular velocities of all joints of the robot according to the momentum planning algorithm comprise:
instructions for obtaining actual joint angular velocities of the robot;
instructions for calculating an actual momentum of the robot according to the actual joint angular velocities;
instructions for constructing a counter-rotational momentum corresponding to the actual momentum; and
instructions for calculating the second set of joint angular velocities according to the counter-rotational momentum.

### Claim 9 (depends on 8)

The robot of claim 8, wherein the actual momentum of the robot is calculated according to the following equation: mfeedback=Aωfeedback, where A represents a predefined mapping matrix, ωfeedback represents the actual angular velocities, and mfeedback represents the actual momentum of the robot.

### Claim 10 (depends on 8)

The robot of claim 8, wherein the counter-rotational momentum is constructed according to the following equation:
m
2
=
[
0
0
0
-
k
x
×
m
feedback
⁡
(
0
)
-
k
y
×
m
feedback
⁡
(
1
)
0
]
,
where mfeedback(0) represents a rotational component of the actual momentum in an x-axis direction, mfeedback(1) represents a rotational component of the actual momentum in a y-axis direction, and m2 represents the counter-rotational momentum.

### Claim 11 (depends on 8)

The robot of claim 8, wherein the second set of joint angular velocities is calculated according to the following equation: ω2=A*m2, where A represents a predefined mapping matrix, A* represents a generalized inverse matrix of A, m2 represents the counter-rotational momentum, and ω2 represents the second set of joint angular velocities.

### Claim 12 (depends on 7)

The robot of claim 7, wherein the instructions for performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities, comprises:
instructions for calculating desired joint angular velocities of the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities;
instructions for calculating desired joint angles of the robot according to the desired joint angular velocities; and
instructions for controlling a pose of the robot according to the desired joint angles.

### Claim 13

A non-transitory computer-readable storage medium storing one or more programs to be executed in a robot, the one or more programs, when being executed by one or more processors of the legged robot, causing the robot to perform processing comprising:
estimating a first set of joint angular velocities of all joints of the robot according to a balance control algorithm;
estimating a second set of joint angular velocities of all joints of the robot according to a momentum planning algorithm;
estimating a third set of joint angular velocities of all joints of the robot according to a pose return-to-zero algorithm; and
performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities;
wherein estimating the second set of joint angular velocities of all joints of the robot according to the momentum planning algorithm comprises:
obtaining actual joint angular velocities of the robot;
calculating an actual momentum of the robot according to the actual joint angular velocities;
constructing a counter-rotational momentum corresponding to the actual momentum; and
calculating the second set of joint angular velocities, by using a predefined mapping matrix and a generalized inverse matrix of the predefined mapping matrix, according to the counter-rotational momentum.

### Claim 14 (depends on 13)

The computer-readable storage medium of claim 13, wherein the actual momentum of the robot is calculated according to the following equation: mfeedback=Aωfeedback, where A represents the predefined mapping matrix, ωfeedback represents the actual angular velocities, and mfeedback represents the actual momentum of the robot.

### Claim 15 (depends on 13)

The computer-readable storage medium of claim 13, wherein the counter-rotational momentum is constructed according to the following equation:
m
2
=
[
0
0
0
-
k
x
×
m
feedback
⁡
(
0
)
-
k
y
×
m
feedback
⁡
(
1
)
0
]
,
where mfeedback(0) represents a rotational component of the actual momentum in an x-axis direction, mfeedback(1) represents a rotational component of the actual momentum in a y-axis direction, and m2 represents the counter-rotational momentum.

### Claim 16 (depends on 15)

The computer-readable storage medium of claim 15, wherein kx and ky are preset proportional constants, kx∈(0,1), and ky∈(0,1).

### Claim 17 (depends on 13)

The computer-readable storage medium of claim 13, wherein estimating the third set of joint angular velocities of all joints of the robot according to the pose return-to-zero algorithm comprises:
obtaining an actual pose of each end of the robot, wherein the ends of the robot comprise ends of arms of the robot or ends of feet of the robot;
calculating a desired speed of each end of the robot according to the actual pose and a predefined initial pose of each end of the robot;
calculating joint angular velocities of the robot according to the desired speed of each end of the robot; and
combining the joint angular velocities of the robot into the third joint angular velocities.

### Claim 18 (depends on 17)

The computer-readable storage medium of claim 17, wherein the joint angular velocities of the robot are calculated according to the following equation: ω3,i=Jivi, where vi represents the desired speed of an i-th end of the robot, Ji represents a kinematic Jacobian matrix from a robot coordinate system to an i-th end coordinate system, ω3,i represents angular velocities of all joints in an i-th chain.

### Claim 19 (depends on 13)

The computer-readable storage medium of claim 13, wherein performing pose control on the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities, comprises:
calculating desired joint angular velocities of the robot according to the first set of joint angular velocities, the second set of joint angular velocities, and the third set of joint angular velocities;
calculating desired joint angles of the robot according to the desired joint angular velocities; and
controlling a pose of the robot according to the desired joint angles.

### Claim 20 (depends on 19)

The computer-readable storage medium of claim 19, wherein the desired joint angles of the robot is calculated by integrating the desired joint angular velocities of the robot.