WO2022247170A1 - 机器人平衡控制方法、装置、机器人控制设备及存储介质 - Google Patents

机器人平衡控制方法、装置、机器人控制设备及存储介质 Download PDF

Info

Publication number
WO2022247170A1
WO2022247170A1 PCT/CN2021/131677 CN2021131677W WO2022247170A1 WO 2022247170 A1 WO2022247170 A1 WO 2022247170A1 CN 2021131677 W CN2021131677 W CN 2021131677W WO 2022247170 A1 WO2022247170 A1 WO 2022247170A1
Authority
WO
WIPO (PCT)
Prior art keywords
motion
expected
plantar
robot
parameters
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/CN2021/131677
Other languages
English (en)
French (fr)
Inventor
谢岩
熊友军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to US17/710,949 priority Critical patent/US12134194B2/en
Publication of WO2022247170A1 publication Critical patent/WO2022247170A1/zh
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Program-controlled manipulators
    • B25J9/16—Program controls
    • B25J9/1602—Program controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Program-controlled manipulators
    • B25J9/16—Program controls
    • B25J9/1656—Program controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Program controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Program-controlled manipulators
    • B25J9/16—Program controls
    • B25J9/1656—Program controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Program controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Definitions

  • the present application relates to the technical field of robot control, in particular, to a robot balance control method, device, robot control equipment and storage medium.
  • humanoid robot is an important research branch of existing robot technology.
  • the premise of humanoid robots to realize their own walking and operating capabilities is that they can maintain balance in complex environments.
  • the balance control ability of humanoid robots is the key issue that restricts humanoid robots from becoming practical.
  • the purpose of this application includes providing a robot balance control method, device, robot control equipment and storage medium, which can correct the trajectory of the robot's motion planning in real time according to the real motion status of the robot, and reduce the impact of the robot's environment on the humanoid robot.
  • the motion balance interference of the humanoid robot can be achieved, and the body movement of the humanoid robot can be in line with the law of motion of the robot while achieving the expected motion balance effect, so as to improve the safety of the robot operation.
  • the present application provides a robot balance control method, the method comprising:
  • the motion status information and the expected state parameters calculate the expected driving parameters of the humanoid robot that currently satisfy the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints ;
  • the operating state of each real joint in the humanoid robot is controlled according to the expected driving parameters, so that the humanoid robot is kept balanced according to the expected motion track.
  • the step of performing parameter adaptation and adjustment on the motion planning trajectory according to the motion status information to obtain an expected motion trajectory matching the motion status information includes:
  • the plantar planned pose and plantar planned velocity in the motion planning track are matched and replaced to obtain the expected plantar pose and plantar expected velocity of the foot in the desired track;
  • the planned plantar pose and planned velocity of the foot in the motion planning trajectory are directly used as The expected position and posture of the sole of the foot and the expected velocity of the sole of the foot in the desired motion trajectory;
  • the gravity of the humanoid robot is used to update the parameters of the planned plantar force parameters of the two feet in the motion planning trajectory, so as to obtain the expected plantar force parameters of the two feet in the expected motion trajectory.
  • the expected state parameters include the expected acceleration of the sole, the expected linear dynamic force of the center of mass, and the expected acceleration of the waist
  • the motion planning trajectory includes respective planning state parameters corresponding to the sole, the center of mass, and the waist
  • the planned state parameters include the planned acceleration of the sole, the planned line dynamics of the center of mass, and the planned acceleration of the waist.
  • the expected pose of the balance requirement part is subtracted from the actual pose, and the expected velocity of the balance requirement part is subtracted from the actual velocity to obtain the corresponding The pose difference value and speed difference value of ;
  • the force control parameter distribution constraints include joint moment distribution constraints and plantar force parameter distribution constraints, and the humanoid
  • the steps for the robot to meet the expected driving parameters of the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints at the same time include:
  • the third to-be-recursive driving parameter is substituted into the third relational expression corresponding to the waist control demand, the expected plantar force parameter trend equation corresponding to the plantar control demand, Solve the convex quadratic programming problem in the joint moment inequality and the plantar force parameter inequality group, and obtain the driving parameters to be output that can be used as the expected driving parameters in the fourth null space, wherein the second The correlation expression is used to represent the correlation between the driving parameters to be optimized and the desired state parameters of the waist.
  • the step of force controlling the expected driving parameters of the parameter distribution constraints further includes:
  • the solution of the convex quadratic programming problem of any one of the first null space, the second null space, the third null space and the fourth null space fails, or the first null space
  • the total time-consuming solution of the convex quadratic programming problem corresponding to the second null space, the third null space and the fourth null space is greater than the cycle duration corresponding to the current control cycle, then the humanoid robot
  • the used driving parameters corresponding to the previous control cycle of the current control cycle are used as the expected driving parameters.
  • ⁇ min is used to represent the lower limit of joint torque of the humanoid robot
  • ⁇ max is used to represent the upper limit of joint torque of the humanoid robot
  • S a is used to represent the drivable joint selection matrix of the humanoid robot
  • M is used to represent the The current inertia matrix of the humanoid robot
  • G is used to represent the current gravity matrix of the humanoid robot
  • C is used to represent the current nonlinear matrix of the humanoid robot produced by centrifugal force and Coriolis force
  • J is used to represent the current gravity matrix of the humanoid robot
  • the Jacobian matrix of the plantar force acting point is used to represent the angular acceleration of each joint included in the driving parameters to be optimized
  • F is used to represent the plantar force parameters included in the driving parameters to be optimized.
  • the expression of the plantar force parameter inequality group is as follows:
  • F z is used to represent the component of the plantar force vector in the Z-axis direction in the Cartesian coordinate system in the plantar force parameters included in the driving parameters to be optimized
  • Fx is used to represent the plantar force included in the driving parameters to be optimized
  • F y is used to represent the Y-axis direction of the plantar force vector in the Cartesian coordinate system in the plantar force parameters included in the driving parameters to be optimized
  • ⁇ y is used to represent the component of the plantar moment vector in the Y-axis direction in the Cartesian coordinate system in the plantar force parameters included in the driving parameters to be optimized
  • ⁇ x is used to represent the driving parameters to be optimized.
  • d x,min is used to represent the lower boundary of the plantar support domain in the direction of the X-axis in the Cartesian coordinate system
  • d x,max is used to indicate the upper limit of the plantar support domain in the direction of the X-axis in the Cartesian coordinate system
  • d y,min is used to indicate the lower limit of the plantar support domain in the direction of the Y-axis in the Cartesian coordinate system
  • Boundary, d y, max is used to indicate the upper boundary of the plantar support domain in the direction of the Y axis in the Cartesian coordinate system
  • ⁇ is used to indicate the friction coefficient between the sole of the humanoid robot and the support surface of the foot.
  • the present application provides a robot balance control device, the device comprising:
  • the motion data acquisition module is used to obtain the current motion status information and motion planning trajectory of the humanoid robot
  • a trajectory correction and adaptation module configured to perform parameter adaptation and adjustment on the motion planning trajectory according to the motion status information, to obtain an expected motion trajectory that matches the motion status information
  • the expected state determination module is used to determine the expected state parameters of the humanoid robot tending to the expected motion trajectory at the sole, center of mass and waist of the humanoid robot according to the motion condition information;
  • the expected drive calculation module is used to calculate that the humanoid robot currently satisfies the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameters according to the motion status information and the desired state parameters Expected driving parameters for distributional constraints;
  • the joint balance control module is used to control the running state of each real joint in the humanoid robot according to the expected driving parameters, so as to keep the humanoid robot in balance according to the expected motion track.
  • the present application provides a robot control device, which includes a processor and a memory, the memory stores a computer program that can be executed by the processor, and the processor can execute the computer program to realize the aforementioned embodiments
  • the robot balance control method described in any one is not limited to a robot control device, which includes a processor and a memory, the memory stores a computer program that can be executed by the processor, and the processor can execute the computer program to realize the aforementioned embodiments.
  • the present application provides a storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the robot balance control method described in any one of the foregoing embodiments is implemented.
  • the application After obtaining the current motion status information and motion planning trajectory of the humanoid robot, the application will adapt and adjust the parameters of the motion planning trajectory to obtain the desired motion trajectory that matches the motion status information, and then determine the humanoid robot based on the motion status information
  • the expected state parameters tend to the desired motion trajectory at the sole, center of mass, and waist respectively, and according to the motion status information and the expected state parameters, calculate that the humanoid robot currently satisfies the robot dynamics requirements, the sole control requirements, and the center of mass control. demand, waist control demand, and the expected driving parameters of force control parameter distribution constraints, and then control the running state of each real joint in the humanoid robot according to the obtained expected driving parameters, so as to plan the trajectory of the robot motion according to the real motion status of the robot.
  • Real-time corrections are made to reduce the interference of the robot's environment on the motion balance of the humanoid robot, ensuring that the body movements of the humanoid robot conform to the robot's motion laws while achieving the expected motion balance effect, and improve the safety of the robot's operation.
  • FIG. 1 is a schematic diagram of the composition of a robot control device provided in an embodiment of the present application
  • FIG. 2 is a schematic flowchart of a robot balance control method provided in an embodiment of the present application
  • FIG. 3 is a schematic flow chart of substeps included in step S220 in FIG. 2;
  • FIG. 4 is a schematic flow chart of substeps included in step S230 in FIG. 2;
  • FIG. 5 is one of the schematic flow charts of the substeps included in step S240 in FIG. 2;
  • FIG. 6 is the second schematic flow diagram of the sub-steps included in step S240 in FIG. 2;
  • FIG. 7 is a schematic diagram of the composition of the robot balance control device provided by the embodiment of the present application.
  • Icons 10-robot control equipment; 11-memory; 12-processor; 13-communication unit; 14-sensing unit; 100-robot balance control device; 110-motion data acquisition module; 120-trajectory correction adaptation module; 130-desired state determination module; 140-desired driving calculation module; 150-joint balance control module.
  • the existing robot balance control scheme divides the overall balance control task into multiple control subtasks according to the system composition (for example, ground force control, momentum control, whole body dynamics control, etc.), and then Use the zero-space projection method to decouple each control subtask, so that the low-priority control subtask is in the null space of the high-priority control subtask, so as to ensure that the low-priority control subtask does not affect the high-priority control subtask execution, thereby avoiding control interference among multiple control subtasks.
  • the system composition for example, ground force control, momentum control, whole body dynamics control, etc.
  • this robot balance control scheme does not consider the interference effect of the robot's environment (for example, shaking or bumpy plantar support surface) on each control subtask, and also does not consider the operating safety restrictions of the robot itself, resulting in the final output The control results often cannot achieve the expected motion balance effect.
  • the embodiment of the present application achieves the aforementioned object by providing a robot balance control method, device, robot control device and storage medium.
  • FIG. 1 is a schematic diagram of the composition of a robot control device 10 provided by an embodiment of the present application.
  • the robot control device 10 is used to regulate the motion status of the humanoid robot, so that by correcting the motion planning trajectory of the humanoid robot in real time, the corrected planned trajectory is consistent with the humanoid robot's Match the real motion conditions to reduce the interference of the robot's environment on the motion balance of the humanoid robot, and ensure that the body movement of the humanoid robot conforms to the robot's motion laws while achieving the expected motion balance effect by adjusting the joint operating state, improving the safety of the robot operation sex.
  • the robot control device 10 may be remotely connected to the humanoid robot, or may be integrated with the humanoid robot.
  • the humanoid robot may include a robot upper body, a hip joint driving structure, and two mechanical leg structures. Structural connection, the upper body of the robot is connected with the hip joint driving structure, and the position where the hip joint driving structure is connected with the upper body of the robot is between two mechanical leg structures.
  • Two mutually orthogonal rotatable drive structures may be provided at the position where the hip joint drive structure is connected to each mechanical leg structure, so as to realize the alternate position change between the two mechanical leg structures, and each A rotatable drive structure is provided at the knee joint of the mechanical leg structure for realizing the knee-bending operation of the corresponding mechanical leg structure.
  • the robot control device 10 may include a robot balance control device 100 , a memory 11 , a processor 12 , a communication unit 13 and a sensing unit 14 .
  • the components of the memory 11 , the processor 12 , the communication unit 13 and the sensor unit 14 are directly or indirectly electrically connected to each other to realize data transmission or interaction.
  • the memory 11 , the processor 12 , the communication unit 13 and the sensing unit 14 can be electrically connected to each other through one or more communication buses or signal lines.
  • the memory 11 can be, but not limited to, Random Access Memory (Random Access Memory, RAM), Read Only Memory (Read Only Memory, ROM), Programmable Read-Only Memory (Programmable Read-Only Memory, PROM), Erasable Programmable Read-Only Memory (EPROM), Electric Erasable Programmable Read-Only Memory (EEPROM), etc.
  • RAM Random Access Memory
  • ROM Read Only Memory
  • PROM Programmable Read-Only Memory
  • PROM Programmable Read-Only Memory
  • EPROM Erasable Programmable Read-Only Memory
  • EEPROM Electric Erasable Programmable Read-Only Memory
  • the processor 12 may be an integrated circuit chip capable of processing signals.
  • Described processor 12 can be general-purpose processor, comprises central processing unit (Central Processing Unit, CPU), graphics processing unit (Graphics Processing Unit, GPU) and network processor (Network Processor, NP), digital signal processor (DSP) ), application-specific integrated circuits (ASICs), off-the-shelf programmable gate arrays (FPGAs) or other programmable logic devices, discrete gate or transistor logic devices, and discrete hardware components.
  • the general-purpose processor may be a microprocessor, or the processor may be any conventional processor, etc., and may realize or execute various methods, steps, and logic block diagrams disclosed in the embodiments of the present application.
  • the communication unit 13 is used to establish a communication connection between the robot control device 10 and other electronic devices through the network, and send and receive data through the network, wherein the network includes a wired communication network and a wireless communication network.
  • Communications network For example, the robot control device 10 may obtain the motion planning trajectory for the humanoid robot from the operation planning device through the communication unit 13, and send a motion control instruction to the humanoid robot through the communication unit 13, making the humanoid robot move according to the motion control instruction.
  • the motion planning trajectory is a pre-planned motion trajectory corresponding to the humanoid robot.
  • the sensing unit 14 is used to perceive the motion status information of the humanoid robot, such as the humanoid robot in each joint (including each real joint of the robot and the space posture used to describe the robot waist).
  • the actual angular velocity at each virtual joint) is monitored, the plantar force data used to represent the real plantar force parameter value at the biped sole of the humanoid robot is monitored, and each body part of the humanoid robot (such as , waist, sole and center of mass) to monitor the actual pose and actual velocity.
  • the robot balance control device 100 includes at least one software function module that can be stored in the memory 11 or in the operating system of the robot control device 10 in the form of software or firmware.
  • the processor 12 can be used to execute executable modules stored in the memory 11 , such as software function modules and computer programs included in the robot balance control device 100 .
  • the robot balance control device 100 can correct the robot's motion planning trajectory in real time according to the real motion status of the robot, reduce the interference of the robot's environment on the motion balance of the humanoid robot, and ensure that the body movement of the humanoid robot achieves the desired motion balance effect At the same time, it conforms to the law of robot motion and improves the safety of robot operation.
  • FIG. 1 is only a schematic composition diagram of the robot control device 10, and the robot control device 10 may also include more or less components than those shown in FIG. 1, or have A different configuration than that shown in Figure 1.
  • Each component shown in Fig. 1 may be implemented by hardware, software or a combination thereof.
  • the present application realizes the foregoing functions by providing a robot balance control method applied to the above robot control device 10 .
  • the robot balance control method provided by the embodiment of the present application will be described accordingly below.
  • FIG. 2 is a schematic flowchart of a robot balance control method provided in an embodiment of the present application.
  • the robot balance control method shown in FIG. 2 is as follows.
  • Step S210 acquiring the current motion status information and motion planning trajectory of the humanoid robot.
  • the robot control device 10 when the robot control device 10 controls the humanoid robot to move, it will determine that the humanoid robot is currently in contact with the sole support surface for supporting the humanoid robot to maintain a standing state.
  • the supporting leg is then used as the origin of the world coordinate system at the point of action of the supporting surface in contact with the supporting leg, and a Cartesian coordinate system is established to describe the movement of the humanoid robot.
  • the positive direction of the X-axis in the established Cartesian coordinate system usually indicates the forward direction of the humanoid robot, and the positive direction of the Z-axis in the Cartesian coordinate system is perpendicular to the support surface of the sole upward.
  • the robot control device 10 can detect the current corresponding motion status information of the humanoid robot in the Cartesian coordinate system through the sensing unit 14 .
  • the motion status information may include the force data on the soles of the humanoid robot's feet (including the real plantar force vector and the real plantar moment vector), the current actual pose and posture of the humanoid robot at the soles, the center of mass and the waist.
  • the actual speed, the actual angular velocity and the actual angular velocity of the humanoid robot at each joint including each real joint of the robot and each virtual joint used to describe the space pose of the robot waist), etc.
  • Step S220 performing parameter adaptation and adjustment on the motion planning trajectory according to the motion status information to obtain a desired motion trajectory matching the motion status information.
  • the robot control device 10 After the robot control device 10 acquires the current motion status information corresponding to the humanoid robot and the motion planning trajectory that needs to be realized currently, it will perform relevant parameters on the motion planning trajectory according to the current motion status information.
  • the adaptation adjustment operation, and the adjusted motion planning trajectory is used as the expected trajectory matching the current motion status information, so that the humanoid robot can reduce the impact of the robot's environment on the simulated motion when it moves according to the expected trajectory. Human-robot movement balance disturbance.
  • FIG. 3 is a schematic flowchart of substeps included in step S220 in FIG. 2 .
  • the robot control device 10 can analyze the current motion status information of the humanoid robot to determine the contact status of the humanoid robot's feet with the sole support surface in the current environment, and then based on The contact condition of the supporting surface of the robot's feet adjusts the parameters of the current motion planning trajectory to obtain the desired motion trajectory adapted to the motion condition information.
  • the robot control device 10 can realize the aforementioned functions by executing substeps S221 to S228 of the step S220 shown in FIG. 3 .
  • Sub-step S221 calculate the current zero-moment point positions of the two feet of the humanoid robot according to the force data on the soles of the feet in the motion status information.
  • the biped plantar force data includes the actual plantar force data of the left foot and the right foot of the humanoid robot, wherein the actual plantar force data includes the actual plantar force vector of the corresponding foot in the Cartesian coordinate system
  • the robot control device 10 can use the following formula to calculate the current zero-moment point position for the left foot and the right foot respectively:
  • x ZMP is used to represent the coordinate value of the zero-moment point corresponding to the foot in the X-axis direction of the Cartesian coordinate system
  • y ZMP is used to represent the zero-moment point point corresponding to the foot in the Y-axis direction of the Cartesian coordinate system
  • the coordinate value of F′ x is used to represent the component of the actual plantar force vector of the corresponding foot in the X-axis direction of the Cartesian coordinate system
  • F′ y is used to represent the actual plantar force vector of the corresponding foot in the Cartesian coordinates
  • F′ z is used to represent the component of the actual plantar force vector of the corresponding foot in the direction of the Z axis in the Cartesian coordinate system
  • T y is used to represent the actual plantar force of the corresponding foot
  • T x is used to represent the component of the moment vector on the Y axis direction under the Cartesian coordinate system
  • Sub-step S222 judging whether the respective zero-moment point positions of the two feet of the humanoid robot are within the range of the corresponding plantar support domain.
  • the plantar support domain is used to represent the minimum polygonal area formed by the contact points between the plantar surface of the corresponding foot of the humanoid robot and the plantar support surface.
  • the current movement status information corresponding to the foot can be directly used.
  • the actual plantar pose of the foot is replaced by the plantar planned pose of the foot in the motion planning trajectory to obtain the expected plantar pose of the foot in the expected motion trajectory, and the current motion status information is directly used
  • the actual plantar velocity corresponding to the foot is replaced by the planned plantar velocity of the foot in the motion planning trajectory to obtain the expected plantar pose of the foot in the expected motion trajectory.
  • Sub-step S224 for the foot whose zero-moment point position of the humanoid robot is not within the range of the corresponding plantar support domain, directly use the planned plantar pose and planned velocity of the foot in the motion planning trajectory as The expected plantar pose and expected velocity of the foot in the expected motion trajectory.
  • the current motion of the foot can be directly used
  • the plantar planning pose in the planned trajectory is used as the expected plantar pose of the foot in the expected motion trajectory
  • the planned velocity of the foot in the current motion planning trajectory is directly used as the foot
  • Sub-step S225 using the expected plantar pose of the humanoid robot's feet to update the parameters of the center of mass planned pose in the motion planning trajectory to obtain the expected pose of the center of mass in the expected motion trajectory.
  • the relationship between the desired pose of the center of mass in the desired motion trajectory and the desired pose of the soles of the feet of the humanoid robot can be expressed by the following formula:
  • r COM, ref (x, y) is used to indicate the position coordinate value of the desired pose of the center of mass in the X-axis and Y-axis directions of the Cartesian coordinate system
  • r LF, ref (x, y) is used to indicate The position coordinates of the desired plantar pose of the left foot of the humanoid robot in the X-axis and Y-axis directions of the Cartesian coordinate system
  • r RF,ref (x, y) is used to represent the desired plantar pose of the right foot of the humanoid robot
  • the position coordinate value on the X-axis and the Y-axis direction of the Cartesian coordinate system, rCOM,ref (z) is used to represent the position coordinate value of the expected pose of the center of mass on the Z-axis direction of the Cartesian coordinate system
  • r LF,ref (z) is used to represent the position coordinate value of the expected pose of the left foot of the humanoid robot in the Z-axi
  • the robot control device 10 can use the above formula to update the parameters of the center of mass planning pose in the motion planning trajectory, and obtain the matching expected centroid pose in the corresponding expected motion trajectory.
  • sub-step S226 update the parameters of the center-of-mass planning velocity in the motion planning trajectory by using the expected velocity of the feet of the humanoid robot to obtain the expected velocity of the center of mass in the expected movement trajectory.
  • the relationship between the expected velocity of the center of mass in the expected trajectory and the expected velocity of the soles of the humanoid robot's feet can be expressed by the following formula:
  • v COM, ref is used to represent the expected speed of the center of mass
  • v LF, ref is used to represent the expected speed of the plantar of the left foot of the humanoid robot
  • v RF, ref is used to represent the expected speed of the plantar of the right foot of the humanoid robot.
  • the robot control device 10 can use the above formula to update the parameters of the center-of-mass planning velocity in the motion planning trajectory, so as to obtain the matching expected velocity of the center of mass in the corresponding expected motion trajectory.
  • Sub-step S227 set the waist planning pose and waist planning velocity in the motion planning trajectory to zero respectively, and obtain the waist expected pose and waist velocity in the desired motion trajectory.
  • the gravity of the humanoid robot is used to update the parameters of the planned plantar force parameters of the two feet in the motion planning trajectory, so as to obtain the expected plantar force parameters of the two feet in the expected motion trajectory.
  • the component of the planned plantar force vector included in the planned plantar force parameter of the left foot in the motion planning trajectory in the Z-axis direction in the Cartesian coordinate system can be set as half the gravity of the humanoid robot size, and set the component of the planned plantar force vector included in the planned plantar force parameter of the right foot in the motion planning trajectory in the Z-axis direction under the Cartesian coordinate system to half the gravity of the humanoid robot, and then retain all The remaining planned plantar force parameters of the left foot and right foot in the motion planning trajectory are obtained to obtain the expected plantar force parameters of the two feet in the desired motion trajectory.
  • the present application can determine the contact status of the humanoid robot's feet with the plantar support surface in the current environment by executing the above sub-steps S221 to sub-step S228, and based on the contact status of the robot's biped feet with the support surface of the current situation.
  • the parameters of the motion planning trajectory are adjusted to obtain the desired motion trajectory adapted to the motion status information, so as to complete the real-time correction of the robot motion planning trajectory and reduce the interference of the robot's environment on the motion balance of the humanoid robot.
  • Step S230 according to the motion condition information, determine the expected state parameters of the humanoid robot at the soles, the center of mass, and the waist, respectively tending to the expected motion trajectories.
  • the robot control device 10 After the robot control device 10 determines the current expected motion track that matches the motion condition information, it will correspondingly determine the corresponding positions of the humanoid robot at the sole, center of mass, and waist. The differences in the motion status of each of the motion status information when compared with the desired motion trajectory are used to determine the desired state parameters of the soles, the center of mass and the waist to achieve a motion balance effect that matches the current desired motion trajectory.
  • the expected state parameters include the expected acceleration of the soles of both feet, the expected linear dynamic force of the center of mass, and the expected acceleration of the waist, and the expected acceleration of the soles needs to use the planned state of the soles included in the motion planning trajectory.
  • parameter i.e. the planning acceleration of the sole
  • the expected linear dynamic force of the center of mass needs to utilize the planning state parameters of the center of mass (i.e. the planned linear dynamic force of the center of mass) included in the motion planning trajectory
  • the expected acceleration of the waist needs to be The calculation is performed by using the planned state parameters of the waist (that is, the planned acceleration of the waist) included in the motion planning trajectory.
  • step S230 may include sub-steps S231 to S233, determining the desired states of the soles of the humanoid robot, the center of mass and the waist of the humanoid robot for the desired trajectory according to the robot control law parameter.
  • Sub-step S231 for each balance requirement part in the sole, center of mass and waist, subtract the expected pose and actual pose of the balance requirement part, and subtract the expected speed and actual speed of the balance requirement part Calculate and obtain the corresponding pose difference value and speed difference value.
  • the robot control device 10 when the robot control device 10 faces the three balance demand parts of the sole, center of mass and waist, it will calculate the expected pose and posture corresponding to the balance demand part at the current moment for each balance demand part.
  • a difference parameter value that matches the motion control function of the balance-demanding part is selected from the obtained pose difference value and speed difference value.
  • the motion control function is used to adjust the motion condition of the part corresponding to the balance requirement to an expected condition.
  • the control algorithm principles used by the motion control functions corresponding to different balance demand parts can be the same or different.
  • any one of the PID control algorithm, PI control algorithm, PD control algorithm, and P control algorithm can be used for the motion control function of the sole, and the PID control algorithm, PI control algorithm, and PD control algorithm can be used for the motion control function of the center of mass.
  • Any one of the PID control algorithm, the P control algorithm, the PD control algorithm and the P control algorithm can also be used for the motion control function of the waist.
  • different motion control functions for the same balance demand part may use different calculation parameters to perform related calculations of desired state parameters due to different algorithm principles involved. Therefore, after the robot control device 10 determines the pose difference value and velocity difference value corresponding to the sole, center of mass or waist at the current moment, it will follow the currently selected motion control function from the determined pose difference value and velocity In the difference value, the value of the difference parameter involved in the motion control function is selected.
  • the difference parameter values involved in the motion control function include the pose difference value and the speed difference value.
  • the motion The control function can be expressed as:
  • a F,d a' F,ref +K p,F (r F, ⁇ )+K d,F (v F, ⁇ );
  • the motion control function of the sole is implemented based on the PID control algorithm
  • the difference parameter values involved in the motion control function include pose difference and speed difference.
  • the motion control function can be expressed as:
  • a F,d a' F,ref +K p,F (r F, ⁇ )+K d,F (v F, ⁇ )+K i,F ⁇ r F, ⁇ ;
  • the motion control function of the sole of the foot is implemented based on the PI control algorithm
  • the difference parameter values involved in the motion control function include the pose difference value.
  • the motion control function can be expressed as:
  • a F,d a' F,ref +K p,F (r F, ⁇ )+K i,F ⁇ r F, ⁇ ;
  • the motion control function of the sole is implemented based on the P control algorithm
  • the difference parameter values involved in the motion control function include the pose difference value.
  • the motion control function can be expressed as:
  • a F,d a' F,ref +K p,F (r F, ⁇ ).
  • a F, d is used to represent the expected acceleration of the sole
  • a' F, ref is used to represent the planned acceleration of the sole in the motion planning trajectory
  • K p, F is used to represent the control of the plantar proportional control link
  • K i, F are used to represent the control coefficient of the plantar integral control link
  • K d, F are used to represent the control coefficient of the plantar differential control link
  • r F, ⁇ are used to represent the difference in foot pose
  • v F, ⁇ is used to represent the speed difference value of the sole.
  • the difference parameter values involved in the motion control function include pose difference value and speed difference value, at this time the motion control
  • the function can be expressed as:
  • the motion control function of the center of mass is implemented based on the PID control algorithm
  • the difference parameter values involved in the motion control function include pose difference and speed difference.
  • the motion control function can be expressed as:
  • the motion control function of the center of mass is implemented based on the PI control algorithm
  • the difference parameter values involved in the motion control function include the pose difference value.
  • the motion control function can be expressed as:
  • the motion control function of the center of mass is implemented based on the P control algorithm
  • the difference parameter values involved in the motion control function include the pose difference value.
  • the motion control function can be expressed as:
  • the difference parameter values involved in the motion control function include pose difference value and speed difference value.
  • the motion control function The function can be expressed as:
  • a T,d a' T,ref +K p,T (r T, ⁇ )+K d,T (v T, ⁇ );
  • the motion control function of the waist is implemented based on the PID control algorithm
  • the difference parameter values involved in the motion control function include pose difference value and speed difference value.
  • the motion control function can be expressed as:
  • a T,d a' T,ref +K p,T (r T, ⁇ )+K d,T (v T, ⁇ )+K i,T ⁇ r T, ⁇ ;
  • the motion control function of the waist is implemented based on the PI control algorithm
  • the difference parameter values involved in the motion control function include the pose difference value.
  • the motion control function can be expressed as:
  • a T,d a' T,ref +K p,T (r T, ⁇ )+K i,T ⁇ r T, ⁇ ;
  • the motion control function of the waist is implemented based on the P control algorithm
  • the difference parameter values involved in the motion control function include the pose difference value.
  • the motion control function can be expressed as:
  • a T,d a' T,ref +K p,T (r T, ⁇ ).
  • a T, d is used to represent the expected acceleration of the waist
  • a' T, ref is used to represent the planning acceleration of the waist in the motion planning trajectory
  • K p, T is used to represent the control coefficient of the waist proportional control link
  • K i, T are used to represent the control coefficient of the waist integral control link
  • K d, T are used to represent the control coefficient of the waist differential control link
  • r T, ⁇ are used to represent the waist pose difference value
  • v T, ⁇ are used to represent Velocity difference value for the waist.
  • sub-step S233 the planning state parameter corresponding to the balance demand part and the selected difference parameter value are substituted into the motion control function of the balance demand part for parameter calculation, and the desired state parameter of the balance demand part is obtained.
  • the robot control device 10 determines the difference parameter values related to the matching of the motion control function corresponding to the above-mentioned three balance demand parts of the sole, center of mass and waist at the current moment, the corresponding balance demand parts
  • the planned state parameters and the selected difference parameter values are substituted into the corresponding motion control function for parameter calculation, and the desired state parameters of the balance demanded part are obtained.
  • the specific motion control function can be specifically designed with reference to the above-mentioned implementation of the three balance requirements of the sole, center of mass and waist.
  • the present application can determine the expected state parameters corresponding to the soles, center of mass and waist of the humanoid robot for the expected motion trajectory according to the robot control law by executing the above sub-steps S231 to S233.
  • step S240 calculate the humanoid robot that currently satisfies the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints. expected driving parameters.
  • the robot control device 10 determines the current desired state parameters of the humanoid robot at the soles, center of mass and waist respectively, it will simultaneously satisfy the kinematics laws of the robot and the plantar control effect. , center of mass control effect, waist control effect, and device control angle of the force control parameter distribution restriction effect related to the safe operation of the robot, and determine the corresponding matching expected driving parameters to control the operating conditions of each real joint of the humanoid robot,
  • the adjusted specific state of each real joint satisfies the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints involved in the robot motion balance effect, thereby ensuring that the humanoid robot The movement of the robot conforms to the law of motion of the robot while achieving the expected motion effect, improving the safety of the robot's operation and realizing the balance of the robot's motion.
  • the robot dynamics requirement can be expressed as ensuring that the movement body action of the humanoid robot conforms to the robot kinematics
  • the sole control requirement can be expressed as ensuring that the sole of at least one foot of the humanoid robot does not leave the plantar support
  • the center of mass control requirement can be expressed as ensuring that the humanoid robot does not fall over
  • the waist control requirement can be expressed as ensuring that the humanoid robot can truly restore its balance
  • the force control parameter distribution constraints are used to represent the safe operation of the robot
  • the specific numerical distribution of the required force control parameters, the force control parameter distribution constraints may include joint moment distribution constraints and plantar force parameter distribution constraints.
  • the step S240 may include sub-steps S241 to S247, so as to calculate a balance that can ensure that the body action of the humanoid robot achieves the expected motion balance effect and conforms to the motion of the robot through the sub-steps S241 to S247.
  • Regular expectations drive parameters.
  • the expected driving parameters may be composed of expected angular accelerations of joints of the humanoid robot and expected plantar force parameters of the feet of the humanoid robot.
  • the expected plantar force parameters include the components of the expected plantar force vector corresponding to the foot in the X-axis, Y-axis and Z-axis directions in the Cartesian coordinate system, and the expected plantar moment vector corresponding to the foot in the Cartesian coordinate system The components in the X-axis, Y-axis and Z-axis directions below.
  • Sub-step S241 substituting the motion status information in the first null space into the floating base dynamics equation corresponding to the dynamic requirements of the robot and the joint torque inequality corresponding to the joint torque distribution constraints, and performing a convex quadratic calculation on the driving parameters to be optimized.
  • the programming problem is solved to obtain the first optimized driving parameters in the first null space.
  • the driving parameter to be optimized is a specific type of driving parameter corresponding to the desired driving parameter, and the driving parameter to be optimized may be composed of angular acceleration of each joint of the robot and plantar force parameters of the robot's feet.
  • the floating base dynamics equation corresponding to the robot dynamics requirement can be expressed by the following formula:
  • S f is used to represent the current floating base selection matrix of the humanoid robot
  • M is used to represent the current inertia matrix of the humanoid robot
  • G is used to represent the current gravity matrix of the humanoid robot
  • C is used to represent the current
  • J is used to represent the Jacobian matrix of the current plantar force action point of the humanoid robot
  • J is used to represent the angular acceleration of each joint included in the driving parameters to be optimized
  • F is used to represent the plantar force parameters included in the driving parameters to be optimized.
  • M, G, and C can be calculated by the robot control device 10 according to the current motion status information.
  • joint moment inequality corresponding to the joint moment distribution constraints can be substituted into the full dynamic equation of the humanoid robot for expression, wherein the full dynamic equation of the humanoid robot can be expressed by the following formula:
  • ⁇ min is used to represent the lower limit of joint torque of the humanoid robot
  • ⁇ max is used to represent the upper limit of joint torque of the humanoid robot
  • S a is used to represent the drivable joint selection matrix of the humanoid robot
  • M is used to represent the The current inertia matrix of the humanoid robot
  • G is used to represent the current gravity matrix of the humanoid robot
  • C is used to represent the current nonlinear matrix of the humanoid robot produced by centrifugal force and Coriolis force
  • J is used to represent the current gravity matrix of the humanoid robot
  • the Jacobian matrix of the plantar force acting point is used to represent the angular acceleration of each joint included in the driving parameters to be optimized
  • F is used to represent the plantar force parameters included in the driving parameters to be optimized.
  • the robot control device 10 can substitute the current motion state information into the floating basis dynamic equation and the joint moment inequality in the first null space representing the highest priority robot dynamic demand, and then These two expressions refer to the standard form of the convex quadratic programming problem to construct the corresponding QP (Quadratic Programming, convex quadratic programming) problem, and then use the open source QP solver to solve the converted QP problem to complete the optimization
  • the preliminary optimization process of the driving parameters obtains the first optimized driving parameters in the first null space satisfying the robot dynamic requirements and the distribution constraints of some force control parameters.
  • the QP problem constructed under the first null space can be expressed as:
  • x1 is used to represent the parameter to be optimized corresponding to the driving parameter to be optimized under the first null space
  • v1 is used to represent the slack variable under the first null space
  • A1 is used to represent the first zero space
  • b 1 is used to represent the second task coefficient matrix of the control task corresponding to the first null space
  • D 1 is used to represent the joint moment inequality in QP
  • f 1 is used to represent the second bias influence coefficient of the joint moment inequality in the QP problem
  • 2 is used to represent the vector two-norm.
  • Sub-step S242 according to the null space projection matrix of the first null space, the first free vector of the second null space and the first optimized driving parameters, calculate the first driving parameters to be recursed in the second null space.
  • the first to-be-recursive driving parameter of the second null space and the null space projection matrix of the first null space, the first free vector of the second null space, and the first optimized driving The parameter relationship between parameters can be expressed by the following formula:
  • x2 is used to represent the first to-be-recursive driving parameter under the second null space
  • N1 is used to represent the null space projection matrix of the first null space
  • u 2 is used to represent the first free vector of the second null space.
  • Sub-step S243 substituting the first to-be-recursive driving parameter into the plantar force parameter inequality group corresponding to the first correlation expression corresponding to the plantar control demand, the joint moment inequality, and the plantar force parameter distribution constraint condition in the second null space
  • the convex quadratic programming problem is solved in , and the second optimized driving parameters in the second null space are obtained.
  • the first correlation expression is used to represent the correlation between the driving parameter to be optimized and the desired state parameter (expected acceleration) of the sole of the foot, and the first correlation expression can adopt the following formula sub to express:
  • J F is used to represent the Jacobian matrix of the humanoid robot foot
  • Derivatives of the Jacobian matrix used to represent the sole of the humanoid robot It is used to represent the angular velocity of each joint in the current motion status information of the humanoid robot, is used to represent the angular acceleration of each joint included in the driving parameters to be optimized
  • a F, d is used to represent the expected acceleration of the sole of the foot.
  • the plantar force parameter inequality group corresponding to the plantar force parameter distribution constraints is used to describe the plantar force vector distribution of the humanoid robot when no sliding occurs and the plantar force vector distribution of the humanoid robot when no rotation occurs.
  • the moment vector distribution situation, the expression of the plantar force parameter inequality group at this moment is as follows:
  • F z is used to represent the component of the plantar force vector in the Z-axis direction in the Cartesian coordinate system in the plantar force parameters included in the driving parameters to be optimized
  • Fx is used to represent the plantar force included in the driving parameters to be optimized
  • F y is used to represent the Y-axis direction of the plantar force vector in the Cartesian coordinate system in the plantar force parameters included in the driving parameters to be optimized
  • ⁇ y is used to represent the component of the plantar moment vector in the Y-axis direction in the Cartesian coordinate system in the plantar force parameters included in the driving parameters to be optimized
  • ⁇ x is used to represent the driving parameters to be optimized.
  • d x,min is used to represent the lower boundary of the plantar support domain in the direction of the X-axis in the Cartesian coordinate system
  • d x,max is used to indicate the upper limit of the plantar support domain in the direction of the X-axis in the Cartesian coordinate system
  • d y,min is used to indicate the lower limit of the plantar support domain in the direction of the Y-axis in the Cartesian coordinate system
  • Boundary, d y, max is used to indicate the upper boundary of the plantar support domain in the direction of the Y axis in the Cartesian coordinate system
  • ⁇ is used to indicate the friction coefficient between the sole of the humanoid robot and the support surface of the foot.
  • the robot control device 10 may substitute the current motion condition information and the first driving parameter to be recursively into the first correlation expression, the In the above joint moment inequality and the plantar force parameter inequality group, then construct the corresponding QP problem with reference to the standard form of the convex quadratic programming problem for these three expressions, and then solve the QP problem of the second null space, thus in On the basis of not destroying the control task corresponding to the first null space, complete the secondary optimization process on the driving parameters to be optimized, and obtain the dynamic requirements of the robot, all force control parameter distribution constraints and part of the plantar control The second optimized driving parameter under the second null space of demand.
  • the QP problem constructed under the second null space can be expressed as:
  • sub-step S244 according to the null space projection matrix of the second null space, the second free vector of the third null space and the second optimized driving parameters, the second to-be-recursive driving parameters of the third null space are calculated.
  • the relationship between the second to-be-recursive driving parameter of the third null space and the null space projection matrix of the second null space, the second free vector of the third null space, and the second optimized driving parameter can be expressed by the following formula:
  • x 3 is used to represent the second to be recursive driving parameters under the third null space
  • N 2 is used to represent the null space projection matrix of the second null space
  • u 3 is used to represent the second free vector of the third null space.
  • Sub-step S245 in the third null space, substitute the second to-be-recursive driving parameters into the second relational expression corresponding to the center of mass control requirement, the joint moment inequality and the plantar force parameter inequality group to solve the convex quadratic programming problem, and obtain The third optimized driving parameter under the third null space.
  • the second correlation expression is used to represent the correlation between the driving parameter to be optimized and the desired state parameter (desired line power) of the center of mass, and the second correlation expression can be as follows sub to express:
  • a G, l is used to represent the momentum matrix corresponding to the center of mass of the humanoid robot, is used to represent the derivative of the momentum matrix corresponding to the center of mass of the humanoid robot, It is used to represent the angular velocity of each joint in the current motion status information of the humanoid robot, used to represent the angular acceleration of each joint included in the driving parameters to be optimized, Desired linear dynamics used to represent the center of mass.
  • the robot control device 10 may substitute the current motion condition information and the second driving parameter to be recursively into the second correlation expression, the In the above joint moment inequality and the plantar force parameter inequality group, then construct the corresponding QP problem with reference to the standard form of the convex quadratic programming problem for these three expressions, and then solve the QP problem of the third null space, thus in On the basis of not destroying the corresponding control tasks of the first null space and the second null space, complete the three optimization processes of the driving parameters to be optimized, and obtain the dynamic requirements of the robot, the center of mass control requirements, and all force control parameters.
  • the QP problem constructed under the third null space can be expressed as:
  • x 3 is used to represent the second recursive driving parameter corresponding to the driving parameter to be optimized under the third null space
  • v 3 is used for To represent the slack variable under the third null space
  • a 3 is used to represent the first task coefficient matrix of the control task corresponding to the third null space
  • b 3 is used to represent the corresponding The second task coefficient matrix for the control task.
  • the third to-be-recursive driving parameter of the fourth null space and the null space projection matrix of the third null space, the third free vector of the fourth null space, and the third optimized driving The parameter relationship between parameters can be expressed by the following formula:
  • x 4 is used to represent the third to be recursive driving parameter under the 4th null space
  • N 3 is used to represent the null space projection matrix of the 3rd null space
  • u 4 is used to represent the third free vector of the fourth null space.
  • Sub-step S247 in the fourth null space, substitute the third to-be-recursive driving parameter into the third association relationship expression corresponding to the waist control demand, the expected plantar force parameter trend equation corresponding to the plantar control demand, the joint moment inequality and The convex quadratic programming problem is solved in the plantar force parameter inequality group, and the driving parameters to be output that can be used as expected driving parameters in the fourth null space are obtained.
  • the third correlation expression is used to represent the correlation between the driving parameter to be optimized and the desired state parameter (expected acceleration) of the waist, and the third correlation expression can use the following formula Make an expression:
  • J T is used to represent the Jacobian matrix of the waist of the humanoid robot
  • Derivative of the Jacobian matrix used to represent the waist of the humanoid robot It is used to represent the angular velocity of each joint in the current motion status information of the humanoid robot, is used to represent the angular acceleration of each joint included in the driving parameters to be optimized, and a T, d are used to represent the expected acceleration of the waist.
  • the expected plantar force parameter trend equation corresponding to the plantar control demand is used to make the plantar force parameters included in the driving parameters to be optimized reach the desired plantar force parameter, and at this time the expected plantar force parameter trend equation can be expressed as :
  • FL is used to indicate the plantar force parameters of the left foot of the humanoid robot included in the driving parameters to be optimized
  • FL ,ref is used to indicate the expected plantar force parameters of the left foot of the humanoid robot
  • FR is used to indicate the parameters of the left foot of the humanoid robot to be optimized.
  • the included plantar force parameters of the right foot of the humanoid robot, F R,ref are used to represent the expected plantar force parameters of the right foot of the humanoid robot.
  • the robot control device 10 may substitute the current motion condition information and the third driving parameter to be recursively into the third correlation expression, the In the expected plantar force parameter trend equation, the joint moment inequality and the described plantar force parameter inequality group, then construct the corresponding QP problem with reference to the standard form of the convex quadratic programming problem for these four expressions, then for the fourth zero Solve the QP problem of the space, so as not to destroy the corresponding control tasks of the first null space, the second null space and the third null space, complete the four optimization processes of the driving parameters to be optimized, and satisfy The drive parameters to be output in the fourth null space of the robot dynamics requirements, center of mass control requirements, distribution constraint conditions of all force control parameters, waist control requirements and all plantar control requirements.
  • the QP problem constructed under the fourth null space can be expressed as:
  • x4 is used to represent the third driving parameter to be recursively corresponding to the driving parameter to be optimized under the fourth null space
  • v4 is used to represent the slack variable under the fourth null space
  • A4 is used to represent The first task coefficient matrix of the control task corresponding to the fourth null space
  • b 4 is used to represent the second task coefficient matrix of the control task corresponding to the fourth null space.
  • the present application can calculate the expected driving parameters that can ensure that the body action of the humanoid robot achieves the expected motion balance effect and conforms to the law of motion of the robot by executing the above sub-steps S241 to S247.
  • step S240 may further include substeps S248 and S249 after performing the above substeps S241 to S247, so as to further improve the simulated Operational safety of human-robots.
  • Sub-step S249 if the convex quadratic programming problem of any one of the first null space, the second null space, the third null space and the fourth null space fails to be solved, or the first null space, the second null space, the fourth null space.
  • the total time spent on solving the convex quadratic programming problem corresponding to the third null space and the fourth null space is greater than the cycle duration corresponding to the current control cycle, then the corresponding used time of the humanoid robot in the previous control cycle of the current control cycle driving parameters as expected driving parameters.
  • the present application can further improve the operational safety of the humanoid robot by performing the above sub-steps S248 to S249.
  • step S250 control the running state of each real joint in the humanoid robot according to the desired driving parameters, so that the humanoid robot maintains balance according to the desired motion trajectory.
  • the robot control device 10 determines the expected driving parameters that currently meet the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints.
  • the expected driving parameters extract the expected angular acceleration of each joint of the humanoid robot and the expected plantar force parameters of the feet of the humanoid robot, and then the expected angular acceleration of each joint of the humanoid robot and the humanoid robot.
  • the expected plantar force parameters of the robot's feet are substituted into the expected joint torque calculation expression to solve the current expected joint torque of the humanoid robot, and then transmit the expected joint torque to the force of each real joint of the humanoid robot Control the motor, so as to control the humanoid robot to maintain balance according to the desired motion trajectory, and achieve the desired motion balance effect.
  • the calculation expression of the expected joint torque can be expressed by the following formula:
  • the present application can perform real-time corrections to the robot motion planning trajectory according to the real motion status of the robot by executing the above steps S210 to S250, and ensure that the body movement of the humanoid robot conforms to the robot motion law while achieving the expected motion balance effect. Improve the safety of robot operation.
  • this application implements the aforementioned functions by dividing the robot balance control device 100 into functional modules.
  • the specific composition of the robot balance control device 100 provided in the present application will be described accordingly below.
  • FIG. 7 is a schematic composition diagram of a robot balance control device 100 provided in an embodiment of the present application.
  • the robot balance control device 100 may include a motion data acquisition module 110 , a trajectory correction and adaptation module 120 , a desired state determination module 130 , a desired drive calculation module 140 and a joint balance control module 150 .
  • the motion data acquisition module 110 is used to acquire the current motion status information and motion planning trajectory of the humanoid robot. Wherein, the motion data acquiring module 110 may execute the above step S210 accordingly.
  • the trajectory modification and adaptation module 120 is configured to perform parameter adaptation and adjustment on the motion planning trajectory according to the motion status information, to obtain an expected motion trajectory that matches the motion status information. Wherein, the trajectory correction and adaptation module 120 may execute the above-mentioned step S220 and sub-steps S221 to S228 accordingly.
  • the expected state determination module 130 is used to determine the expected state parameters of the humanoid robot tending to the expected motion track at the sole, center of mass and waist respectively according to the motion condition information. Wherein, the desired state determination module 130 may execute the above step S230 and substeps S231 to S233 accordingly.
  • the expected drive calculation module 140 is used to calculate the expectation that the humanoid robot currently satisfies the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints according to the motion status information and the expected state parameters. drive parameters. Wherein, the expected driving calculation module 140 may execute the above-mentioned step S240 and sub-steps S241 to S249 accordingly.
  • the joint balance control module 150 is used to control the running state of each real joint in the humanoid robot according to the expected driving parameters, so as to keep the humanoid robot balanced according to the expected motion trajectory. Wherein, the joint balance control module 150 may execute the above step S250 accordingly.
  • each block in a flowchart or block diagram may represent a module, program segment, or part of code that includes one or more Executable instructions.
  • the functions noted in the block may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or they may sometimes be executed in the reverse order, depending upon the functionality involved.
  • each block of the block diagrams and/or flowchart illustrations, and combinations of blocks in the block diagrams and/or flowchart illustrations can be implemented by a dedicated hardware-based system that performs the specified function or action , or may be implemented by a combination of dedicated hardware and computer instructions.
  • each functional module in each embodiment of the present application may be integrated to form an independent part, each module may exist independently, or two or more modules may be integrated to form an independent part. If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a storage medium.
  • the technical solution of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in the various embodiments of the present application.
  • the aforementioned storage media include: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disc, etc., which can store program codes. .
  • the motion planning trajectory will be carried out.
  • Parameter adaptation and adjustment to obtain the expected motion trajectory that matches the motion status information and then determine the expected state parameters of the humanoid robot that tend to the desired motion trajectory at the soles, center of mass, and waist of the humanoid robot according to the motion status information, and according to the motion status information Information and expected state parameters, calculate the expected driving parameters of the humanoid robot that currently meet the robot dynamics requirements, plantar control requirements, center of mass control requirements, waist control requirements and force control parameter distribution constraints, and then according to the obtained expected driving parameters.
  • the running state of each real joint in the humanoid robot is controlled, so that the trajectory of the robot's motion planning can be corrected in real time according to the real motion status of the robot, and on the basis of reducing the interference of the robot's environment on the motion balance of the humanoid robot, it can ensure that the humanoid robot
  • the body movement of the robot conforms to the law of robot motion while achieving the expected motion balance effect, improving the safety of robot operation.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)
  • Toys (AREA)

Abstract

一种机器人平衡控制方法，包括获取仿人机器人当前的运动状况信息以及运动规划轨迹；根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹；根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数；根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数；按照所述期望驱动参数对所述仿人机器人中各真实关节的运行状态进行控制，使所述仿人机器人按照所述期望运动轨迹保持平衡。该方法可以实现运动规划轨迹的实时修正，降低环境的运动平衡干扰，实现预期运动平衡效果，提升机器人运行安全性。此外还包括相应的装置、机器人控制设备及存储介质。

Description

机器人平衡控制方法、装置、机器人控制设备及存储介质
相关申请的交叉引用
本申请要求于2021年5月26日提交中国专利局的申请号为202110579167.8、名称为“机器人平衡控制方法、装置、机器人控制设备及存储介质”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请涉及机器人控制技术领域，具体而言，涉及一种机器人平衡控制方法、装置、机器人控制设备及存储介质。
背景技术
随着科学技术的不断发展，机器人技术因具有极大的研究价值及应用价值得到了各行各业的广泛重视，其中仿人机器人便是现有机器人技术的一项重要研究分支。而对仿人机器人而言，仿人机器人实现自身的行走和操作能力的前提是自身能够在复杂环境中保持平衡，仿人机器人的平衡控制能力是制约仿人机器人走向实用的关键问题。
申请内容
有鉴于此，本申请的目的包括提供一种机器人平衡控制方法、装置、机器人控制设备及存储介质，能够根据机器人真实运动状况对机器人运动规划轨迹进行实时修正，降低机器人所处环境对仿人机器人的运动平衡干扰，并使仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。
为了实现上述目的，本申请实施例采用的技术方案如下：
第一方面，本申请提供一种机器人平衡控制方法，所述方法包括：
获取仿人机器人当前的运动状况信息以及运动规划轨迹；
根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹；
根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数；
根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数；
按照所述期望驱动参数对所述仿人机器人中各真实关节的运行状态进行控制，使所述仿人机器人按照所述期望运动轨迹保持平衡。
在可选的实施方式中，所述根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹的步骤，包括：
根据所述运动状况信息中的双足足底受力数据计算所述仿人机器人双足当前各自的零力矩点位置；
判断所述仿人机器人双足各自的零力矩点位置是否处于对应足底支撑域范围内；
针对所述仿人机器人的零力矩点位置处于对应足底支撑域范围内的足部，采用所述运动状况信息中与该足部对应的足底位姿及足底速度，对该足部在所述运动规划轨迹中的足底规划位姿及足底规划速度进行匹配式替换，得到该足部在所述期望运动轨迹中的足底期望位姿及足底期望速度；
针对所述仿人机器人的零力矩点位置未处于对应足底支撑域范围内的足部，直接将该足部在所述运动规划轨迹中的足底规划位姿及足底规划速度，分别作为该足部在所述期望运动轨迹中的足底期望位姿及足底期望速度；
采用所述仿人机器人双足的足底期望位姿对所述运动规划轨迹中的质心规划位姿进行参数更新，得到所述期望运动轨迹中的质心期望位姿；
采用所述仿人机器人双足的足底期望速度对所述运动规划轨迹中的质心规划速度进行参数更新，得到所述期望运动轨迹中的质心期望速度；
将所述运动规划轨迹中的腰部规划位姿与腰部规划速度分别置为零，得到所述期望运动轨迹中的腰部期望位姿及腰部期望速度；
采用所述仿人机器人的重力对所述运动规划轨迹中的双足各自的规划足底力参数进行参数更新，得到所述期望运动轨迹中的双足各自的期望足底力参数。
在可选的实施方式中，所述期望状态参数包括足底的期望加速度、质心的期望线动力及腰部的期望加速度，所述运动规划轨迹包括足底、质心和腰部各自对应的规划状态参数，所述规划状态参数包括足底的规划加速度、质心的规划线动力及腰部的规划加速度，所述运动状况信息包括足底、质心和腰部各自的实际位姿及实际速度，所述根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数的步骤，包括：
针对足底、质心和腰部中的每项平衡需求部位，将该平衡需求部位的期望位姿与实际位姿进行减法运算，并将该平衡需求部位的期望速度与实际速度进行减法运算，得到对应的位姿差异值及速度差异值；
在得到的位姿差异值与速度差异值中选取与该平衡需求部位的运动控制函数匹配的差异参数值；
将该平衡需求部位所对应的规划状态参数及选取的差异参数值代入该平衡需求部位的运动控制函数中进行参数计算，得到该平衡需求部位的期望状态参数。
在可选的实施方式中，所述力控参数分布约束条件包括关节力矩分布约束条件及足底力参数分布约束条件，所述根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数的步骤，包括：
在第一零空间内将所述运动状况信息代入到所述机器人动力学需求所对应的浮动基动力学方程以及所述关节力矩分布约束条件所对应的关节力矩不等式中针对待优化驱动参数进行凸二次规划问题求解，得到处于所述第一零空间下的第一优化驱动参数；
根据所述第一零空间的零空间投影矩阵、第二零空间的第一自由向量以及所述第一优化驱动参数，计算所述第二零空间的第一待递归驱动参数；
在所述第二零空间内将所述第一待递归驱动参数代入与所述足底控制需求对应的第一关联关系表达式、所述关节力矩不等式及所述足底力参数分布约束条件所对应的足底力参数不等式组中进行凸二次规划问题求解，得到处于所述第二零空间下的第二优化驱动参数，其中所述第一关联关系表达式用于表示待优化驱动参数与足底的期望状态参数之间的关联关系；
根据所述第二零空间的零空间投影矩阵、第三零空间的第二自由向量以及所述第二优化驱动参数，计算所述第三零空间的第二待递归驱动参数；
在所述第三零空间内将所述第二待递归驱动参数代入与所述质心控制需求对应的第二关联关系表达式、所述关节力矩不等式及所述足底力参数不等式组中进行凸二次规划问题求解，得到处于所述第三零空间下的第三优化驱动参数，其中所述第二关联关系表达式用于表示待优化驱动参数与质心的期望状态参数之间的关联关系；
根据所述第三零空间的零空间投影矩阵、第四零空间的第三自由向量以及所述第三优化驱动参数，计算所述第四零空间的第三待递归驱动参数；
在所述第四零空间内将所述第三待递归驱动参数代入与所述腰部控制需求对应的第三关联关系表达式、与所述足底控制需求对应的期望足底力参数趋向等式、所述关节力矩不等式及所述足底力参数不等式组中进行凸二次规划问题求解，得到处于所述第四零空间下的可作为所述期望驱动参数的待输出驱动参数，其中所述第二关联关系表达式用于表示待优化驱动参数与腰部的期望状态参数之间的关联关系。
在可选的实施方式中，所述根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数的步骤，还包括：
若所述第一零空间、所述第二零空间、所述第三零空间及所述第四零空间各自对应的凸二次规划问题均求解成功，并且凸二次规划问题的求解总耗时小于当前控制周期所对应的周期时长，则直接将求解出的处于所述第四零空间下的待输出驱动参数作为所述期望驱动参数；
若所述第一零空间、所述第二零空间、所述第三零空间及所述第四零空间中任一零空间的凸二次规划问题求解失败，或者所述第一零空间、所述第二零空间、所述第三零空间及所述第四零空间所对应的凸二次规划问题的求解总耗时大于当前控制周期所对应的周期时长，则将所述仿人机器人在当前控制周期的上一控制周期下对应的已使用驱动参数作为所述期望驱动参数。
在可选的实施方式中，所述关节力矩不等式的表达式如下所示：
Figure PCTCN2021131677-appb-000001
其中，τ min用于表示仿人机器人的关节力矩下限边界，τ max用于表示仿人机器人的关节力矩上限边界，S a用于表示仿人机器人的可驱动关节选择矩阵，M用于表示仿人机器人当前的惯量矩阵，G用于表示仿人机器人当前的重力矩阵，C用于表示仿人机器人当前的由离心力和科里奥利力产生的非线性矩阵，J用于表示仿人机器人当前足底力作用点的雅可比矩阵，
Figure PCTCN2021131677-appb-000002
用于表示所述待优化驱动参数所包括的各关节角加速度，F用于表示所述待优化驱动参数所包括的足底力参数。
在可选的实施方式中，所述足底力参数不等式组的表达式如下所示：
Figure PCTCN2021131677-appb-000003
其中，F z用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的Z轴方向上的分量，F x用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的X轴方向上的分量，F y用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的Y轴方向上的分量，τ y用于表示待优化驱动参数所包括的足底力参数中的足底力矩矢量在笛卡尔坐标系下的Y轴方向上的分量，τ x用于表示待优化驱动参数所包括的足底力参数中的足底力矩矢量在笛卡尔坐标系下的X轴方向上的分量，d x,min用于表示足底支撑域在笛卡尔坐标系下的X轴方向上的下限边界，d x,max用于表示足底支撑域在笛卡尔坐标系下的X轴方向上的上限边界，d y,min用于表示足底支撑域在笛卡尔坐标系下的Y轴方向上的下限边界，d y,max用于表示足底支撑域在笛卡尔坐标系下的Y轴方向上的上限边界，μ用于表示仿人机器人足底与足底支撑面之间的摩擦力系数。
第二方面，本申请提供一种机器人平衡控制装置，所述装置包括：
运动数据获取模块，用于获取仿人机器人当前的运动状况信息以及运动规划轨迹；
轨迹修正适配模块，用于根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹；
期望状态确定模块，用于根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数；
期望驱动计算模块，用于根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制 需求及力控参数分布约束条件的期望驱动参数；
关节平衡控制模块，用于按照所述期望驱动参数对所述仿人机器人中各真实关节的运行状态进行控制，使所述仿人机器人按照所述期望运动轨迹保持平衡。
第三方面，本申请提供一种机器人控制设备，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，实现前述实施方式中任意一项所述的机器人平衡控制方法。
第四方面，本申请提供一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现前述实施方式中任意一项所述的机器人平衡控制方法。
在此情况下，本申请实施例的有益效果包括以下内容：
本申请在获取到仿人机器人当前的运动状况信息以及运动规划轨迹后，会对运动规划轨迹进行参数适配调整，得到与运动状况信息匹配的期望运动轨迹，而后根据运动状况信息确定仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数，并根据运动状况信息以及期望状态参数，计算仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数，接着按照得到的期望驱动参数对仿人机器人中各真实关节的运行状态进行控制，从而在根据机器人真实运动状况对机器人运动规划轨迹进行实时修正，降低机器人所处环境对仿人机器人的运动平衡干扰的基础上，确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。
为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本申请实施例提供的机器人控制设备的组成示意图；
图2为本申请实施例提供的机器人平衡控制方法的流程示意图；
图3为图2中的步骤S220包括的子步骤的流程示意图；
图4为图2中的步骤S230包括的子步骤的流程示意图；
图5为图2中的步骤S240包括的子步骤的流程示意图之一；
图6为图2中的步骤S240包括的子步骤的流程示意图之二；
图7为本申请实施例提供的机器人平衡控制装置的组成示意图。
图标：10-机器人控制设备；11-存储器；12-处理器；13-通信单元；14-传感单元；100-机器人平衡控制装置；110-运动数据获取模块；120-轨迹修正适配模块；130-期望状态确定模块；140-期望驱动计算模块；150-关节平衡控制模块。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
在本申请的描述中，需要理解的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。对于本领域的普通技术人员而言，可以具体情况理解上述术语在 本申请中的具体含义。
申请人通过辛苦调研发现，现有的机器人平衡控制方案是将整体的平衡控制任务按系统组成划分为多个控制子任务(例如，地面作用力控制、动量控制、全身动力学控制等)，而后使用零空间投影的方法对各控制子任务进行解耦，使低优先级控制子任务处于高优先级控制子任务的零空间内，从而确保低优先级控制子任务不影响高优先级控制子任务的执行，从而避免多个控制子任务之间的控制干扰。但这种机器人平衡控制方案并未考虑机器人所处环境(例如，晃动或颠簸的足底支撑面)对各控制子任务的干扰影响，同时也并未考虑机器人本身的运行安全限制，导致最终输出的控制结果往往并不能达到预期运动平衡效果。
在此情况下，为确保仿人机器人能够结合真实运动状况对机器人运动规划轨迹进行实时修正，降低机器人所处环境对仿人机器人的运动平衡干扰，并确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。本申请实施例通过提供一种机器人平衡控制方法、装置、机器人控制设备及存储介质实现前述目的。
下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互结合。
请参照图1，图1是本申请实施例提供的机器人控制设备10的组成示意图。在本申请实施例中，所述机器人控制设备10用于对仿人机器人的运动状况进行调控，以通过对仿人机器人的运动规划轨迹进行实时修正，使修正后的规划轨迹与仿人机器人的真实运动状况相匹配，降低机器人所处环境对仿人机器人的运动平衡干扰，并通过调节关节运行状态确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。其中，所述机器人控制设备10可以与仿人机器人远程通信连接，也可以与所述仿人机器人集成在一起。
在本申请实施例中，所述仿人机器人可以包括机器人上身、髋关节驱动结构及两个机械腿结构，两个机械腿结构分别位于所述髋关节驱动结构两侧，并与该髋关节驱动结构连接，所述机器人上身与所述髋关节驱动结构连接，且所述髋关节驱动结构与所述机器人上身连接的位置处于两个机械腿结构之间。所述髋关节驱动结构的与每个机械腿结构连接的位置处可均设置有两个相互正交的可旋转驱动结构，用以实现两个机械腿结构 之间的位置交替变换，而每个机械腿结构的膝关节处设置有一个可旋转驱动结构，用于实现对应机械腿结构的屈膝操作。
在本实施例中，所述机器人控制设备10可以包括机器人平衡控制装置100、存储器11、处理器12、通信单元13及传感单元14。所述存储器11、所述处理器12、所述通信单元13及所述传感单元14各个元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，所述存储器11、所述处理器12、所述通信单元13及所述传感单元14这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。
在本实施例中，所述存储器11可以是，但不限于，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-Only Memory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，所述存储器11用于存储计算机程序，所述处理器12在接收到执行指令后，可相应地执行所述计算机程序。
在本实施例中，所述处理器12可以是一种具有信号的处理能力的集成电路芯片。所述处理器12可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、图形处理器(Graphics Processing Unit，GPU)及网络处理器(Network Processor，NP)、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件中的至少一种。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。
在本实施例中，所述通信单元13用于通过网络建立所述机器人控制设备10与其他电子设备之间的通信连接，并通过所述网络收发数据，其中所述网络包括有线通信网络及无线通信网络。例如，所述机器人控制设备10可以通过所述通信单元13从作业规划设备处获取针对所述仿人机器人的运动规划轨迹，并通过所述通信单元13向所述仿人机器人发送运动控制指令，使所述仿人机器人按照所述运动控制指令进行运动。其中，所述运动规划轨迹为与所述仿人机器人对应的预先规划好的运动轨迹。
在本实施例中，所述传感单元14用于对仿人机器人的运动状况信息进行感知，比如对仿人机器人在各个关节(包括机器人的各真实关节以及用于描述机器人腰部空间位 姿的各虚拟关节)处的实际角速度进行监测，对仿人机器人的双足足底处的用于表示真实足底力参数数值的足底受力数据进行监测，并对仿人机器人的各个形体部位(例如，腰部、足底及质心)的实际位姿及实际速度进行监测。
在本实施例中，所述机器人平衡控制装置100包括至少一个能够以软件或固件的形式存储于所述存储器11中或者在所述机器人控制设备10的操作系统中的软件功能模块。所述处理器12可用于执行所述存储器11存储的可执行模块，例如所述机器人平衡控制装置100所包括的软件功能模块及计算机程序等。所述机器人平衡控制装置100能够根据机器人真实运动状况对机器人运动规划轨迹进行实时修正，降低机器人所处环境对仿人机器人的运动平衡干扰，并确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。
可以理解的是，图1所示的框图仅为所述机器人控制设备10的一种组成示意图，所述机器人控制设备10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。
在本申请中，为确保所述机器人控制设备10能够根据机器人真实运动状况对机器人运动规划轨迹进行实时修正，并确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性，本申请通过提供应用于上述机器人控制设备10的机器人平衡控制方法实现前述功能。下面对本申请实施例提供的机器人平衡控制方法进行相应描述。
请参照图2，图2是本申请实施例提供的机器人平衡控制方法的流程示意图。在本实施例中，图2所示的机器人平衡控制方法的如下文所示。
步骤S210，获取仿人机器人当前的运动状况信息以及运动规划轨迹。
在本实施例中，当所述机器人控制设备10控制所述仿人机器人进行运动时，会确定所述仿人机器人当前与足底支撑面接触的用于支撑所述仿人机器人保持站立状态的支撑腿，而后将与该支撑腿接触的支撑面作用点作为世界坐标系的原点，建立笛卡尔坐标系来对该仿人机器人的运动情况进行描述。其中，建立出的笛卡尔坐标系下的X轴正方向通常表示所述仿人机器人的前进方向，笛卡尔坐标系下的Z轴正方向垂直于足底支撑面向上。由此，所述机器人控制设备10可通过所述传感单元14对所述仿人机器人当前在笛卡尔坐标系下对应的运动状况信息进行探测。
其中，所述运动状况信息可以包括仿人机器人双足足底受力数据(包括真实足底力矢量及真实足底力矩矢量)、仿人机器人当前在足底、质心和腰部处的实际位姿及实际速度、仿人机器人在各个关节(包括机器人的各真实关节以及用于描述机器人腰部空间位姿的各虚拟关节)处的实际角速度和实际角速度等。
步骤S220，根据运动状况信息对运动规划轨迹进行参数适配调整，得到与运动状况信息匹配的期望运动轨迹。
在本实施例中，当所述机器人控制设备10获取到所述仿人机器人当前对应的运动状况信息以及当前需要实现的运动规划轨迹后，会根据当前运动状况信息对该运动规划轨迹进行相关参数的适配调整操作，并将调整后的运动规划轨迹作为与当前运动状况信息匹配的期望运动轨迹，使仿人机器人在按照所述期望运动轨迹进行运动的情况下，降低机器人所处环境对仿人机器人的运动平衡干扰。
可选地，请参照图3，图3是图2中的步骤S220包括的子步骤的流程示意图。在本申请实施例中，所述机器人控制设备10可通过对仿人机器人当前的运动状况信息进行分析，以确定仿人机器人双足在当前环境下各自与足底支撑面的接触状况，进而基于机器人双足的支撑面接触状况对当前的运动规划轨迹进行参数调整，得到与所述运动状况信息适配的期望运动轨迹。为此，所述机器人控制设备10可通过执行图3所示的所述步骤S220的子步骤S221～子步骤S228实现前述功能。
子步骤S221，根据运动状况信息中的双足足底受力数据计算仿人机器人双足当前各自的零力矩点位置。
在本实施例中，所述双足足底受力数据包括仿人机器人左足和右足各自的实际足底力数据，其中所述实际足底力数据包括对应足部的实际足底力矢量在笛卡尔坐标系下的X轴、Y轴及Z轴方向上的分量，和对应足部的实际足底力矩矢量在笛卡尔坐标系下的X轴、Y轴及Z轴方向上的分量。所述机器人控制设备10可采用如下公式针对左足和右足分别计算当前的零力矩点位置：
Figure PCTCN2021131677-appb-000004
其中，x ZMP用于表示对应足部的零力矩点在笛卡尔坐标系的X轴方向上的坐标值， y ZMP用于表示对应足部的零力矩点在笛卡尔坐标系的Y轴方向上的坐标值，F′ x用于表示对应足部的实际足底力矢量在笛卡尔坐标系下的X轴方向上的分量，F′ y用于表示对应足部的实际足底力矢量在笛卡尔坐标系下的Y轴方向上的分量，F′ z用于表示对应足部的实际足底力矢量在笛卡尔坐标系下的Z轴方向上的分量，T y用于表示对应足部的实际足底力矩矢量在笛卡尔坐标系下的Y轴方向上的分量，T x用于表示对应足部的实际足底力矩矢量在笛卡尔坐标系下的X轴方向上的分量，d用于表示所述传感单元14的中心位置与对应足部的足底之间的垂直距离。
子步骤S222，判断仿人机器人双足各自的零力矩点位置是否处于对应足底支撑域范围内。
在本实施例中，所述足底支撑域用于表示所述仿人机器人的对应足部的足底面与足底支撑面接触点构成的最小多边形区域。所述机器人控制设备10在确定出仿人机器人的左足和右足各自在当前对应的零力矩点位置后，会相应地根据所述仿人机器人的左足和右足各自在当前对应的足底位置，确定左足和右足各自对应的足底支撑域范围，而后分别针对左足和右足，判断对应足部的零力矩点位置是否处于该足部当前的足底支撑域范围内。
子步骤S223，针对仿人机器人的零力矩点位置处于对应足底支撑域范围内的足部，采用运动状况信息中与该足部对应的足底位姿及足底速度，对该足部在运动规划轨迹中的足底规划位姿及足底规划速度进行匹配式替换，得到该足部在期望运动轨迹中的足底期望位姿及足底期望速度。
在本实施例中，若所述仿人机器人的左足和右足中的某个足部的零力矩点位置处于对应足底支撑域范围内，则可直接采用当前运动状况信息中与该足部对应的实际足底位姿，对该足部在所述运动规划轨迹中的足底规划位姿进行替换，得到该足部在期望运动轨迹中的足底期望位姿，并且直接采用当前运动状况信息中与该足部对应的实际足底速度，对该足部在所述运动规划轨迹中的足底规划速度进行替换，得到该足部在期望运动轨迹中的足底期望位姿。
子步骤S224，针对仿人机器人的零力矩点位置未处于对应足底支撑域范围内的足部，直接将该足部在运动规划轨迹中的足底规划位姿及足底规划速度，分别作为该足部在期望运动轨迹中的足底期望位姿及足底期望速度。
在本实施例中，若所述仿人机器人的左足和右足中的某个足部的零力矩点位置未处于对应足底支撑域范围内，则可直接采用该足部在当前的所述运动规划轨迹中的足底规划位姿，作为该足部在期望运动轨迹中的足底期望位姿，并且直接采用该足部在当前的所述运动规划轨迹中的足底规划速度，作为该足部在期望运动轨迹中的足底期望位姿。
子步骤S225，采用仿人机器人双足的足底期望位姿对运动规划轨迹中的质心规划位姿进行参数更新，得到期望运动轨迹中的质心期望位姿。
在本实施例中，所述期望运动轨迹中的质心期望位姿与所述仿人机器人双足的足底期望位姿之间的关联关系可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000005
其中，r COM,ref(x,y)用于表示所述质心期望位姿在笛卡尔坐标系的X轴及Y轴方向上的位置坐标值，r LF,ref(x,y)用于表示仿人机器人左足的足底期望位姿在笛卡尔坐标系的X轴及Y轴方向上的位置坐标值，r RF,ref(x,y)用于表示仿人机器人右足的足底期望位姿在笛卡尔坐标系的X轴及Y轴方向上的位置坐标值，r COM,ref(z)用于表示所述质心期望位姿在笛卡尔坐标系的Z轴方向上的位置坐标值，r LF,ref(z)用于表示仿人机器人左足的足底期望位姿在笛卡尔坐标系的Z轴方向上的位置坐标值，r RF,ref(z)用于表示仿人机器人右足的足底期望位姿在笛卡尔坐标系的Z轴方向上的位置坐标值，Constant用于表示固定位置偏置。
由此，所述机器人控制设备10可采用上述式子对所述运动规划轨迹中的质心规划位姿进行参数更新，得到对应期望运动轨迹中匹配的质心期望位姿。
子步骤S226，采用仿人机器人双足的足底期望速度对运动规划轨迹中的质心规划速度进行参数更新，得到期望运动轨迹中的质心期望速度。
在本实施例中，所述期望运动轨迹中的质心期望速度与所述仿人机器人双足的足底期望速度之间的关联关系可采用如下式子进行表达：
v COM,ref＝(v LF,ref+v RF,ref)/2
其中，v COM,ref用于表示所述质心期望速度，v LF,ref用于表示仿人机器人左足的足 底期望速度，v RF,ref用于表示仿人机器人右足的足底期望速度。
由此，所述机器人控制设备10可采用上述式子对所述运动规划轨迹中的质心规划速度进行参数更新，得到对应期望运动轨迹中匹配的质心期望速度。
子步骤S227，将运动规划轨迹中的腰部规划位姿与腰部规划速度分别置为零，得到期望运动轨迹中的腰部期望位姿及腰部期望速度。
子步骤S228，采用仿人机器人的重力对运动规划轨迹中的双足各自的规划足底力参数进行参数更新，得到期望运动轨迹中的双足各自的期望足底力参数。
在本实施例中，可通过将所述运动规划轨迹中的左足的规划足底力参数所包括的规划足底力矢量在笛卡尔坐标系下的Z轴方向上的分量置为仿人机器人的一半重力大小，并将所述运动规划轨迹中的右足的规划足底力参数所包括的规划足底力矢量在笛卡尔坐标系下的Z轴方向上的分量置为仿人机器人的一半重力大小，而后保留所述运动规划轨迹中剩余的左足和右足各自的规划足底力参数，得到期望运动轨迹中的双足各自的期望足底力参数。
由此，本申请可通过执行上述子步骤S221～子步骤S228，确定仿人机器人双足在当前环境下各自与足底支撑面的接触状况，并基于机器人双足的支撑面接触状况对当前的运动规划轨迹进行参数调整，得到与所述运动状况信息适配的期望运动轨迹，以完成对机器人运动规划轨迹的实时修正，降低机器人所处环境对仿人机器人的运动平衡干扰。
步骤S230，根据运动状况信息确定仿人机器人在足底、质心和腰部处分别趋向于期望运动轨迹的期望状态参数。
在本实施例中，当所述机器人控制设备10确定出当前与所述运动状况信息匹配的期望运动轨迹后，会相应地确定出所述仿人机器人在足底、质心和腰部处分别对应的运动状况信息各自在与所述期望运动轨迹对比时的运动状况差异，进而确定出足底、质心和腰部各自要达到与当前期望运动轨迹匹配的运动平衡效果所需的期望状态参数。
其中，所述期望状态参数包括双足足底的期望加速度、质心的期望线动力及腰部的期望加速度，而所述足底的期望加速度需利用所述运动规划轨迹所包括的足底的规划状态参数(即足底的规划加速度)进行计算，所述质心的期望线动力需利用所述运动规划轨迹所包括的质心的规划状态参数(即质心的规划线动力)，所述腰部的期望加速度需 利用所述运动规划轨迹所包括的腰部的规划状态参数(即腰部的规划加速度)进行计算。
可选地，请参照图4，图4是图2中的步骤S230包括的子步骤的流程示意图。在本申请实施例中，所述步骤S230可以包括子步骤S231～子步骤S233，确定出符合机器人控制规律的针对期望运动轨迹的所述仿人机器人的足底、质心和腰部各自对应的期望状态参数。
子步骤S231，针对足底、质心和腰部中的每项平衡需求部位，将该平衡需求部位的期望位姿与实际位姿进行减法运算，并将该平衡需求部位的期望速度与实际速度进行减法运算，得到对应的位姿差异值及速度差异值。
在本实施例中，所述机器人控制设备10在面对足底、质心和腰部三项平衡需求部位时，会针对每项平衡需求部位计算该平衡需求部位在当前时刻下对应的期望位姿与获取到的实际位姿之间的差异状况，以及该平衡需求部位在当前时刻下对应的期望速度与获取到的实际速度之间的差异状况。即将该平衡需求部位的期望位姿与实际位姿进行减法运算，得到对应的位姿差异值；将该平衡需求部位的期望速度与实际速度进行减法运算，得到对应的速度差异值。
子步骤S232，在得到的位姿差异值与速度差异值中选取与该平衡需求部位的运动控制函数匹配的差异参数值。
在本实施例中，所述运动控制函数用于将对应平衡需求部位的运动状况调整到预期状况。不同平衡需求部位所对应的运动控制函数所采用的控制算法原理可以相同，也可以不同。比如，针对足底的运动控制函数可采用PID控制算法、PI控制算法、PD控制算法及P控制算法中的任意一种，针对质心的运动控制函数可采用PID控制算法、PI控制算法、PD控制算法及P控制算法中的任意一种，针对腰部的运动控制函数也可采用PID控制算法、PI控制算法、PD控制算法及P控制算法中的任意一种。
其中，针对同一平衡需求部位的不同运动控制函数各自也因涉及到的算法原理不同而可能采用不同的计算参数进行期望状态参数的相关运算。因此，所述机器人控制设备10在确定出足底、质心或腰部在当前时刻对应的位姿差异值及速度差异值后，会按照当前选定的运动控制函数从确定出的位姿差异值及速度差异值中，选取该运动控制函数所涉及的差异参数值。
在本实施例的一种实施方式中，若足底的运动控制函数基于PD控制算法实现时， 则该运动控制函数所涉及的差异参数值包括位姿差异值及速度差异值，此时该运动控制函数可表示为：
a F,d＝a' F,ref+K p,F(r F,Δ)+K d,F(v F,Δ)；
若足底的运动控制函数基于PID控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值及速度差异值，此时该运动控制函数可表示为：
a F,d＝a' F,ref+K p,F(r F,Δ)+K d,F(v F,Δ)+K i,F∑r F,Δ；
若足底的运动控制函数基于PI控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值，此时该运动控制函数可表示为：
a F,d＝a' F,ref+K p,F(r F,Δ)+K i,F∑r F,Δ；
若足底的运动控制函数基于P控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值，此时该运动控制函数可表示为：
a F,d＝a' F,ref+K p,F(r F,Δ)。
其中，a F,d用于表示足底的期望加速度，a′ F,ref用于表示所述运动规划轨迹中的足底的规划加速度，K p,F用于表示足底比例控制环节的控制系数，K i,F用于表示足底积分控制环节的控制系数，K d,F用于表示足底微分控制环节的控制系数，r F,Δ用于表示足底的位姿差异值，v F,Δ用于表示足底的速度差异值。
在本实施例的一种实施方式中，若质心的运动控制函数基于PD控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值及速度差异值，此时该运动控制函数可表示为：
Figure PCTCN2021131677-appb-000006
若质心的运动控制函数基于PID控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值及速度差异值，此时该运动控制函数可表示为：
Figure PCTCN2021131677-appb-000007
若质心的运动控制函数基于PI控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值，此时该运动控制函数可表示为：
Figure PCTCN2021131677-appb-000008
若质心的运动控制函数基于P控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值，此时该运动控制函数可表示为：
Figure PCTCN2021131677-appb-000009
其中，
Figure PCTCN2021131677-appb-000010
用于表示质心的期望线动力，
Figure PCTCN2021131677-appb-000011
用于表示所述运动规划轨迹中的质心的规划线动力(其数值为
Figure PCTCN2021131677-appb-000012
a′ c,ref用于表示所述运动规划轨迹中的质心的规划加速度，m用于表示所述仿人机器人的质量)，K p,C用于表示质心比例控制环节的控制系数，K i,C用于表示质心积分控制环节的控制系数，K d,C用于表示质心微分控制环节的控制系数，r C,Δ用于表示质心的位姿差异值，v C,Δ用于表示质心的速度差异值。
在本实施例的一种实施方式中，若腰部的运动控制函数基于PD控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值及速度差异值，此时该运动控制函数可表示为：
a T,d＝a' T,ref+K p,T(r T,Δ)+K d,T(v T,Δ)；
若腰部的运动控制函数基于PID控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值及速度差异值，此时该运动控制函数可表示为：
a T,d＝a' T,ref+K p,T(r T,Δ)+K d,T(v T,Δ)+K i,T∑r T,Δ；
若腰部的运动控制函数基于PI控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值，此时该运动控制函数可表示为：
a T,d＝a' T,ref+K p,T(r T,Δ)+K i,T∑r T,Δ；
若腰部的运动控制函数基于P控制算法实现时，则该运动控制函数所涉及的差异参数值包括位姿差异值，此时该运动控制函数可表示为：
a T,d＝a' T,ref+K p,T(r T,Δ)。
其中，a T,d用于表示腰部的期望加速度，a′ T,ref用于表示所述运动规划轨迹中的腰部的规划加速度，K p,T用于表示腰部比例控制环节的控制系数，K i,T用于表示腰部积分控制环节的控制系数，K d,T用于表示腰部微分控制环节的控制系数，r T,Δ用于表示腰部的位姿差异值，v T,Δ用于表示腰部的速度差异值。
子步骤S233，将该平衡需求部位所对应的规划状态参数及选取的差异参数值代入该平衡需求部位的运动控制函数中进行参数计算，得到该平衡需求部位的期望状态参数。
在本实施例中，当机器人控制设备10在确定出上述足底、质心和腰部三项平衡需求部位各自在当前时刻对应的涉及运动控制函数匹配的差异参数值后，会将对应平衡需求部位的规划状态参数与选取的差异参数值代入对应的运动控制函数进行参数计算，得到该平衡需求部位的期望状态参数。其中具体的运动控制函数可参照上述关于足底、质心和腰部三项平衡需求部位的实施方式进行具体设计。
由此，本申请可通过执行上述子步骤S231～子步骤S233，确定出符合机器人控制规律的针对期望运动轨迹的所述仿人机器人的足底、质心和腰部各自对应的期望状态参数。
请再次参照图2，步骤S240，根据运动状况信息以及期望状态参数，计算仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数。
在本实施例中，所述机器人控制设备10在确定出所述仿人机器人当前在足底、质心和腰部处分别对应的期望状态参数后，会从同时满足机器人运动学规律、足底控制效果、质心控制效果、腰部控制效果以及与机器人安全运行相关的力控参数分布限制效果的器件控制角度，确定出对应匹配的期望驱动参数来对该仿人机器人的各真实关节的运行状况进行控制，以通过调整后的各真实关节的具体状态同时满足机器人运动平衡效果所涉及的机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件，从而确保仿人机器人的机体动作在实现预期运动效果的同时符合机器人运动规律，提升机器人运行安全性，实现机器人运动平衡。
下面对本申请通过引入零空间投影与凸二次规划问题优化的概念，来求得符合当前期望运动轨迹的满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数的实例进行详细描述。其中，所述机器人动力学需求可表示为确保仿人机器人的运动机体动作符合机器人运动学，所述足底控制需求可表示为确保仿人机器人的至少一个足部的足底不离开足底支撑面，所述质心控制需求可表示为确保仿人机器人不发生倾倒，所述腰部控制需求可表示为确保仿人机器人能够真正恢复平衡，所述力控参数分布约束条件用于表示实现机器人安全运行所需的力控参数的具体数值分布，所述力控参数分布约束条件可以包括关节力矩分布约束条件及足底 力参数分布约束条件。
可选地，请参照图5，图5是图2中的步骤S240包括的子步骤的流程示意图之一。在本实施例中，所述步骤S240可以包括子步骤S241～子步骤S247，以通过所述子步骤S241～子步骤S247计算出能够确保仿人机器人的机体动作实现预期运动平衡效果且符合机器人运动规律的期望驱动参数。其中，所述期望驱动参数可由所述仿人机器人各关节的期望角加速度和所述仿人机器人双足的期望足底力参数组成。所述期望足底力参数包括对应足部的期望足底力矢量在笛卡尔坐标系下的X轴、Y轴及Z轴方向上的分量，和对应足部的期望足底力矩矢量在笛卡尔坐标系下的X轴、Y轴及Z轴方向上的分量。
子步骤S241，在第一零空间内将运动状况信息代入到机器人动力学需求所对应的浮动基动力学方程以及关节力矩分布约束条件所对应的关节力矩不等式中针对待优化驱动参数进行凸二次规划问题求解，得到处于第一零空间下的第一优化驱动参数。
在本实施例中，所述待优化驱动参数为与所述期望驱动参数对应的具体驱动参数类型，所述待优化驱动参数可由机器人各关节的角加速度和机器人双足的足底力参数组成。所述机器人动力学需求所对应的浮动基动力学方程可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000013
其中，S f用于表示仿人机器人当前的浮动基选择矩阵，M用于表示仿人机器人当前的惯量矩阵，G用于表示仿人机器人当前的重力矩阵，C用于表示仿人机器人当前的由离心力和科里奥利力产生的非线性矩阵，J用于表示仿人机器人当前足底力作用点的雅可比矩阵，
Figure PCTCN2021131677-appb-000014
用于表示所述待优化驱动参数所包括的各关节角加速度，F用于表示所述待优化驱动参数所包括的足底力参数。其中，M、G、C可由所述机器人控制设备10根据当前的运动状况信息计算得到。
而所述关节力矩分布约束条件所对应的关节力矩不等式可代入仿人机器人的全动力学方程进行表达，其中所述仿人机器人的全动力学方程可采用如下式子进行表示：
Figure PCTCN2021131677-appb-000015
此时，所述关节力矩不等式的表达式如下所示：
Figure PCTCN2021131677-appb-000016
其中，τ min用于表示仿人机器人的关节力矩下限边界，τ max用于表示仿人机器人的 关节力矩上限边界，S a用于表示仿人机器人的可驱动关节选择矩阵，M用于表示仿人机器人当前的惯量矩阵，G用于表示仿人机器人当前的重力矩阵，C用于表示仿人机器人当前的由离心力和科里奥利力产生的非线性矩阵，J用于表示仿人机器人当前足底力作用点的雅可比矩阵，
Figure PCTCN2021131677-appb-000017
用于表示所述待优化驱动参数所包括的各关节角加速度，F用于表示所述待优化驱动参数所包括的足底力参数。
因此，所述机器人控制设备10可在代表最高优先级的机器人动力学需求的第一零空间内将当前的运动状况信息代入到所述浮动基动力学方程以及所述关节力矩不等式中，而后对这两个表达式参照凸二次规划问题的标准形式构建对应的QP(Quadratic Programming，凸二次规划)问题，接着使用开源QP求解器对转换得到的QP问题进行求解，完成对所述待优化驱动参数的初步优化处理，得到满足所述机器人动力学需求以及部分力控参数分布约束条件的第一零空间下的第一优化驱动参数。
在此过程中，所述第一零空间下构建出的QP问题可表示为：
Figure PCTCN2021131677-appb-000018
s.t.D 1x 1-f 1≤v 1
其中，x 1用于表示所述第一零空间下的与待优化驱动参数对应的需优化参数，v 1用于表示所述第一零空间下的松弛变量，A 1用于表示所述第一零空间所对应的控制任务的第一任务系数矩阵，b 1用于表示所述第一零空间所对应的控制任务的第二任务系数矩阵，D 1用于表示所述关节力矩不等式在QP问题中的第一偏置影响系数，f 1用于表示所述关节力矩不等式在QP问题中的第二偏置影响系数，||·|| 2用于表示向量二范数。
子步骤S242，根据第一零空间的零空间投影矩阵、第二零空间的第一自由向量以及第一优化驱动参数，计算第二零空间的第一待递归驱动参数。
在本实施例中，所述第二零空间的第一待递归驱动参数与所述第一零空间的零空间投影矩阵、所述第二零空间的第一自由向量以及所述第一优化驱动参数之间的参数关联关系可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000019
其中，x 2用于表示所述第二零空间下的第一待递归驱动参数，N 1用于表示所述第一零空间的零空间投影矩阵，
Figure PCTCN2021131677-appb-000020
用于表示所述第一优化驱动参数，u 2用于表示所述第 二零空间的第一自由向量。
子步骤S243，在第二零空间内将第一待递归驱动参数代入与足底控制需求对应的第一关联关系表达式、关节力矩不等式及足底力参数分布约束条件所对应的足底力参数不等式组中进行凸二次规划问题求解，得到处于第二零空间下的第二优化驱动参数。
在本实施例中，所述第一关联关系表达式用于表示待优化驱动参数与足底的期望状态参数(期望加速度)之间的关联关系，所述第一关联关系表达式可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000021
其中，J F用于表示仿人机器人足底的雅可比矩阵，
Figure PCTCN2021131677-appb-000022
用于表示所述仿人机器人足底的雅可比矩阵导数，
Figure PCTCN2021131677-appb-000023
用于表示所述仿人机器人当前的运动状况信息中的各关节角速度，
Figure PCTCN2021131677-appb-000024
用于表示所述待优化驱动参数所包括的各关节角加速度，a F,d用于表示足底的期望加速度。
而所述足底力参数分布约束条件所对应的足底力参数不等式组用于描述所述仿人机器人在不发生滑动时的足底力矢量分布状况以及所述仿人机器人在不发生转动时的足底力矩矢量分布状况，此时所述足底力参数不等式组的表达式如下所示：
Figure PCTCN2021131677-appb-000025
其中，F z用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的Z轴方向上的分量，F x用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的X轴方向上的分量，F y用于表示待优化驱动参数所包 括的足底力参数中的足底力矢量在笛卡尔坐标系下的Y轴方向上的分量，τ y用于表示待优化驱动参数所包括的足底力参数中的足底力矩矢量在笛卡尔坐标系下的Y轴方向上的分量，τ x用于表示待优化驱动参数所包括的足底力参数中的足底力矩矢量在笛卡尔坐标系下的X轴方向上的分量，d x,min用于表示足底支撑域在笛卡尔坐标系下的X轴方向上的下限边界，d x,max用于表示足底支撑域在笛卡尔坐标系下的X轴方向上的上限边界，d y,min用于表示足底支撑域在笛卡尔坐标系下的Y轴方向上的下限边界，d y,max用于表示足底支撑域在笛卡尔坐标系下的Y轴方向上的上限边界，μ用于表示仿人机器人足底与足底支撑面之间的摩擦力系数。
此时，所述机器人控制设备10可在代表次优先级的足底控制需求的第二零空间内将当前运动状况信息以及第一待递归驱动参数代入到所述第一关联关系表达式、所述关节力矩不等式及所述足底力参数不等式组中，而后对这三个表达式参照凸二次规划问题的标准形式构建对应的QP问题，接着对第二零空间的QP问题进行求解，从而在不破坏第一零空间所对应的控制任务的基础上，完成对所述待优化驱动参数的二次优化处理，得到满足所述机器人动力学需求、全部力控参数分布约束条件及部分足底控制需求的第二零空间下的第二优化驱动参数。
在此过程中，所述第二零空间下构建出的QP问题可表示为：
Figure PCTCN2021131677-appb-000026
其中，
Figure PCTCN2021131677-appb-000027
用于表示针对第一零空间所对应的QP问题求解出的优化松弛变量，x 2用于表示所述第二零空间下的与待优化驱动参数对应的第一待递归驱动参数，v 2用于表示所述第二零空间下的松弛变量，A 2用于表示所述第二零空间所对应的控制任务的第一任务系数矩阵，b 2用于表示所述第二零空间所对应的控制任务的第二任务系数矩阵，D 2用于表示所述足底力参数不等式组在QP问题中的第一偏置影响系数，f 2用于表示所述足底力参数不等式组在QP问题中的第二偏置影响系数。
子步骤S244，根据第二零空间的零空间投影矩阵、第三零空间的第二自由向量以及第二优化驱动参数，计算第三零空间的第二待递归驱动参数。
在本实施例中，所述第三零空间的第二待递归驱动参数与所述第二零空间的零空间 投影矩阵、第三零空间的第二自由向量以及第二优化驱动参数之间的参数关联关系可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000028
其中，x 3用于表示所述第三零空间下的第二待递归驱动参数，N 2用于表示所述第二零空间的零空间投影矩阵，
Figure PCTCN2021131677-appb-000029
用于表示所述第二优化驱动参数，u 3用于表示所述第三零空间的第二自由向量。
子步骤S245，在第三零空间内将第二待递归驱动参数代入与质心控制需求对应的第二关联关系表达式、关节力矩不等式及足底力参数不等式组中进行凸二次规划问题求解，得到处于第三零空间下的第三优化驱动参数。
在本实施例中，所述第二关联关系表达式用于表示待优化驱动参数与质心的期望状态参数(期望线动力)之间的关联关系，所述第二关联关系表达式可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000030
其中，A G,l用于表示所述仿人机器人质心所对应的动量矩阵，
Figure PCTCN2021131677-appb-000031
用于表示所述仿人机器人质心所对应的动量矩阵导数，
Figure PCTCN2021131677-appb-000032
用于表示所述仿人机器人当前的运动状况信息中的各关节角速度，
Figure PCTCN2021131677-appb-000033
用于表示所述待优化驱动参数所包括的各关节角加速度，
Figure PCTCN2021131677-appb-000034
用于表示质心的期望线动力。
此时，所述机器人控制设备10可在代表更次优先级的质心控制需求的第三零空间内将当前运动状况信息以及第二待递归驱动参数代入到所述第二关联关系表达式、所述关节力矩不等式及所述足底力参数不等式组中，而后对这三个表达式参照凸二次规划问题的标准形式构建对应的QP问题，接着对第三零空间的QP问题进行求解，从而在不破坏第一零空间及第二零空间各自对应的控制任务的基础上，完成对所述待优化驱动参数的三次优化处理，得到满足所述机器人动力学需求、质心控制需求、全部力控参数分布约束条件及部分足底控制需求的第三零空间下的第三优化驱动参数。
在此过程中，所述第三零空间下构建出的QP问题可表示为：
Figure PCTCN2021131677-appb-000035
其中，
Figure PCTCN2021131677-appb-000036
用于表示针对第一零空间所对应的QP问题求解出的优化松弛变量，x 3用于表示所述第三零空间下的与待优化驱动参数对应的第二待递归驱动参数，v 3用于表示所述第三零空间下的松弛变量，A 3用于表示所述第三零空间所对应的控制任务的第一任务系数矩阵，b 3用于表示所述第三零空间所对应的控制任务的第二任务系数矩阵。
子步骤S246，根据第三零空间的零空间投影矩阵、第四零空间的第三自由向量以及第三优化驱动参数，计算第四零空间的第三待递归驱动参数。
在本实施例中，所述第四零空间的第三待递归驱动参数与所述第三零空间的零空间投影矩阵、所述第四零空间的第三自由向量以及所述第三优化驱动参数之间的参数关联关系可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000037
其中，x 4用于表示所述第四零空间下的第三待递归驱动参数，N 3用于表示所述第三零空间的零空间投影矩阵，
Figure PCTCN2021131677-appb-000038
用于表示所述第三优化驱动参数，u 4用于表示所述第四零空间的第三自由向量。
子步骤S247，在第四零空间内将第三待递归驱动参数代入与腰部控制需求对应的第三关联关系表达式、与足底控制需求对应的期望足底力参数趋向等式、关节力矩不等式及足底力参数不等式组中进行凸二次规划问题求解，得到处于第四零空间下的可作为期望驱动参数的待输出驱动参数。
在本实施例中，所述第三关联关系表达式用于表示待优化驱动参数与腰部的期望状态参数(期望加速度)之间的关联关系，所述第三关联关系表达式可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000039
其中，J T用于表示所述仿人机器人腰部的雅可比矩阵，
Figure PCTCN2021131677-appb-000040
用于表示所述仿人机器人腰部的雅可比矩阵导数，
Figure PCTCN2021131677-appb-000041
用于表示所述仿人机器人当前的运动状况信息中的各关节角速度，
Figure PCTCN2021131677-appb-000042
用于表示所述待优化驱动参数所包括的各关节角加速度，a T,d用于表示腰部 的期望加速度。
而与足底控制需求对应的期望足底力参数趋向等式用于使所述待优化驱动参数所包括的足底力参数达到期望足底力参数，此时所述期望足底力参数趋向等式可表达为：
Figure PCTCN2021131677-appb-000043
其中，F L用于表示待优化驱动参数所包括的仿人机器人左足的足底力参数，F L,ref用于表示仿人机器人左足的期望足底力参数，F R用于表示待优化驱动参数所包括的仿人机器人右足的足底力参数，F R,ref用于表示仿人机器人右足的期望足底力参数。
此时，所述机器人控制设备10可在代表最低优先级的腰部控制需求的第四零空间内将当前运动状况信息以及第三待递归驱动参数代入到所述第三关联关系表达式、所述期望足底力参数趋向等式、所述关节力矩不等式及所述足底力参数不等式组中，而后对这四个表达式参照凸二次规划问题的标准形式构建对应的QP问题，接着对第四零空间的QP问题进行求解，从而在不破坏第一零空间、第二零空间及第三零空间各自对应的控制任务的基础上，完成对所述待优化驱动参数的四次优化处理，得到满足所述机器人动力学需求、质心控制需求、全部力控参数分布约束条件、腰部控制需求及全部足底控制需求的第四零空间下的待输出驱动参数。
在此过程中，所述第四零空间下构建出的QP问题可表示为：
Figure PCTCN2021131677-appb-000044
其中，x 4用于表示所述第四零空间下的与待优化驱动参数对应的第三待递归驱动参数，v 4用于表示所述第四零空间下的松弛变量，A 4用于表示所述第四零空间所对应的控制任务的第一任务系数矩阵，b 4用于表示所述第四零空间所对应的控制任务的第二任务系数矩阵。
由此，本申请可通过执行上述子步骤S241～子步骤S247，计算出能够确保仿人机器人的机体动作实现预期运动平衡效果且符合机器人运动规律的期望驱动参数。
可选地，请参照图6，图6是图2中的步骤S240包括的子步骤的流程示意图之一。 在本实施例中，所述步骤S240在执行完上述子步骤S241～子步骤S247之后，还可以包括子步骤S248及子步骤S249，以通过所述子步骤S248～子步骤S249进一步提升所述仿人机器人的运行安全性。
子步骤S248，若第一零空间、第二零空间、第三零空间及第四零空间各自对应的凸二次规划问题均求解成功，并且凸二次规划问题的求解总耗时小于当前控制周期所对应的周期时长，则直接将求解出的处于第四零空间下的待输出驱动参数作为期望驱动参数。
子步骤S249，若第一零空间、第二零空间、第三零空间及第四零空间中任一零空间的凸二次规划问题求解失败，或者第一零空间、第二零空间、第三零空间及第四零空间所对应的凸二次规划问题的求解总耗时大于当前控制周期所对应的周期时长，则将仿人机器人在当前控制周期的上一控制周期下对应的已使用驱动参数作为期望驱动参数。
由此，本申请可通过执行上述子步骤S248～子步骤S249进一步提升所述仿人机器人的运行安全性。
请再次参照图2，步骤S250，按照期望驱动参数对仿人机器人中各真实关节的运行状态进行控制，使仿人机器人按照期望运动轨迹保持平衡。
在本实施例中，当所述机器人控制设备10确定出当前满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数后，可通过从所述期望驱动参数提取出所述仿人机器人各关节的期望角加速度和所述仿人机器人双足的期望足底力参数，而后将所述仿人机器人各关节的期望角加速度和所述仿人机器人双足的期望足底力参数代入到期望关节力矩计算表达式中，求解出所述仿人机器人当前的期望关节力矩，而后将所述期望关节力矩传输到所述仿人机器人各真实关节的力控电机处，从而控制所述仿人机器人按照期望运动轨迹保持平衡，达到期望运动平衡效果。其中，所述期望关节力矩计算表达式可采用如下式子进行表达：
Figure PCTCN2021131677-appb-000045
其中，
Figure PCTCN2021131677-appb-000046
用于表示所述期望驱动参数中所述仿人机器人各关节的期望角加速度，F d用于表示所述期望驱动参数中所述仿人机器人双足的期望足底力参数，τ d用于表示仿人机器人当前的期望关节力矩。
由此，本申请可通过执行上述步骤S210～步骤S250，根据机器人真实运动状况对机器人运动规划轨迹进行实时修正，并确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。
在本申请中，为确保所述机器人控制设备10能够通过所述机器人平衡控制装置100执行上述机器人平衡控制方法，本申请通过对所述机器人平衡控制装置100进行功能模块划分的方式实现前述功能。下面对本申请提供的机器人平衡控制装置100的具体组成进行相应描述。
请参照图7，图7是本申请实施例提供的机器人平衡控制装置100的组成示意图。在本申请实施例中，所述机器人平衡控制装置100可以包括运动数据获取模块110、轨迹修正适配模块120、期望状态确定模块130、期望驱动计算模块140及关节平衡控制模块150。
运动数据获取模块110，用于获取仿人机器人当前的运动状况信息以及运动规划轨迹。其中，所述运动数据获取模块110可相应执行上述步骤S210。
轨迹修正适配模块120，用于根据运动状况信息对运动规划轨迹进行参数适配调整，得到与运动状况信息匹配的期望运动轨迹。其中，所述轨迹修正适配模块120可相应执行上述步骤S220以及子步骤S221～子步骤S228。
期望状态确定模块130，用于根据运动状况信息确定仿人机器人在足底、质心和腰部处分别趋向于期望运动轨迹的期望状态参数。其中，所述期望状态确定模块130可相应执行上述步骤S230以及子步骤S231～子步骤S233。
期望驱动计算模块140，用于根据运动状况信息以及期望状态参数，计算仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数。其中，所述期望驱动计算模块140可相应执行上述步骤S240以及子步骤S241～子步骤S249。
关节平衡控制模块150，用于按照期望驱动参数对仿人机器人中各真实关节的运行状态进行控制，使仿人机器人按照期望运动轨迹保持平衡。其中，所述关节平衡控制模块150可相应执行上述步骤S250。
需要说明的是，本申请实施例所提供的机器人平衡控制装置100，其基本原理及产 生的技术效果与前述的机器人平衡控制方法相同。为简要描述，本实施例部分未提及之处，可参考上述的针对机器人平衡控制方法的描述内容。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
综上所述，在本申请提供的机器人平衡控制方法、装置、机器人控制设备及存储介质中，本申请在获取到仿人机器人当前的运动状况信息以及运动规划轨迹后，会对运动规划轨迹进行参数适配调整，得到与运动状况信息匹配的期望运动轨迹，而后根据运动状况信息确定仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数，并根据运动状况信息以及期望状态参数，计算仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数，接着按照得到的期望驱动参数对仿人机器人中各真实关节的运行状态进 行控制，从而在根据机器人真实运动状况对机器人运动规划轨迹进行实时修正，降低机器人所处环境对仿人机器人的运动平衡干扰的基础上，确保仿人机器人的机体动作在实现预期运动平衡效果的同时符合机器人运动规律，提升机器人运行安全性。
以上所述，仅为本申请的各种实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应当以权利要求的保护范围为准。

Claims (10)

  1. 一种机器人平衡控制方法，其特征在于，所述方法包括：
    获取仿人机器人当前的运动状况信息以及运动规划轨迹；
    根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹；
    根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数；
    根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数；
    按照所述期望驱动参数对所述仿人机器人中各真实关节的运行状态进行控制，使所述仿人机器人按照所述期望运动轨迹保持平衡。
  2. 根据权利要求1所述的方法，其特征在于，所述根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹的步骤，包括：
    根据所述运动状况信息中的双足足底受力数据计算所述仿人机器人双足当前各自的零力矩点位置；
    判断所述仿人机器人双足各自的零力矩点位置是否处于对应足底支撑域范围内；
    针对所述仿人机器人的零力矩点位置处于对应足底支撑域范围内的足部，采用所述运动状况信息中与该足部对应的足底位姿及足底速度，对该足部在所述运动规划轨迹中的足底规划位姿及足底规划速度进行匹配式替换，得到该足部在所述期望运动轨迹中的足底期望位姿及足底期望速度；
    针对所述仿人机器人的零力矩点位置未处于对应足底支撑域范围内的足部，直接将该足部在所述运动规划轨迹中的足底规划位姿及足底规划速度，分别作为该足部在所述期望运动轨迹中的足底期望位姿及足底期望速度；
    采用所述仿人机器人双足的足底期望位姿对所述运动规划轨迹中的质心规划位姿 进行参数更新，得到所述期望运动轨迹中的质心期望位姿；
    采用所述仿人机器人双足的足底期望速度对所述运动规划轨迹中的质心规划速度进行参数更新，得到所述期望运动轨迹中的质心期望速度；
    将所述运动规划轨迹中的腰部规划位姿与腰部规划速度分别置为零，得到所述期望运动轨迹中的腰部期望位姿及腰部期望速度；
    采用所述仿人机器人的重力对所述运动规划轨迹中的双足各自的规划足底力参数进行参数更新，得到所述期望运动轨迹中的双足各自的期望足底力参数。
  3. 根据权利要求2所述的方法，其特征在于，所述期望状态参数包括足底的期望加速度、质心的期望线动力及腰部的期望加速度，所述运动规划轨迹包括足底、质心和腰部各自对应的规划状态参数，所述规划状态参数包括足底的规划加速度、质心的规划线动力及腰部的规划加速度，所述运动状况信息包括足底、质心和腰部各自的实际位姿及实际速度，所述根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数的步骤，包括：
    针对足底、质心和腰部中的每项平衡需求部位，将该平衡需求部位的期望位姿与实际位姿进行减法运算，并将该平衡需求部位的期望速度与实际速度进行减法运算，得到对应的位姿差异值及速度差异值；
    在得到的位姿差异值与速度差异值中选取与该平衡需求部位的运动控制函数匹配的差异参数值；
    将该平衡需求部位所对应的规划状态参数及选取的差异参数值代入该平衡需求部位的运动控制函数中进行参数计算，得到该平衡需求部位的期望状态参数。
  4. 根据权利要求1-3中任意一项所述的方法，其特征在于，所述力控参数分布约束条件包括关节力矩分布约束条件及足底力参数分布约束条件，所述根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数的步骤，包括：
    在第一零空间内将所述运动状况信息代入到所述机器人动力学需求所对应的浮动基动力学方程以及所述关节力矩分布约束条件所对应的关节力矩不等式中针对待优化 驱动参数进行凸二次规划问题求解，得到处于所述第一零空间下的第一优化驱动参数；
    根据所述第一零空间的零空间投影矩阵、第二零空间的第一自由向量以及所述第一优化驱动参数，计算所述第二零空间的第一待递归驱动参数；
    在所述第二零空间内将所述第一待递归驱动参数代入与所述足底控制需求对应的第一关联关系表达式、所述关节力矩不等式及所述足底力参数分布约束条件所对应的足底力参数不等式组中进行凸二次规划问题求解，得到处于所述第二零空间下的第二优化驱动参数，其中所述第一关联关系表达式用于表示待优化驱动参数与足底的期望状态参数之间的关联关系；
    根据所述第二零空间的零空间投影矩阵、第三零空间的第二自由向量以及所述第二优化驱动参数，计算所述第三零空间的第二待递归驱动参数；
    在所述第三零空间内将所述第二待递归驱动参数代入与所述质心控制需求对应的第二关联关系表达式、所述关节力矩不等式及所述足底力参数不等式组中进行凸二次规划问题求解，得到处于所述第三零空间下的第三优化驱动参数，其中所述第二关联关系表达式用于表示待优化驱动参数与质心的期望状态参数之间的关联关系；
    根据所述第三零空间的零空间投影矩阵、第四零空间的第三自由向量以及所述第三优化驱动参数，计算所述第四零空间的第三待递归驱动参数；
    在所述第四零空间内将所述第三待递归驱动参数代入与所述腰部控制需求对应的第三关联关系表达式、与所述足底控制需求对应的期望足底力参数趋向等式、所述关节力矩不等式及所述足底力参数不等式组中进行凸二次规划问题求解，得到处于所述第四零空间下的可作为所述期望驱动参数的待输出驱动参数，其中所述第二关联关系表达式用于表示待优化驱动参数与腰部的期望状态参数之间的关联关系。
  5. 根据权利要求4所述的方法，其特征在于，所述根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数的步骤，还包括：
    若所述第一零空间、所述第二零空间、所述第三零空间及所述第四零空间各自对应的凸二次规划问题均求解成功，并且凸二次规划问题的求解总耗时小于当前控制周期所对应的周期时长，则直接将求解出的处于所述第四零空间下的待输出驱动参数作为所述 期望驱动参数；
    若所述第一零空间、所述第二零空间、所述第三零空间及所述第四零空间中任一零空间的凸二次规划问题求解失败，或者所述第一零空间、所述第二零空间、所述第三零空间及所述第四零空间所对应的凸二次规划问题的求解总耗时大于当前控制周期所对应的周期时长，则将所述仿人机器人在当前控制周期的上一控制周期下对应的已使用驱动参数作为所述期望驱动参数。
  6. 根据权利要求4所述的方法，其特征在于，所述关节力矩不等式的表达式如下所示：
    Figure PCTCN2021131677-appb-100001
    其中，τ min用于表示仿人机器人的关节力矩下限边界，τ max用于表示仿人机器人的关节力矩上限边界，S a用于表示仿人机器人的可驱动关节选择矩阵，M用于表示仿人机器人当前的惯量矩阵，G用于表示仿人机器人当前的重力矩阵，C用于表示仿人机器人当前的由离心力和科里奥利力产生的非线性矩阵，J用于表示仿人机器人当前足底力作用点的雅可比矩阵，
    Figure PCTCN2021131677-appb-100002
    用于表示所述待优化驱动参数所包括的各关节角加速度，F用于表示所述待优化驱动参数所包括的足底力参数。
  7. 根据权利要求4所述的方法，其特征在于，所述足底力参数不等式组的表达式如下所示：
    Figure PCTCN2021131677-appb-100003
    其中，F z用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔 坐标系下的Z轴方向上的分量，F x用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的X轴方向上的分量，F y用于表示待优化驱动参数所包括的足底力参数中的足底力矢量在笛卡尔坐标系下的Y轴方向上的分量，τ y用于表示待优化驱动参数所包括的足底力参数中的足底力矩矢量在笛卡尔坐标系下的Y轴方向上的分量，τ x用于表示待优化驱动参数所包括的足底力参数中的足底力矩矢量在笛卡尔坐标系下的X轴方向上的分量，d x,min用于表示足底支撑域在笛卡尔坐标系下的X轴方向上的下限边界，d x,max用于表示足底支撑域在笛卡尔坐标系下的X轴方向上的上限边界，d y,min用于表示足底支撑域在笛卡尔坐标系下的Y轴方向上的下限边界，d y,max用于表示足底支撑域在笛卡尔坐标系下的Y轴方向上的上限边界，μ用于表示仿人机器人足底与足底支撑面之间的摩擦力系数。
  8. 一种机器人平衡控制装置，其特征在于，所述装置包括：
    运动数据获取模块，用于获取仿人机器人当前的运动状况信息以及运动规划轨迹；
    轨迹修正适配模块，用于根据所述运动状况信息对所述运动规划轨迹进行参数适配调整，得到与所述运动状况信息匹配的期望运动轨迹；
    期望状态确定模块，用于根据所述运动状况信息确定所述仿人机器人在足底、质心和腰部处分别趋向于所述期望运动轨迹的期望状态参数；
    期望驱动计算模块，用于根据所述运动状况信息以及所述期望状态参数，计算所述仿人机器人当前同时满足机器人动力学需求、足底控制需求、质心控制需求、腰部控制需求及力控参数分布约束条件的期望驱动参数；
    关节平衡控制模块，用于按照所述期望驱动参数对所述仿人机器人中各真实关节的运行状态进行控制，使所述仿人机器人按照所述期望运动轨迹保持平衡。
  9. 一种机器人控制设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，实现权利要求1-7中任意一项所述的机器人平衡控制方法。
  10. 一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1-7中任意一项所述的机器人平衡控制方法。
PCT/CN2021/131677 2021-05-26 2021-11-19 机器人平衡控制方法、装置、机器人控制设备及存储介质 Ceased WO2022247170A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US17/710,949 US12134194B2 (en) 2021-05-26 2022-03-31 Robot balance control method, robot controller and computer readable storage medium

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110579167.8 2021-05-26
CN202110579167.8A CN113290559B (zh) 2021-05-26 2021-05-26 机器人平衡控制方法、装置、机器人控制设备及存储介质

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US17/710,949 Continuation US12134194B2 (en) 2021-05-26 2022-03-31 Robot balance control method, robot controller and computer readable storage medium

Publications (1)

Publication Number Publication Date
WO2022247170A1 true WO2022247170A1 (zh) 2022-12-01

Family

ID=77325219

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/131677 Ceased WO2022247170A1 (zh) 2021-05-26 2021-11-19 机器人平衡控制方法、装置、机器人控制设备及存储介质

Country Status (2)

Country Link
CN (1) CN113290559B (zh)
WO (1) WO2022247170A1 (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113290559B (zh) * 2021-05-26 2022-11-11 深圳市优必选科技股份有限公司 机器人平衡控制方法、装置、机器人控制设备及存储介质
CN113733106B (zh) * 2021-11-05 2022-04-15 深圳市优必选科技股份有限公司 力控仿人机器人的全身控制方法、装置、设备及介质
CN114253260B (zh) * 2021-12-08 2023-08-18 深圳市优必选科技股份有限公司 机器人步态规划方法及装置、运动规划设备和存储介质
CN114326769B (zh) * 2021-12-28 2024-03-29 深圳市优必选科技股份有限公司 机器人运动矫正方法及装置、机器人控制设备和存储介质
CN116736749B (zh) * 2022-03-01 2024-09-17 腾讯科技（深圳）有限公司 构建机器人的控制器的方法和机器人
CN115042178B (zh) * 2022-06-27 2025-05-23 清华大学 保证接触安全的机器人强化学习方法、装置、设备及介质
CN117406787A (zh) * 2022-06-30 2024-01-16 北京小米机器人技术有限公司 姿态控制方法、装置、机器人、存储介质和程序产品
CN116985110B (zh) * 2022-07-25 2026-01-30 腾讯科技（深圳）有限公司 控制足式机器人的方法和装置及足式机器人
CN116985114B (zh) * 2022-09-30 2025-07-25 腾讯科技（深圳）有限公司 机器人控制方法和机器人
CN115771163B (zh) * 2022-11-17 2025-03-07 国网智能科技股份有限公司 足式机器人系统及方法
CN118131801B (zh) * 2022-11-24 2025-06-03 北京小米机器人技术有限公司 运动控制方法及装置、运动轨迹生成方法及装置
CN116079735B (zh) * 2023-02-08 2025-03-07 清华大学 足式机器人的一步预测全身控制方法、装置及足式机器人
CN118990466A (zh) * 2023-05-17 2024-11-22 腾讯科技（深圳）有限公司 机器人的控制方法、装置、设备及存储介质
CN121043111A (zh) * 2024-05-29 2025-12-02 腾讯科技（深圳）有限公司 机器人的控制方法、装置、芯片、设备及存储介质
CN119987186B (zh) * 2025-02-07 2026-01-13 中山大学 六自由度轮足机器人控制方法、系统、装置及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2343161A1 (en) * 2010-01-11 2011-07-13 Samsung Electronics Co., Ltd. Walking robot and method of controlling the same
US9586316B1 (en) * 2015-09-15 2017-03-07 Google Inc. Determination of robotic step path
CN108115669A (zh) * 2016-11-26 2018-06-05 沈阳新松机器人自动化股份有限公司 一种机器人浮动控制方法、装置及系统
CN108858208A (zh) * 2018-09-05 2018-11-23 鲁东大学 一种复杂地形仿人机器人自适应平衡控制方法、装置和系统
CN111872941A (zh) * 2020-08-06 2020-11-03 深圳市优必选科技股份有限公司 平衡控制方法、装置、仿人机器人及可读存储介质
CN112775976A (zh) * 2021-02-05 2021-05-11 深圳市优必选科技股份有限公司 任务执行控制方法、装置、控制设备及可读存储介质
CN112792808A (zh) * 2020-12-24 2021-05-14 珞石（山东）智能科技有限公司 基于变结构滤波器的工业机器人在线轨迹规划方法及装置
CN113290559A (zh) * 2021-05-26 2021-08-24 深圳市优必选科技股份有限公司 机器人平衡控制方法、装置、机器人控制设备及存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6823569B2 (ja) * 2017-09-04 2021-02-03 本田技研工業株式会社 目標ｚｍｐ軌道の生成装置
CN109703645B (zh) * 2018-12-29 2020-05-22 深圳市优必选科技有限公司 机器人质心位置调整方法、装置、机器人和数据存储介质
CN111098300B (zh) * 2019-12-13 2021-06-04 深圳市优必选科技股份有限公司 一种机器人平衡控制方法、装置、可读存储介质及机器人
CN112643679B (zh) * 2020-12-30 2022-07-08 乐聚(深圳)机器人技术有限公司 机器人运动控制方法、装置、控制器及存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2343161A1 (en) * 2010-01-11 2011-07-13 Samsung Electronics Co., Ltd. Walking robot and method of controlling the same
US9586316B1 (en) * 2015-09-15 2017-03-07 Google Inc. Determination of robotic step path
CN108115669A (zh) * 2016-11-26 2018-06-05 沈阳新松机器人自动化股份有限公司 一种机器人浮动控制方法、装置及系统
CN108858208A (zh) * 2018-09-05 2018-11-23 鲁东大学 一种复杂地形仿人机器人自适应平衡控制方法、装置和系统
CN111872941A (zh) * 2020-08-06 2020-11-03 深圳市优必选科技股份有限公司 平衡控制方法、装置、仿人机器人及可读存储介质
CN112792808A (zh) * 2020-12-24 2021-05-14 珞石（山东）智能科技有限公司 基于变结构滤波器的工业机器人在线轨迹规划方法及装置
CN112775976A (zh) * 2021-02-05 2021-05-11 深圳市优必选科技股份有限公司 任务执行控制方法、装置、控制设备及可读存储介质
CN113290559A (zh) * 2021-05-26 2021-08-24 深圳市优必选科技股份有限公司 机器人平衡控制方法、装置、机器人控制设备及存储介质

Also Published As

Publication number Publication date
CN113290559B (zh) 2022-11-11
CN113290559A (zh) 2021-08-24

Similar Documents

Publication Publication Date Title
WO2022247170A1 (zh) 机器人平衡控制方法、装置、机器人控制设备及存储介质
CN111891252B (zh) 一种四足仿生机器人的身体姿态斜坡自适应控制方法
Klemm et al. LQR-assisted whole-body control of a wheeled bipedal robot with kinematic loops
US12070863B2 (en) Dynamic planning controller
US12134194B2 (en) Robot balance control method, robot controller and computer readable storage medium
Feng et al. Optimization‐based full body control for the darpa robotics challenge
US12172309B2 (en) Systems and methods for learning agile locomotion for multiped robots
JP2024512235A (ja) 脚車輪型ロボットの制御方法、装置、脚車輪型ロボット及び機器
Park et al. Development and control of a single-wheel robot: Practical Mechatronics approach
Chan et al. Review of modelling and control of two-wheeled robots
US12263597B2 (en) Robot localization using variance sampling
CN111872941A (zh) 平衡控制方法、装置、仿人机器人及可读存储介质
US8781628B2 (en) Walking robot and control method thereof
CN115179272A (zh) 机器人运动控制方法、机器人及系统
EP4112234B1 (en) Legged robot and method for controlling legged robot
Takasugi et al. Real-time perceptive motion control using control barrier functions with analytical smoothing for six-wheeled-telescopic-legged robot tachyon 3
Chen et al. Integrable whole-body orientation coordinates for legged robots
CN115488893B (zh) 一种基于mpc的四足机器人状态修正方法及系统
CN115755594B (zh) 一种四足机器人稳定运动的控制方法及系统
CN120540362B (zh) 一种轮腿式平衡机器人的控制方法及系统
Xie et al. Gait optimization and energy-based stability for biped locomotion using large-scale programming
CN119987186B (zh) 六自由度轮足机器人控制方法、系统、装置及存储介质
Jiang et al. A Nonlinear MPC and Hybrid Whole-body Control Framework for Optimizing Agile Motions in Quadruped Robots
Sani et al. The development of a self-balancing robot based on complementary filter and arduino
CN120178747B (zh) 双轮足机器人的导纳控制方法、装置及双轮足机器人

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21942733

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21942733

Country of ref document: EP

Kind code of ref document: A1