Robot control method and device, electronic equipment and storage medium

Abstract

The invention provides a robot control method and device, electronic equipment and a storage medium. The method comprises the following steps: acquiring a state parameter and a rotation parameter of a to-be-observed part of the robot at a first moment; determining a centroid acceleration of a centroid point of the to-be-observed part based on the state parameter and the rotation parameter; calling a pre-constructed kinetic model, and determining an external force value borne by the to-be-observed part 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. The external force borne by the robot is observed on the basis of the pre-constructed kinetic model by combining the state parameters and the rotation parameters of the robot, the precision of external force value observation can be improved, and therefore the control precision of the robot is improved.

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 similar structure to 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 period 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 the related art, when observing the external force acting on the robot, the robot model is greatly simplified. Since the robot model is overly simplified, the accuracy of the external force value observation 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 state parameters and rotation parameters of the robot and observing the external force applied to the robot based on a pre-constructed dynamic model, the accuracy of the external force value observation 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, including: an acquisition module, used to acquire state parameters and rotation parameters of a part to be observed of the robot at a first moment; a first determination module, 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; a second determination module, used to call a pre-constructed dynamic model, and 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, used to control 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 also 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 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 point 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 parameter; 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; 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 diagram 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 diagram; 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 the robot performs a grasping task, first, the state parameters and rotation parameters of the robot's part to be observed at the 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; the 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 the second moment is determined; the second moment is later than the first moment; 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 the 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 through the state parameters and rotation parameters, thereby providing a reliable data basis for the prediction of the external force value; on the basis of calculating the accurate centroid acceleration, 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, thereby improving the control accuracy of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0023] Figure 3 It is a schematic diagram of the process of constructing a kinetic model provided in an embodiment of the present application;

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

[0025] Figure 5 is a flow chart of a method for determining a gain matrix provided in an embodiment of the present application;

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

[0027] Figure 7 is a schematic diagram of the structure of a humanoid robot provided in an embodiment of the present application;

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

[0029] Fig. 9is a flow chart of the extended Kalman filter algorithm provided in the embodiment of the present application;

[0030] Fig. 10A It is a waveform diagram of an observer when the robot provided in 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] Fig. 10B It 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 acted upon by an external force in the negative direction of the Y axis;

[0032] Fig. 10C It 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;

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

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

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

[0036] Fig.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 the present application clearer, the present application will be further described in detail below in conjunction with the accompanying drawings. The described embodiments should not be regarded as limiting the present application. All other embodiments obtained by ordinary technicians in the 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 describe a subset of all possible embodiments, but it can 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 instructions are added. In the following description, the terms "first\second\third" involved are merely used to distinguish similar objects and do not represent a specific ordering of the objects. It can be understood that "first\second\third" can be interchanged in 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 a part of a computer program with a predetermined function, and works together with other related parts to achieve a predetermined goal, and can be implemented in whole or in part by using software, hardware (such as processing circuits or memories) 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 function 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 the 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-constructed dynamic model is called, and based on the centroid acceleration and state parameters, the external force value of the part to be observed at the second moment is determined; the second moment is later than the first moment; 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 the 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 through the state parameters and rotation parameters, thereby providing a reliable data basis for the prediction of the external force value; on the basis of calculating the accurate centroid acceleration, the pre-constructed dynamic model is called to observe the external force value of the robot, which can improve the accuracy of the external force value observation, thereby improving 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, laptop computers, 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 embodiment of the present application is described in detail below with reference to the accompanying drawings.

[0045] Figure 1 1 is an optional flow chart of the robot control method provided in the embodiment of the present application, and the method can be applied to electronic devices. The following will take the electronic device as a server as an example for exemplary description. Figure 1 As shown, the method includes the following steps S101 to S104:

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

[0047] Here, the robot can be a humanoid robot, a monopod robot or a multipod robot. Among them, the humanoid robot can be used in the service industry as a customer service staff or shopping guide robot, and can also be used in the medical field for patient rehabilitation training, or to assist the elderly with limited mobility to complete daily activities; due to its simple structure, the monopod robot can be used in extreme working environments, such as working in a small space, or cleaning the exterior walls of high-rise buildings; the multipod robot can be used in geological surveys. Due to its better stability and multi-terrain adaptability, it can conduct geological surveys and sample collection in mountainous areas and jungles. It can also be used in agricultural operations, such as planting, fertilizing or picking in complex terrain. The part to be observed refers to the part of the robot that needs to observe the external force value, which can be the robot's leg, torso or the whole body; the first moment refers to the moment before the current moment. If the current moment is 10:04, the first moment can be 10:03.

[0048] The state parameters are quantities that describe the physical state of the part to be observed at the first moment. The 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 certain 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] Rotation parameters are quantities that describe the rotational motion state of the part to be observed, usually including rotation angle, angular velocity (rotation speed of the part to be observed about a certain rotation axis) and angular acceleration (rate of change of angular velocity), etc. Rotation parameters can be obtained through sensors, or they can be inferred and estimated through known rotation 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 refers to a device used to measure the position information of the part to be observed in space. The position sensor can be a laser rangefinder, ultrasonic sensor or other device. The position sensor can directly measure the position of the part to be observed in a specific coordinate system. A speed sensor is a device used to measure the movement speed of the part to be observed. The speed sensor can include a gyroscope, a speed measuring motor or a laser speed meter. A rotation parameter sensor is a device used to measure the rotational motion parameters of the part to be observed, such as a gyroscope or an inertial measurement unit. The inertial measurement unit integrates multiple sensors and can provide more comprehensive rotation state information.

[0052] By collecting the position parameters, speed parameters and rotation parameters of the sensor, the complete state of the observed part at the first moment can be obtained, which provides a reliable data basis for subsequent external force value observation and robot control, reduces observation errors and improves 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 is a schematic diagram of a process of collecting the rotation parameters of the part to be observed at the first moment by using a rotation parameter sensor provided in an embodiment of the present application, Figure 2 It is shown 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] The mass sensor is a device used to measure the mass of the part to be observed, which 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 trunk mass of the robot (if the robot has arms, the trunk mass also includes the arm mass), leg mass and total mass.

[0056] Step S202, using a distance sensor, collects 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, etc. The rotation axis is the axis of rotation of the observed part at a specific moment. For a symmetrical object, the symmetry axis of the object is usually the rotation axis; for a robot, the rotation axis can be determined by the robot designer, such as the rotation axis of each joint of the robot, or determined based on the precession phenomenon of the gyroscope under the action of external force. The precession phenomenon means that when the gyroscope is subjected to an external force, the rotation axis of the gyroscope will rotate perpendicular to the direction of the external force instead of deflecting in the direction of the external force. The centroid refers to the geometric center of an object.

[0058] Step S203, 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.

[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 according to the mass, the first distance and the second distance, which can be achieved by the following method: first, according to the mass and the first distance, the first moment of inertia of the part to be observed at the third moment is determined; then, according to the mass and the second distance, the second moment of inertia of the part to be observed at the first moment is determined; finally, according to 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 is the quantity that describes the inertia of the part to be observed when it rotates around the axis of rotation, which depends on the mass of the part to be observed and the distance between the centroid of the part to be observed and the axis of rotation; the rate of change of the moment of inertia is the rate of change of the moment of inertia of the part to be observed at different times over 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]

[0064] Among them, I 1 represents the first moment of inertia, m represents the mass of the part to be observed, r 1 Indicates 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, and the calculation formula is shown in formula (2):

[0066]

[0067] Among them, I 2 represents the second moment of inertia, m represents the mass of the part to be observed, and r 2 Indicates 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 according to 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]

[0070] in, represents the rate of change of the moment of inertia at the first moment, and Δt represents 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 speed of the centroid of the observed part in space over time.

[0073] In some embodiments, 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 point includes a first centroid point of the first part and a second centroid point of the second part; in step S102, based on the state parameter and the rotation parameter, the centroid acceleration of the centroid point of the part to be observed is determined, 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 point equation is a formula for calculating the position of the robot's balance point during movement. When the balance point is within the support surface, the total torque of the robot 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, which determines the stability of the robot in this posture.

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

[0076]

[0077] Among them, p x 、p y 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; m b Represents the mass of the robot body (including the arm); m l represents the mass of the robot's legs; M represents the total mass of the robot; l represents the distance from the sole of the foot to the centroid of the torso in the y-axis direction; g is the gravitational acceleration; x b ,y b 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; s ,y s 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 moving process; c x,b 、c y,b 、c z,b It represents the offset of the robot’s torso mass center relative to the torso centroid; c x,t 、c y,t 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 trunk 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; c x,s 、c y,s 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.

[0078] The center of mass refers to the center point of the mass distribution of an object. For objects with regular shapes 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, and then calculate the offset between the center of mass of each part and the overall centroid, calculate the weighted average of the offsets between the center of mass of all parts and the overall centroid, determine the weighted average as the offset of the center of mass of the object relative to the centroid of the object, and add the coordinates of the centroid to the offset to obtain the coordinates of the center of mass of the object.

[0079] 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. 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 of the center of mass 1 relative to the centroid is 1 = (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) according to the offset and the centroid position.

[0080] In some embodiments, when using a three-particle inverted pendulum model to observe external force values, 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 forces on the trunk and the swing leg as an example: according to the zero-point moment equation represented 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 moment equation in 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 are all unknown parameters and cannot be directly obtained. 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 trunk, it can be approximately assumed that Then, the above assumptions are substituted into formula (4) and formula (5) respectively, and the external force disturbance term of the trunk is added to the calculation result; the first centroid acceleration obtained by solving is and The expressions of are shown in formula (6) and formula (7):

[0081]

[0082] Among them, f b,x represents the external force on the robot's trunk in the x-axis direction; p x Indicates the position of the zero moment point on the x-axis; c z,s 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 trunk on the z-axis; c z,t 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 z-axis; b Indicates the position of the robot's torso centroid on the z-axis; It represents the rate of change of the moment of inertia of the robot trunk on the y-axis.

[0083]

[0084] Among them, f b,y represents the external force on the robot's trunk in the y-axis direction; p y 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 trunk on the x-axis.

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

[0086]

[0087] in, σ 2 =gm b (c x,b +x b );f s,x 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.

[0088]

[0089] in, σ2 =gm b (c y,b +y b );f s,y 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 swinging 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 in this application.

[0090] 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.

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

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

[0093] Step S301, determining the state equation and the observation equation according to the obtained preset state variables and preset observation quantities.

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

[0095] 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):

[0096]

[0097] Among them, f b,z Represents the external force in the z-axis direction on the robot trunk; f s,z Indicates the external force in the z-axis direction on the robot's swinging legs; [] T It represents the transpose of a matrix.

[0098]

[0099] The state equation F(X) is obtained by differentiating the expression of the preset state variable. The expression of the state equation F(X) is shown in formula (12):

[0100]

[0101] in ε 5 =gm b (c y,b +y b ); ε 8 =gm b (c x,b +x b );f t,z represents the external force in the z-axis direction on the robot's supporting legs; c z,t It represents the offset of the center of mass of the robot's supporting leg on the z-axis relative to the midpoint of the line connecting the supporting foot and the torso.

[0102] Let the observed quantity Y = H(X) and obtain the observation equation. The expression of the observation equation is shown in formula (13):

[0103]

[0104] Among them, ε 1 to ε 8 The expression of is consistent with F(X), and this application will not repeat it here.

[0105] Step S302, discretizing the state equation and the observation equation respectively to obtain a discretized state equation and a discretized observation equation.

[0106] 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.

[0107] 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):

[0108]

[0109] Among them, X(k+1) represents the state quantity at time k+1; X(k) represents the state quantity at time k; dt represents the discrete time interval.

[0110] Since the observation of the observed quantity is directly carried out 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 of the observation equation is shown in formula (15):

[0111] Y(k+1)=H(X(k+1)) Formula (15)

[0112] Among them, Y(k+1) represents the observation value at time k+1.

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

[0114] 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.

[0115] In some embodiments, see Figure 4 , Figure 4 is a schematic diagram of a flow chart for 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 parameter, which can be achieved by the following steps S1031 to S1035:

[0116] 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.

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

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

[0119] X(k+1|k)=Fk(X(k|k)) Formula (16)

[0120] Among them, X(k+1|k) represents the prior estimate at time k+1; X(k|k) represents the optimal estimate of the system state at time k, and the optimal estimate of the system state at time k is obtained based on the prediction at time k-1; Fk() represents the discrete state equation at time k.

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

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

[0123] 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.

[0124] 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 the observation operation value.

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

[0126] Step S1034, determining the a posteriori 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, the preset gain matrix, the actual observation value and the observation operation value at the second moment.

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

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

[0129] X(k+1|k+1)=X(k+1|k)+K(k+1)[Y(k+1)-H(X(k+1|k))] Formula (17)

[0130] Among them, X(k+1|k+1) is the posterior estimate at the second moment; X(k+1|k) is the prior estimate at the second moment; K(k+1) is the preset gain matrix; Y(k+1) is the actual observation value at the second moment; H(X(k+1|k) is the observation operation value at the second moment.

[0131] 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:

[0132] Step S501: determine 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.

[0133] 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 cannot be fully captured by the model. Observation noise refers to the random error of sensors or measuring devices during the measurement process, reflecting the random difference between the measured value and the true value.

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

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

[0136] P(k+1|k)=f(k)P(k|k)f T (k)+Q k Formula (18)

[0137] Wherein, P(k+1|k) represents the second error covariance matrix; P(k|k) is the first error covariance matrix corresponding to the state parameters; Q k is the first noise covariance matrix, which is the covariance matrix of the system process noise; f(k) is the Jacobian matrix of the equation Fk(X(k|k); f T (k) 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.

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

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

[0140] K(k+1)=P(k+1|k)h T (k)[h(k)P(k+1|k)h T (k)+R k+1 ]-1 Formula (19)

[0141] Wherein, K(k+1) is the gain matrix corresponding to the second moment; P(k+1|k) represents the second error covariance matrix; R k+1 is the covariance matrix of the measurement noise at time k+1; h(k) is the Jacobian matrix of the equation H(X(k|k)).

[0142] In some embodiments, after determining the posterior estimation value 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):

[0143] P(k+1|k+1)=[IK(k+1)H(k)]P(k+1|k) Formula (20)

[0144] Among them, P(k+1|k+1) represents the third error covariance matrix; I is the unit matrix; K(k+1) is the gain matrix corresponding to the second moment; P(k+1|k) is the second error covariance matrix.

[0145] Step S1035, extracting the external force value from the posterior estimation value.

[0146] After calculating formula (17), the posterior estimation value at the second moment is obtained. The posterior estimation value is data in matrix form, and the posterior estimation value 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 legs in the three directions of x, y, and z can be directly extracted according to the posterior estimation value calculated by formula (17).

[0147] 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 first moment) (i.e., the a posteriori estimate at the previous moment). Then, the gain matrix can be updated according to the error covariance matrix of the a priori estimate, and the optimal estimate of the state quantity at the current moment (i.e., the a posteriori estimate) can be determined based on the actual observed quantity 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 applied to the robot at the current moment.

[0148] In some embodiments, it is necessary to determine whether the external force on the robot is considerable. If the external force on the robot is predicted to be considerable based on the dynamic model, all state variables can be observed based on the dynamic model. If it is not considerable, the state variables cannot be observed. Determining whether the external force on the robot is considerable can be achieved by the following method:

[0149] First, according to formula (21), the partial differential of the observation equation H(X) corresponding to the preset observation quantity with respect to the preset state variable X is solved:

[0150]

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

[0152] Then, let L 0 FH=H(X), using L 0 FH takes the partial derivative of X and multiplies it with F(X) to calculate L 1 FH, L 1 The expression of FH is shown in formula (22):

[0153]

[0154] Among them, α 1 =m l (z b +z s ) 2 ; α 5 =gm b +gm l ;

[0155] Similarly, we can calculate L 2 FH to L 17 FH, L 2 FH to L 17 The expression of FH is shown in formula (23) and formula (24):

[0156]

[0157] It should be noted that L 2 FH to L 17 The solution process of FH and L 1 The solution process of FH is similar. For the sake of simplicity, this application will not further explain L 2 FH to L 17 Expand the expression of FH.

[0158] Next, let the matrix G = [L 1 F 2 FH … L 17 FH] T , the observability matrix dG of the three-particle inverted pendulum model can be solved. The expression of the observability matrix dG is shown in formula (25):

[0159]

[0160] Finally, it can be determined that the rank of the observability matrix dG of the three-particle inverted pendulum model is 18, which is equal to the dimension of the preset state variable X. That is, the external forces on the robot's trunk and swinging legs are observable. Therefore, the external forces f on the robot's trunk and swinging legs at different times can be observed based on the dynamic model expression. b,x 、f b,y 、f b,z 、f s,x 、f s,y and f s,z .

[0161] Step S104, controlling the robot based on the external force value.

[0162] 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 having unpredictable behaviors such as tipping over, sudden acceleration, etc. due to the influence of the external force.

[0163] 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:

[0164] Step S1041, constructing a force distribution map of the robot based on the external force values ​​of the N parts to be observed.

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

[0166] In some embodiments, after determining the external force values ​​of N parts to be observed, the direction of the external force on the robot can be determined according to the momentum theorem, and the direction of the robot's momentum change is the same as the direction of the external force; by analyzing the change in the robot's speed, the direction of the external force can be indirectly inferred. For example, if the robot slows down during the forward movement, 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 cause the state based on the known motion state of the robot (speed, acceleration, position change, etc.). For example, the direction of the external force applied to the robot can be derived by combining the acceleration and speed changes of each part to be observed of the robot with a known dynamic model. The specific method for inferring the direction of the external force is not limited in this application.

[0167] 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 (with arrows indicating the direction and magnitude of the force for each observed part), a heat map (with a color gradient slope indicating the distribution of the magnitude of the force, the darker the color, the greater the force), a three-dimensional mechanical model (indicating the force conditions of each part of the robot in three-dimensional space), etc. The specific construction method is not limited in this application.

[0168] Step S1042, based on the force distribution diagram, determine the compensation force for the robot and the force position of the compensation force.

[0169] 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.

[0170] In some embodiments, the force distribution diagram can be analyzed, and the external forces in different directions on the observed part 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 direction to offset the influence of the external force. The external forces in all directions received by the robot on the observed part can also be vector-summed to calculate the direction and magnitude of the resultant force received by the observed part, and a compensation force equal to the magnitude and opposite to the resultant force can be applied to the observed part according to the direction and magnitude of the resultant force to offset the influence of the external force.

[0171] In some embodiments, there may be multiple force-bearing positions of the compensation force, such as if there are multiple parts of the robot to be observed, and a force-bearing position of the compensation force may be determined for each part to be observed; the force-bearing position of the compensation force may also be one, such as determining an intermediate position based on multiple parts to be observed of the robot, or determining a position for concentrated compensation based on the size and direction of the external forces acting on multiple parts to be observed, as the force-bearing position of the compensation force.

[0172] Step S1043, controlling the robot to apply a force equal to the compensation force at the force-bearing position.

[0173] In some embodiments, the joints of the robot are usually equipped with motors or servo motors, and the force or torque at the joints is adjusted by controlling the current or voltage of the motors to complete the application of the compensation force. For example, when the robot is subjected to an external force from the back to the front during walking, the motors of the robot's torso or legs can be controlled to increase the force in the opposite direction of the external force to prevent the robot from tipping over.

[0174] 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, which may be disturbed by external forces during the welding process, such as the touch of workers. 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, which 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. The robot may be disturbed by external forces during the transportation or installation process, such as external force interference caused by ground bumps. Through the robot control method provided in the embodiment of the present application, the external force exerted on the construction robot can be observed, and the construction robot can be controlled according to the observed external force value to ensure stable transportation and safe installation of materials.

[0175] 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 the 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 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 the pre-constructed 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.

[0176] The following is an explanation of an exemplary application of the embodiments of the present application in a practical application scenario.

[0177] The embodiment of the present application provides a robot external force disturbance observation control method, which can estimate the external force disturbance to the robot when the robot is standing and walking. The external force can be a real external force, such as a push from a human, or a virtual external force, such as the disturbance to the robot caused by bumps on the road. Since the external force acting on the robot will affect the stability of the robot, it is necessary to estimate the external force so as to compensate for the external force in the control and keep the robot stable.

[0178] The robot control method provided in the embodiment of the present application is applicable to a broad humanoid robot, see 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 trunk, a head, two legs, and two arms. Figure 8 , Figure 8 Schematic diagram of a three-mass 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 f acting on the trunk and the swinging leg in the x-axis, y-axis, and z-axis directions. b,x 、f b,y 、f b,z 、f s,x 、f s,y and f s,z .

[0179] 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 an extended Kalman filter algorithm to estimate the state quantities, and finally obtains the external force disturbance observation value of the humanoid robot. Fig. 9 , Fig. 9 : is a flow chart of the extended Kalman filter algorithm provided in the embodiment of the present application. The extended Kalman filter algorithm can be implemented by following steps S601 to S605:

[0180] Step S601, setting preset state variables and preset observation quantities.

[0181] In the implementation process, the preset state variables and the preset observation quantities can be set according to the actual application scenario, the state equation and the observation equation can be determined according to the preset state variables and the preset observation quantities, and the state equation and the observation equation can be discretized to obtain the discrete state equation and the discrete observation equation. For the specific implementation process, please refer to the contents corresponding to formula (10) to formula (13).

[0182] Step S602, determining a priori estimation values ​​and a second error covariance matrix.

[0183] In 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; for the specific implementation process, refer to formula (14) to formula (16), and the content corresponding to formula (18).

[0184] Step S603: determine the gain matrix.

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

[0186] Step S604, determining the posterior estimation value.

[0187] In the implementation process, the a posteriori estimate value can be determined according to the a priori estimate value and the gain matrix; for the specific implementation method, please refer to the content corresponding to formula (17).

[0188] Step S605: update the second error covariance matrix to obtain a third error covariance matrix.

[0189] 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).

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

[0191] 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 external force disturbance.

[0192] See also FIG. 10A to FIG. 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 respond quickly to 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. Fig. 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 respond quickly to observe the size of the external force applied to the torso and its change curve over time. Fig. 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 magnitude of the external force applied to the torso, and the external force is relatively stable. After the external force is suddenly withdrawn, the observed value quickly drops to near 0. Fig. 10D This is a waveform diagram of the observer when the robot is in the stepping 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 the stepping state, a stable external force in the positive direction of the x-axis is applied to the robot trunk, and the observer can quickly respond to the observed external force on the trunk. After the external force is suddenly withdrawn, the observed value returns to the level before the external force was applied. In order to avoid redundancy, the external force observations of other directions, scenes and legs are not shown in this application.

[0193] It should be noted that since the humanoid robot does not completely conform to the three-particle 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.

[0194] See also Fig.11 , Fig.11 It 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.

[0195] 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, which improves 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 of the trunk external force, the supporting leg external force and the swing leg external force, and control the robot according to the observed external force values. At the same time, it has good flexibility and can be combined by selecting different preset state quantities and preset observation quantities, so as to be suitable for observing multiple external force disturbances.

[0196] Based on the robot control method described in the above embodiment, Fig.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 may be a device in an electronic device (e.g., a robot). The robot control device may be implemented in software, which may be software in the form of programs and plug-ins, etc., including the following software modules: an acquisition module 101, an image mapping module 102, a force analysis module 103, and a control module 104. These modules are logical, and therefore may be arbitrarily combined or further split according to the functions implemented.

[0197] Among them, the acquisition module 101 is used to obtain the state parameters and rotation parameters of the robot's part to be observed 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.

[0198] In some embodiments, the acquisition module 101 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.

[0199] In some embodiments, the rotation parameter sensor includes a mass sensor and a distance sensor; the acquisition module 101 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.

[0200] In some embodiments, the rotation parameters include the rate of change of the moment of inertia; the acquisition module 101 is also 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.

[0201] In some embodiments, 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 point 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.

[0202] In some embodiments, the device also includes a construction module, which is used to: determine the state equation and the observation equation based on the acquired 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.

[0203] 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 parameter; 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.

[0204] In some embodiments, the second determination module 103 is further used to: determine a first error covariance matrix corresponding to the state parameters, a first noise covariance matrix at the first moment, and a second noise covariance matrix at the second moment; determine a 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.

[0205] 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 also used to: construct a force distribution diagram 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 diagram; and control the robot to apply a force equal to the compensation force at the force position.

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

[0207] An embodiment of the present application provides an electronic device, which may be a robot. Fig.13 Schematic diagram of the structure of the robot provided in the embodiment of the present application. Fig.13 As shown, the robot 130 includes: at least one processor 131 ( Fig.13 Only one is shown), a memory 132, and computer executable instructions 133 stored in the memory 132 and executable on at least one processor 131, the processor 131 executing the executable instructions 133 to implement the steps of any of the above-mentioned robot control method embodiments.

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

[0209] 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, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc.

[0210] In some embodiments, the memory 132 may be an internal storage unit of the robot 130, such as a hard disk or memory of the robot 130. In other embodiments, the memory 132 may also be an external storage device of the robot 130, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the robot 130. Further, the memory 132 may also include both an internal storage unit of the robot 130 and an external storage device. The memory 132 is used to store an operating system, an application program, a boot loader, data, and other programs, such as program codes of a computer program. The memory 132 may also be used to temporarily store data that has been output or is to be output.

[0211] The present application embodiment 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 be caused to execute the robot control method provided by the present application embodiment, for example, Figure 1 The robot control method is shown.

[0212] The 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. The processor of the electronic device reads the computer executable instructions from the computer readable storage medium, and the processor executes the computer executable instructions, so that the electronic device executes the robot control method described in the embodiment of the present application.

[0213] 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.

[0214] 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.

[0215] 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 coordinated files (e.g., files storing one or more modules, subroutines, or code portions).

[0216] As an 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 at multiple sites and interconnected by a communication network.

[0217] The above is only an embodiment of the present application and is not intended to limit the protection scope of the present application. Any modifications, equivalent substitutions and improvements made within the spirit and scope of the present application are included in the protection scope 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 the first moment; Determining the centroid acceleration of the centroid point of the part to be observed based on the state parameter and the rotation parameter; Calling a pre-built dynamic model, based on the centroid acceleration and the state parameter, determines the external force value of 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: The position parameter and the speed parameter of the part to be observed at the first moment are respectively collected by a position sensor and a speed sensor; the state parameter includes the position parameter and the speed parameter; 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 means of a rotation parameter sensor comprises: The mass sensor is used to collect the mass of the part to be observed; By means of 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 are collected; the third moment is 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 The rotation parameters include the rate of change of the moment of inertia; 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, characterized in that 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 point 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 comprises: 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; 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 observation operation on the priori estimated value at the second moment, and obtaining an observation operation value; Determine 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 calculation value; The external force value is extracted from the a posteriori 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; Determine 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 comprises: Constructing a force distribution diagram of the robot based on the external force values ​​of N parts to be observed; Based on the force distribution diagram, determining a compensation force for the robot and a force position of the compensation force; 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, used to acquire state parameters and rotation parameters of the robot's to-be-observed part at a first moment; A first determination module, used to determine the centroid acceleration of the centroid point of the part to be observed based on the state parameter and the rotation parameter; A second determination module is used to call a pre-built dynamic model to determine the external force value of the part to be observed at a second moment based on the centroid acceleration and the state parameter; 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; A processor, 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