Robot control method, device, electronic device and storage medium

Abstract

The present application provides a robot control method, device, electronic device, and storage medium; the method comprises: obtaining the state parameters and rotation parameters of a robot's part to be observed at a first moment; determining the centroid acceleration of the centroid point of the part to be observed based on the state parameters and rotation parameters; calling a pre-constructed dynamic model to determine the value of the external force applied to the part to be observed at a second moment based on the centroid acceleration and state parameters; the second moment being later than the first moment; and controlling the robot based on the external force value. By combining the robot's state parameters and rotation parameters and observing the external force applied to the robot based on a pre-constructed dynamic model, the present application can improve the accuracy of the external force observation, thereby improving the accuracy of the robot's control.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a robot control method, device, electronic device, and storage medium. Background Art

[0002] Humanoid robots have a structure similar to that of humans and can imitate human actions in various scenarios and tasks. With the development of artificial intelligence, humanoid robots have also ushered in a golden age of development. Humanoid robots will inevitably be affected by external forces to varying degrees when they are stationary or in motion. Unknown external forces may cause the robot's stable state to be broken or produce unpredictable behavior.

[0003] In related technologies, when observing the external force acting on a robot, the robot model is greatly simplified. Since the robot model is overly simplified, the accuracy of observing the external force value is reduced, thereby reducing the control accuracy of the robot. Summary of the Invention

[0004] The embodiments of the present application provide a robot control method, device, electronic device and storage medium. By combining the robot's state parameters and rotation parameters and observing the external forces acting on the robot based on a pre-built dynamic model, the accuracy of observing the external force values ​​can be improved, thereby improving the control accuracy of the robot.

[0005] The technical solution of the embodiment of the present application is implemented as follows:

[0006] An embodiment of the present application provides a robot control method, which includes: obtaining state parameters and rotation parameters of a part to be observed of the robot at a first moment; determining the centroid acceleration of the centroid point of the part to be observed based on the state parameters and the rotation parameters; calling a pre-constructed dynamic model, and determining the external force value applied to the part to be observed at a second moment based on the centroid acceleration and the state parameters; the second moment is later than the first moment; and controlling the robot based on the external force value.

[0007] An embodiment of the present application provides a robot control device, comprising: an acquisition module for acquiring state parameters and rotation parameters of a part to be observed of the robot at a first moment; a first determination module for determining the centroid acceleration of the centroid point of the part to be observed based on the state parameters and the rotation parameters; a second determination module for calling a pre-constructed dynamic model to determine the external force value applied to the part to be observed at a second moment based on the centroid acceleration and the state parameters; the second moment is later than the first moment; and a control module for controlling the robot based on the external force value.

[0008] In the above scheme, the acquisition module is also used to: respectively collect the position parameters and speed parameters of the part to be observed at the first moment through a position sensor and a speed sensor; the state parameters include the position parameters and the speed parameters; and collect the rotation parameters of the part to be observed at the first moment through a rotation parameter sensor.

[0009] In the above scheme, the rotation parameter sensor includes a mass sensor and a distance sensor; the acquisition module is also used to: collect the mass of the part to be observed through the mass sensor; collect the first distance between the centroid of the part to be observed and the rotation axis of the part to be observed at a third moment, and the second distance between the centroid and the rotation axis of the part to be observed at the first moment through the distance sensor; the third moment is earlier than the first moment; determine the rotation parameter of the part to be observed at the first moment based on the mass, the first distance and the second distance.

[0010] In the above scheme, the rotation parameters include the rate of change of the moment of inertia; the acquisition module is further used to: determine the first moment of inertia of the part to be observed at the third moment based on the mass and the first distance; determine the second moment of inertia of the part to be observed at the first moment based on the mass and the second distance; determine the rate of change of the moment of inertia of the part to be observed at the first moment based on the first moment of inertia and the second moment of inertia.

[0011] In the above scheme, the part to be observed includes a first part and a second part, and accordingly, the state parameter includes a first state parameter of the first part and a second state parameter of the second part, the rotation parameter includes a first rotation parameter of the first part and a second rotation parameter of the second part, and the centroid includes a first centroid point of the first part and a second centroid point of the second part; the first determination module is also used to: call a preset zero moment point equation, and determine the centroid acceleration of the first centroid point based on the first state parameter, the first rotation parameter and the second state parameter; call the zero moment point equation, and determine the centroid acceleration of the second centroid point based on the second state parameter, the second rotation parameter, the first state parameter and the centroid acceleration of the first centroid point.

[0012] In the above scheme, the device also includes a construction module, which is used to: determine the state equation and the observation equation based on the obtained preset state variables and preset observation quantities; discretize the state equation and the observation equation respectively to obtain a discretized state equation and a discretized observation equation; and construct the dynamic model based on the discretized state equation and the discretized observation equation.

[0013] In the above scheme, the second determination module is also used to: call the discretized state equation in the dynamic model, and determine the prior estimation value of the part to be observed at the second moment based on the centroid acceleration and the state parameters; obtain the actual observation value of the part to be observed at the second moment; call the discretized observation equation in the dynamic model, perform observation operation on the prior estimation value at the second moment, and obtain the observation operation value; determine the posterior estimation value of the part to be observed at the second moment based on the prior estimation value of the part to be observed at the second moment, the preset gain matrix, the actual observation value at the second moment and the observation operation value; and extract the external force value from the posterior estimation value.

[0014] In the above scheme, the second determination module is also used to: determine the first error covariance matrix corresponding to the state parameters, the first noise covariance matrix at the first moment and the second noise covariance matrix at the second moment; determine the second error covariance matrix corresponding to the prior estimate value based on the first error covariance matrix and the first noise covariance matrix; and determine the gain matrix corresponding to the second moment based on the second error covariance matrix and the second noise covariance matrix.

[0015] In the above scheme, the number of the parts to be observed is N, where N is an integer greater than 1; the control module is also used to: construct a force distribution map of the robot based on the external force values ​​of the N parts to be observed; determine the compensation force for the robot and the force position of the compensation force based on the force distribution map; and control the robot to apply a force equal to the compensation force at the force position.

[0016] An embodiment of the present application provides an electronic device, comprising: a memory for storing computer-executable instructions; and a processor for implementing the robot control method provided in the embodiment of the present application when executing the computer-executable instructions stored in the memory.

[0017] An embodiment of the present application provides a computer-readable storage medium storing a computer program or executable instructions. When the computer program or computer-executable instructions are executed by a processor, the robot control method provided in the embodiment of the present application is implemented.

[0018] An embodiment of the present application provides a computer program product, which includes computer-executable instructions, and the computer-executable instructions are stored in a computer-readable storage medium; wherein, when a processor of an electronic device reads the computer-executable instructions from the computer-readable storage medium and executes the computer-executable instructions, the robot control method provided in the embodiment of the present application is implemented.

[0019] The embodiments of the present application have the following beneficial effects:

[0020] When a robot performs a grasping task, it first obtains the state parameters and rotation parameters of the robot's part to be observed at a first moment. Based on the state parameters and rotation parameters, it determines the centroid acceleration of the centroid of the part to be observed. Then, it calls a pre-built dynamic model and, based on the centroid acceleration and state parameters, determines the external force value applied to the part to be observed at a second moment. The second moment is later than the first moment. The robot is then controlled based on the external force value. In this way, by combining the state parameters and rotation parameters, more comprehensive state information about the part to be observed can be provided. The centroid acceleration is a direct result of the force applied to the part to be observed. The state parameters and rotation parameters can be used to accurately calculate the centroid acceleration of the part to be observed, thus providing a reliable data basis for external force value prediction. Based on the accurate centroid acceleration calculated, the pre-built dynamic model is called to observe the external force value applied to the robot, which can improve the accuracy of the external force value observation and thus improve the accuracy of the robot control. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 This is an optional flowchart of the robot control method provided in the embodiment of the present application;

[0022] Figure 2 This is a schematic diagram of a process for collecting the rotation parameters of a part to be observed at a first moment by using a rotation parameter sensor provided in an embodiment of the present application;

[0023] Figure 3 Schematic diagram of the process of constructing a kinetic model provided in the embodiment of the present application;

[0024] Figure 4 This is a schematic diagram of a process for determining the external force value applied to the part to be observed at the second moment, provided in an embodiment of the present application;

[0025] Figure 5 Schematic diagram of the process of determining the gain matrix provided in the embodiment of the present application;

[0026] Figure 6 This is a schematic diagram of a process for controlling a robot based on an external force value provided in an embodiment of the present application;

[0027] Figure 7 Schematic diagram of the structure of the humanoid robot provided in the embodiment of the present application;

[0028] Figure 8 Schematic diagram of a three-mass inverted pendulum model of a humanoid robot provided in an embodiment of the present application;

[0029] Figure 9Schematic diagram of the extended Kalman filter algorithm provided in the embodiment of the present application;

[0030] Figure 10A This is a waveform diagram of an observer when the robot provided by an embodiment of the present application is in a standing state and is subjected to a time-varying external force in the positive direction of the X-axis;

[0031] Figure 10B This is a waveform diagram of an observer when the robot provided by an embodiment of the present application is in a standing state and subjected to an external force in the negative direction of the Y axis;

[0032] Figure 10C This is a waveform diagram of an observer when the robot provided by an embodiment of the present application is in a standing state and subjected to a stable external force in the positive direction of the X-axis;

[0033] Figure 10D This is a waveform diagram of an observer when the robot provided in an embodiment of the present application is in a stepping state and subjected to a stable external force in the positive direction of the X-axis;

[0034] Figure 11 This is a schematic diagram of an external force disturbance observation framework provided in an embodiment of the present application;

[0035] Figure 12 This is a structural block diagram of a robot control device provided in an embodiment of the present application;

[0036] Figure 13 It is a schematic diagram of the structure of the robot provided in the embodiment of the present application. DETAILED DESCRIPTION

[0037] In order to make the purpose, technical solutions and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings. The described embodiments should not be regarded as limiting this application. All other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0038] In the following description, reference is made to “some embodiments”, which describes a subset of all possible embodiments, but it will be understood that “some embodiments” may be the same subset or different subsets of all possible embodiments and may be combined with each other without conflict.

[0039] If similar descriptions of "first / second" appear in the application documents, the following explanation is added. In the following description, the terms "first\second\third" involved are merely used to distinguish similar objects and do not represent a specific order for the objects. It can be understood that "first\second\third" can be interchanged with a specific order or sequence where permitted, so that the embodiments of the present application described herein can be implemented in an order other than that illustrated or described herein.

[0040] In the embodiments of the present application, the term "module" or "unit" refers to a computer program or portion of a computer program that has a predetermined function and works together with other related parts to achieve a predetermined goal. It can be implemented in whole or in part using software, hardware (such as processing circuits or memory), or a combination thereof. Similarly, a processor (or multiple processors or memories) can be used to implement one or more modules or units. In addition, each module or unit can be part of an overall module or unit that includes the functionality of the module or unit.

[0041] Unless otherwise defined, all technical and scientific terms used in the embodiments of the present application have the same meanings as those commonly understood by those skilled in the art. The terms used in the embodiments of the present application are only for the purpose of describing the embodiments of the present application and are not intended to limit the present application.

[0042] The embodiment of the present application provides a robot control method. When controlling the robot, first, the state parameters and rotation parameters of the robot's part to be observed at a first moment are obtained; based on the state parameters and rotation parameters, the centroid acceleration of the centroid point of the part to be observed is determined; a pre-built dynamic model is called, and based on the centroid acceleration and state parameters, the external force value of the part to be observed at a second moment is determined; the second moment is later than the first moment; and the robot is controlled based on the external force value. In this way, by combining the state parameters and rotation parameters, more comprehensive state information of the part to be observed can be provided; the centroid acceleration is a direct result of the force on the part to be observed, and the centroid acceleration of the part to be observed can be accurately calculated using the state parameters and rotation parameters, thereby providing a reliable data basis for external force value prediction; based on the accurate centroid acceleration calculated, the pre-built dynamic model is called to observe the external force value of the robot, which can improve the accuracy of the external force value observation and thus improve the control efficiency of the robot.

[0043] The robot control method provided in the embodiments of the present application can be applied to electronic devices such as robots, laptops, tablet computers, desktop computers, smart home appliances, and smart vehicle-mounted devices. The embodiments of the present application do not impose any restrictions on the specific types of electronic devices.

[0044] The robot control method provided in the embodiments of the present application is described in detail below with reference to the accompanying drawings.

[0045] Figure 1 This is an optional flow chart of the robot control method provided in the embodiment of the present application, which can be applied to electronic devices. The following will be explained by taking the electronic device as an example. Figure 1 As shown, the method includes the following steps S101 to S104:

[0046] Step S101: obtaining the state parameters and rotation parameters of the part to be observed of the robot at the first moment.

[0047] Here, the robot can be a humanoid robot, a unipedal robot, or a multi-legged robot. Humanoid robots can be used in the service industry as customer service personnel or shopping guides. They can also be used in the medical field for patient rehabilitation training or to assist elderly people with limited mobility in completing daily activities. Unipedal robots, due to their simple structure, can be used in extreme operating environments, such as working in confined spaces or cleaning the exterior walls of high-rise buildings. Multi-legged robots can be used in geological surveys. Due to their improved stability and adaptability to multiple terrains, they can conduct geological surveys and sample collection in mountainous areas and jungles. They can also be used in agricultural operations, such as planting, fertilizing, or harvesting in complex terrain. The part to be observed refers to the part of the robot where the external force value needs to be measured, which can be the robot's legs, torso, or entire body. The first moment refers to the moment before the current moment. For example, if the current moment is 10:04, the first moment could be 10:03.

[0048] State parameters are quantities that describe the physical state of the part to be observed at the first moment. State parameters may include: position parameters: describing the position of the part to be observed in space, such as the position parameters of x, y, and z represented by the coordinate system; velocity parameters: describing the movement speed of the part to be observed, such as linear velocity (the speed at which the part to be observed moves in a specific direction during movement); acceleration parameters: describing the movement acceleration of the part to be observed, such as linear acceleration (the rate of change of linear velocity); the state parameters of the robot can be measured by sensors.

[0049] Rotational parameters are quantities that describe the rotational motion state of the observed part, and usually include rotation angle, angular velocity (the rotational speed of the observed part about a certain rotation axis) and angular acceleration (the rate of change of angular velocity). Rotational parameters can be obtained through sensors, or unknown rotational parameters can be inferred and estimated using known rotational parameters, such as inferring angular velocity and angular acceleration by combining existing angle information with time.

[0050] In some embodiments, step S101 can be implemented by the following method: first, the position parameters and speed parameters of the part to be observed at the first moment are respectively collected by a position sensor and a speed sensor; the state parameters include position parameters and speed parameters; then, the rotation parameters of the part to be observed at the first moment are collected by a rotation parameter sensor.

[0051] A position sensor is a device used to measure the spatial location of an observed part. A position sensor can be a laser rangefinder, ultrasonic sensor, or other device. It can directly measure the position of the observed part in a specific coordinate system. A speed sensor is a device used to measure the speed of movement of the observed part. Speed ​​sensors can include gyroscopes, speed measuring motors, or laser speed meters. A rotational parameter sensor is a device used to measure the rotational motion parameters of the observed part, such as a gyroscope or inertial measurement unit. An inertial measurement unit integrates multiple sensors to provide more comprehensive rotational state information.

[0052] By collecting the position parameters, velocity parameters and rotation parameters of the sensor, the complete state of the observed part at the first moment can be obtained, providing a reliable data basis for subsequent external force value observation and robot control, reducing observation errors and improving the control efficiency of the robot.

[0053] In some embodiments, the rotation parameter sensor includes a mass sensor and a distance sensor; see Figure 2 , Figure 2 This is a flow chart of collecting the rotation parameters of the part to be observed at the first moment by using a rotation parameter sensor provided by an embodiment of the present application. Figure 2 It shows that the rotation parameter sensor is used to collect the rotation parameter of the part to be observed at the first moment, which can be achieved by the following steps S201 to S203:

[0054] Step S201: collecting the mass of the part to be observed through a mass sensor.

[0055] A mass sensor is a device used to measure the mass of a part to be observed, and can provide mass data of the part to be observed under static or dynamic conditions. The mass of the part to be observed includes the robot's torso mass (if the robot has arms, the torso mass also includes the arm mass), leg mass, and total mass.

[0056] Step S202: Using a distance sensor, collect a first distance between the centroid of the part to be observed and the rotation axis of the part to be observed at a third moment, and a second distance between the centroid and the rotation axis of the part to be observed at the first moment; the third moment is earlier than the first moment.

[0057] A distance sensor is a device used to measure the distance between two points. It can be a laser sensor, ultrasonic sensor, or other device. The rotation axis is the axis about which the observed part rotates at a specific moment. For symmetrical objects, this axis of symmetry is usually the rotation axis. For robots, the rotation axis can be determined by the robot designer. For example, it can be the rotation axis of each joint of the robot, or it can be determined based on the precession phenomenon of a gyroscope under external forces. Precession refers to the phenomenon in which a gyroscope's rotation axis rotates perpendicular to the direction of the external force, rather than deflecting in the direction of the force. The centroid is the geometric center of an object.

[0058] Step S203: determining a rotation parameter of the part to be observed at the first moment according to the mass, the first distance, and the second distance.

[0059] The rotation parameters of the part to be observed at the first moment are calculated based on the collected mass of the part to be observed, the first distance between the centroid of the part to be observed and the rotation axis of the part to be observed at the third moment, and the second distance between the centroid of the part to be observed and the rotation axis of the part to be observed at the first moment.

[0060] In some embodiments, the rotation parameters include the rate of change of the moment of inertia; in step S203, the rotation parameters of the part to be observed at the first moment are determined based on the mass, the first distance and the second distance, which can be achieved by the following method: first, based on the mass and the first distance, the first moment of inertia of the part to be observed at the third moment is determined; then, based on the mass and the second distance, the second moment of inertia of the part to be observed at the first moment is determined; finally, based on the first moment of inertia and the second moment of inertia, the rate of change of the moment of inertia of the part to be observed at the first moment is determined.

[0061] The moment of inertia describes the inertia of the observed part when it rotates about the axis of rotation. It depends on the mass of the observed part and the distance between the centroid of the observed part and the axis of rotation. The rate of change of the moment of inertia describes the rate of change of the moment of inertia of the observed part over time at different moments in time. The first moment is later than the third moment.

[0062] In some embodiments, first, according to the mass of the part to be observed and the first distance, the first moment of inertia of the part to be observed at the third moment is determined, and the calculation formula is shown in formula (1):

[0063] Formula (1)

[0064] in, represents the first moment of inertia, m represents the mass of the part to be observed, and r1 represents the first distance.

[0065] Then, according to the mass of the part to be observed and the second distance, the second moment of inertia of the part to be observed at the first moment is determined. The calculation formula is shown in formula (2):

[0066] Formula (2)

[0067] in, represents the second moment of inertia, m represents the mass of the part to be observed, and r2 represents the second distance.

[0068] Next, the rate of change of the moment of inertia of the part to be observed at the first moment is determined based on the first moment of inertia, the second moment of inertia, and the time difference between the third moment and the first moment. The calculation formula is shown in formula (3):

[0069] Formula (3)

[0070] in, represents the rate of change of moment of inertia at the first moment, Indicates the time difference between the third moment and the first moment.

[0071] Step S102: determining the centroid acceleration of the centroid point of the part to be observed based on the state parameter and the rotation parameter.

[0072] Centroid acceleration refers to the rate of change of the centroid speed of the observed part in space with time.

[0073] In some embodiments, the part to be observed includes a first part and a second part, and accordingly, the state parameter includes a first state parameter of the first part and a second state parameter of the second part, the rotation parameter includes a first rotation parameter of the first part and a second rotation parameter of the second part, and the centroid includes a first centroid point of the first part and a second centroid point of the second part; in step S102, the centroid acceleration of the centroid point of the part to be observed is determined based on the state parameter and the rotation parameter, which can be achieved by the following method: first, calling a preset zero moment point equation, and determining the centroid acceleration of the first centroid point based on the first state parameter, the first rotation parameter and the second state parameter; then, calling the zero moment point equation, and determining the centroid acceleration of the second centroid point based on the second state parameter, the second rotation parameter, the first state parameter, and the centroid acceleration of the first centroid point.

[0074] The zero-torque equation is a formula for calculating the position of the robot's equilibrium point during motion. When the equilibrium point is within the support surface, the robot's total torque on the ground is zero. The support surface refers to the area where the robot's various support points (such as the soles of the feet) contact the ground. This area determines the robot's stability in that posture.

[0075] In some embodiments, the external forces acting on the robot are observed using a three-mass inverted pendulum model. The three-mass inverted pendulum model is a mathematical model used to simulate and study the balance and gait control of multi-link robots (such as bipedal robots). The three-mass inverted pendulum model has three masses connected by rigid rods, which are similar to different body parts of the robot, such as the swing leg, the support leg, and the torso. The swing leg refers to the leg of the robot that leaves the ground and swings in the air during walking or moving, and the support leg refers to the leg of the robot that is in contact with the ground and provides support at a certain moment. In the three-mass inverted pendulum model, the motion and mechanical behavior of each mass can be described by dynamic equations. In order to ensure the balance of the system, it is usually necessary to calculate the zero-point torque position of the system. The zero-point torque position can be calculated by the zero-point torque equation. When the external force disturbance acting on the robot is not considered, the zero-point torque equation can be expressed as Formula (4) and Formula (5):

[0076] Formula (4)

[0077] Formula (5)

[0078] in, 、 It represents the position of the zero moment point on the x-axis and y-axis, that is, the coordinates on the x-axis and y-axis; represents the mass of the robot torso (including the arms); It represents the mass of the robot’s legs; It represents the total mass of the robot; It represents the distance from the sole of the foot to the centroid of the torso in the y-axis direction; g is the acceleration due to gravity; 、 It represents the position of the robot torso centroid on the x-axis and y-axis; 、 represents the centroid acceleration of the robot torso on the x-axis and y-axis; 、 It represents the position of the centroid of the robot's swing foot on the x-axis and y-axis; 、 It represents the centroid acceleration of the swing foot on the x-axis and y-axis. The robot swing foot refers to the foot that leaves the ground and moves in the air during the robot's walking or movement; 、 、 It represents the offset of the robot's torso mass center relative to the torso centroid; 、 It represents the displacement of the center of mass of the supporting leg relative to the midpoint of the line connecting the supporting foot and the torso on the x-axis and y-axis. The supporting foot refers to the part of the foot under the supporting leg that is in direct contact with the ground. 、 It represents the offset of the center of mass of the swing leg relative to the midpoint of the line connecting the swing foot and the torso on the x-axis and y-axis.

[0079] The center of mass refers to the center point of the mass distribution of an object. For objects with regular shape and uniform density, the center of mass usually coincides with the centroid. In this case, the center of mass is the centroid. For objects with non-uniform mass distribution, it is necessary to calculate the offset between the center of mass and the centroid according to the mass distribution of the object, and determine the center of mass of the object based on the centroid and the offset. When determining the offset, the object can be divided into several small parts. Assuming that the center of mass of each part is known, the offset between the center of mass of each part and the overall centroid is calculated, and the weighted average of the offsets between the center of mass of all parts and the overall centroid is calculated. The weighted average is determined as the offset of the center of mass of the object relative to the centroid of the object, and the coordinates of the centroid are added to the offset to obtain the coordinates of the center of mass of the object.

[0080] For example: there is an object A with uneven mass distribution. Object A can be divided into two parts with relatively uniform mass distribution, namely part 1 and part 2. Among them, the mass of part 1 is 2KG, and the position coordinates of the center of mass 1 are (1, 2, 1). The mass of part 2 is 3KG, and the position coordinates of the center of mass 2 are (4, 3, 3). Assume that the centroid of object A is known, which is (2, 2.5, 2); then, calculate the offset of the center of mass of the two parts relative to the centroid. For part 1, the offset 1 of the center of mass 1 relative to the centroid = (1, 2, 1) - (2, 2.5, 2) = (-1, -0.5, -1), for part 2, the offset of the center of mass 2 relative to the centroid is 2 = (4, 3, 3) - (2, 2.5, 2) = (2, 0.5, 1), and the total mass of object A is 5KG; then, calculate the offset of the center of mass of object A relative to the centroid D = [2×(-1, -0.5, -1) + 3×(2, 0.5, 1)] / 5 = (0.8, 0.1, 0.2); finally, determine the center of mass position R = (2, 2.5, 2) + (0.8, 0.1, 0.2) = (2.8, 2.6, 2.2) based on the offset and the centroid position.

[0081] In some embodiments, when observing the external force value using a three-particle inverted pendulum model, any two or three of the trunk, swing leg, and support leg can be selected for observation. The embodiment of the present application takes the observation of the external force on the trunk and swing leg as an example: according to the zero-point torque equation expressed by formula (4) and formula (5), the centroid acceleration of the first centroid is determined based on the first state parameter, the first rotation parameter, and the second state parameter, and the external force disturbance on the part to be observed is also considered when calculating the centroid acceleration. The calculation process of the centroid acceleration of the first centroid is as follows: First, due to the zero-point torque equation in formula (4) and formula (5), the first state parameter, the first rotation parameter, and the second state parameter are used to calculate the centroid acceleration. 、 、 and are all unknown parameters and cannot be directly calculated. Therefore, the embodiment of the present application adopts an approximate method: since the total displacement of the swinging foot in the x and y directions during the movement of the robot is twice the displacement of the torso, it can be approximately assumed that =2* , =2* ; Then, substitute the above assumptions into formula (4) and formula (5) respectively, and add the external force disturbance term of the torso into the calculation results; solve the first centroid acceleration and The expressions of are shown in formula (6) and formula (7):

[0082] Formula (6)

[0083] in, Indicates the external force in the x-axis direction on the robot torso; Indicates the position of the zero moment point on the x-axis; It represents the offset of the center of mass of the swing leg relative to the midpoint of the line connecting the swing foot and the torso on the z-axis; It represents the offset of the center of mass of the supporting leg relative to the midpoint of the line connecting the supporting foot and the torso on the z-axis; Indicates the position of the robot torso centroid on the z-axis; It represents the rate of change of the moment of inertia of the robot torso on the y-axis.

[0084] Formula (7)

[0085] in, Represents the external force in the y-axis direction on the robot torso; Indicates the position of the zero moment point on the y-axis; It represents the rate of change of the moment of inertia of the robot torso on the x-axis.

[0086] Then, we calculate and Substitute the expression into formula (4) and formula (5) again, and add the external force disturbance term of the swing leg to the calculation result; solve the second centroid acceleration and The expressions of are shown in formula (8) and formula (9):

[0087] Formula (8)

[0088] in, ; ; Represents the external force in the x-axis direction on the robot's swinging legs; It represents the rate of change of the moment of inertia of the robot's swinging leg on the y-axis.

[0089] Formula (9)

[0090] in, ; ; Represents the external force in the y-axis direction on the robot's swinging leg; It represents the rate of change of the moment of inertia of the robot's swing leg on the x-axis; the other parameters in formula (6) to formula (9) have been described in detail in formula (4) and will not be repeated here in this application.

[0091] Step S103 , calling a pre-built dynamic model, and determining the external force value of the part to be observed at a second moment based on the centroid acceleration and the state parameters; the second moment is later than the first moment.

[0092] A dynamic model is a mathematical model used to describe the motion laws of an object or system when it is subjected to forces or other effects. A kinematic model predicts the motion behavior of a system by analyzing the relationship between physical quantities such as force, mass, and acceleration.

[0093] In some embodiments, before step S103, a kinetic model needs to be pre-built. Figure 3 , Figure 3 Schematic diagram of the process of constructing a kinetic model provided in an embodiment of the present application. The kinetic model can be constructed by following steps S301 to S303:

[0094] Step S301: Determine the state equation and observation equation based on the acquired preset state variables and preset observation quantities.

[0095] State variables are variables that describe the internal state of a system. State variables cover all necessary information about the system at a certain moment and determine the future evolution of the system. Observable quantities are measurable output quantities from the outside of the system. State equations describe how the system state changes over time. Observation equations describe the relationship between the system's observable quantities and state variables.

[0096] In some embodiments, the expression of the preset state variable is shown in formula (10), and the expression of the preset observation quantity is shown in formula (11):

[0097] Formula (10)

[0098] in, Indicates the external force in the z-axis direction on the robot torso; Indicates the external force in the z-axis direction on the robot's swinging legs; It represents the transpose of a matrix.

[0099] Formula (11)

[0100] Derivative the expression of the preset state variable to obtain the state equation , equation of state The expression of is shown in formula (12):

[0101] Formula (12)

[0102] in ; ; ; ; ; ; ; ; Indicates the external force in the z-axis direction on the robot's supporting legs; It represents the offset of the center of mass of the robot's supporting leg relative to the midpoint of the line connecting the supporting foot and the torso on the z-axis.

[0103] Let the observed quantity Y= , we get the observation equation, and the expression of the observation equation is shown in formula (13):

[0104] (Formula 13)

[0105] in, to The expression of The present application will not elaborate on this.

[0106] Step S302 : discretize the state equation and the observation equation respectively to obtain a discretized state equation and a discretized observation equation.

[0107] Discretization refers to the process of converting a continuous-time system model into a discrete-time system model. The main purpose of discretization is to enable the continuous-time system to run and be solved on a digital computer or digital controller.

[0108] In some embodiments, the state equation can be discretized by the forward Euler method, the backward Euler method, or the trapezoidal method. The specific discretization method is not limited in this application. The discretized state equation obtained after the state equation is discretized is shown in formula (14):

[0109] Formula (14)

[0110] in, Represents the state quantity at time k+1; Represents the state quantity at time k; Represents a discrete time interval.

[0111] Since the observation of the observation quantity is performed directly at discrete time points, the discretization of the observation equation is actually to sample the continuous time observation quantity directly at each sampling moment. The discretized observation equation obtained after discretization is shown in formula (15):

[0112] Formula (15)

[0113] in, Represents the observation at time k+1.

[0114] Step S303: constructing a dynamic model based on the discretized state equation and the discretized observation equation.

[0115] The discretized state equation and the discretized observation equation are combined to construct a complete dynamic model. The dynamic model includes the state transition and observation mechanism of the system, which is used to predict the future state and estimate the current state through observation data.

[0116] In some embodiments, see Figure 4 , Figure 4 is a flow chart of determining the external force value of the part to be observed at the second moment provided by an embodiment of the present application, Figure 4 It is shown that in step S103, the pre-built dynamic model is called to determine the external force value of the part to be observed at the second moment based on the centroid acceleration and the state parameters. This can be achieved by the following steps S1031 to S1035:

[0117] Step S1031 , calling the discretized state equation in the dynamic model, and determining the prior estimation value of the part to be observed at the second moment based on the centroid acceleration and the state parameters.

[0118] The prior estimate refers to a predicted value or estimated value of the current state based on the existing data before the measurement data at the current moment is obtained.

[0119] In some embodiments, the discretized state equation in the dynamic model is called to predict the prior estimated value of the observed part at the second moment through the extended Kalman filter algorithm. The state prediction equation is shown in formula (16):

[0120] Formula (16)

[0121] in, represents the prior estimate at time k+1; represents the optimal estimate of the system state at time k, which is obtained based on the prediction at time k-1; represents the discrete state equation at time k.

[0122] Step S1032: obtaining the actual observation value of the part to be observed at the second moment.

[0123] The actual observation value refers to the value directly obtained through the sensor or measuring device, which reflects the real state of the system at that moment.

[0124] In some embodiments, the position data of the part to be observed at the second moment can be obtained through a position sensor, and the speed data of the large observation part at the second moment can be obtained through a speed sensor; the specific actual observation value type can be determined according to actual conditions, and this application does not limit it here.

[0125] Step S0133: calling the discretized observation equation in the dynamic model, performing observation operation on the prior estimation value at the second moment, and obtaining an observation operation value.

[0126] In some embodiments, the prior estimate at the second moment is converted into a quantity that can be compared with the actual observation value by discretizing the observation equation.

[0127] Step S1034 , determining a posterior estimation value of the part to be observed at the second moment based on the prior estimation value of the part to be observed at the second moment, a preset gain matrix, the actual observation value at the second moment, and the observation operation value.

[0128] The posterior estimate is the estimated value of the system state obtained by correcting the prior estimate based on the actual observation data after obtaining the actual observation data at the current moment.

[0129] In some embodiments, the prior estimate obtained in step S1031 is corrected based on the actual observation value at the second moment (time k+1) to obtain the posterior estimate of the part to be observed at the second moment. The correction equation is shown in formula (17):

[0130] Formula (17)

[0131] in, is the posterior estimate at the second moment; is the prior estimate at the second moment; is the preset gain matrix; is the actual observation value at the second moment; is the observed calculation value at the second moment.

[0132] In some embodiments, see Figure 5 , Figure 5 is a flow chart of a method for determining a gain matrix provided in an embodiment of the present application, Figure 5 It is shown that the gain matrix can be determined by the following steps S501 to S503:

[0133] Step S501 : determining a first error covariance matrix corresponding to a state parameter, a first noise covariance matrix at a first moment, and a second noise covariance matrix at a second moment.

[0134] The error covariance matrix represents the covariance of the estimation error and is used to describe the uncertainty of state estimation. The error covariance matrix can be directly obtained by extending the Kalman filter algorithm. The noise covariance matrix describes the covariance of the system process noise or observation noise and is used to quantify the impact of noise on the system state. The system process noise refers to the random difference between the actual change of the system state and the model prediction during the system dynamic process, reflecting the impact of random interference or unknown factors on the system state that the model cannot fully capture. The observation noise refers to the random error of the sensor or measuring equipment during the measurement process, reflecting the random difference between the measured value and the true value.

[0135] Step S502 : determining a second error covariance matrix corresponding to the priori estimate according to the first error covariance matrix and the first noise covariance matrix.

[0136] In some embodiments, the second error covariance matrix prediction equation is shown in formula (18):

[0137] Formula (18)

[0138] in, represents the second error covariance matrix; is the first error covariance matrix corresponding to the state parameters; is the first noise covariance matrix, which is the covariance matrix of the system process noise; It is an equation The Jacobian matrix of ; is the transposed matrix of the Jacobian matrix, which is a matrix used to describe the partial derivatives of a multivariate vector-valued function at a certain point.

[0139] Step S503: Determine a gain matrix corresponding to the second moment based on the second error covariance matrix and the second noise covariance matrix.

[0140] In some embodiments, the calculation formula of the gain matrix corresponding to the second moment is shown in formula (19):

[0141] Formula (19)

[0142] in, is the gain matrix corresponding to the second moment; represents the second error covariance matrix; is the covariance matrix of the measurement noise at time k+1; For the equation The Jacobian matrix of .

[0143] In some embodiments, after determining the posterior estimate at the second moment, it is also necessary to update the second error covariance matrix at the second moment to obtain a third error covariance matrix. The expression of the third error covariance matrix is ​​shown in formula (20):

[0144] Formula (20)

[0145] in, represents the third error covariance matrix; I is the identity matrix; is the gain matrix corresponding to the second moment; is the second error covariance matrix.

[0146] Step S1035: extract the external force value from the posterior estimation value.

[0147] After calculating formula (17), the posterior estimate at the second moment is obtained. The posterior estimate is data in matrix form, and the posterior estimate corresponds to the preset state variable X, and the external force disturbance term is included in the preset state variable X. Therefore, the external force values ​​​​acting on the robot's torso and swing leg in the three directions of x, y, and z can be directly extracted based on the posterior estimate calculated by formula (17).

[0148] It should be noted that the Extended Kalman Filter can predict the state quantity (i.e., the prior estimate) at the current moment (i.e., the second moment) based on the optimal estimate of the state quantity at the previous moment (i.e., the posterior estimate at the previous moment). The gain matrix can then be updated based on the error covariance matrix of the prior estimate, and the optimal estimate of the state quantity at the current moment (i.e., the posterior estimate) can be determined based on the actual observations at the current moment and the updated gain matrix. In other words, the Extended Kalman Filter is insensitive to the initial value of the state quantity and can quickly estimate the state quantity at the current moment and determine the external force acting on the robot at the current moment.

[0149] In some embodiments, it is necessary to determine whether the external force applied to the robot is substantial. If the external force applied to the robot is substantial, as predicted by the dynamic model, all state variables can be observed based on the dynamic model. If the external force applied to the robot is substantial, no state variables can be observed. Determining whether the external force applied to the robot is substantial can be achieved by the following methods:

[0150] First, according to formula (21), the observation equation corresponding to the preset observation quantity is solved: Partial derivative of the preset state variable X:

[0151] Formula (21)

[0152] Among them, H1, H2...H12 represent the rows of H(X), and X1, X2...X18 represent the rows of X.

[0153] Then, let ,use Find the partial derivative of X and compare it with Multiplication, calculation , The expression of is shown in formula (22):

[0154] Formula (22)

[0155] in, ; ; ; ; ; ; .

[0156] Similarly, we can calculate to , to The expressions of are shown in formula (23) and formula (24):

[0157] Formula (23)

[0158] Formula (24)

[0159] It should be noted that to The solution process and The solution process is similar to that of to Expand the expression.

[0160] Next, let the matrix , the observability matrix dG of the three-particle inverted pendulum model can be obtained. The expression of the observability matrix dG is shown in formula (25):

[0161] Formula (25)

[0162] Finally, we can determine that the observability matrix dG of the three-point inverted pendulum model has a rank of 18, which is equal to the dimension of the preset state variable X. This means that the external forces acting on the robot's trunk and swinging legs are observable. Therefore, we can observe the external forces acting on the robot's trunk and swinging legs at different times based on the dynamic model expression: 、 、 、 、 and .

[0163] Step S104: Control the robot based on the external force value.

[0164] After calculating the external force value applied to the robot, the robot can be controlled according to the magnitude of the external force applied to the robot to prevent the robot from producing unpredictable behaviors such as tipping over or sudden acceleration due to the influence of the external force.

[0165] In some embodiments, the number of sites to be observed is N, where N is an integer greater than 1; see Figure 6 , Figure 6 is a schematic diagram of a flow chart of controlling a robot based on an external force value provided in an embodiment of the present application. Figure 6 It is shown that in step S104, the robot is controlled based on the external force value, which can be achieved by the following steps S1041 to S1043:

[0166] Step S1041: construct a force distribution map of the robot based on the external force values ​​of the N parts to be observed.

[0167] A force distribution diagram is a graphical representation used to show the force conditions of various observed parts of a robot. The force distribution diagram usually shows the magnitude, direction, and distribution pattern of the external force applied to each observed part.

[0168] In some embodiments, after determining the external force values ​​for N observed locations, the direction of the external force acting on the robot can be determined based on the momentum theorem. The direction of the robot's momentum change is the same as the direction of the external force. The direction of the external force can be indirectly inferred by analyzing changes in the robot's velocity. For example, if the robot decelerates while moving forward, it can be inferred that the direction of the external force is opposite to its forward direction. The principle of inverse dynamics can also be used to determine the direction of the external force that may have caused this state based on the robot's known motion state (speed, acceleration, position change, etc.). For example, the direction of the external force applied to the robot can be inferred by combining the acceleration and velocity changes of each observed location of the robot with a known dynamic model. This application does not limit the specific method for inferring the direction of the external force.

[0169] After determining the direction of the external force, the force distribution map of the robot can be constructed according to the magnitude and direction of the external force. The force distribution map includes a vector map (arrows indicate the direction and magnitude of the force for each observed part), a heat map (the gradient color slope indicates the distribution of the force, the darker the color, the greater the force), a three-dimensional mechanical model (indicates the force conditions of each part of the robot in three-dimensional space), etc. The specific construction method is not limited in this application.

[0170] Step S1042: Determine the compensation force for the robot and the position where the compensation force is applied based on the force distribution diagram.

[0171] The compensation force is used to offset or weaken the external force applied to the robot. The main function of the compensation force is to enable the robot to maintain the expected motion trajectory or posture under the influence of interference or unbalanced force; the force position of the compensation force refers to the position where the compensation force is applied on the robot structure or system. The appropriate force position can effectively offset the influence of external force.

[0172] In some embodiments, the force distribution diagram can be analyzed, and the external forces acting on the part to be observed in different directions can be compensated separately. For example, if the robot is disturbed by external forces of different magnitudes in the x-axis and y-axis directions, a compensation force of the same magnitude as the external force can be applied in the opposite direction of the force to offset the influence of the external force; the external forces in all directions acting on the robot on the part to be observed can also be vector-summed to calculate the direction and magnitude of the resultant force acting on the part to be observed, and a compensation force equal to the magnitude and opposite direction of the resultant force can be applied to the part to be observed according to the direction and magnitude of the resultant force to offset the influence of the external force.

[0173] In some embodiments, there may be multiple locations where the compensation force is applied, such as if there are multiple parts of the robot to be observed, and a location where the compensation force is applied may be determined for each part to be observed. The location where the compensation force is applied may also be one, such as determining an intermediate location based on multiple parts to be observed of the robot, or determining a location where concentrated compensation can be performed based on the magnitude and direction of the external forces acting on multiple parts to be observed, as the location where the compensation force is applied.

[0174] Step S1043: Control the robot to apply a force equal to the compensation force at the force-bearing position.

[0175] In some embodiments, the robot's joints are typically equipped with motors or servo motors. By controlling the motor's current or voltage, the force or torque at the joint is adjusted to apply compensatory force. For example, if the robot is subjected to an external force from behind or forward while walking, the motors in the robot's torso or legs can be controlled to increase force in the opposite direction of the external force to prevent the robot from tipping over.

[0176] The robot control method provided in the embodiment of the present application can be widely used in a variety of scenarios, such as manufacturing, logistics, construction and other scenarios. In the manufacturing scenario, the robot can be a mobile welding robot. The mobile welding robot may be disturbed by external forces during the welding process, such as the touch of a worker. Through the robot control method provided in the embodiment of the present application, the external force value received by the welding robot can be observed, and the robot can be controlled according to the observed external force value to ensure the accuracy of the welding position. In the field of logistics, the robot can be a handling robot. The handling robot may be disturbed by external forces of different sizes during the process of handling goods, such as changes in the weight of the goods. Through the robot control method provided in the embodiment of the present application, the external force value received by the handling robot can be observed, and the handling robot can be controlled according to the observed external force value to prevent the goods from falling. In the construction field, the robot may be a construction robot, which is responsible for transporting and installing construction materials during the construction of high-rise buildings. During the transportation or installation process, the robot may be disturbed by external forces, such as external force interference caused by ground bumps. Through the robot control method provided in the embodiment of the present application, the external forces acting on the construction robot can be observed, and the construction robot can be controlled according to the observed external force values ​​to ensure stable transportation and safe installation of materials.

[0177] The robot control method provided in the embodiment of the present application can provide more comprehensive state information of the part to be observed by combining state parameters and rotation parameters; the centroid acceleration is a direct result of the force applied to the part to be observed, and the centroid acceleration of the part to be observed can be accurately calculated through the state parameters and rotation parameters, thereby providing a reliable data basis for the prediction of external force values; on the basis of calculating the accurate centroid acceleration, calling a pre-built dynamic model to observe the external force value applied to the robot can improve the accuracy of the external force value observation, thereby improving the control accuracy of the robot.

[0178] The following describes an exemplary application of the embodiments of the present application in a practical application scenario.

[0179] The present invention provides a method for observing and controlling external force disturbances on a robot. This method estimates the external force disturbances experienced by the robot while it is standing and walking. These external forces can be real, such as a human push, or virtual, such as the disturbance caused by a bumpy road. Because external forces acting on the robot can affect its stability, it is necessary to estimate these external forces so that they can be compensated for during control to maintain robot stability.

[0180] The robot control method provided in the embodiment of the present application is applicable to a broad range of humanoid robots. Figure 7 , Figure 7is a schematic diagram of the structure of a humanoid robot provided in an embodiment of the present application. The humanoid robot has a body structure similar to that of a human, including a torso, a head, two legs, and two arms. Figure 8 , Figure 8 This is a schematic diagram of a three-point inverted pendulum model of a humanoid robot provided in an embodiment of the present application, including the center of mass of the trunk, the center of mass of the supporting leg, the center of mass of the swinging leg, and the external forces on the trunk and the swinging leg in the x-axis, y-axis, and z-axis directions. 、 、 、 、 and .

[0181] The embodiment of the present application establishes a dynamic model of a humanoid robot based on a three-particle inverted pendulum model, selects appropriate state quantities and observation quantities according to the external force disturbance observation requirements, establishes state equations and observation equations, and uses the extended Kalman filter algorithm to estimate the state quantities, and finally obtains the external force disturbance observation value of the humanoid robot. Figure 9 , Figure 9 This is a flow chart of the extended Kalman filter algorithm provided in an embodiment of the present application. The extended Kalman filter algorithm can be implemented by following steps S601 to S605:

[0182] Step S601: setting preset state variables and preset observation quantities.

[0183] During the implementation process, preset state variables and preset observation quantities can be set according to the actual application scenario, the state equation and observation equation can be determined based on the preset state variables and preset observation quantities, and the state equation and observation equation can be discretized to obtain discrete state equations and discrete observation equations; for the specific implementation process, please refer to the corresponding contents of formulas (10) to (13).

[0184] Step S602: Determine a priori estimation values ​​and a second error covariance matrix.

[0185] During the implementation process, the prior estimation value and the second error covariance matrix corresponding to the prior estimation value can be determined according to the discrete state equation; the specific implementation process can be found in formulas (14) to (16), as well as the content corresponding to formula (18).

[0186] Step S603: determine the gain matrix.

[0187] During the implementation process, the gain matrix can be determined based on the second error covariance matrix and the discrete observation equation; for the specific implementation process, please refer to the content corresponding to formula (19).

[0188] Step S604: Determine the posterior estimation value.

[0189] During the implementation process, the posterior estimation value can be determined based on the prior estimation value and the gain matrix; for the specific implementation method, please refer to the content corresponding to formula (17).

[0190] Step S605: Update the second error covariance matrix to obtain a third error covariance matrix.

[0191] During the implementation process, the second error covariance matrix can be updated according to the gain matrix to obtain the third error covariance matrix; for the specific implementation method, please refer to the content corresponding to formula (20).

[0192] It should be noted that the specific implementation process of steps S601 to S605 has been described in detail above, and this application will not repeat it here. For the specific implementation process, please refer to the corresponding contents of method (10) to formula (20).

[0193] The extended Kalman filter algorithm used in the embodiments of the present application is a filtering algorithm applied to nonlinear systems, and is widely used in state estimation and observation in various scenarios. It should be understood that the extended Kalman filter algorithm will calculate the prior estimate of the system state quantity at this moment (the above-mentioned second moment) based on the posterior estimate of the system state quantity at the previous moment (the above-mentioned first moment), and then update the gain matrix, and then use the observation quantity at this moment (the actual observation value at the above-mentioned second moment) and the gain matrix to determine the posterior estimate of the state quantity at this moment. Therefore, the extended Kalman filter algorithm is insensitive to the initial value of the state quantity and can quickly estimate the observation value of the external force disturbance.

[0194] See also 10A to 10D 10A is a waveform diagram of the observer when the robot provided in the embodiment of the present application is in a standing state and is subjected to a time-varying external force in the positive direction of the X-axis. It can be seen that when the robot is in a standing state, when a time-varying external force in the positive direction of the x-axis is applied to the robot torso, the observer can quickly respond and observe the magnitude of the external force on the torso and its changes over time. After the external force is suddenly withdrawn, the observed value quickly drops to near 0. Figure 10B This is a waveform diagram of the observer when the robot provided in the embodiment of the present application is in a standing state and is subjected to an external force in the negative direction of the Y-axis. It can be seen that when the robot is in a standing state, an external force in the negative direction of the Y-axis is slowly applied to the robot torso. When the external force is applied to the maximum value and then gradually reduced, the observer can quickly respond and observe the magnitude of the external force applied to the torso and its change curve over time. Figure 10C This is a waveform diagram of the observer when the robot provided in the embodiment of the present application is in a standing state and is subjected to a stable external force in the positive direction of the x-axis. It can be seen that when the robot is in a standing state, a stable external force in the positive direction of the x-axis is suddenly applied to the robot torso. The observer can quickly respond to the observed external force on the torso, and the external force is relatively stable. After the external force is suddenly withdrawn, the observed value quickly drops to near 0. Figure 10D This is a waveform diagram of the observer when a stable external force in the positive x-axis direction is applied to the robot's torso during stepping, as provided in an embodiment of this application. It can be seen that when the robot is in the stepping state, a stable external force in the positive x-axis direction is applied to the robot's torso. The observer can quickly respond to the observed external force on the torso. After the external force is suddenly removed, the observed value returns to the level before the external force was applied. To avoid redundant description, this application does not further illustrate the external force observations in other directions, scenarios, and legs.

[0195] It should be noted that since the humanoid robot does not completely conform to the three-mass inverted pendulum model, especially when stepping, the observer in the embodiment of the present application has a small amount of noise when stepping, but it does not affect the overall effectiveness of the observer.

[0196] See also Figure 11 , Figure 11 This is a schematic diagram of an external force disturbance observation framework provided in an embodiment of the present application, which is used to describe the robot control method provided in an embodiment of the present application. The external force disturbance observation framework includes six parts: a humanoid robot 1101, kinematic parameters 1102, a three-particle inverted pendulum model 1103, an inertial sensor (IMU, Inertial Measurement Unit) 1104, a state estimation 1105 and an external force disturbance observation 1106.

[0197] The robot control method provided in the embodiment of the present application adds two leg mass points compared to the traditional three-dimensional inverted pendulum model and takes into account the moment of inertia. The dynamic model used in the embodiment of the present application is more accurate, improving the accuracy of external force disturbance observation. The robot control method provided in the embodiment of the present application can observe three external force disturbances: the external force of the trunk, the external force of the supporting leg, and the external force of the swinging leg, and control the robot based on the observed external force values. At the same time, it has great flexibility and can be combined by selecting different preset state quantities and preset observation quantities, thus being suitable for observing multiple external force disturbances.

[0198] Based on the robot control method described in the above embodiment, Figure 12 A structural block diagram of a robot control device 100 provided in an embodiment of the present application is shown. The robot control device can be a device in an electronic device (for example, a robot). The robot control device can be implemented in software, which can be software in the form of programs and plug-ins, etc., including the following software modules: an acquisition module 101, a first determination module 102, a second determination module 103 and a control module 104. These modules are logical and can therefore be arbitrarily combined or further split according to the functions implemented.

[0199] Among them, the acquisition module 101 is used to obtain the state parameters and rotation parameters of the part to be observed of the robot at the first moment; the first determination module 102 is used to determine the centroid acceleration of the centroid point of the part to be observed based on the state parameters and the rotation parameters; the second determination module 103 is used to call a pre-constructed dynamic model to determine the external force value of the part to be observed at the second moment based on the centroid acceleration and the state parameters; the second moment is later than the first moment; the control module 104 is used to control the robot based on the external force value.

[0200] In some embodiments, the acquisition module 101 is further used to: respectively collect the position parameters and speed parameters of the part to be observed at the first moment through a position sensor and a speed sensor; the state parameters include the position parameters and the speed parameters; and collect the rotation parameters of the part to be observed at the first moment through a rotation parameter sensor.

[0201] In some embodiments, the rotation parameter sensor includes a mass sensor and a distance sensor; the acquisition module 101 is further used to: collect the mass of the part to be observed through the mass sensor; collect the first distance between the centroid of the part to be observed and the rotation axis of the part to be observed at a third moment, and the second distance between the centroid and the rotation axis of the part to be observed at the first moment through the distance sensor; the third moment is earlier than the first moment; determine the rotation parameter of the part to be observed at the first moment based on the mass, the first distance and the second distance.

[0202] In some embodiments, the rotation parameters include the rate of change of the moment of inertia; the acquisition module 101 is further used to: determine the first moment of inertia of the part to be observed at the third moment based on the mass and the first distance; determine the second moment of inertia of the part to be observed at the first moment based on the mass and the second distance; determine the rate of change of the moment of inertia of the part to be observed at the first moment based on the first moment of inertia and the second moment of inertia.

[0203] In some embodiments, the part to be observed includes a first part and a second part, and accordingly, the state parameter includes a first state parameter of the first part and a second state parameter of the second part, the rotation parameter includes a first rotation parameter of the first part and a second rotation parameter of the second part, and the centroid includes a first centroid point of the first part and a second centroid point of the second part; the first determination module 102 is also used to: call a preset zero moment point equation, and determine the centroid acceleration of the first centroid point based on the first state parameter, the first rotation parameter and the second state parameter; call the zero moment point equation, and determine the centroid acceleration of the second centroid point based on the second state parameter, the second rotation parameter, the first state parameter and the centroid acceleration of the first centroid point.

[0204] In some embodiments, the device also includes a construction module for: determining the state equation and the observation equation based on the obtained preset state variables and preset observation quantities; discretizing the state equation and the observation equation respectively to obtain a discretized state equation and a discretized observation equation; and constructing the dynamic model based on the discretized state equation and the discretized observation equation.

[0205] In some embodiments, the second determination module 103 is further used to: call the discretized state equation in the dynamic model, and determine the prior estimation value of the part to be observed at the second moment based on the centroid acceleration and the state parameters; obtain the actual observation value of the part to be observed at the second moment; call the discretized observation equation in the dynamic model, perform observation operation on the prior estimation value at the second moment, and obtain the observation operation value; determine the posterior estimation value of the part to be observed at the second moment based on the prior estimation value of the part to be observed at the second moment, a preset gain matrix, the actual observation value at the second moment, and the observation operation value; and extract the external force value from the posterior estimation value.

[0206] In some embodiments, the second determination module 103 is further used to: determine a first error covariance matrix corresponding to the state parameter, a first noise covariance matrix at the first moment, and a second noise covariance matrix at the second moment; determine the second error covariance matrix corresponding to the prior estimate based on the first error covariance matrix and the first noise covariance matrix; and determine the gain matrix corresponding to the second moment based on the second error covariance matrix and the second noise covariance matrix.

[0207] In some embodiments, the number of the parts to be observed is N, where N is an integer greater than 1; the control module 104 is further used to: construct a force distribution map of the robot based on the external force values ​​of the N parts to be observed; determine the compensation force for the robot and the force position of the compensation force based on the force distribution map; and control the robot to apply a force equal to the compensation force at the force position.

[0208] It should be noted that the description of the device embodiment of the present application is similar to the description of the method embodiment described above, and has similar beneficial effects as the method embodiment, so it will not be repeated. For technical details not disclosed in the device embodiment, please refer to the description of the method embodiment of the present application for understanding.

[0209] An embodiment of the present application provides an electronic device, which may be a robot. Figure 13 This is a schematic diagram of the structure of the robot provided in the embodiment of the present application. Figure 13 As shown, the robot 130 includes: at least one processor 131 ( Figure 13 Only one is shown in the figure), a memory 132 and computer executable instructions 133 stored in the memory 132 and executable on at least one processor 131. When the processor 131 executes the executable instructions 133, the steps of any of the above-mentioned robot control method embodiments are implemented.

[0210] The robot may include but is not limited to a processor 131 and a memory 132. It will be understood by those skilled in the art that Figure 13 This is merely an example of the robot 130 and does not constitute a limitation on the robot 130 . The robot 130 may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the robot 130 may also include input and output devices, network access devices, etc.

[0211] The processor 131 may be a central processing unit (CPU), or other general-purpose processors, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPG), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. A general-purpose processor may be a microprocessor or any conventional processor.

[0212] In some embodiments, the memory 132 may be an internal storage unit of the robot 130, such as the robot 130's hard drive or memory. In other embodiments, the memory 132 may also be an external storage device of the robot 130, such as a plug-in hard drive, a Smart Media Card (SMC), a Secure Digital (SD) card, a flash memory card, etc. Furthermore, the memory 132 may include both the robot 130's internal storage unit and an external storage device. The memory 132 is used to store operating systems, application programs, boot loaders, data, and other programs, such as computer program code. The memory 132 may also be used to temporarily store data that has been output or is about to be output.

[0213] The embodiment of the present application provides a computer-readable storage medium in which computer-executable instructions are stored. When the computer-executable instructions are executed by a processor, the processor will execute the robot control method provided by the embodiment of the present application, for example, Figure 1 The robot control method is shown.

[0214] The present invention provides a computer program product comprising computer-executable instructions stored in a computer-readable storage medium. A processor of an electronic device reads the computer-executable instructions from the computer-readable storage medium and executes the computer-executable instructions, causing the electronic device to perform the robot control method described in the present invention.

[0215] In some embodiments, the computer-readable storage medium may be a memory such as RAM, ROM, flash memory, magnetic surface memory, optical disk, or CD-ROM; or may be various devices including one or any combination of the above memories.

[0216] In some embodiments, computer-executable instructions may be in the form of a program, software, software module, script, or code, written in any form of programming language (including compiled or interpreted languages, or declarative or procedural languages), and may be deployed in any form, including as a stand-alone program or as a module, component, subroutine, or other unit suitable for use in a computing environment.

[0217] As an example, computer-executable instructions may, but need not, correspond to a file in a file system, may be stored as part of a file that stores other programs or data, such as in one or more scripts in a HyperText Markup Language (HTML) document, in a single file dedicated to the program in question, or in multiple coordinating files (e.g., files storing one or more modules, subroutines, or code portions).

[0218] By way of example, computer-executable instructions may be deployed to be executed on one electronic device, or on multiple electronic devices located at one site, or on multiple electronic devices distributed across multiple sites and interconnected by a communication network.

[0219] The above description is merely an embodiment of the present application and is not intended to limit the scope of protection of the present application. Any modifications, equivalent replacements, and improvements made within the spirit and scope of the present application are included in the scope of protection of the present application.

Claims

1. A robot control method, characterized in that: The method comprises: Obtaining state parameters and rotation parameters of the robot's part to be observed at a first moment; the state parameters include position parameters and speed parameters; the rotation parameters include a rate of change of moment of inertia; Determining the centroid acceleration of the centroid point of the part to be observed based on the state parameter and the rotation parameter; Invoking a pre-built dynamic model, based on the centroid acceleration and the state parameter, to determine the external force value applied to the part to be observed at a second moment; the second moment is later than the first moment; The robot is controlled based on the external force value.

2. The method according to claim 1, characterized in that The step of obtaining the state parameters and rotation parameters of the robot's part to be observed at the first moment includes: Using a position sensor and a speed sensor, respectively, the position parameter and the speed parameter of the part to be observed at the first moment are collected; The rotation parameter of the part to be observed at the first moment is collected by a rotation parameter sensor.

3. The method according to claim 2, characterized in that The rotation parameter sensor includes a mass sensor and a distance sensor; The step of collecting the rotation parameter of the part to be observed at the first moment by using a rotation parameter sensor includes: collecting the mass of the part to be observed by the mass sensor; collecting, by the distance sensor, a first distance between the centroid of the part to be observed and the rotation axis of the part to be observed at a third moment, and a second distance between the centroid and the rotation axis of the part to be observed at the first moment; the third moment being earlier than the first moment; The rotation parameter of the part to be observed at the first moment is determined according to the mass, the first distance, and the second distance.

4. The method according to claim 3, characterized in that Determining the rotation parameter of the part to be observed at the first moment according to the mass, the first distance, and the second distance includes: determining a first moment of inertia of the part to be observed at the third moment according to the mass and the first distance; determining a second moment of inertia of the part to be observed at the first moment according to the mass and the second distance; The rate of change of the moment of inertia of the part to be observed at the first moment is determined according to the first moment of inertia and the second moment of inertia.

5. The method according to claim 1, wherein The part to be observed includes a first part and a second part, and correspondingly, the state parameter includes a first state parameter of the first part and a second state parameter of the second part, the rotation parameter includes a first rotation parameter of the first part and a second rotation parameter of the second part, and the centroid includes a first centroid point of the first part and a second centroid point of the second part; The determining of the centroid acceleration of the centroid point of the part to be observed based on the state parameter and the rotation parameter includes: Calling a preset zero-moment point equation to determine the centroid acceleration of the first centroid point based on the first state parameter, the first rotation parameter, and the second state parameter; The zero moment point equation is called to determine the centroid acceleration of the second centroid point based on the second state parameter, the second rotation parameter, the first state parameter, and the centroid acceleration of the first centroid point.

6. The method according to claim 1, characterized in that The method further comprises: Determine the state equation and the observation equation according to the obtained preset state variables and preset observation quantities; Discretizing the state equation and the observation equation respectively to obtain a discretized state equation and a discretized observation equation; The dynamic model is constructed based on the discretized state equation and the discretized observation equation.

7. The method according to claim 6, characterized in that The calling of the pre-built dynamic model to determine the external force value of the part to be observed at the second moment based on the centroid acceleration and the state parameter includes: calling the discretized state equation in the dynamic model, and determining a priori estimated value of the part to be observed at the second moment based on the centroid acceleration and the state parameter; Obtaining an actual observation value of the part to be observed at the second moment; calling the discretized observation equation in the dynamic model, performing an observation operation on the prior estimate at the second moment, and obtaining an observation operation value; Determining a posterior estimate of the part to be observed at the second moment based on the a priori estimate of the part to be observed at the second moment, a preset gain matrix, the actual observation value at the second moment, and the observation calculation value; The external force value is extracted from the posterior estimate.

8. The method according to claim 7, characterized in that The method further comprises: Determine a first error covariance matrix corresponding to the state parameter, a first noise covariance matrix at the first moment, and a second noise covariance matrix at the second moment; Determining a second error covariance matrix corresponding to the priori estimate according to the first error covariance matrix and the first noise covariance matrix; The gain matrix corresponding to the second moment is determined based on the second error covariance matrix and the second noise covariance matrix.

9. The method according to claim 1, characterized in that The number of the parts to be observed is N, where N is an integer greater than 1; The controlling the robot based on the external force value includes: Constructing a force distribution map of the robot based on the external force values ​​of N parts to be observed; determining a compensation force for the robot and a position where the compensation force is applied based on the force distribution diagram; The robot is controlled to apply a force equal to the compensation force at the force-bearing position.

10. A robot control device, characterized in that: include: An acquisition module, configured to acquire state parameters and rotation parameters of the robot's part to be observed at a first moment; the state parameters include position parameters and speed parameters; the rotation parameters include a rate of change of moment of inertia; A first determining module is configured to determine a centroid acceleration of a centroid point of the part to be observed based on the state parameter and the rotation parameter; a second determination module, configured to call a pre-built dynamic model and determine, based on the centroid acceleration and the state parameter, a value of the external force applied to the part to be observed at a second moment; The second moment is later than the first moment; A control module is used to control the robot based on the external force value.

11. An electronic device, characterized in that: include: a memory for storing computer-executable instructions; The processor is configured to implement the robot control method according to any one of claims 1 to 9 when executing the computer executable instructions stored in the memory.

12. A computer-readable storage medium, characterized in that Computer executable instructions or computer programs are stored, and when the computer executable instructions or computer programs are executed by a processor, the robot control method according to any one of claims 1 to 9 is implemented.

Images