CN113442171B - Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method - Google Patents
Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method Download PDFInfo
- Publication number
- CN113442171B CN113442171B CN202110742012.1A CN202110742012A CN113442171B CN 113442171 B CN113442171 B CN 113442171B CN 202110742012 A CN202110742012 A CN 202110742012A CN 113442171 B CN113442171 B CN 113442171B
- Authority
- CN
- China
- Prior art keywords
- robot
- state
- pitch
- roll
- attitude
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
Abstract
The invention discloses a robot dynamic stability judging method, which comprises the steps of firstly constructing a state variable, wherein the state variable is divided into an actual state value and a target state value, secondly caching the state difference value of the actual state value and the target state value into a queue, and finally calculating the state deviation and the variance of the state difference value in the cache to comprehensively judge the dynamic stability of a robot, thereby realizing more accurate and comprehensive evaluation of the stability of the robot in the dynamic motion process. In addition, a robot dynamic self-adaptive attitude control method is provided, an integral mode is used for real-time adjustment of a compensation term by using a dynamic self-adaptive compensation technology, speed-related factors are used for adjusting attitude deviation in different directions caused by speed, the attitude of a robot body can be dynamically and self-adaptively adjusted to keep the stability of the robot body, and the attitude control effect of the robot is improved.
Description
Technical Field
The invention relates to the field of robot motion control, in particular to a robot dynamic stability judging method and a dynamic self-adaptive attitude control method.
Background
The dynamic stability of the mobile robot refers to the stability of the robot in the process of fast moving, and in order to achieve the ideal high-speed operation effect, the robot needs to be controlled constantly to keep dynamic balance. The conventional method for determining the dynamic stability of a mobile robot is mainly a Zero Moment Point (ZMP) method in which the resultant moment of the ground reaction force at this point is only a vertical component and the horizontal component is zero, and the dynamic stability of the robot is obtained by determining the relative positional relationship between the ZMP and the support plane, and the ZMP is considered to be more stable as it approaches the center of the support plane and is considered to be completely unstable (and may fall down) as it is out of the support plane. However, for onboard devices such as cameras, shaking of the body position and wiggling of the pose are both adverse factors, which puts higher demands on controlling the dynamic motion stability of the robot. The ZMP can only obtain the stability of the dimension of the instantaneous force, is slightly influenced by the shaking of the position of the robot body and is independent of the twisting of the posture of the robot body, and the ZMP method is not enough to judge the shaking of the position of the robot body, the twisting of the posture and the like in the moving process of the robot body. Therefore, the ZMP method cannot meet the need for determination of the dynamic stability of the robot in such cases.
When the robot moves, the body attitude deviation is caused by different reasons, such as uneven mass distribution of the body, deviation of the center of gravity caused by the change of the position of the limbs during movement, high movement speed, uneven ground, the characteristic of the movement law or other problems. In order to correct such a deviation of the posture, it is a conventional practice to make an adjustment of the center of gravity of the robot, reduce the moving speed, and the like. Due to the variety and possible variations of the causes of fuselage attitude deviations, the problem cannot be fully solved by calibration of the centroid position alone, using a higher precision sensor. For the problem of long-time deviation of the attitude of the fuselage, the traditional method has low robustness and is difficult to adapt to various conditions and various problem reasons.
Disclosure of Invention
Therefore, the invention provides a robot dynamic stability judging method and a dynamic self-adaptive attitude control method, so as to better solve the problem of accurately, comprehensively and reliably evaluating the stability degree of the robot in the dynamic motion process, and dynamically and self-adaptively adjust the attitude of the robot body to keep the stability of the robot body.
The invention mainly adopts the following technical scheme:
a robot dynamic stability judging method comprises the following steps:
step A, constructing a state variable, wherein the state variable is formed by combining the body position and the posture of a robot, the state variable is divided into an actual state value and a target state value, the actual state value is obtained by using a Kalman filtering algorithm according to the real-time posture information of the robot, and the target state value is obtained by real-time adjustment according to the motion control of the robot;
b, recording the state difference value of the actual state value and the target state value and caching the state difference value into a queue, wherein the length of the queue is set as the sampling frequency of one period in the movement process of the robot;
step C, judging the dynamic stability of the robot according to the state deviation and the state variance;
the state deviation and the state variance are obtained by respectively calculating according to a plurality of state difference values sampled and obtained in one period of the robot movement, the state deviation reflects the deviation degree of the actual state value and the target state value of the robot, and the state variance reflects the control stability of the actual state value of the robot.
Further, the queue is a first-in first-out finite length queue.
Further, the state deviation is an average value of a plurality of state difference values sampled and acquired in one period, and the state variance is a variance of the plurality of state difference values sampled and acquired in one period.
Further, the state deviation and the state variance are used for analyzing the influence of different parameters on the dynamic stability of the robot by a control variable method. For example, to compare the effect of certain parameters on the dynamic stability of the robot under various conditions, such as different motion control algorithms, different structures, or different assembly methods, to prefer.
Further, the method is used for a legged robot, and the cycle is set to be a complete gait cycle.
Furthermore, the robot is subjected to attitude control based on integral compensation of the state deviation and considering the influence of the speed on the attitude deviation.
Preferably, the following steps are included in one control cycle:
step 1: acquiring a current state value and a target state value;
step 2: recording the current state deviation, and updating a buffer queue;
and step 3: calculating the state deviation and variance of the last motion period;
and 4, step 4: judging whether the current speed is close to static, if so, updating a constant term of the attitude compensation formula, otherwise, updating a first power term related to the speed in the attitude compensation formula; the current speed comprises the lateral speed of the current fuselage and the fore-and-aft speed of the current fuselage;
and 5: compensating the target attitude;
and 6: updating the original motion control algorithm;
and 7: and (5) entering the step 1 again, and adjusting the control algorithm to enter the next movement period.
Preferably, the attitude compensation formula is as follows:
Rolltarget＝Rolldesired+vy·Droll,1+Droll,0
Pitchtarget＝Pitchdesired+vx·Dpitch,1+Dpitch,0
wherein RolldesiredRepresenting the original fuselage roll angle, PitchdesiredRepresenting the original fuselage pitch angle, vySpeed, v, representing the left and right lateral directions of the robotxRepresenting the speed of the robot in the front-to-rear direction, RolltargetRepresents the target roll angle of the fuselage, PitchtargetRepresenting a target pitch angle of the fuselage; droll,1、Droll,0、Dpitch,1And Dpitch,0The initial value is set to zero.
Further, said Droll,1、Droll,0、Dpitch,1And Dpitch,0The specific updating method comprises the following steps:
when the lateral speed of the front fuselage is smaller than the speed range of the robot when the front fuselage is still in place, D is updatedroll,0＝Droll,0-drollΔ t, wherein drollIs the current roll angle deviation of the robot, Δ t is the ratio of the control time interval to the motion period, otherwise D is updatedroll,1＝Droll,1-droll·Δt/vyWherein v isyThe current left-right lateral moving speed of the robot;
updating D when the fore-aft speed of the front fuselage is less than the speed range of one robot when the robot is stationary in placepitch,0＝Dpitch,0-dpitchΔ t, wherein dpitchIs the robot current pitch angle deviation, Δ t is the ratio of the control time interval to the motion period, otherwise D is updatedpitch,1＝Dpitch,1-dpitch·Δt/vxWherein v isxIs the current forward and backward moving speed of the robot;
further, the compensation value of the target attitude can be set to a numerical range according to the walking gradient of the robot. Compared with the prior art, the invention has the advantages that:
(1) according to the method for judging the dynamic stability of the robot, the state deviation and the variance of a complete motion period are used as the stability criterion according to actual requirements, the influence of instantaneous abnormal values is eliminated, the stable state of the robot during motion can be more objectively, comprehensively and accurately quantified, and meanwhile the obtained state deviation can further help to improve the motion stability of the robot.
(2) The invention provides a robot dynamic self-adaptive attitude control method, which is based on integral compensation of state deviation and considers speed factors to adjust attitude deviation in real time, can overcome attitude errors during movement caused by mass center position deviation on a mechanical structure and uneven ground, can also overcome the problem of unstable body attitude of a robot caused by other reasons, does not need manual intervention or a complicated calibration process, has the advantages of strong universality, smooth adjustment and the like, and improves the attitude control effect of the robot.
Drawings
Fig. 1 is a flow chart of a robot dynamic adaptive attitude control method of the present invention.
Detailed Description
The following describes in more detail embodiments of the present invention with reference to the schematic drawings. The advantages and features of the present invention will become more apparent from the following description. It is to be noted that the drawings are in a very simplified form and are not to precise scale, which is provided for the purpose of facilitating and clearly illustrating embodiments of the present invention.
Firstly, a robot coordinate system is defined, the robot is taken as a reference origin, the front is the positive direction of an X axis, the left side is the positive direction of a Y axis, and the anticlockwise rotation is positive, namely the X axis is the front-back direction, and the Y axis is the left-right direction. The angle of rotation about the forward axis is indicated by roll angle and the angle of rotation about the right axis is indicated by pitch angle.
The invention provides a method for judging the dynamic stability of a robot, which is used for calculating and evaluating the dynamic stability of the robot by using real-time data of a motion state and a target state of the robot. Firstly, a state variable is constructed and divided into an actual state value and a target state value, then, state difference values of the actual state value and the target state value are cached to a queue, and finally, state deviation and variance of the state difference values in the cache are calculated to comprehensively judge the dynamic stability of the robot.
The present embodiment is described by taking a legged robot as an example, and specifically includes the following steps:
step A, constructing a state variable: the state variable is formed by combining the body position and the posture of the robot, so that the comprehensive and accurate quantification of various influence factors in the self movement of the robot is realized, and the state variable is divided into an actual state value (a current state value) and a target state value. The actual state value is obtained by measuring the angle of each joint by using a position encoder arranged on the joint of the robot, measuring the acceleration and the body orientation by using an inertial sensor arranged at the position of the mass center of the body of the robot, and calculating by using kinematics and a Kalman filtering algorithm. The target state value, namely the target posture after the robot motion control adjustment is obtained through the following formula:
Rolltarget＝Rolldesired+vy·Droll,1+Droll,0
Pitchtarget＝Pitchdesired+vx·Dpitch,1+Dpitch,0
wherein RolldesiredAnd PitchdesiredRespectively representing the roll angle and pitch angle of the fuselage desired by the user, i.e. the original desired attitude, this value being specified by the user, for example by pushing a rocker when operating the machine using a remote control, the value being normally zero; v. ofyAnd vxRespectively representing the speeds of the left and right sides and the front and back directions of the robot; rolltargetAnd PitchtargetRespectively representing a target roll angle and a target pitch angle in the motion control system, namely a target attitude; droll,1，Droll,0，Dpitch,1，Dpitch,0All initial values are zero, where Droll,1Representing a first order coefficient of attitude compensation for the Roll (i.e. Roll) direction with respect to the y-direction velocity, Droll,0Representing attitude-compensated zeroth order coefficients for Roll direction，Dpitch,1A first order coefficient, D, representing attitude compensation for Pitch (i.e., Pitch) direction with respect to velocity in the x directionpitch,0Representing the attitude compensation zeroth order coefficient for the Pitch (i.e., Pitch) direction.
B, recording the state difference value of the actual state value and the target state value and caching the state difference value to a queue: the state variables can be sampled, the difference value between the actual state value and the target state value of the robot at each sampling moment is recorded, the difference values are cached through a first-in first-out queue, and the length of the queue is set as the sampling times of one period in the motion process of the robot. The corresponding motion period is different according to different implementation objects. For a legged robot, this cycle is a complete gait cycle, and in this embodiment, the time of one cycle is 400ms, and the sampling frequency in one cycle is 500 times/s.
And C, judging the dynamic stability of the robot according to the state deviation and the state variance: and calculating the average value of the buffered robot state difference values as a state deviation, and calculating the variance of the robot state difference values as a state variance. The state deviation can reflect the deviation degree of the actual state value and the target state value of the robot, namely the deviation degree of the state in the motion process of the robot, and the state variance can reflect the control stability of the actual state value of the robot, namely the jitter degree of the state in the motion process of the robot. A smaller state deviation means that the control result is closer to the target, and a smaller state variance means that the control result is more stable and the jitter is smaller. The state deviation reflects the deviation degree, the state variance reflects the stability, and the indexes can be used for comparing the influence of a certain parameter in different motion control algorithms, or different structures, or different assembly modes and the like on the dynamic motion stability of the robot through a control variable method so as to optimize.
The discrimination method provided by the invention meets the requirement of a developer for debugging the motion control method of the robot to improve the motion stability of the robot, uses the first-in first-out queue with the length of one motion cycle as a buffer, averages the influence of different states at different stages in one motion cycle, obtains state deviation and variance with reference value, eliminates the influence of instantaneous abnormal values, and simultaneously does not bring too much delay. The stability of the robot in motion can be judged more objectively, more comprehensively and more accurately and quantitatively by using the state deviation and the variance of the motion period, and the obtained state deviation can further help to improve the motion stability of the robot.
The invention also provides a robot dynamic self-adaptive attitude control method, which adopts a dynamic self-adaptive compensation technology, uses an integral mode for real-time adjustment of a compensation term, uses a speed-related factor for attitude deviation in different directions caused by speed for adjustment, and can increase or decrease a compensation value of a target attitude according to the actual speed. As can be seen from fig. 1, in a control cycle, the specific method includes the following steps:
step 1: acquiring a current state value and a target state value, and then entering a self-adaptive compensation attitude adjustment control algorithm;
step 2: recording the current state deviation, and updating a buffer queue;
and step 3: calculating the state deviation and variance of the last motion period (the past motion period);
and 4, step 4: judging whether the current speed is close to static, if so, updating a constant term of the attitude compensation formula, otherwise, updating a first power term about the speed in the attitude compensation formula, wherein the current speed is respectively adjusted according to the lateral speed and the front and back speed of the fuselage:
if the current lateral speed of the fuselage is less than a speed range (≦ 0.01m/s) in which the robot can be considered to be stationary in place, i.e. when the lateral speed of the fuselage is less than or equal to 0.01m/s, D is updatedroll,0＝Droll,0-drollΔ t, wherein drollIs the current roll angle deviation, Δ t is the ratio of the control time interval to the motion period, otherwise D is updatedroll,1＝Droll,1-droll·Δt/vyWherein v isyIs the current left-right lateral moving speed of the robot. In this embodiment, suppose aWhen a load is applied to the robot and the position of the load is shifted to the left, the actual posture (mainly the Roll angle Roll) of the robot when stepping on the spot will have a deviation, assuming that the deviation is-0.1 rad (rad means radian, 1 radian is about equal to 57 degrees). As a result of stepping in place, we assume that the current speed fluctuates between-0.001 m/s to +0.001 m/s. Assume that the desired Roll angle Roll is 0 (original desired attitude), i.e., remains horizontal.
Assuming that the current time of the first sampling point is-0.1 rad, the current Roll angle Roll is-0.1 rad, and the current speed is 0.001m/s, the buffer queue is still empty, and the current state deviation is put into the buffer queue, and then the deviation of the whole queue is calculated, and the result is-0.1 rad. Since the velocity is less than 0.01m/s, according to formula Droll,0＝Droll,0-drollΔ t update Droll,00.1 × 0.002 ═ 0.0002rad, Roll was updated according to the formulatargetIs 0.0002 rad. Since the target attitude is positive 0.0002rad, the robot is controlled to adjust the attitude in the Roll angle Roll positive direction, assuming that 0.00002rad is actually adjusted when the second sample point arrives.
Assuming that the current sampling point is the second sampling point, the current Roll angle Roll is-0.09998 rad, the current speed is-0.001 m/s, the current state deviation is put into a buffer queue, and then the deviation of the whole queue is calculated, and the result is [ -0.1, -0.09998 [ -0.1 [ -0.09998 ]]Average of-0.09999, since the velocity is still less than 0.01m/s, according to equation Droll,0＝Droll,0-drollΔ t update Droll,00.0002+0.09999 × 0.002 ═ 0.00039998rad, and Roll was calculated according to the formulatargetIs 0.00039998 rad. Because the Roll angle of the target posture increases toward the positive direction, the robot is controlled to adjust the posture toward the Roll angle positive direction, and the robot is supposed to actually adjust 0.000019rad when the second sampling point arrives.
Assuming that it is now the third sampling point, the Roll angle Roll is-0.099961 rad, and the speed is 0.001m/s, the process continues as above.
Assuming that the current sampling point is at the 501 th sampling point moment, after the current state deviation is put into a queue, because the length of the queue limited by the current state deviation exceeds the length of the queue, the data of the oldest sampling point at the moment can be popped up so as to keep the latest historical state deviation data in one period in the queue, and the attitude compensation value is continuously updated and controlled;
if the current fore-aft speed of the fuselage is less than a range of speeds that can be considered as the robot being stationary in place (≦ 0.01m/s), i.e., when the fore-aft speed of the fuselage is less than 0.01m/s, D is updatedpitch,0＝Dpitch,0-dpitchΔ t, wherein dpitchIs the current pitch angle deviation, at is the ratio of the control time interval to the motion period, otherwise D is updatedpitch,1＝Dpitch,1-dpitch·Δt/vxWherein v isxIs the current forward and backward moving speed of the robot. In this embodiment, it is assumed that the actual attitude of the robot when stepping (which is mainly the Pitch angle Pitch) is deviated due to the load applied to the robot and the position of the load is deviated, and the deviation is assumed to be-0.1 rad (rad means radian, 1 radian is approximately equal to 57 degrees). Since it is stepping in place, we assume that the current speed fluctuates between-0.001 m/s to +0.001 m/s. Assume that the desired Pitch angle Pitch is 0 (original desired attitude), i.e., remains horizontal.
Assuming that the current sampling point time is the first sampling point time, the current Pitch angle Pitch is-0.1 rad, and the current speed is 0.001m/s, the buffer queue is still empty, the current state deviation is put into the buffer queue, and then the deviation of the whole queue is calculated, and the result is-0.1 rad, because the speed is less than 0.01m/s, the speed is according to the formula Dpitch,0＝Dpitch,0-dpitchΔ t update Dpitch,00.1 × 0.002 ═ 0.0002rad, and Pitch is updated according to the formulatargetIs 0.0002 rad. Since the target attitude is positive 0.0002rad, the robot is controlled to adjust the attitude in the positive direction of Pitch angle Pitch, assuming that 0.00002rad is actually adjusted when the second sampling point arrives.
Assuming that the current sampling point is the second sampling point, the current Pitch angle Pitch is-0.09998 rad, the current speed is-0.001 m/s, the current state deviation is put into the buffer queue, and then the deviation of the whole queue is calculated, the result is [ -0.1-0.09998]Average of-0.09999, since the velocity is still less than 0.01m/s, according to equation Dpitch,0＝Dpitch,0-dpitchΔ t update Dpitch,00.0002+0.09999 × 0.002 ═ 0.00039998rad, and Pitch was calculated according to the formulatargetIs 0.00039998 rad. Because the target attitude Pitch angle increases again in the positive direction, the robot is controlled to adjust the attitude in the positive Pitch angle Pitch direction, and the second sampling point is assumed to actually adjust 0.000019rad again when arriving.
Assuming that the current sampling point is the third sampling point, the current Pitch angle Pitch is-0.099961 rad, and the current speed is 0.001m/s, the process is continued according to the above steps.
Assuming that the current state deviation is at the 501 th sampling point, after the current state deviation is put into the queue, because the length of the queue is exceeded, the data of the oldest sampling point at the moment can be popped up, so as to keep the latest historical state deviation data of one period in the queue, and continuously update the attitude compensation value and perform control.
And 5: compensating for target attitude by updating Droll,1，Droll,0，Dpitch,1，Dpitch,0Recalculating the compensation value for the target attitude, adding the compensation value to the original desired attitude, i.e. using the calculated Roll in "step AtargetAnd PitchtargetThe target attitude compensation is realized by the formula (2); and the compensation value of the target attitude can be limited by a numerical range according to the actual application requirement, so that excessive compensation is prevented. In this embodiment, the application scenario of the legged robot is mostly seen in slopes or stairs, and the compensation value has a value range greater than-30 ° and less than +30 °.
Step 6: updating the original motion control algorithm, and updating the target attitude of the original motion control algorithm of the robot to achieve the aim of correcting the attitude deviation of the robot;
and 7: and (5) entering the step 1 again, and adjusting the control algorithm to enter the next movement period.
In the adaptive attitude adjustment control method provided by the invention, the target attitude compensation in the step 5 is adjusted in real time by using an integral mode, and the average state deviation in the historical motion period at each sampling moment is used for carrying out smooth adjustment on the target attitude compensation value in real time. The control method is based on integral compensation of state deviation, simultaneously considers the influence of speed on the attitude deviation, improves the attitude control effect of the robot, can overcome the attitude error during movement caused by deviation of the position of a mass center on a mechanical structure, can also overcome the attitude error during movement caused by uneven ground, can also be used for overcoming the problem of unstable body attitude of the robot caused by other reasons, does not need manual intervention and complicated calibration process, and has the advantages of strong universality, smooth regulation and the like.
In summary, the present invention provides a method for determining dynamic stability of a robot and a method for controlling dynamic adaptive attitude of a robot, which use a state deviation and a variance with reference values, eliminate the influence of instantaneous abnormal values, achieve more accurate and comprehensive evaluation of stability of the robot during dynamic motion, dynamically adaptively adjust the attitude of the robot to keep the body stable, and improve the attitude control effect of the robot by taking into account the influence of speed on the attitude deviation based on the integral compensation of the state deviation.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and those skilled in the art can make various changes, modifications, substitutions and alterations without departing from the principle and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.
Claims (7)
1. A robot dynamic self-adaptive attitude control method based on a dynamic stability discrimination method is characterized in that attitude control is carried out on a robot based on integral compensation of state deviation and considering the influence of speed on the attitude deviation; the robot dynamic self-adaptive attitude control method comprises the following steps in one control cycle:
step 1: acquiring a current state value and a target state value;
and 2, step: recording the current state deviation, and updating a buffer queue;
and step 3: calculating the state deviation and variance of the last motion period;
and 4, step 4: judging whether the current speed is close to static, if so, updating a constant term of the attitude compensation formula, otherwise, updating a first power term related to the speed in the attitude compensation formula; the current speed comprises the lateral speed of the current fuselage and the fore-and-aft speed of the current fuselage;
and 5: compensating the target attitude;
step 6: updating the original motion control algorithm;
and 7: step 1 is carried out again, the control algorithm is adjusted to enter the next movement period,
the attitude compensation formula is as follows:
Rolltarget＝Rolldesired+vy·Droll,1+Droll,0
Pitchtarget＝Pitchdesired+vx·Dpitch,1+Dpitch,0
wherein RolldesiredRepresenting the original fuselage roll angle, PitchdesiredRepresenting the original fuselage pitch angle, vyRepresenting the velocity, v, of the robot in the left and right directionsxRepresenting the speed of the robot in the front-to-rear direction, RolltargetRepresenting the target roll angle of the fuselage, PitchtargetRepresenting a target pitch angle of the fuselage; droll,1、Droll,0、Dpitch,1And Dpitch,0Initial value set to zero, Droll,1Representing a first order coefficient of attitude compensation for the roll direction with respect to the y-direction velocity, Droll,0Representing the attitude-compensated zeroth-order coefficient, D, for the roll directionpitch,1Representing a first order coefficient of attitude compensation for the pitch direction with respect to the x-direction velocity, Dpitch,0Representing an attitude-compensated zeroth order coefficient for a pitch direction; said Droll,1、Droll,0、Dpitch,1And Dpitch,0The specific updating method comprises the following steps:
when the lateral speed of the front fuselage is less than the speed range of a robot standing still in placeUpdate Droll,0＝Droll,0-drollΔ t, wherein drollIs the current roll angle deviation of the robot, Δ t is the ratio of the control time interval to the motion period, otherwise D is updatedroll,1＝Droll,1-droll·Δt/vyWherein v isyThe current left-right lateral moving speed of the robot is obtained;
updating D when the fore-aft speed of the front fuselage is less than the speed range of one robot when the robot is stationary in placepitch,0＝Dpitch,0-dpitchΔ t, wherein dpitchIs the deviation of the current pitch angle of the robot, delta t is the ratio of the control time interval to the motion period, otherwise D is updatedpitch,1＝Dpitch,1-dpitch·Δt/vxWherein v isxIs the current forward and backward movement speed of the robot.
2. The robot dynamic adaptive attitude control method based on the dynamic stability determination method according to claim 1, wherein the dynamic stability determination method comprises the following steps:
step A, constructing a state variable, wherein the state variable is formed by combining the body position and the posture of a robot, the state variable is divided into an actual state value and a target state value, the actual state value is obtained by using a Kalman filtering algorithm according to the real-time posture information of the robot, and the target state value is obtained by real-time adjustment according to the motion control of the robot;
b, recording the state difference value of the actual state value and the target state value and caching the state difference value into a queue, wherein the length of the queue is set as the sampling frequency of one period in the movement process of the robot;
step C, judging the dynamic stability of the robot according to the state deviation and the state variance;
the state deviation and the state variance are obtained by respectively calculating according to a plurality of state difference values sampled and obtained in one period of the robot movement, the state deviation reflects the deviation degree of the actual state value and the target state value of the robot, and the state variance reflects the control stability of the actual state value of the robot.
3. The method according to claim 2, wherein in step B, the queue is a first-in first-out finite-length queue.
4. The method as claimed in claim 2, wherein the state deviation is an average value of a plurality of state differences sampled in one period, and the state variance is a variance of the plurality of state differences sampled in one period.
5. The method as claimed in claim 2, wherein the state deviation and the state variance are used to analyze the influence of different parameters on the dynamic stability of the robot by a control variable method.
6. The method as claimed in claim 2, wherein the dynamic stability determination method is applied to a legged robot, and the period is set as a complete gait period.
7. The method as claimed in claim 1, wherein the compensation value for the target attitude can be set to a value range according to the walking gradient of the robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110742012.1A CN113442171B (en) | 2021-07-01 | 2021-07-01 | Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110742012.1A CN113442171B (en) | 2021-07-01 | 2021-07-01 | Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113442171A CN113442171A (en) | 2021-09-28 |
| CN113442171B true CN113442171B (en) | 2022-06-24 |
Family
ID=77814708
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110742012.1A Active CN113442171B (en) | 2021-07-01 | 2021-07-01 | Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113442171B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114371613B (en) * | 2021-12-27 | 2023-08-29 | 同济大学 | Online learning-based upper body posture control method for humanoid robot |
| CN114516048B (en) * | 2022-02-21 | 2024-01-09 | 乐聚(深圳)机器人技术有限公司 | Zero point debugging method and device for robot, controller and storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN108638128A (en) * | 2018-05-24 | 2018-10-12 | 哈工大机器人（合肥）国际创新研究院 | A kind of real-time method for monitoring abnormality and its system of industrial robot |
| CN108871323A (en) * | 2018-04-25 | 2018-11-23 | 珠海全志科技股份有限公司 | A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment |
| CN110712201A (en) * | 2019-09-20 | 2020-01-21 | 同济大学 | Robot multi-joint self-adaptive compensation method based on perceptron model and stabilizer |
| CN111896007A (en) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | Quadruped robot attitude calculation method for compensating foot-ground impact |
| AU2019304101A1 (en) * | 2018-07-16 | 2021-02-04 | Fastbrick Ip Pty Ltd | Active damping system |
| CN112536797A (en) * | 2020-12-01 | 2021-03-23 | 南京航空航天大学 | Comprehensive compensation method for position and attitude errors of industrial robot |
-
2021
- 2021-07-01 CN CN202110742012.1A patent/CN113442171B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN108871323A (en) * | 2018-04-25 | 2018-11-23 | 珠海全志科技股份有限公司 | A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment |
| CN108638128A (en) * | 2018-05-24 | 2018-10-12 | 哈工大机器人（合肥）国际创新研究院 | A kind of real-time method for monitoring abnormality and its system of industrial robot |
| AU2019304101A1 (en) * | 2018-07-16 | 2021-02-04 | Fastbrick Ip Pty Ltd | Active damping system |
| CN110712201A (en) * | 2019-09-20 | 2020-01-21 | 同济大学 | Robot multi-joint self-adaptive compensation method based on perceptron model and stabilizer |
| CN111896007A (en) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | Quadruped robot attitude calculation method for compensating foot-ground impact |
| CN112536797A (en) * | 2020-12-01 | 2021-03-23 | 南京航空航天大学 | Comprehensive compensation method for position and attitude errors of industrial robot |
Non-Patent Citations (1)
| Title |
|---|
| 基于自适应动态面算法的旋翼式飞行器姿态控制研究;凌帅;《中国优秀硕士学位论文全文数据库 (基础科学辑)》;20200817(第8期);第13-39页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113442171A (en) | 2021-09-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113442171B (en) | Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method | |
| JP6922641B2 (en) | Angular velocity derivation device and angular velocity derivation method | |
| US8600552B2 (en) | Information processing method, apparatus, and computer readable medium | |
| JP3314101B2 (en) | Coordinate measuring device having a method and a control device for measuring shape elements | |
| CN109003292B (en) | Moving target tracking method based on switch Kalman filter | |
| CN109579824B (en) | Self-adaptive Monte Carnot positioning method integrating two-dimensional code information | |
| EP1721710A1 (en) | Leg-equipped robot and leg-equipped robot walk control method | |
| US20120303162A1 (en) | Mobile object controller and floor surface estimator | |
| US20100106451A1 (en) | Zero-Point Correction Apparatus and Method for an Angular Speed Sensor | |
| WO2019087357A1 (en) | Calibration device, calibration method, and control device | |
| CN110071676B (en) | Vibration suppression method and device for flexible robot joint servo system | |
| US20190160676A1 (en) | Robot and collision detection method therefor | |
| CN107450316B (en) | Sampling adaptive robust control method of chip mounter driving system | |
| JP2008129813A (en) | Mobile robot with flexible arm | |
| CN111168672B (en) | Robot motion planning method, system, computer device and storage medium | |
| CN112693449A (en) | Transverse and longitudinal coupling control method under limit working condition of unmanned vehicle | |
| CN108871323B (en) | High-precision navigation method of low-cost inertial sensor in locomotive environment | |
| CN110440756B (en) | Attitude estimation method of inertial navigation system | |
| CN117091592A (en) | Gesture resolving method, gesture resolving device, and computer storage medium | |
| KR19990063535A (en) | DEVICE AND METHOD FOR CONTROLLING MOVING OBJECT | |
| CN111016882B (en) | Vehicle control signal calculation method, device, equipment and storage medium | |
| JP3536084B2 (en) | Gait control method for walking robot | |
| US20120303161A1 (en) | Robot controller | |
| JP6859917B2 (en) | Angular velocity derivation device and angular velocity derivation method | |
| CN113568314A (en) | Small celestial body hovering constant thrust control method based on switching boundary compensation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |