Humanoid robot complex terrain balance control method based on state and disturbance observer

Abstract

The invention provides a humanoid robot complex terrain balance control method based on a state and disturbance observer, and belongs to the technical field of robot application. The problem that the humanoid robot is unstable when walking in a complex environment is solved. According to the technical scheme, the method comprises the following steps that 1, a simplified humanoid robot'spring-damping 'inverted pendulum model is established; 2, designing a state and disturbance observer; step 3, based on the step 2, designing a state feedback controller taking a real-time observation object of the state and disturbance observer as input; 4, selecting an appropriate controller pole and an appropriate controller observation point; and 5, simulation verification of attitude balance control of the humanoid robot under the complex terrain is carried out. The method has the beneficial effects that the robustness of the robot to interference and the task execution capability can be improved particularly in disaster rescue and complex environment operation.

Description

Technical Field

[0001] The present invention relates to the field of robot application technology, and in particular to a complex terrain balance control method for a humanoid robot based on a state and disturbance observer. Background Art

[0002] In the field of modern robotics, humanoid robots have gradually become an important research direction in the fields of service robots, medical rehabilitation and complex industrial operations due to their flexibility and adaptability. However, due to the high degree of freedom of joints and complex kinematic structures of humanoid robots, when faced with the unpredictability of the external environment and the uncertainty of their own system parameters, the robots are often subject to unexpected external disturbances (such as collisions, uneven ground) which can lead to a decrease in control system performance and even robot instability. Therefore, improving the posture control performance of humanoid robots and ensuring the coordination and stability of the entire system are technical difficulties in humanoid robot research.

[0003] The paper "Balance Control of Humanoid Robot Based on PID in Dynamic Environment" designs a posture control method based on PID controller to meet the needs of humanoid robots to maintain posture balance in dynamic environments. This method has two shortcomings: (1) When facing large external disturbances, PID control may be difficult to effectively maintain posture balance; (2) It shows certain limitations when dealing with complex nonlinear systems and is difficult to cope with coupling effects and dynamic changes in multi-degree-of-freedom systems.

[0004] The paper "Fuzzy Logic Control of Bipedal Robot Posture Stability" designs a posture control method based on fuzzy logic to meet the needs of bipedal robots to maintain posture stability in complex environments. This method has two shortcomings: (1) Fuzzy logic control relies on a complex set of rules. When the complexity of the system increases, the number of fuzzy logic rules will also increase significantly, resulting in an increase in the amount of calculation and affecting real-time performance. (2) Fuzzy rules are designed based on specific environmental conditions. When the external environment changes significantly, the adaptability of fuzzy logic control may be insufficient, affecting the stability of posture control.

[0005] How to solve the above technical problems is the subject faced by the present invention. Summary of the invention

[0006] The purpose of the present invention is to provide a complex terrain balance control method for a humanoid robot based on a state and disturbance observer, aiming to improve the stability of the humanoid robot when walking in a complex environment, especially the adaptability in disaster rescue and complex working environments. The controller realizes real-time monitoring and adjustment of the robot's dynamic posture and external interference by designing a state and disturbance dual-action observer, ensuring that the robot can still maintain balance and perform tasks in various uncertain environments.

[0007] The inventive concept of the present invention is as follows: the present invention takes into account that the impact force generated when the robot walks on an unknown uneven ground will affect its posture balance, so inspired by the traditional inverted pendulum model, a simplified "spring-damper" inverted pendulum model of a humanoid robot is established; then, a state and disturbance dual-acting observer is designed to observe in real time the inclination angle, angular velocity and continuous disturbance of the center of mass of the humanoid robot during walking; secondly, on the basis of the observer, a state feedback controller with the real-time observation object of the state and disturbance observer as input is designed to maintain the posture balance of the robot when it is subjected to unknown disturbances in complex terrain; finally, according to the different stages of the humanoid robot's walking process, the present invention designs a variety of controller schemes, and determines the optimal model parameters through system identification and appropriate selection of poles to ensure that the robot can maintain a stable balance posture in various environments.

[0008] In order to achieve the above-mentioned invention object, the technical solution adopted by the present invention is specifically: a complex terrain balance control method of a humanoid robot based on a state and disturbance observer, comprising the following steps:

[0009] Step 1: Considering that the impact force generated when the robot walks on an unknown uneven ground will affect its posture balance, a simplified humanoid robot "spring-damper" inverted pendulum model is established, inspired by the traditional inverted pendulum model. This model can effectively absorb the negative effects of the impact.

[0010] Step 2, for the humanoid robot subject to unpredictable interference in complex terrain, a state and disturbance observer is designed to observe in real time the inclination angle, angular velocity and continuous disturbance of the center of mass of the humanoid robot during walking;

[0011] Step 3, based on step 2, designing a state feedback controller with the real-time observation object of the state and disturbance observer as input, so as to maintain the posture balance of the robot when it is subject to unknown disturbances in complex terrain;

[0012] Step 4: for the observer-based state feedback controller designed above, determine the specific model parameters of the humanoid robot's walking process through system identification, select appropriate controller poles and controller observation points, and divide the robot into a coronal plane model and a sagittal plane model in the single support stage, and a coronal plane model and a sagittal plane model in the double support stage according to the walking stage and the overall structure of the robot;

[0013] Step 5: Based on the Aelos humanoid standard platform, conduct simulation verification of the posture balance control of the humanoid robot in complex terrain and evaluate the actual performance of the posture balance controller based on the state and disturbance observer.

[0014] Furthermore, in step 1, the motion equation of the simplified humanoid robot "spring-damper" inverted pendulum model is as follows:

[0015]

[0016] Where m is the mass, l is the length of the connecting rod, c is the damping constant, k is the spring constant, g is the acceleration of gravity, and θ is the swing angle. is the angular velocity of the pendulum, is the angular acceleration, u θ is the input angle of the inverted pendulum; if θ and u θ is small, sinθ≈θ and lu θ ≈u, then the equation of motion can be approximated as follows:

[0017]

[0018] Furthermore, in step 2, it is assumed that the disturbance F d , the input is u, and the state space equation is:

[0019]

[0020]

[0021] The above equation can be simply expressed as follows:

[0022]

[0023]

[0024] in:

[0025]

[0026] Therefore, the observer is designed as follows:

[0027]

[0028] Where L is the observer gain determined by the pole placement method.

[0029] Furthermore, in step 3, if the state is known, a state feedback controller can be used to maintain balance, and the formula is as follows:

[0030]

[0031] Where K is the control gain determined by the pole placement method, r is the reference input value, N is a constant, α is the total gain of the controller change effect, and COM ref is the desired center of mass position. The transfer function between r and θ is as follows:

[0032]

[0033] Furthermore, in step 3, in order to maintain the posture balance of the humanoid robot, an observer-based state feedback controller is used, and the input u is defined as follows:

[0034]

[0035] In the formula, is the estimated state quantity, is the estimated disturbance. The state feedback controller can effectively eliminate the estimated disturbance, and the formula is as follows:

[0036]

[0037] Furthermore, in step 4, the humanoid robot has a single-support stage and a double-support stage during walking, and the model of the humanoid robot in each stage is different in the sagittal plane and the coronal plane, so the specific parameters of the four models need to be determined through system identification.

[0038] Furthermore, in step 4, the analytical solution of ZMP is:

[0039]

[0040] in:

[0041]

[0042]

[0043]

[0044] Assume that the peak value P of ZMP output 1 and P 2 ,

[0045] P 1 =(t 1 ,y 1 )

[0046] P 2 =(t 2 ,y 2 )

[0047] The damping constant and spring constant are calculated as follows:

[0048]

[0049]

[0050] The observer poles and controller poles are appropriately selected by trial and error.

[0051] Furthermore, in step 4, the humanoid robot detects phase changes by force / torque sensors on its feet during walking. When the robot enters a new stage, such as from the double support stage to the single support stage or from the single support stage to the double support stage, a new controller is replaced, and the total gain α of the new controller is new Increase smoothly from 0 to 1t in a short time sh , and the total gain α of the previous controller pre By reducing from 1 to 0, the conversion can be completed instantly, which effectively improves the stability of the humanoid robot when changing its posture.

[0052] Compared with the prior art, the present invention has the following beneficial effects:

[0053] 1. The present invention designs a posture balance controller based on a state and disturbance observer, which can estimate and eliminate the influence of external disturbances in real time. Compared with traditional PID control and fuzzy logic control, the present invention significantly improves the stability of the robot's posture balance when facing external interference, especially in complex and dynamic environments, and has stronger robustness.

[0054] 2. In response to the multi-degree-of-freedom and nonlinear problems of humanoid robots during walking, the present invention designs multiple controller solutions for different environments and parameters, which can adaptively adjust the controller parameters and effectively maintain the stability of the robot's posture when the system parameters change or the environment changes suddenly, overcoming the limitations of traditional methods in dealing with multi-degree-of-freedom coupling effects.

[0055] 3. Compared with fuzzy logic control that relies on a large number of rules, the present invention realizes a relatively simple and efficient control method by designing an attitude balance controller based on a state and disturbance observer. While ensuring the control accuracy, it greatly reduces the amount of calculation, improves the real-time performance and operating efficiency of the controller, and is suitable for high-dynamic working environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.

[0057] Figure 1 The present invention is a flowchart of the working process of the humanoid robot complex terrain balance controller based on the state and disturbance observer.

[0058] Figure 2 It is a mathematical model diagram of the "spring-damping" inverted pendulum in the present invention.

[0059] Figure 3 It is a system structure diagram of the humanoid robot in the present invention.

[0060] Figure 4 This is a gain scheduling diagram for a controller to smoothly transition when the walking phase of the humanoid robot moves from a double-support stage to a single-support stage in the present invention.

[0061] Figure 5 This is a real picture of the AELOS SMART robot under the AELOS humanoid standard platform of the present invention.

[0062] Figure 6 The natural response curve diagram of the present invention shows the angle change of the "spring-damped" inverted pendulum over time without control input.

[0063] Figure 7 Observer performance graph of the present invention; comparing the angle estimated by the observer with the true angle.

[0064] Figure 8 It is a closed-loop response curve diagram in the present invention; it shows the system response under the state feedback controller. DETAILED DESCRIPTION

[0065] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. Of course, the specific implementation methods described here are only used to explain the present invention and are not used to limit the present invention.

[0066] Example 1

[0067] See also Figures 1 to 8 By comparing with the traditional controller without the observer, the control effect of this embodiment on the posture balance of the humanoid robot under complex terrain is fully demonstrated.

[0068] In order to test the performance of different control methods, four controllers were designed: controller A (traditional PID controller), controller B (fuzzy logic controller), controller C (LQR controller), and controller D (humanoid robot complex terrain balance controller based on state and disturbance observer). During the experiment, a variety of disturbance conditions were set, including random impact force and uneven terrain, in order to test the controller's responsiveness in an uncertain environment. The experiment focused on the following performance indicators: attitude error (angle and position error from the target attitude), steady-state error (residual error after the system stabilizes), and control signal energy (control input intensity required by the controller to maintain balance), and the following controller performance comparison table 1 was obtained:

[0069] Table 1 Controller performance comparison table

[0070]

[0071] The experimental results show that this embodiment performs well in all indicators, especially in the face of sudden disturbances, controller D can restore balance in the shortest time, with the smallest attitude error and steady-state error, and relatively low control signal energy. In addition, controller C is second, also showing good response speed and low error, while controllers A and B recover slowly in the case of strong interference and have relatively large errors.

[0072] From the perspective of controller implementation, this embodiment can capture system status and disturbance information in real time, and use the state feedback control algorithm to effectively eliminate external interference, greatly improving the robustness and control accuracy of the system. In contrast, although traditional PID and fuzzy controllers are simple, they are prone to large control errors when facing complex environments and strong disturbances. The LQR controller performs better than PID and fuzzy controllers in terms of control signal energy and state feedback control, but there is still a problem of large errors.

[0073] In general, the above-mentioned traditional control methods have certain limitations when adjusting the posture balance of humanoid robots, especially when dealing with external interference, the control effect and system robustness of the traditional control methods are still to be improved compared with this embodiment. These comparative effects provide an important basic comparison for the effectiveness verification of the method of this embodiment in subsequent embodiments.

[0074] Example 2

[0075] See also Figures 1 to 8 , the technical solution provided by this embodiment is as follows: Figure 5 As shown, the AELOS humanoid standard platform is adopted, and the robot model is AELOS SMART. This embodiment proposes a complex terrain balance controller for a humanoid robot based on a state and disturbance observer to realize the posture balance control of the humanoid robot in a complex environment. First, a simplified "spring-damping" inverted pendulum model of a humanoid robot is established. Secondly, a state and disturbance dual-action observer is designed to observe the inclination angle, angular velocity and continuous disturbance of the center of mass of the humanoid robot in real time during walking. Then, based on the observer, a state feedback controller is designed, and the zero moment point of the robot is set as the output, and the inclination angle, angular velocity and disturbance of the center of mass are set as the state. By using the posture and interference information provided by the observer, the controller can dynamically adjust the control input of the robot to ensure the balance of the robot in a complex environment. Finally, a variety of controller schemes are designed for the motion characteristics of the humanoid robot in different environments (such as parameters or motion directions of different legs). Through system identification, the optimal controller parameters are determined to ensure that the robot can maintain balance in each walking stage.

[0076] The method flow of this embodiment is as follows Figure 1 As shown, the specific method is as follows:

[0077] 1. Considering the impact force generated when the robot walks on an unknown uneven ground, a simplified humanoid robot "spring-damper" inverted pendulum model is established based on the traditional inverted pendulum model to effectively absorb the negative effects caused by the impact.

[0078] The robot used in this example uses high-gain position control for all actuators, taking into account the damping effects from the mechanical hardware, joint controllers, and the sole material. The equation of motion for the simplified humanoid robot "spring-damper" inverted pendulum model is as follows:

[0079]

[0080] Where m is the mass, l is the length of the connecting rod, c is the damping constant, k is the spring constant, g is the acceleration of gravity, and θ is the swing angle. is the angular velocity of the pendulum, is the angular acceleration, u θ is the input angle of the inverted pendulum. If θ and u θ is small, sinθ≈θ and lu θ ≈u, then the equation of motion can be approximated as follows:

[0081]

[0082] like Figure 2 As shown, the forces and moments at the ankle joint are measured and the ZMP of the robot along the x-axis is calculated as follows:

[0083]

[0084] where F z and M y are the z-direction ground reaction force and y-direction ground reaction force measured by the FT sensor at the ankle. y is also the reaction torque of the spring and damper, so it can be written in terms of the action of the spring and damper as:

[0085]

[0086] Furthermore, if the z-direction motion is small, it can be assumed that F z The same as the weight of the robot, the equation for ZMP is

[0087]

[0088] 2. Aiming at the unpredictable interference of humanoid robots in complex terrain, a state and disturbance observer is designed to observe the inclination angle, angular velocity and continuous disturbance of the center of mass of the humanoid robot in real time during walking.

[0089] Overall exercise program Figure 3 When the desired ZMP is planned, the pattern generator generates the motion reference trajectory of COM and swing foot based on the online preview control. Assuming the disturbance F d , the state space equation is:

[0090]

[0091]

[0092] The above equation can be simply expressed as:

[0093]

[0094] y=C′z+D′u

[0095] in:

[0096]

[0097] Therefore, the observer is designed as follows:

[0098]

[0099] Where L is the observer gain determined by the pole placement method.

[0100] 3. Based on step 2, design a state feedback controller. The proposed controller is placed Figure 3 In the dotted box, the control input of the robot is adjusted according to the estimated posture and disturbance to maintain the balance of the robot. The feedback controller formula for maintaining the balance state is as follows:

[0101]

[0102] Where K is the control gain determined by the pole placement method, r is the reference input value, N is a constant, α is the total gain of the controller change effect, and COM ref is the desired center of mass position. The transfer function between r and θ is as follows. The transfer function between r and θ is as follows:

[0103]

[0104] A state feedback controller based on a state observer is used, and the input u is defined as follows:

[0105]

[0106] The state feedback controller can effectively eliminate the estimated disturbance, and the formula is as follows:

[0107]

[0108] 4. Based on step 3, for the humanoid robot with different modeling parameters for several legs and / or movement directions, perform system identification for four situations and design corresponding controllers.

[0109] The humanoid robot has a single support phase and a double support phase during walking. At the same time, the humanoid robot model is different in the sagittal plane and the coronal plane, so the observer and controller are designed for each plane. The damping and spring constants of the simple model are determined by performing system identification on the four models, and the controller poles and observer poles are appropriately selected by trial and error, as shown in Table 2.

[0110] Table 2 Different control parameters of four models

[0111]

[0112] During walking, when the robot enters a new phase, such as from double support phase to single support phase or from single support phase to double support phase, the controller needs to be replaced. This phase change is detected by the force / torque sensor on the foot. To achieve instantaneous phase transition, the total gain α of the new controller is new Increase smoothly from 0 to 1t in a short time sh , and the total gain α of the previous controller pre Decrease from 1 to 0, such as Figure 4 shown.

[0113] 5. Based on the Aelos humanoid standard platform, conduct simulation verification of the posture balance control of the humanoid robot in complex terrain, and evaluate the actual performance of the posture balance controller based on the state and disturbance observer.

[0114] First, a simplified humanoid robot "spring-damper" inverted pendulum model is established through MATLAB / Simulink. The state matrix A, input matrix B, output matrix C, and transfer matrix D of the system are defined through the state space model to capture the dynamic characteristics of the robot, and the natural recovery ability of the system without control input is verified through free response simulation, such as Figure 6 As shown. Next, a state and disturbance dual-action observer is designed to estimate the inclination angle, angular velocity and continuous disturbance of the robot's center of mass in real time. The observer selects the appropriate gain L through the pole configuration method, constructs the state space observer model, and simulates and verifies the observer's estimation accuracy of the system state, as shown in Figure 7 As shown. The results show that the observer can accurately track the true state and provide reliable state estimation for the feedback controller. Finally, the state feedback controller is designed based on the state information provided by the observer, the control gain K is determined by the pole placement method, and a closed-loop control system is constructed, as shown in Figure 8The simulation results show that the controller can effectively improve the response speed and stability of the system, enabling the humanoid robot to quickly restore balance when disturbed. The simulation of the closed-loop system response verifies the robustness of the state feedback controller, which can significantly improve the posture control performance, especially when dealing with dynamic changes in complex environments.

[0115] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A complex terrain balance control method for a humanoid robot based on a state and disturbance observer, characterized in that: The following steps are involved: Step 1: Considering that the impact force generated when the robot walks on an unknown uneven ground will affect its posture balance, a simplified "spring-damper" inverted pendulum model of a humanoid robot is established, inspired by the traditional inverted pendulum model; Step 2, for the humanoid robot subject to unpredictable interference in complex terrain, a state and disturbance observer is designed to observe in real time the inclination angle, angular velocity and continuous disturbance of the center of mass COM of the humanoid robot during walking; Step 3, based on step 2, designing a state feedback controller with the real-time observation object of the state and disturbance observer as input, so as to maintain the posture balance of the robot when it is subjected to unknown disturbances in complex terrain; Step 4: for the observer-based state feedback controller designed above, determine the specific model parameters of the humanoid robot's walking process through system identification, select appropriate controller poles and controller observation points, and divide the robot into a coronal plane model and a sagittal plane model in the single support stage, and a coronal plane model and a sagittal plane model in the double support stage according to the walking stage and the overall structure of the robot; Step 5: Based on the Aelos humanoid standard platform, conduct simulation verification of the posture balance control of the humanoid robot in complex terrain and evaluate the actual performance of the posture balance controller based on the state and disturbance observer.

2. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 1 is characterized in that: In step 1, the motion equation of the simplified humanoid robot "spring-damper" inverted pendulum model is as follows: Where m is the mass, l is the length of the connecting rod, c is the damping constant, k is the spring constant, g is the acceleration due to gravity, and θ is the swing angle. is the angular velocity of the pendulum, is the angular acceleration, u θ is the input angle of the inverted pendulum.

3. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 1 is characterized in that: In step 2, it is assumed that the disturbance F d , the input is u, and the disturbance is defined as an additional element of the state, then the state space equation is: The above equation can be expressed in the following form: in: Therefore, the observer is designed as follows: Where L is the observer gain determined by the pole placement method.

4. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 1 is characterized in that: In step 3, if the state is known, a state feedback controller is used to maintain balance, and the formula is as follows: Where K is the control gain determined by the pole placement method, r is the reference input value, N is a constant, α is the total gain of the controller change effect, and COM ref For the desired center of mass position, the transfer function between r and θ is as follows:

5. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 1 is characterized in that: In step 3, in order to maintain the posture balance of the humanoid robot, an observer-based state feedback controller is used, and the input u is defined as follows: In the formula, is the estimated state quantity, is the estimated disturbance, and the state feedback controller eliminates the estimated disturbance. The formula is as follows:

6. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 1 is characterized in that: In step 4, the humanoid robot has a single support stage and a double support stage during walking, and the model of the humanoid robot in each stage is different in the sagittal plane and the coronal plane. Therefore, the specific parameters of the four models are determined through system identification, including the coronal plane model and the sagittal plane model of the single support stage, and the coronal plane model and the sagittal plane model of the double support stage.

7. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 5 is characterized in that: In step 4, the analytical solution of the zero moment point ZMP is: in: Assume the peak values ​​of ZMP output are P1 and P2, P1=(t1,y1) P2=(t2,y2) The damping constant and spring constant are calculated as follows: The observer poles and controller poles are appropriately selected by trial and error.

8. The complex terrain balance control method of a humanoid robot based on a state and disturbance observer according to claim 6 is characterized in that: In step 4, when the humanoid robot is walking, the force or torque sensor on the foot detects the phase change. When entering a new stage, from the double support stage to the single support stage or from the single support stage to the double support stage, the controller needs to be replaced in time. To ensure that the conversion is performed instantly, the total gain α of the new controller is designed. new Increase smoothly from 0 to 1t in a short time sh , the total gain α of the previous controller pre Decrease from 1 to 0.

Images