Control method for eliminating upper body posture shake of biped robotTechnical Field
The invention relates to the technical field of automatic control, in particular to a control method for eliminating upper body posture shaking of a biped robot.
Background
The humanoid robot is a system with multiple degrees of freedom, nonlinearity, complex kinematics and dynamic characteristics. Research on humanoid robots has gone through a difficult process ranging from biped to humanoid, from static to dynamic walking and operation, from planar movement to uneven ground walking. The double-leg structure of the robot is similar to that of a human body, and the robot has better maneuverability compared with the traditional wheeled and tracked robots, and particularly shows superiority on uneven ground, stairs and occasions with only discrete and discontinuous contact points with the ground. However, the biped robot has the characteristic of unstable nature and is easy to fall down.
In order for the robot to walk, the walking track of the robot, i.e., the dynamic gait, needs to be given, including the angles of the respective joints of the articulated robot. The dynamic gait of the robot is an inherent, periodic motion that is generated based on the overall dynamics of the biped robot. Due to the coupling of constraints and the complexity of the kinetic equations, dynamic gait calculations require an optimization process. Therefore, dynamic gait is generally achieved by off-line calculation methods. That is, dynamic gait is generally generated assuming that the bipedal robot model and surrounding environment are known. In fact, the real walking environment of the biped robot cannot be completely the same as the set environment and conditions, and due to the change of the surrounding environment of the robot or the generation of unknown conditions, if the robot is completely executed mechanically according to the dynamic gait planned in advance, the planned dynamic gait is not corrected and controlled in real time, the upper body of the robot is likely to shake, and abnormal phenomena such as instability and even falling are likely to occur.
Therefore, how to eliminate the upper body posture shake of the biped robot is a problem that needs to be solved urgently by those skilled in the art.
Disclosure of Invention
In view of this, the present invention provides a control method for eliminating the upper body posture shake of a biped robot, which controls the waist joint of the robot to eliminate the upper body shake.
In order to achieve the purpose, the invention adopts the following technical scheme:
a control method for eliminating the upper body posture shake of a biped robot is disclosed, wherein a gyroscope and an accelerometer are arranged on the robot; the gyroscope is used for measuring angular velocity, and the accelerometer is used for measuring acceleration;
the control method comprises the following steps:
acquiring the actual angular velocity of the upper body of the robot;
calculating a first correction amount based on the actual angular velocity and the desired angular velocity;
calculating a second correction amount based on a planned gait and an actual posture of the robot generated in advance;
acquiring the acceleration of the upper body of the robot;
calculating a third correction amount based on the acceleration and the desired acceleration;
calculating the upper body posture correction quantity of the robot based on the first correction quantity and the second correction quantity;
and adjusting the waist joint based on the upper body posture correction quantity and the third correction quantity so as to eliminate the upper body posture shake of the robot.
Preferably, the calculation of the first correction amount based on the actual angular velocity and the desired angular velocity is specifically as follows:
wherein,is the desired angular velocity of the lumbar joint, omega is the actual angular velocity of the lumbar joint, KvIs the velocity feedback gain.
Preferably, the second correction amount is calculated based on the planned gait and the actual posture of the robot generated in advance as follows:
wherein,planning the posture of the upper body of the robot, wherein tau is the actual posture of the upper body of the robot, KpIs the proportional feedback gain.
Preferably, the third correction amount is calculated based on the acceleration and the desired acceleration as follows:
wherein,for the desired acceleration value, α is the actual acceleration value, and D (q) is the waist joint inertia matrix.
Preferably, calculating the robot upper body posture correction amount based on the first correction amount and the second correction amount specifically includes: theta is equal to theta1+θ2，
Wherein, theta1Is a first correction amount, theta2Is the second correction amount.
Preferably, the planning gait is calculated in an off-line manner, and the waist joint inertia matrix d (q) is obtained by modeling the whole dynamics of the robot.
Preferably, the method further comprises the step of detecting a contact force F between the feet of the robot and the ground at different moments through a pressure detection circuit, and if the contact force F changes, the robot is in a motion state at the current moment, and a fourth correction quantity is calculated by using the following formula;
wherein, KrIs a predetermined proportionality coefficient, and 0 < Kr＜1；
And calculating the upper body posture correction amount of the robot based on the first correction amount, the second correction amount and the fourth correction amount.
Preferably, the pressure detection circuit includes: a pressure sensor circuit and a detection signal amplifying circuit,
wherein the pressure sensor circuit comprises a full bridge detection circuit;
the detection signal amplification circuit includes: an amplifier and a sliding varistor;
the full-bridge detection circuit is connected with a positive phase input end and a negative phase input end of the amplifier, and the slide rheostat is connected between a seventh pin and an eighth pin of the amplifier in parallel; and the output end of the amplifier is connected with a diode.
According to the technical scheme, compared with the prior art, the control method for eliminating the upper body posture shake of the biped robot is disclosed, the current parameters of the humanoid robot are measured, and the correction quantity is calculated based on various measured values, so that the upper body shake is eliminated by adjusting and controlling the waist joint, and the aim of stably walking the robot is fulfilled.
Moreover, the control method for eliminating the upper body posture shake of the biped robot has the advantages of wide application range, simple calculation and more comprehensive consideration of factors.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
FIG. 1 is a schematic diagram of a control method for eliminating the upper body posture shake of a biped robot according to the present invention;
fig. 2 is a schematic diagram of a pressure detection circuit provided in the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Referring to the attached figure 1, the embodiment of the invention discloses a control method for eliminating the upper body posture shake of a biped robot, wherein a gyroscope and an accelerometer are installed on the robot; the gyroscope is used for measuring angular velocity, and the accelerometer is used for measuring acceleration;
the control method comprises the following steps:
acquiring the actual angular velocity of the upper body of the robot;
calculating a first correction amount based on the actual angular velocity and the desired angular velocity;
calculating a second correction amount based on a planned gait and an actual posture of the robot generated in advance;
acquiring the acceleration of the upper body of the robot;
calculating a third correction amount based on the acceleration and the desired acceleration;
calculating the upper body posture correction quantity of the robot based on the first correction quantity and the second correction quantity;
and adjusting the waist joint based on the upper body posture correction quantity and the third correction quantity so as to eliminate the upper body posture shake of the robot.
When the robot actually walks, the control of the waist joint is equal to the dynamic gait of the off-line planning plus the real-time correction, and the reference value obtained by the two is input into the servo driver to control the humanoid robot to walk. Wherein the dynamic gait is generated given the robot model and the circumstances of the surroundings. The invention adopts PD control to reduce the shaking of the upper body, and the specific control method refers to the attached figure 1.
Moreover, in the prior art, most control methods only consider detecting the actual angular velocity and perform correction and adjustment according to the actual angular velocity, but special situations such as: the ground is abnormal, the robot suddenly steps into a pit, the instantaneous influence is very large, but the correction is not accurate only by detecting the angular velocity through the gyroscope, the change of the upper body of the robot can be sensed through the accelerometer, the adjustment is carried out based on a robot dynamic model, and the correction result can be more accurate.
The method provided by the invention is also suitable for other robots with upper body posture shaking.
Referring to fig. 1, to further optimize the above technical solution, the first correction amount is calculated based on the actual angular velocity and the desired angular velocity as follows:
wherein,is the desired angular velocity of the lumbar joint, omega is the actual angular velocity of the lumbar joint, KvIs the velocity feedback gain.
Referring to fig. 1, in order to further optimize the above technical solution, the second correction amount is calculated based on the planned gait and the actual posture of the robot generated in advance as follows:
wherein,planning the posture of the upper body of the robot, wherein tau is the actual posture of the upper body of the robot, KpIs the proportional feedback gain.
Referring to fig. 1, to further optimize the above technical solution, the third correction amount is calculated based on the acceleration and the expected acceleration as follows:
wherein,for the desired acceleration value, α is the actual acceleration value, and D (q) is the waist joint inertia matrix.
Referring to fig. 1, in order to further optimize the above technical solution, calculating the robot upper body posture correction amount based on the first correction amount and the second correction amount specifically includes: theta is equal to theta1+θ2，
Wherein, theta1Is a first correction amount, theta2Is the second correction amount.
It should be explained here that the planning gait is calculated off-line and the lumbar joint inertia matrix d (q) is obtained by modeling the robot as a whole body dynamics.
Therefore, when the control method provided by the invention is used for controlling the upper body shaking of the robot, the current parameters of the robot are considered, the influence of the pre-established robot dynamic model on the robot control is also considered, and the factors are considered comprehensively.
Moreover, the control method for eliminating the upper body posture shake of the biped robot only adjusts the waist joint, does not influence the landing time of the robot, can play a role in the whole walking engineering or operation process of the robot, and has universal applicability.
In order to further optimize the technical scheme, the method further comprises the steps of detecting the contact force F between the feet of the robot and the ground at different moments through a pressure detection circuit, and if the contact force F changes, the robot is in a motion state at the current moment, and calculating a fourth correction quantity by using the following formula;
wherein, KrIs a predetermined proportionality coefficient, and 0 < Kr＜1；
And calculating the upper body posture correction amount of the robot based on the first correction amount, the second correction amount and the fourth correction amount.
It should be noted that, the robot needs to take into account different factors during the moving process and the stationary process, and if the robot shakes during the moving process, the robot needs to be corrected by using the fourth correction amount.
Referring to fig. 2, in order to further optimize the above technical solution, the pressure detection circuit includes: a pressure sensor circuit and a detection signal amplifying circuit;
wherein the pressure sensor circuit comprises a full bridge detection circuit;
the detection signal amplification circuit includes: an amplifier and a sliding varistor;
the full-bridge detection circuit is connected with a positive phase input end and a negative phase input end of the amplifier, and the slide rheostat is connected between a seventh pin and an eighth pin of the amplifier in parallel; and the output end of the amplifier is connected with a diode.
Referring to fig. 2, R9-R12 are full bridge detection circuits, the right side is a signal amplification circuit, and the millivolt level signal of the pressure sensor is amplified and converted into a range distinguishable by an analog-to-digital converter, the amplifier can be an AD620 amplifier, and the amplifier is powered by a ± 5V power supply, in order to ensure the stability of the amplified signal, the power input end is subjected to voltage stabilization protection, the amplification factor can be adjusted by a slide rheostat R3, and the calculation formula of the amplification factor is:
wherein G is the amplification factor of the amplifier, and R3 is the current resistance of the slide rheostat R3.
In addition, the signal output end of the amplifier is also connected with a voltage stabilizing diode, and particularly, referring to fig. 2, voltage protection is realized.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.