Quadruped robot control method based on auxiliary air injection

Abstract

The invention relates to the technical field of quadruped robot control, in particular to a quadruped robot control method based on auxiliary air injection. The method comprises the following steps: constructing a kinetic equation of the quadruped robot; the posture data of the quadruped robot are collected in real time through a sensor, and the posture of the quadruped robot is calculated; the unbalance degree of the quadruped robot is calculated according to the poses, and whether an air injection device is started or not is judged; and when the air injection device is started, the air injection angle, the air injection time and the air injection force are controlled by adopting a PID self-adaptive control algorithm until the quadruped robot restores to be stable. When the quadruped robot moves at a high speed, the robot easily loses balance due to inertia and external interference. The air injection device can quickly generate counter-acting force at the moment of unbalance to help the robot to recover balance and prevent the robot from falling down or being out of control.

Description

Technical Field

[0001] The invention relates to the technical field of quadruped robot control, and in particular to a quadruped robot control method based on auxiliary air jet. Background Art

[0002] Quadruped robots, as a type of bionic robot, have demonstrated their unique advantages in many fields. They can adapt to complex terrain and perform a variety of tasks, such as search and rescue, exploration, and transportation. However, quadruped robots under high-speed movement face a key problem: during rapid movement, the robot is prone to lose balance, resulting in falling or loss of control. This instability not only limits the robot's movement speed and efficiency, but may also cause damage to the robot itself and the surrounding environment.

[0003] Existing quadruped robots usually rely on complex mechanical structures and sensor systems to maintain balance. These systems include inertial measurement units (IMUs), force sensors, and vision systems, which monitor the robot's posture and environmental information in real time and adjust joint angles and gait to maintain stability. However, these methods often have difficulty coping with sudden balance problems during high-speed movement. For example, when the robot encounters irregular terrain or is subjected to external impact, traditional balance control methods may not respond in time, causing the robot to lose balance. Summary of the invention

[0004] The present invention discloses a control method of a quadruped robot based on auxiliary jetting. A pair of jetting devices capable of adjusting jetting angle, jetting time and jetting intensity are symmetrically installed at the center positions on both sides of the quadruped robot. The specific control method is as follows:

[0005] Construct dynamic equations for quadruped robots;

[0006] Collect the quadruped robot's posture data in real time through sensors and calculate the quadruped robot's posture;

[0007] Calculate the imbalance degree of the quadruped robot according to its position and posture, and determine whether to start the jet device;

[0008] When the jet device is started, the PID adaptive control algorithm is used to control the jet angle, jet time and jet force until the quadruped robot returns to stability.

[0009] Furthermore, the dynamic equation of the quadruped robot is as follows:

[0010]

[0011] Where t∈R represents the independent variable, q∈R n represents coordinates, Indicates speed, represents acceleration, represents an uncertain parameter, and u∈R n is the control input. is comprehensive but unknown, representing all possible boundaries of σ;

[0012] M(q(t),σ(t),t) is the mass matrix, is the Coriolis force / centrifugal force. g(q(t),σ(t),t) represents gravity, represents external interference. Functions M(.), C(.), g(.), F(.) are continuous;

[0013] M:=[M ij ] 3×3 ,C:=[C il ] 3×6 ,g:=[g i ] 3×1

[0014] Among them, i=1,2,3,j=1,2,3,l=1,…,6.

[0015] Furthermore, solving the quadruped robot posture includes:

[0016] The position of the quadruped robot is expressed in three-dimensional coordinates as follows:

[0017]

[0018] Among them, x, y, and z represent the horizontal coordinate, vertical coordinate, and height of the robot in the world coordinate system, respectively.

[0019] Direction, expressed in Euler angles, defines θ x is the pitch angle Pitch, θ y Roll angle Roll, θ z is the yaw angle; represents the rotation angle of a coordinate or coordinate system around the X-axis, Y-axis and Z-axis respectively; the corresponding basic rotation transformation matrices are:

[0020]

[0021] The attitude transformation matrix of coordinate system B relative to coordinate system A is:

[0022]

[0023] In the formula, c represents cosine, s represents sin;

[0024] Combining position and orientation, the quadruped robot posture equation is expressed as a six-dimensional vector:

[0025]

[0026] Where P is the position vector and C is the Euler angle vector.

[0027] Furthermore, the design of the jet device includes:

[0028] Size, shape, material and air jets.

[0029] Furthermore, the imbalance degree of the quadruped robot is calculated as follows:

[0030] The quadruped robot’s posture data, including acceleration, angular velocity, and contact force, are collected in real time through sensors;

[0031] Filter the collected posture data;

[0032] The quadruped robot posture equation is used to calculate the current posture of the robot, including the belief angle, roll angle and yaw angle. The specific formula is as follows:

[0033] Pitch Angle

[0034]

[0035] Roll Angle

[0036]

[0037] Yaw angle

[0038] θ z =θ z +w z ·Δt

[0039] Where, is the angular velocity measurement of the gyroscope on the Z axis and is the time step.

[0040] The Kalman filter algorithm is used to fuse the data from the accelerometer and gyroscope to update the quadruped robot’s posture estimation;

[0041] Dynamically adjust the posture estimation parameters according to the motion state of the quadruped robot and environmental changes;

[0042] When the pitch angle is greater than the corresponding threshold, the quadruped robot tilts forward or backward, and it is judged that the front or rear legs are unbalanced in weight, and the jet device is activated;

[0043] When the rolling angle is greater than the corresponding threshold, the quadruped robot tilts left or right, judging that the left or right leg is unbalanced in weight, and the jet device is activated;

[0044] When the yaw angle is greater than the corresponding threshold, the quadruped robot rotates around the vertical axis, which is judged as a deviation from the forward direction, and the jet device is activated.

[0045] Furthermore, the PID adaptive control algorithm is used to control the jet angle, jet time and jet strength. The specific method is as follows:

[0046] Calculate the pitch angle deviation and roll angle deviation Δθ of the quadruped robot y and yaw angle deviation Δθ z , the specific formula is as follows:

[0047] Δθ x =θ x,t -θ x,c

[0048] Δθ y =θ y,t -θ y,c

[0049] Δθ z =θ z,t -θ z,c

[0050] In the formula, θ x,t is the target pitch angle, θ x,c is the current pitch angle, θ y,t is the target roll angle, θ y,c is the current roll angle, θ z,t is the target yaw angle, θ z,c is the current yaw angle;

[0051] According to the pitch angle deviation Δθ x , roll angle deviation Δθ y and yaw angle deviation Δθ z , calculate the required correction torque in each direction:

[0052]

[0053] Determine the specific jet device to be started based on the correction force direction moment;

[0054] Calculate the jet angle and jet vector force of the jet device It can be expressed as:

[0055]

[0056] Among them, T i is the torque of the jet injection device, r i is the distance of the jet device relative to the robot's center of mass, F i is the thrust of the jet, α i is the horizontal angle, β i is the vertical angle;

[0057] Horizontal angle calculation formula:

[0058]

[0059] Among them, Ty and T x are the correction moments in the rolling and pitching directions respectively;

[0060] When α i = 0, the thrust of the jet device is fully used to correct the pitch angle;

[0061] when or When , the thrust is completely used to correct the roll angle;

[0062] The calculation formula of vertical direction angle is:

[0063]

[0064] Among them, T z is the correction torque in the yaw angle direction, F i is the thrust of the jet device;

[0065] When β i = 0, the thrust acts only on the horizontal plane and does not produce a vertical component;

[0066] when or , the thrust is fully used to correct the yaw angle.

[0067] Furthermore, when the quadruped robot recovers stability, the quadruped robot is controlled to resume quadruped driving under the current inertia. The specific method is as follows:

[0068] Allocate the proportion of total torque borne by the jet device and the leg drive system;

[0069] Adjust feedback control of leg movements;

[0070] Improved attitude control model;

[0071] Design imbalance detection mechanisms;

[0072] Establish priority control logic for jet devices;

[0073] Predict and control the motion trajectory and posture of the quadruped robot to complete inertial transfer.

[0074] Furthermore, the proportion of the total torque borne by the jet device and the leg drive system is allocated as follows:

[0075] Define the maximum torque capacity T of the jet device and leg drive system in each attitude angle direction (jet,max) and T (leg,max) ;

[0076] Define the proportional factors α and β of the jet device and the leg drive system. These two factors represent the proportion of the total torque borne by the jet device and the leg drive system respectively, so that:

[0077] α+β=1

[0078] α and β are adjusted dynamically according to the severity of the current attitude deviation and the energy state of the jet device.

[0079]

[0080] The total correction torque is decomposed to the jet and leg control systems using proportional factors:

[0081]

[0082] in

[0083] T jet,x =αT x ,T jet,y =αT y ,T jet,z =αT z

[0084] T leg,x =βT x ,T leg,y =βT y ,T leg,z =βT z .

[0085] Furthermore, an imbalance detection mechanism is designed. The specific method is as follows:

[0086] Predict whether the quadruped robot may be unbalanced. The specific formula is:

[0087]

[0088] Among them, Δθ x is the pitch angle deviation, Δθ y is the roll angle deviation, Δθ max,x is the maximum allowable deviation of the pitch angle, Δθ max,y is the maximum allowable deviation of the roll angle; w max,x and w max,y is the angular velocity threshold in the pitch and roll directions; a max is the maximum permissible value of the three-axis acceleration; w1, w2, w3, w4, w5 are the weights of each indicator, which are used to adjust the contribution of each parameter to the imbalance risk;

[0089] Set an imbalance risk threshold R max , when R>R max When the engine is running, the jet device is activated to assist in restoring balance;

[0090] 10. The control method of a quadruped robot based on auxiliary air jet according to claim 7, characterized in that the motion trajectory and posture of the quadruped robot are predicted and controlled, and the specific method is as follows:

[0091] Monitor the robot's speed, acceleration and attitude angle in real time, and adjust the movement of the leg device through feedback control. The specific method is as follows:

[0092] When the jet device starts to work, the inertial force is: That is, the thrust of the jet device, and its torque is

[0093] The thrust of the jet is The jet time is t, and the mass of the robot is m. After the jet ends, the speed of the robot changes to:

[0094]

[0095] Inertial force

[0096] Moment of inertia: The distance from the jet device to the center of mass is r, then the moment of inertia is It can be expressed as:

[0097]

[0098] Where × represents the vector cross product;

[0099] The state vector of the robot is x, and the control output is u, then the feedback control algorithm can be expressed as:

[0100] u=Kx

[0101] Where K is the gain matrix of the feedback controller;

[0102] After the jet is activated, the feedback controller can add consideration for the inertial effects:

[0103] u=K(x+Δx)

[0104] Among them, Δx is the state increment calculated according to the direction and magnitude of inertia;

[0105] When the direction of the inertial force is known in advance, the swing frequency and direction of the quadruped gait are adjusted in advance to conform to the inertial force and reduce imbalance. The specific methods are as follows:

[0106] The robot's gait period is T, and the gait phase is Then the gait adjustment algorithm can be expressed as:

[0107]

[0108] in, is the gait phase adjustment calculated based on the inertial direction and magnitude;

[0109] Gait phase adjustment It can be calculated based on the inertia direction and magnitude:

[0110] Where k is a constant used to adjust the sensitivity of gait phase adjustment;

[0111] Use forward inertia to complete inertia transfer.

[0112] Due to the adoption of the above technical solution, the beneficial effects of the present invention are as follows:

[0113] 1. When the quadruped robot moves at high speed, it is easy to lose balance due to inertia and external interference. The jet device can quickly generate a reaction force at the moment of imbalance to help the robot restore its balance and avoid falling or losing control.

[0114] 2. The jet device system can help the quadruped robot maintain stability on different terrains and enhance its adaptability and maneuverability in complex environments.

[0115] 3. By maintaining balance, the robot can use energy more efficiently and reduce energy waste and reduced movement efficiency caused by imbalance.

[0116] 4. The jet device system can be used as part of the robot control system to assist the main control system in posture adjustment and balance control, simplifying the control algorithm and hardware design. With the assistance of the jet device, the robot control system can focus more on motion path planning and task execution, reducing the control difficulty and complexity.

[0117] 5. During high-speed movement, a robot falling down may cause serious mechanical damage or mission failure. The jet device system can effectively prevent falling down and improve the safety and reliability of the robot.

[0118] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0119] The accompanying drawings of the present invention are as follows.

[0120] Figure 1 Schematic diagram of the three-dimensional structure of the auxiliary jet quadruped robot.

[0121] Figure 2 It is a control flow diagram of the present invention. DETAILED DESCRIPTION

[0122] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0123] A quadruped robot based on assisted air jet, such as Figure 1 As shown in the figure, a pair of jet devices with adjustable jet angle, jet time and jet intensity are symmetrically installed at the center of both sides of the quadruped robot. In order to achieve the stability of the quadruped robot during high-speed movement, an innovative jet device is designed, and its structure and installation position are determined. The main function of the jet device is to quickly provide a reaction force when the robot loses balance to help the robot regain balance. The following is a detailed description of the design details and installation position of the jet device.

[0124] The location of the jets needs to be carefully chosen to ensure that they can provide the maximum reaction force when the robot loses balance. We chose the following two key locations: symmetrical positions on both sides of the robot. The working principle of the jets is based on Newton's third law, which is action and reaction. When the robot loses balance, the control system detects the change in posture and triggers the corresponding jets. The jets quickly release high-pressure gas through the internal compressed gas reservoir to generate reaction force through the jet nozzle.

[0125] The structural design of the jet device mainly considers the following aspects: size, shape, material and jet port design.

[0126] Size and shape: The size of the jet device should be moderate, ensuring sufficient jet volume while avoiding excessive increase in weight and volume of the robot. This embodiment chooses a cylindrical design with a diameter of about 5 cm and a height of about 10 cm. This shape is not only easy to install, but also provides a stable jet effect.

[0127] Material: The shell of the jet device is made of high-strength aluminum alloy to ensure its durability and impact resistance in high-speed movement. The internal jet pipe is made of high-temperature resistant titanium alloy to withstand the high temperature generated during jetting.

[0128] Jet port design: The design of the jet port is crucial, as it directly affects the reaction force effect of the jet device. We have adopted a multi-hole jet port design, with 8 small holes with a diameter of 1 cm on each jet device, evenly distributed on the side of the cylinder. This design can provide a uniform jet effect and ensure the effectiveness of the reaction force.

[0129] The design and location of the jets provide several advantages:

[0130] Fast response: The jet device can be activated within milliseconds to provide reaction force and help the robot quickly regain balance.

[0131] Uniform reaction force: The multi-hole air jet design ensures the uniform distribution of the reaction force, avoiding damage to the robot caused by excessive local reaction force.

[0132] Flexibility: The jet head of the jet device can rotate at any angle in space, enabling it to provide reaction force in all directions when the robot loses balance, enhancing the overall stability of the robot.

[0133] In order to ensure that the quadruped robot can respond in real time and restore balance during high-speed movement, an efficient jet control algorithm is designed. The core function of the algorithm is to monitor the robot's posture in real time and trigger the jet device when necessary to provide reaction force to help the robot restore balance.

[0134] The first step of the jet control algorithm is to monitor the robot's posture in real time. Multi-sensor fusion technology is used to obtain the robot's posture data by combining inertial measurement unit IMU, force sensor and vision system.

[0135] In order to accurately measure the posture of the quadruped robot, we choose the inertial measurement unit IMU as the main sensor. IMU includes accelerometers and gyroscopes, which can measure the acceleration and angular velocity of the robot in real time. Accelerometer: used to measure the acceleration of the robot in three axes. Gyroscope: used to measure the angular velocity of the robot in three axes. IMU is usually installed at the center of the robot to ensure the accuracy and stability of the measurement data. Force sensor: The force sensor is installed on the joints and feet of the robot to measure the contact force and reaction force between the robot and the ground. These data help to determine whether the robot loses balance. Vision system: The vision system includes cameras and depth sensors to obtain environmental information and terrain features around the robot. These data help to determine whether the robot encounters irregular terrain or external impact. Through multi-sensor fusion technology, we can obtain comprehensive and accurate posture data to provide reliable input for the jet control algorithm.

[0136] A control method of a quadruped robot based on auxiliary air jet, such as Figure 2 As shown, the specific steps are as follows:

[0137] S1. Construct the dynamic equations of the quadruped robot.

[0138] The main controller controls the quadruped robot to move with four active legs. The sensor measurement module detects the real-time monitoring of the robot's posture and environmental changes.

[0139] The dynamic equation of the quadruped robot during motion can be expressed as:

[0140]

[0141] Where t∈R represents the independent variable, q∈R n represents coordinates, Indicates speed, represents acceleration, represents an uncertain parameter, and u∈R n is the control input. is comprehensive but unknown, representing all possible bounds of σ. In addition, M(q(t),σ(t),t) is the mass matrix, is the Coriolis force / centrifugal force. g(q(t),σ(t),t) represents gravity, Represents external interference. Functions M(.), C(.), g(.), F(.) are continuous.

[0142] M:=[M ij ] 3×3 ,C:=[C il ] 3×6 ,g:=[g i ] 3×1

[0143] Where i=1,2,3,j=1,2,3,l=1,…,6.

[0144] S2. Collect the posture data of the quadruped robot in real time through sensors and calculate the position and posture of the quadruped robot.

[0145] The attitude equation of a quadruped robot usually involves describing the position and orientation of the robot body in three-dimensional space. The attitude equation can be divided into two parts: position and orientation. The position is usually expressed in three-dimensional coordinates, while the orientation can be expressed in rotation matrices and Euler angles.

[0146] The position of a quadruped robot is usually expressed in three-dimensional coordinates, namely:

[0147]

[0148] Among them, x, y, and z represent the horizontal coordinate, vertical coordinate, and height of the robot in the world coordinate system, respectively.

[0149] Euler AnglesEuler Angles use three independent angles and predefined rotation axes and rotation orders to represent the posture of a rigid body. Different rotation axes and rotation orders can express different postures.

[0150] Define θ x is the pitch angle Pitch, θ yRoll angle Roll, θ z is the yaw angle. represents the rotation angle of a coordinate or coordinate system around the X-axis, Y-axis and Z-axis respectively. The corresponding basic rotation transformation matrices are:

[0151]

[0152] Therefore, the attitude transformation matrix of coordinate system B relative to coordinate system A is:

[0153]

[0154] In the above formula, c represents cosine and s represents sin.

[0155] The above formula establishes the relationship between the Euler angle and the attitude transformation matrix. Conversely, under the premise of determining an attitude transformation matrix, the corresponding Euler angle can be obtained by calculating the above formula.

[0156] Combining the position and orientation, the posture of the quadruped robot can be expressed as a six-dimensional vector:

[0157]

[0158] Where P is the position vector and C is the Euler angle vector.

[0159] S3. Calculate the imbalance degree of the quadruped robot according to the position and posture, and determine whether to start the jet device.

[0160] The robot's posture data, including acceleration, angular velocity, contact force and environmental information, are collected in real time through sensors. Since sensor data may be subject to noise and interference, the Kalman filter algorithm is used to filter the data to improve the accuracy and stability of the data.

[0161] The attitude solution algorithm is used to calculate the current attitude of the robot through the filtered data, including the pitch angle, roll angle and yaw angle.

[0162] Euler angle method: The Euler angle method is a commonly used posture representation method that describes the posture of an object through three angles, namely pitch angle, roll angle and yaw angle. For quadruped robots, the Euler angle method can be used to monitor and control the robot's posture in real time to ensure its stability during high-speed movement.

[0163] Pitch angle: The pitch angle represents the angle of rotation of the robot around the X axis. The pitch angle is calculated using the acceleration data measured by the accelerometer:

[0164]

[0165] Where, are the measurements of the accelerometer on the X, Y, and Z axes, respectively.

[0166] Roll angle: The roll angle indicates the angle of rotation of the robot around the Y axis. The roll angle is calculated using the acceleration data measured by the accelerometer:

[0167]

[0168] Yaw: The yaw angle represents the angle of rotation of the robot around the Z axis. Since the accelerometer cannot directly measure the yaw angle, it is usually necessary to combine the data of the gyroscope and magnetometer to calculate it. The angular velocity data measured by the gyroscope is integrated over time to obtain the change in the yaw angle:

[0169] θ z =θ z +w z ·Δt

[0170] Where, is the angular velocity measurement of the gyroscope on the Z axis and is the time step.

[0171] Through the above calculations, the robot's pitch angle, roll angle, and yaw angle can be calculated in real time. In order to ensure the accuracy and stability of the attitude, we need to update the attitude. Through the Kalman filter algorithm, the data of the accelerometer and gyroscope are fused to obtain a more accurate attitude estimation. According to the robot's motion state and environmental changes, the parameters of the attitude estimation are dynamically adjusted to ensure the accuracy and stability of the attitude.

[0172] Through posture calculation, the robot's tilt angle can be detected in real time and it can be determined whether it loses balance.

[0173] Pitch angle detection: The pitch angle indicates the angle at which the robot tilts forward or backward. When the pitch angle exceeds a preset threshold, the robot is judged to be tilted forward or backward. In this embodiment, the pitch angle threshold is set to ±10 degrees.

[0174] Roll angle detection: The roll angle indicates the angle at which the robot tilts left or right. When the roll angle exceeds a preset threshold, the robot is judged to be tilted left or right. In this embodiment, the roll angle threshold is set to ±10 degrees.

[0175] Yaw angle detection: The yaw angle indicates the angle at which the robot rotates around the vertical axis. The yaw angle detection is mainly used to determine the direction change of the robot and has little effect on the detection of the tilt angle.

[0176] Real-time feedback: Through the real-time feedback mechanism, the tilt angle detection algorithm is continuously adjusted and optimized. For example, according to each detection result, the parameters of the posture solution are adjusted to improve the detection accuracy.

[0177] Determine the direction of the robot losing balance: Determine the direction of the robot losing balance based on the posture data. For example, when the pitch angle is greater than 10 degrees, the robot is judged to be tilted forward; when the pitch angle is less than -10 degrees, the robot is judged to be tilted backward. Determine the degree of the robot losing balance: Determine the degree of the robot losing balance based on the posture data. For example, when the pitch angle is 15 degrees, the robot is judged to be more unbalanced; when the pitch angle is 5 degrees, the robot is judged to be less unbalanced.

[0178] S4. When the jet device is started, the PID adaptive control algorithm is used to control the jet angle, jet time and jet force until the quadruped robot returns to stability.

[0179] When a quadruped robot moves at high speed, it may experience the following imbalance states:

[0180] Pitch imbalance: The robot tilts forward or backward, and the weight on the front or back legs is uneven.

[0181] Roll imbalance: The robot tilts left or right, and the weight on the left or right leg is uneven.

[0182] Yaw imbalance: The robot rotates around the vertical axis, causing the walking direction to deviate.

[0183] Jet device number: Jet device 1 is located on the left side of the robot (L); Jet device 2 is located on the right side of the robot (R);

[0184] Attitude deviation calculation:

[0185] Pitch: Pitch angle deviation is Δθ x , assuming the target pitch angle is θ x,t , the current pitch angle is θ x,c

[0186] Δθ x =θ x,t -θ x,c

[0187] Roll: Roll angle deviation is Δθ y , let the target rolling angle be θ y,t , the current roll angle is θ y,c

[0188] Δθ y =θ y,t -θ y,c

[0189] Yaw: Yaw angle deviation is Δθ z , assuming the target yaw angle is θ z,t , the current yaw angle is θ z,c

[0190] Δθ z =θ z,t -θ z,c

[0191] Based on these deviations, the control algorithm calculates the required correction torque in each direction:

[0192]

[0193]

[0194] These moments are used to correct the pitch, roll and yaw angles. p , K i and K d are the proportional, integral and differential coefficients respectively.

[0195] If the correction moment is mainly in the pitch and yaw directions, both jets need to be used, because the correction moments for pitch and yaw need to be generated symmetrically. If the correction moment is mainly in the roll direction, only one jet is needed, because the roll moment can be corrected by generating an opposite moment with one jet:

[0196] Δθ y >0, the robot rolls to the right and uses the left jet device.

[0197] Δθ y <0, the robot rolls to the left and uses the right jet device.

[0198] The spray direction is controlled by two angles:

[0199] Horizontal direction angle α: The angle at which the jet device rotates in the horizontal plane, used to correct the roll and pitch angles.

[0200] Vertical angle β: The elevation angle of the jet device in the vertical direction, used to correct the yaw angle.

[0201] Jet vector force of jet device It can be expressed as:

[0202]

[0203] Among them, T i is the torque of the jet injection device, r i is the distance of the jet device relative to the robot's center of mass, F i is the thrust of the jet, α i is the horizontal angle, β i is the vertical angle.

[0204] The horizontal angle controls the component of the jet thrust in the horizontal plane. To generate the correction torque for the pitch and roll angles, α can be calculated i Make the thrust direction consistent with the torque direction:

[0205]

[0206] Among them, T y and T x are the correction moments in the rolling and pitching directions respectively.

[0207] When α i When = 0, the thrust of the jet device is completely used to correct the pitch angle.

[0208] when or , the thrust is completely used to correct the roll angle.

[0209] The calculation formula of vertical direction angle is:

[0210]

[0211] Among them, T z is the correction torque in the yaw angle direction, F i It is the thrust of the jet device.

[0212] When β i When =0, the thrust acts only on the horizontal plane and does not produce a vertical component.

[0213] when or , the thrust is fully used to correct the yaw angle.

[0214] Under the above-mentioned PID control, other devices will be added for auxiliary control.

[0215] The angle adjustment mechanism is used to change the spray angle of the nozzle. Common angle adjustment mechanisms include rotation mechanisms, tilting mechanisms, etc. This embodiment adopts a rotation mechanism.

[0216] Angle sensor: Use angle sensors including rotary encoders, inclination sensors, etc. to monitor the angle of the nozzle in real time and feed the data back to the control system.

[0217] Pressure sensor: monitors the jet pressure and helps determine whether the jet angle needs to be adjusted.

[0218] After the jet device is triggered, the PID adaptive control algorithm is used to calculate the appropriate jet angle, jet time, and jet force according to the imbalance state, and then work to assist the quadruped robot to stabilize. After the jet device works for a period of time, the stability of the quadruped robot is monitored in real time. If the stability standard is met, the mission is terminated.

[0219] S5. When the quadruped robot regains stability, the quadruped robot is controlled to resume quadruped driving under the current inertia. The specific steps are as follows:

[0220] S51, Introducing the coordinated control of the jet device and leg dynamics

[0221] The original dynamic control system of the robot mainly relies on leg movements to maintain balance. When the jet device intervenes, the original leg torque requirement will be reduced because the jet device will provide additional thrust and torque. Therefore:

[0222] The control system should dynamically distribute the thrust source: distribute part of the thrust to the jet device, reduce the frequency and amplitude of leg movements, and reduce mechanical wear. The calculation of the dynamic distribution torque is as follows:

[0223] First, define the maximum torque capacity T of the jet device and leg system in each attitude angle direction (jet,max) and T (leg,max) Generally speaking, the jet device can provide fast and instantaneous large torque, but the energy consumption is high, while the leg system provides smaller torque but energy saving.

[0224] Define the scaling factors α and β of the jet and leg systems such that:

[0225] α+β=1

[0226] These two factors indicate the proportion of the total torque borne by the jet device and the leg system respectively.

[0227] α and β are adjusted dynamically according to the severity of the current attitude deviation and the energy state of the jet device.

[0228] When the attitude deviation is large, it is preferred to increase the α value so that the jet device can bear more torque.

[0229] When the deviation is small and close to stability, gradually reduce the α value to reduce the intervention of the jet device and save energy.

[0230] The adjustment strategy is:

[0231]

[0232] This strategy ensures that the total correction torque T total When it is smaller, it relies more on the leg system, while in T totalWhen larger, it relies more on jet devices.

[0233] The total correction torque is decomposed into the jet and leg systems using proportional factors:

[0234]

[0235] Specifically:

[0236] T jet,x =αT x ,T jet,y =αT y ,T jet,z =αT z

[0237] T leg,x =βT x ,T leg,y =βT y ,T leg,z =βT z

[0238] Optimize control priority: In extreme imbalance situations, use the jet device to quickly stabilize the posture, so that the leg system avoids excessive torque requirements and frequent movements.

[0239] S52, adjusting the feedback control of the leg movement, the specific steps are as follows:

[0240] The feedback control of the original control system continuously adjusts the leg position and angle based on sensor data to maintain stability. Due to the intervention of the jet device, the robot will make rapid posture adjustments in a short period of time, which will introduce additional angular velocity and displacement. To this end:

[0241] Increase the tolerance of the feedback system: avoid over-response of leg movements under the instantaneous thrust of the jet device.

[0242] Dynamically adjust control gain: appropriately reduce leg feedback gain when the jet device intervenes to avoid overcompensation and prevent system oscillation.

[0243] S53, improve the posture control model, the specific steps are as follows:

[0244] When the robot loses balance during high-speed motion, the jet device can quickly provide a balancing torque. Therefore, the posture control model should consider the control effect of the jet device so that the leg dynamics model can cooperate with it instead of taking on the stabilization work alone:

[0245] Introducing a torque sharing control model: When the jet device intervenes, the overall posture torque is decomposed into a form shared by the jet device and the leg control system. This can reduce the burden on leg control.

[0246] Dynamically adjust the stability area of ​​the control model: When the jet device intervenes, the balance range is redefined to allow the robot to remain stable even when it deviates from the center at a large angle.

[0247] S54, redesign the imbalance detection mechanism

[0248] The jet device intervention can respond to the imbalance of attitude more quickly, so the imbalance detection mechanism of the original control system can be adjusted:

[0249] Detect and predict imbalance in advance: Based on multiple parameters such as speed, acceleration, angular velocity, etc., possible imbalance conditions can be judged in advance, and the jet device will be activated immediately once an imbalance signal appears.

[0250] First, the robot has the following real-time sensor data: angular velocity, acceleration a = (a x ,a y ,a z ), speed v = (v x ,v y ,v z ), position and attitude include pitch angle, roll angle and yaw angle.

[0251] In a dynamic environment, the robot can be judged to be unbalanced based on the following criteria:

[0252] Attitude angle deviation: If the pitch angle and roll angle deviate greatly from the normal attitude range, the robot may become unbalanced.

[0253] Too high angular velocity: If the angular velocity exceeds the set threshold, the robot may lose stability due to rapid rotation.

[0254] Abnormal acceleration: If the amplitude of the three-axis acceleration is large, it indicates that the robot may encounter external interference or rapid movement, and there is a risk of imbalance.

[0255] Based on these criteria, we can use the following judgment formula to make predictions:

[0256]

[0257] Among them, Δθ x is the pitch angle deviation, Δθ y is the roll angle deviation, Δθ max,x is the maximum allowable deviation of the pitch angle, Δθ max,y is the maximum allowable deviation of the roll angle; w max,x and w max,y is the angular velocity threshold in the pitch and roll directions; a max is the maximum allowable value of the three-axis acceleration; w1, w2, w3, w4, w5 are the weights of each indicator, which are used to adjust the contribution of each parameter to the imbalance risk.

[0258] By setting an imbalance risk threshold R max , when R>R max When the system considers that the risk of imbalance is high, the jet device should be activated to assist in restoring balance;

[0259] Optimize switching conditions: In extreme posture imbalance, quickly switch to the jet device assisted mode, give up the main role of leg control, and restore balance as quickly as possible.

[0260] S55, establish the priority control logic of the jet device, the specific steps are as follows:

[0261] The intervention of the jet device should have a priority control strategy, that is, under what circumstances the jet device will completely take the lead in balance adjustment, while the leg system will suspend or reduce action. For example: in extreme deviation posture, such as when the roll angle or pitch angle exceeds a certain threshold, switch to the "jet priority" mode; when approaching stability, gradually hand over control to the leg control system to save energy consumption of the jet device and restore the normal dynamic control mode.

[0262] S56, Robot motion prediction and planning based on jet control, the specific steps are as follows:

[0263] The intervention of the jet device will affect the robot's motion trajectory and posture changes, so the control system needs to update the motion trajectory planning to ensure that the posture changes caused by the jet device do not conflict with the expected trajectory. Specific adjustments include:

[0264] Real-time update of posture prediction model: After the jet device is started, the predicted value of the robot's posture is adjusted in real time to dynamically correct the leg movements.

[0265] Plan the motion compensation strategy when the jet device intervenes: For example, under the thrust of the jet device, recalculate the robot's gait planning so that it can smoothly transition to the next action after regaining balance.

[0266] When the auxiliary jet device is started or stopped, the robot may generate forward thrust and torque due to inertia. In order to better control and utilize this inertial force, the specific inertial force and inertial torque can be obtained through dynamic calculation, and a control strategy can be used to solve or utilize the inertial force.

[0267] The inertia force of the jet device when it starts working is: That is, the thrust of the jet device, and its torque is

[0268] Calculation of inertia force after the jet device finishes working:

[0269] The thrust of the jet is The jet time is t, and the mass of the robot is m. After the jet ends, the speed of the robot changes to:

[0270]

[0271] Inertial force

[0272] Moment of inertia: The distance from the jet device to the center of mass is r, then the moment of inertia is It can be expressed as:

[0273]

[0274] Here, × represents the vector cross product.

[0275] For the treatment of inertial force:

[0276] S561, Feedback Control:

[0277] The robot's speed, acceleration and attitude angle are monitored in real time, and the movement of the legs is dynamically adjusted through feedback control to offset the inertial force and inertial moment.

[0278] Force feedback: Based on the direction and magnitude of the inertial force, the robot's leg strength is adjusted in time to generate a force opposite to the inertial force to keep the robot balanced.

[0279] Torque feedback: Adjust the angles of the robot's leg joints according to the direction and magnitude of the inertia moment so that the torque generated by the legs can offset the inertia moment.

[0280] The state vector of the robot is x, and the control output is u, then the feedback control algorithm can be expressed as:

[0281] u=Kx

[0282] Where K is the gain matrix of the feedback controller.

[0283] After the jet is activated, the feedback controller can add consideration for the inertial effects:

[0284] u=K(x+Δx)

[0285] Where Δx is the state increment calculated based on the inertial direction and magnitude.

[0286] S562, make gait adjustments in advance:

[0287] When the direction of the inertial force is known, the swing frequency and direction of the quadruped gait can be adjusted to conform to the inertial force and reduce imbalance. For example, during the period of inertial force, the robot can smoothly complete the transfer of center of gravity by changing the step length of the front and rear legs to reduce the risk of instability.

[0288] The robot's gait period is T, and the gait phase is Then the gait adjustment algorithm can be expressed as:

[0289]

[0290] in, It is the gait phase adjustment calculated based on the inertial direction and magnitude.

[0291] Gait phase adjustment It can be calculated based on the inertia direction and magnitude:

[0292] Here, k is a constant used to adjust the sensitivity of gait phase adjustment.

[0293] S563, using forward inertia:

[0294] After the jet device is activated, the robot can actively use the forward inertia to move, thereby reducing the workload of the jet device. For example, the robot can be allowed to move slightly forward for a short period of time to balance with the jet direction of the jet device.

[0295] Adjust the jet angle of the jet device: The jet device can adjust the jet angle according to the forward inertia of the robot to generate lateral or reverse torque to assist in posture control.

[0296] 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 above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A quadruped robot control method based on auxiliary jet, characterized in that: A pair of jet devices with adjustable jet angle, jet time and jet intensity are symmetrically installed at the center of both sides of the quadruped robot. The specific control method is as follows: Construct dynamic equations for quadruped robots; Collect the quadruped robot's posture data in real time through sensors and calculate the quadruped robot's posture; Calculate the imbalance degree of the quadruped robot according to its position and posture, and determine whether to start the jet device; When the jet device is started, the PID adaptive control algorithm is used to control the jet angle, jet time and jet force until the quadruped robot returns to stability.

2. The quadruped robot control method based on auxiliary jetting as claimed in claim 1, characterized in that: The dynamic equations of the quadruped robot are as follows: Where t∈R represents the independent variable, q∈R n represents coordinates, Indicates speed, represents acceleration, represents an uncertain parameter, and u∈R n is the control input. is comprehensive but unknown, representing all possible boundaries of σ; M(q(t),σ(t),t) is the mass matrix, is the Coriolis force / centrifugal force. g(q(t),σ(t),t) represents gravity, represents external interference. Functions M(.), C(.), g(.), F(.) are continuous; M:=[M ij ] 3×3 ,C:=[C il ] 3×6 ,g:=[g i ] 3×1 Among them, i=1,2,3,j=1,2,3,l=1,…,6.

3. The quadruped robot control method based on auxiliary jetting as claimed in claim 1, characterized in that: Solving the quadruped robot pose includes: The position of the quadruped robot is expressed in three-dimensional coordinates as follows: Among them, x, y, and z represent the horizontal coordinate, vertical coordinate, and height of the robot in the world coordinate system, respectively. Direction, expressed in Euler angles, defines θ x is the pitch angle Pitch, θ y Roll angle Roll, θ z is the yaw angle; represents the rotation angle of a coordinate or coordinate system around the X-axis, Y-axis and Z-axis respectively; the corresponding basic rotation transformation matrices are: The attitude transformation matrix of coordinate system B relative to coordinate system A is: In the formula, c represents cosine, s represents sin; Combining position and orientation, the quadruped robot posture equation is expressed as a six-dimensional vector: Where P is the position vector and C is the Euler angle vector.

4. The quadruped robot control method based on auxiliary air jet according to claim 1, characterized in that: The design of the jet device includes: Size, shape, material and air jets.

5. The quadruped robot control method based on auxiliary air jet according to claim 3, characterized in that: Calculate the imbalance degree of the quadruped robot. The specific method is as follows: The quadruped robot’s posture data, including acceleration, angular velocity, and contact force, are collected in real time through sensors; Filter the collected posture data; The quadruped robot posture equation is used to calculate the current posture of the robot, including the belief angle, roll angle and yaw angle. The specific formula is as follows: Pitch Angle Roll Angle Yaw angle i z =θ z +w z ·Δt Where, is the angular velocity measurement of the gyroscope on the Z axis and is the time step. The Kalman filter algorithm is used to fuse the data from the accelerometer and gyroscope to update the quadruped robot’s posture estimation; Dynamically adjust the posture estimation parameters according to the motion state of the quadruped robot and environmental changes; When the pitch angle is greater than the corresponding threshold, the quadruped robot tilts forward or backward, and it is judged that the front or rear legs are unbalanced in weight, and the jet device is activated; When the rolling angle is greater than the corresponding threshold, the quadruped robot tilts left or right, judging that the left or right leg is unbalanced in weight, and the jet device is activated; When the yaw angle is greater than the corresponding threshold, the quadruped robot rotates around the vertical axis, which is judged as a deviation from the forward direction, and the jet device is activated.

6. The quadruped robot control method based on auxiliary air jet according to claim 5, characterized in that: The PID adaptive control algorithm is used to control the jet angle, jet time and jet strength. The specific method is as follows: Calculate the pitch angle deviation Δθ of the quadruped robot x , roll angle deviation Δθ y and yaw angle deviation Δθ z , the specific formula is as follows: Dth x =θ x,t -θ x,c Dth y =θ y,t -θ y,c Dth z =θ z,t -θ z,c In the formula, θ x,t is the target pitch angle, θ x,c is the current pitch angle, θ y,t is the target roll angle, θ y,c is the current roll angle, θ z,t is the target yaw angle, θ z,c is the current yaw angle; According to the pitch angle deviation Δθ x , roll angle deviation Δθ y and yaw angle deviation Δθ z , calculate the required correction torque in each direction: Determine the specific jet device to be started based on the correction force direction moment; Calculate the jet angle and jet vector force of the jet device It can be expressed as: Among them, T i is the torque of the jet injection device, r i is the distance of the jet device relative to the robot's center of mass, F i is the thrust of the jet, α i is the horizontal angle, β i is the vertical angle; Horizontal angle calculation formula: Among them, T y and T x are the correction moments in the rolling and pitching directions respectively; When α i = 0, the thrust of the jet device is fully used to correct the pitch angle; when or When , the thrust is completely used to correct the roll angle; The calculation formula of vertical direction angle is: Among them, T z is the correction torque in the yaw angle direction, F i is the thrust of the jet device; When β i = 0, the thrust acts only on the horizontal plane and does not produce a vertical component; when or , the thrust is fully used to correct the yaw angle.

7. The quadruped robot control method based on auxiliary air jet according to claim 1, characterized in that: When the quadruped robot regains stability, the quadruped robot is controlled to resume quadruped driving under the current inertia. The specific method is as follows: Allocate the proportion of total torque borne by the jet device and the leg drive system; Adjust feedback control of leg movements; Improved attitude control model; Design imbalance detection mechanisms; Establish priority control logic for jet devices; Predict and control the motion trajectory and posture of the quadruped robot to complete inertial transfer.

8. The quadruped robot control method based on auxiliary air jet according to claim 7, characterized in that: The proportion of the total torque borne by the jet device and the leg drive system is allocated as follows: Define the maximum torque capacity T of the jet device and leg drive system in each attitude angle direction (jet,max) and T (leg,max) ; Define the proportional factors α and β of the jet device and the leg drive system. These two factors represent the proportion of the total torque borne by the jet device and the leg drive system respectively, so that: α+β＝1 α and β are adjusted dynamically according to the severity of the current attitude deviation and the energy state of the jet device. The total correction torque is decomposed to the jet and leg control systems using proportional factors: in T jet,x =αT x ,T jet,y =αT y ,T jet,z =αT z T leg,x =βT x ,T leg,y =βT y ,T leg,z =βT z 。 9. The quadruped robot control method based on auxiliary air jet according to claim 7, characterized in that: Design an imbalance detection mechanism. The specific method is as follows: Predict whether the quadruped robot may be unbalanced. The specific formula is: Among them, Δθ x is the pitch angle deviation, Δθ y is the roll angle deviation, Δθ max,x is the maximum allowable deviation of the pitch angle, Δθ max,y is the maximum allowable deviation of the roll angle; w max,x and w max,y is the angular velocity threshold in the pitch and roll directions; a max is the maximum permissible value of the three-axis acceleration; w1, w2, w3, w4, w5 are the weights of each indicator, which are used to adjust the contribution of each parameter to the imbalance risk; Set an imbalance risk threshold R max , when R>R max When the vehicle is in a state of shock, activate the jet device to assist in restoring balance.

10. The quadruped robot control method based on auxiliary air jet according to claim 7, characterized in that: Predict and control the motion trajectory and posture of the quadruped robot. The specific method is as follows: Monitor the robot's speed, acceleration and attitude angle in real time, and adjust the movement of the leg device through feedback control. The specific method is as follows: When the jet device starts to work, the inertial force is: That is, the thrust of the jet device, and its torque is The thrust of the jet is The jet time is t, and the mass of the robot is m. After the jet ends, the speed of the robot changes to: Inertial force Moment of inertia: The distance from the jet device to the center of mass is r, then the moment of inertia is It can be expressed as: Where × represents the vector cross product; The state vector of the robot is x, and the control output is u, then the feedback control algorithm can be expressed as: u=Kx Where K is the gain matrix of the feedback controller; After the jet is activated, the feedback controller can add consideration for the inertial effects: u=K(x+Δx) Among them, Δx is the state increment calculated according to the direction and magnitude of inertia; When the direction of the inertial force is known in advance, the swing frequency and direction of the quadruped gait are adjusted in advance to conform to the inertial force and reduce imbalance. The specific methods are as follows: The robot's gait period is T, and the gait phase is Then the gait adjustment algorithm can be expressed as: in, is the gait phase adjustment calculated based on the inertial direction and magnitude; Gait phase adjustment It can be calculated based on the inertia direction and magnitude: Where k is a constant used to adjust the sensitivity of gait phase adjustment; Use forward inertia to complete inertia transfer.

Images