Method for controlling attitude of double-wheel-foot robot on non-flat ground

Abstract

The invention provides a non-flat ground attitude control method for a double-wheel-foot robot, and relates to the field of robot control. According to the method, motion of the wheel-legged robot on the non-flat ground is considered, a nonlinear system motion equation is established by considering internal and external disturbance of the robot, a complex system is decoupled into two subsystems for independent control, and a coefficient observer is established for uncertain coefficients of the system; a disturbance observer is established based on the output of a coefficient observer, so that disturbance can be observed more quickly and accurately, a nonlinear prediction model is discretized for the whole system, and the system can be well and accurately controlled by applying model prediction control based on the established observer; meanwhile, it is ensured that the robot still has the recovery capacity under the condition that the posture of the robot deviates from expectation seriously. The defects in the prior art are overcome, and the robot can still have certain movement capacity on the non-flat ground under the condition of large self-disturbance or internal disturbance.

Description

Technical Field

[0001] The invention relates to the technical field of robot control, and in particular to a posture control method for a two-wheeled foot robot on a non-flat ground. Background Art

[0002] The two-wheeled leg robot is a robot that moves and maintains balance through two side-by-side wheels. It is widely used in logistics, inspection, service robots and other fields. Its unique structural design enables it to have high maneuverability in a small space, but also places high demands on its motion control system. The main challenge faced by the two-wheeled leg robot when driving on complex terrain is how to maintain stable balance and precise speed control under various external disturbances and uncertain environments.

[0003] Balance control is crucial for two-wheeled legged robots. If the robot cannot maintain balance, it will not only fail the mission, but may also damage the robot itself or the surrounding environment. From a technical point of view, the balance control of two-wheeled legged robots involves accurate modeling of system dynamics, real-time state estimation, and fast response control. Its core goal is to ensure that the robot can quickly restore the balance state and maintain the predetermined motion trajectory under various dynamic interference conditions.

[0004] In the prior art, commonly used balance control methods include proportional-integral-differential (PID) control and linear quadratic regulation (LQR) control. These methods can achieve balance control of two-wheeled robots to a certain extent, but they also have obvious shortcomings. The PID control method is widely used in industrial control because it is simple and easy to implement. However, when facing complex nonlinear systems and multi-variable coupling, PID control often exhibits problems such as low control accuracy, long adjustment time and sensitivity to parameters. The LQR control method minimizes the state error and control energy of the system by optimizing the control input, and has good theoretical performance. However, LQR control requires an accurate system model and has poor robustness to model uncertainty and external disturbances.

[0005] In addition, when the two-wheeled robot travels on uneven ground, the complexity of the terrain and unpredictable disturbances further increase the difficulty of balance control. When dealing with these problems, the existing technology often requires frequent parameter adjustments of the system, which increases the complexity and computational burden of the control system. At the same time, these methods often show the shortcomings of delayed response and unsatisfactory control effect when dealing with instantaneous large disturbances and complex terrain changes.

[0006] Therefore, how to design a strategy that can achieve high-precision and high-robust balance control on uneven ground has become an important topic in the current research of two-wheeled legged robots. Effectively solving this problem can not only improve the practical application performance of two-wheeled legged robots, but also promote their application in more complex scenarios. Summary of the invention

[0007] The purpose of the present invention is to solve the defects of the above-mentioned prior art and provide a posture control method for a two-wheeled leg robot on non-flat ground. The leg and torso models of the two-wheeled leg robot are simplified, and the nonlinear kinematic equations of the simplified model on non-flat ground are established, the system uncertainty interference and nonlinear terms are estimated, disturbance compensation is performed based on the discretized model, and then the wheel output torque is obtained to achieve posture control on non-flat ground.

[0008] The present invention adopts the following technical solution:

[0009] A method for controlling the posture of a two-wheeled foot robot on a non-flat ground comprises the following steps:

[0010] Step 1. Consider the robot trunk as a spherical rigid body, simplify the five-link parallel leg structure into a single rod, the upper end of the single rod structure is connected to the center of mass of the trunk rigid body, and the lower end is connected to the rotation axis of the wheel;

[0011] Step 2. Establish a slope with a continuously changing angle according to the simplified model of the robot in step 1, and establish a kinematic equation for the simplified model on the slope;

[0012] Step 3. Decouple the complex system into two subsystems for independent control based on the kinematic equations established in step 2, establish a coefficient observer for the system uncertainty coefficient, and establish a disturbance observer based on the output of the coefficient observer to observe the disturbance more quickly and accurately;

[0013] Step 4. Substitute the total disturbance term of step 3 into the equation of step 2 for further simplification, design a prediction model based on the discretized model for the simplified equation and compensate for the disturbance at the same time, and obtain the wheel torque under disturbance compensation; BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is a flow chart of a non-flat ground posture control method for a two-wheeled leg robot of the present invention;

[0015] Figure 2 A schematic diagram of a dynamic model of a non-flat ground posture control method for a two-wheeled legged robot according to the present invention;

[0016] Figure 3 This is a MATLAB simulation effect diagram of a non-flat ground posture control method for a two-wheeled leg robot according to the present invention.

[0017] Figure 4 A schematic diagram of a non-flat ground posture control method for a two-wheeled legged robot according to the present invention; DETAILED DESCRIPTION

[0018] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0019] like Figure 2 As shown, a posture control method of a two-wheeled legged robot on non-flat ground is provided in the present invention, which takes into account the stable balance of the two-wheeled legged robot on non-flat ground. In order to simplify the calculation, the kinematics of the profile single wheel is analyzed. When the slope inclination angle α is unknown, the robot can satisfy the constraints of the body itself while maintaining stable balance on the slope.

[0020] The method of the present invention specifically comprises the following steps:

[0021] Step 1. The entire body is a six-degree-of-freedom entity, which means that the body can freely translate / rotate in the X / Y / Z axis direction. The body is connected to the four thighs through a revolute joint, and the thighs are connected to the calves through a revolute joint. The two calves on the same side are connected to the same wheel to form a unilateral five-link series structure. The robot trunk is regarded as a spherical rigid body, and the five-link parallel leg structure is simplified to a single rod. The upper end of the single rod structure is connected to the center of mass of the trunk rigid body, and the lower end is connected to the rotation axis of the wheel;

[0022] Step 2. Place the simplified robot model in step 1 on a slope with a continuously changing angle, consider the robot's own viscous resistance, the viscous resistance between the robot and the ground, and unknown external disturbances, establish kinematic equations for the model on the slope, and then reduce the established high-order system to two low-order systems;

[0023] Step 3. According to the kinematic equation established in step 2, an uncertain coefficient observer is established to observe the coefficient of the system input item, i.e., the wheel torque. Then, the interference item and the nonlinear item of the system are integrated into the total disturbance item, and a total disturbance observer is established. The observed coefficient is used as one of the inputs of the disturbance observer to observe the total disturbance.

[0024] Step 4. Substitute the total disturbance term and coefficient term observed in step 3 into the equation of step 2 for further simplification. Design a prediction model based on the discretized model for the simplified equation and compensate for the disturbance at the same time. Consider the system constraints at the same time. Under the optimal system balance evaluation function, the wheel torque of the disturbance compensation is directly applied to the wheel.

[0025] A further solution is that the wheel, fuselage, and connecting rod models in step one are all rigid bodies with uniform mass, and all physical quantities are known. There is no relative sliding between the wheel and the slope. The slope is a fixed geometric body and will not move. The slope angle can vary within a certain range.

[0026] A further solution is to establish kinematic equations for the above model.

[0027] The basic principle of the present invention is further described below in conjunction with specific embodiments.

[0028] 1. Motion model establishment

[0029] (1) Since the robot moves on a slope with a position angle, which is different from general planar motion, it is necessary to consider the impact of the inclination angle α on the entire system. Therefore, after considering the slope position inclination angle α and the external unknown disturbance ω (t) , as well as the viscous resistance in the driving system and the viscous resistance of the ground. The specific dynamic model diagram is as follows Figure 2 As shown, the kinetic model is

[0030]

[0031] in It means to find the differential with respect to time, m b is the body mass, m w is the wheel mass, I b is the moment of inertia of the fuselage rotating around the wheel axis, θ b is the angle between the fuselage and the vertical direction, yes

[0032] The differential of the angle between the fuselage and the vertical direction with respect to time is the angular velocity of the angle between the fuselage and the vertical direction, I w is the moment of inertia of the wheel rotating around the axis, l is the distance between the center of mass of the fuselage and the center of mass of the wheel, and r is

[0033] Wheel radius, θ w is the wheel rotation angle, is the wheel angular velocity, is the wheel rotation angular acceleration, τ is the input of the entire system, i.e., the wheel torque, α is the angle between the slope and the horizontal plane, g = 9.8 is the gravitational acceleration constant, ω 1(t) is the disturbance of the fuselage rotation, ω 2(t) is the disturbance of wheel rotation, D b is the viscous resistance in the drive system, D w is the viscous resistance of the ground.

[0034] Simplify the above formula and decouple and get

[0035]

[0036] 2. Coefficient Observer

[0037] (1) Observer

[0038] The angle and angular velocity between the current system body and the vertical direction can be directly measured by the gyroscope. The wheel angle and angular velocity are obtained by low-pass filtering using a 14-bit photoelectric encoder. The system input is the torque control of the brushless motor under FOC control. According to the system state and input, the motion model is further simplified as follows:

[0039]

[0040] In the simplified equation, since the coefficient of the wheel torque τ of the robot system is related to α, and α is unknown, an adaptive observer is established Observe B separately 11 With B 12 size.

[0041] First build B 11 The observer is denoted by B 11 The observed value is The observed value is The observer established is:

[0042]

[0043] Where L1 is the observer gain, γ1 is the adaptive gain, and the initial estimate is set According to the current wheel input τ and state estimation Compute new state estimates According to the current wheel input τ and state error Compute new parameter estimates Repeat the above steps until convergence.

[0044] (2) Selection of observer gain L1 and adaptive gain γ1

[0045] In order to make the observer converge quickly, we set the system time constant τ obs = 0.5 is used to evaluate the fast and slow response characteristics of the observation system and set the extreme points of the observation system To configure the system response characteristics,

[0046] The observer gain L1 can be set as The adaptive gain γ1 is selected based on experience to avoid unnecessary oscillation and instability by considering the robot's own estimated parameters.

[0047] (3) The above (1) and (2) established B 11 The observer of B is established in the same way. 12 Observer, also choose B 12 The observation gain of the observer is L1 = 2, γ1 = 0.1. The input of the coefficient observer is the angular velocity of the wheel rotation, the differential of the angle between the fuselage and the vertical direction relative to time, that is, the angular velocity of the angle between the fuselage and the vertical direction, and the current output torque of the wheel. The output is the observation coefficient, which is also the input of the next total disturbance observer.

[0048] 2. Total disturbance observer

[0049] The simplified system equation is: When the coefficient observation converges, we continue to establish the total disturbance observer and regard h1 and h2 as The disturbance term.

[0050] Setting up the Extended State Observer

[0051]

[0052] in They are h1 is the estimate, L1, L2, L3 are the observer gains, and the observer system poles are configured as -1, -1, -1, so the observer gain L1 = -3 is selected.

[0053] L2=3, L3=-1.

[0054] 3. Prediction Model

[0055] According to the establishment of the above coefficient observer and disturbance observer, the uncertainty coefficient is obtained The observed value of the total disturbance Discretize the model to get:

[0056]

[0057] The quadratic performance index of the system is defined as:

[0058]

[0059] Where x represents the state variable of the fuselage, specifically θ b , θ w , K is a constant equal to 100, R and Q are coefficient matrices, as follows

[0060]

[0061] The linear constraints of the system are

[0062]

[0063] The above formula 1 indicates that the fuselage inclination angle is between -1rad and 1rad, the above formula 2 indicates that the inclination acceleration is between -5rad / s and 5rad / s, the above formula 3 indicates that the wheel rotation speed is between -20rad / s and 20rad / s, and the above formula 4 indicates that the wheel torque is between -1N / M and 1N / M.

[0064] According to the above model, model constraints, and performance indicators, the quadratic programming solver module is substituted to solve the wheel torque sequence. The first value is selected as the current control value to act on the system, so that posture control on uneven ground can be achieved.

[0065] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for controlling the posture of a two-wheeled foot robot on uneven ground, characterized in that: The steps include: Step 1. Establish a motion model of a wheel-legged robot on a non-flat ground, and establish a nonlinear system motion equation based on the internal and external disturbances of the robot; Step 2. Decouple the complex system into two subsystems and control them separately; Step 3. Establish a coefficient observer for the system uncertainty coefficient, and establish a disturbance observer based on the coefficient observer output; Step 4. Substitute the total disturbance term of step 3 into the equation of step 2 for further simplification. Design a prediction model based on the discretized model for the simplified equation and compensate for the disturbance at the same time to obtain the wheel torque under disturbance compensation.

2. A method for controlling the posture of a two-wheeled foot robot on a non-flat ground according to claim 1, characterized in that: In the step 1, the robot's own viscous resistance, the viscous resistance between the robot and the ground, and the unknown external disturbance are considered, and the complex robot model is modeled on a slope with an unknown inclination. The specific motion equation is expressed as follows: Where "" represents the differential with respect to time, τ is the input of the entire system, i.e., the wheel torque, and m b is the body mass, m w is the wheel mass, I b is the moment of inertia of the fuselage rotating around the wheel axis, θ b is the angle between the fuselage and the vertical direction, is the differential of the angle between the fuselage and the vertical direction with respect to time, that is, the angular velocity of the angle between the fuselage and the vertical direction, I w is the moment of inertia of the wheel rotating around the axis, l is the distance between the center of mass of the fuselage and the center of mass of the wheel, r is the radius of the wheel, θ w is the wheel rotation angle, is the angular velocity of the wheel, is the angular acceleration of the wheel, α is the angle between the slope and the horizontal plane, g = 9.8 is the gravitational acceleration constant, ω 1(t) is the disturbance of the fuselage rotation, ω 2(t) is the disturbance of wheel rotation, D b is the viscous resistance in the drive system, D w is the viscous resistance of the ground.

3. A method for controlling the posture of a two-wheeled foot robot on a non-flat ground according to claim 1, characterized in that: In step 2, the high-order robot system is decoupled into two low-order systems: Among them B 11 , B 21 As the uncertain coefficient, the high-order system is decoupled into two inputs, which are wheel torques, and the state variables are the angle between the fuselage and the vertical direction and the wheel rotation angle and its first-order derivative.

4. A method for controlling the posture of a two-wheeled foot robot on a non-flat ground according to claim 1, characterized in that: In step 3, the coefficient of the uncertain term in the system is considered and a coefficient observer is established, which includes the following steps. 11 For example: (1) First establish B 11 The observer is denoted by B 11 The observed value is The observed value is The observer established is: Where L1 is the observer gain, γ1 is the adaptive gain, and the initial estimate is set According to the current wheel input τ and state estimation Compute new state estimates According to the current wheel input τ and state error Compute new parameter estimates Repeat the above steps until convergence; (2) Selection of observer gain L1 and adaptive gain γ1 In order to make the observer converge quickly, we set the system time constant τ obs = 0.5 is used to evaluate the fast and slow response characteristics of the observation system and set the extreme points of the observation system To configure the system response characteristics, the observer gain L1 can be set to The adaptive gain γ1 is selected based on experience to avoid unnecessary oscillation and instability by considering the robot's own estimated parameters.

5. A method for controlling the posture of a two-wheeled foot robot on a non-flat ground according to claim 1, characterized in that: In the step 3, a total disturbance observer is established based on the coefficient observer; Based on step 3, we get B 11 With B 12 The observation value of the second expansion state observer is established as follows in They are h1 is the estimate of h1, L1, L2, L3 are the gains of the observer, and the poles of the observer system are configured to be -1, -1, -1, so the gains of the observer are selected to be L1=-3, L2=3, L3=-1.

6. A method for controlling the posture of a two-wheeled foot robot on a non-flat ground according to claim 1, characterized in that: The uncertainty coefficient and total disturbance observed in step 4 can be added with reasonable constraints according to the system prediction model established in step 2 to obtain the input of the system; Based on step 3, we get B 11 With B 12 The observed values ​​and The observed values ​​are based on the two low-order systems established in step 2, and then based on the fact that the body, motor, etc. are in the real world and have certain constraints: The above formula 1 indicates that the fuselage inclination angle is between -1rad and 1rad, the above formula 2 indicates that the inclination acceleration is between -5rad / s and 5rad / s, the above formula 3 indicates that the wheel rotation speed is between -20rad / s and 20rad / s, and the above formula 4 indicates that the wheel torque is between -1N / M and 1N / M; The quadratic performance index of the system is defined as: Where x represents the state variable of the fuselage, specifically θ b , θ w , K is a constant equal to 100, R and Q are coefficient matrices, as follows According to the above model, model constraints, and performance indicators, the quadratic programming solver module is substituted to solve the wheel torque sequence, and the first value is selected as the current control value to act on the system.

Images