CN117891167A - External force prediction method and device for robot riding balance car and robot - Google Patents
External force prediction method and device for robot riding balance car and robot Download PDFInfo
- Publication number
- CN117891167A CN117891167A CN202311795319.3A CN202311795319A CN117891167A CN 117891167 A CN117891167 A CN 117891167A CN 202311795319 A CN202311795319 A CN 202311795319A CN 117891167 A CN117891167 A CN 117891167A
- Authority
- CN
- China
- Prior art keywords
- robot
- expression
- moment
- state quantity
- external force
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 62
- 238000004458 analytical method Methods 0.000 claims abstract description 13
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 107
- 238000004590 computer program Methods 0.000 claims description 19
- 238000005259 measurement Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 description 18
- 230000001133 acceleration Effects 0.000 description 9
- 238000010586 diagram Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 241000282414 Homo sapiens Species 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 210000003141 lower extremity Anatomy 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 210000001364 upper extremity Anatomy 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/72—Electric energy management in electromobility
Landscapes
- Manipulator (AREA)
Abstract
The application is applicable to the technical field of robots, and particularly relates to an external force prediction method and device for a robot riding balance car and the robot. In the method, when external force applied to the robot when the robot rides the balance car is predicted, a wheel type inverted pendulum model of the robot when the robot rides the balance car can be established, dynamic analysis can be carried out on the wheel type inverted pendulum model, and a dynamic model expression corresponding to the robot is determined. Then, external force suffered by the robot when riding the balance car can be predicted according to the dynamic model expression and the extended Kalman filtering. The method can accurately predict the external force applied to the robot when the robot rides the balance car, so that the external force applied to the robot can be compensated in control to keep the stability of the robot when the robot rides the balance car.
Description
Technical Field
The application belongs to the technical field of robots, and particularly relates to an external force prediction method and device for a robot riding balance car, a robot and a computer readable storage medium.
Background
Humanoid robots are a research hotspot in the field of robots. The upper limb of the humanoid robot has operation capability, the lower limb has movement capability, and the humanoid robot can use vehicles like human beings, for example, can ride a two-wheeled balance car. The humanoid robot riding balance car can give consideration to the operation capability of the humanoid robot and the mobility capability of the wheeled robot. When the robot rides the balance car, if the robot receives an external force, the motion states of the robot and the balance car can be changed. For example, pushing the robot back may cause the robot to tilt forward, the balance car to accelerate forward, and affect the stability of the robot. Therefore, it is necessary to predict the external force applied to the robot while riding the balance car, so that the external force can be compensated for during control, and the stability of the robot while riding the balance car can be maintained.
Disclosure of Invention
The embodiment of the application provides an external force prediction method and device for a robot to ride a balanced vehicle, the robot and a computer readable storage medium, which can accurately predict external force born by the robot to ride the balanced vehicle, so that the external force born by the robot can be compensated in control to maintain the stability of the robot to ride the balanced vehicle.
In a first aspect, an embodiment of the present application provides a method for predicting an external force when a robot rides a balancing vehicle, which may include:
establishing a wheel type inverted pendulum model when the robot rides a balance car;
carrying out dynamics analysis on the wheel type inverted pendulum model, and determining a dynamics model expression corresponding to the robot;
and predicting the external force applied to the robot when riding the balance car according to the dynamic model expression and the extended Kalman filtering.
In the external force prediction method for the robot riding balance vehicle, when external force received by the robot riding balance vehicle is predicted, a wheel type inverted pendulum model of the robot riding balance vehicle can be established, dynamic analysis can be performed on the wheel type inverted pendulum model, and a dynamic model expression corresponding to the robot is determined. Then, external force suffered by the robot when riding the balance car can be predicted according to the dynamic model expression and the extended Kalman filtering. The method can accurately predict the external force applied to the robot when the robot rides the balance car, so that the external force applied to the robot can be compensated in control to keep the stability of the robot when the robot rides the balance car.
In one possible implementation manner, the dynamics model expression includes a state expression and an observation expression, and predicting the external force suffered by the robot when riding the balance according to the dynamics model expression and the extended kalman filter may include:
determining a discretization state expression corresponding to the robot according to the state expression, and determining a discretization observation expression corresponding to the robot according to the observation expression;
and predicting a first predicted state quantity of the robot at a first moment according to the discretization state expression, the discretization observation expression and the extended Kalman filtering, wherein the first predicted state quantity comprises an external force applied to the robot at the first moment.
In one example, the predicting the first predicted state quantity of the robot at the first time according to the discretized state expression, the discretized observation expression, and the extended kalman filter may include:
acquiring a second predicted state quantity of the robot at a second moment, wherein the second moment is a moment before the first moment;
determining a candidate state quantity of the robot at the first moment according to the second predicted state quantity and the discretization state expression;
Acquiring the actual observed quantity of the robot at the first moment;
and correcting the candidate state quantity according to the actual observed quantity of the robot at the first moment, the discretization observed expression and the Kalman gain matrix corresponding to the first moment to obtain a first predicted state quantity of the robot at the first moment.
Optionally, the correcting the candidate state quantity according to the actual observed quantity of the robot at the first moment, the discretized observed expression and the kalman gain matrix corresponding to the first moment to obtain a first predicted state quantity of the robot at the first moment may include:
determining a first predicted state quantity of the robot at the first moment according to the following formula:
X(k+1|k+1)＝X(k+1|k)+K(k+1)[Y(k+1)-H(X(k+1|k))]；
wherein X (k+1|k+1) is a first predicted state quantity of the robot at the first time, X (k+ 1|k) is a candidate state quantity of the robot at the first time, K (k+1) is a kalman gain matrix corresponding to the first time, Y (k+1) is an actual observed quantity of the robot at the first time, and H (X (k+ 1|k)) is a predicted observed quantity at the first time predicted from the candidate state quantity at the first time.
In one possible implementation manner, before correcting the candidate state quantity according to the actual observed quantity of the robot at the first moment, the discretized observed expression and the kalman gain matrix corresponding to the first moment to obtain a first predicted state quantity of the robot at the first moment, the method may further include:
determining a first error covariance matrix corresponding to the second prediction state quantity;
determining a second error covariance matrix corresponding to the candidate state quantity according to the discretization state expression and the first error covariance matrix;
determining a covariance matrix corresponding to the measurement noise at the first moment;
and determining a Kalman gain matrix corresponding to the first moment according to the second error covariance matrix, the discretization observation expression and the covariance matrix corresponding to the measurement noise.
Illustratively, the determining the kalman gain matrix corresponding to the first time according to the second error covariance matrix, the discretized observation expression and the covariance matrix corresponding to the measurement noise may include:
determining a Kalman gain matrix corresponding to the first moment according to the following formula:
K(k+1)＝P(k+1|k)h T (k)[h(k)P(k+1|k)h T (k)+R k+1 ]-1；
Wherein K (k+1) is a kalman gain matrix corresponding to the first time, P (k+ 1|k) is a second error covariance matrix corresponding to the candidate state quantity, H (K) is a jacobian matrix of H (X (k|k)), H (X (k|k)) is a prediction observed quantity at the second time predicted from a second prediction state quantity at the second time, R k+1 And the covariance matrix is corresponding to the measurement noise at the first moment.
In another possible implementation manner, after the determining the kalman gain matrix corresponding to the first time, the method may further include:
and determining a third error covariance matrix corresponding to the first prediction state quantity according to the second error covariance matrix corresponding to the candidate state quantity, the discretization observation expression and the Kalman gain matrix corresponding to the first moment.
In a second aspect, an embodiment of the present application provides an external force prediction device when a robot rides a balancing vehicle, which may include:
the inverted pendulum model building module is used for building a wheel type inverted pendulum model when the robot rides the balance vehicle;
the dynamics analysis module is used for carrying out dynamics analysis on the wheel type inverted pendulum model and determining a dynamics model expression corresponding to the robot;
And the external force prediction module is used for predicting external force applied to the robot when the robot rides the balance car according to the dynamic model expression and the extended Kalman filtering.
In a third aspect, an embodiment of the present application provides a robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor executes the computer program to cause the robot to implement the external force prediction method when the robot rides the balance car according to any one of the first aspect.
In a fourth aspect, embodiments of the present application provide a computer-readable storage medium storing a computer program that, when executed by a robot, causes the robot to implement the method for predicting an external force when the robot rides a balancing vehicle according to any one of the first aspect.
In a fifth aspect, embodiments of the present application provide a computer program product, which, when run on a robot, causes the robot to perform the method of external force prediction when the robot is riding balance as described in any one of the first aspects above.
It will be appreciated that the advantages of the second to fifth aspects may be found in the relevant description of the first aspect, and are not described here again.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following description will briefly introduce the drawings that are needed in the embodiments or the description of the prior art, it is obvious that the drawings in the following description are only some embodiments of the present application, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic illustration of a robotic ride balance vehicle provided in an embodiment of the present application;
fig. 2 is a schematic flow chart of an external force prediction method during robot riding balance according to an embodiment of the present application;
fig. 3 is a schematic view of a wheel type inverted pendulum model provided in an embodiment of the present application;
fig. 4 is a schematic structural diagram of an external force prediction device when a robot rides a balancing car according to an embodiment of the present application;
fig. 5 is a schematic structural diagram of a robot according to an embodiment of the present application.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system configurations, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
It should be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It should also be understood that the term "and/or" as used in this specification and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes such combinations.
As used in this specification and the appended claims, the term "if" may be interpreted as "when..once" or "in response to a determination" or "in response to detection" depending on the context. Similarly, the phrase "if a determination" or "if a [ described condition or event ] is detected" may be interpreted in the context of meaning "upon determination" or "in response to determination" or "upon detection of a [ described condition or event ]" or "in response to detection of a [ described condition or event ]".
In addition, in the description of the present application and the appended claims, the terms "first," "second," "third," and the like are used merely to distinguish between descriptions and are not to be construed as indicating or implying relative importance.
Reference in the specification to "one embodiment" or "some embodiments" or the like means that a particular feature, structure, or characteristic described in connection with the embodiment is included in one or more embodiments of the application. Thus, appearances of the phrases "in one embodiment," "in some embodiments," "in other embodiments," and the like in the specification are not necessarily all referring to the same embodiment, but mean "one or more but not all embodiments" unless expressly specified otherwise. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless expressly specified otherwise.
Humanoid robots are a research hotspot in the field of robots. The upper limbs of the humanoid robot have operation capability, the lower limbs have movement capability, and the humanoid robot can use vehicles like human beings. For example, as shown in fig. 1, the robot may ride a two-wheeled balance car. The humanoid robot riding balance car can give consideration to the operation capability of the humanoid robot and the mobility capability of the wheeled robot. Wherein, in the course of robot riding the balance car, when the robot receives external force, can lead to the motion state of robot and balance car to change. For example, pushing the robot back may cause the robot to tilt forward, the balance car to accelerate forward, and affect the stability of the robot. For example, when the robot rides the balance car, if the robot encounters road jolts, the stability of the robot is also affected. Therefore, it is necessary to predict the external force applied to the robot while riding the balance car, so that the external force can be compensated for during control, and the stability of the robot while riding the balance car can be maintained.
In order to solve the above problems, embodiments of the present application provide a method, an apparatus, a robot, and a computer-readable storage medium for predicting an external force when a robot rides a balancing car. The method can establish a wheel type inverted pendulum model when the robot rides the balance vehicle, and can carry out dynamics analysis on the wheel type inverted pendulum model to determine a dynamics model expression corresponding to the robot. And then, the external force born by the robot during the balance riding process can be predicted according to the dynamic model expression and the extended Kalman filtering so as to accurately predict the external force born by the robot during the balance riding process, so that the external force can be compensated in the control process so as to maintain the stability of the robot during the balance riding process.
Referring to fig. 2, fig. 2 is a schematic flowchart illustrating a method for predicting an external force when a robot rides a balancing car according to an embodiment of the present application. The method may be applied to a robot, for example, a bipedal robot, the leg joints of which may be drive joints. The balance car can be any double-wheel platform with an open double-wheel motor interface. As shown in fig. 2, the method may include:
s201, establishing a wheel type inverted pendulum model when the robot rides a balance car.
Alternatively, establishing a wheeled inverted pendulum model when the robot rides the balance car may refer to simplifying the robot and the balance car together into a wheeled inverted pendulum model.
Referring to fig. 3, fig. 3 shows a schematic diagram of a wheel type inverted pendulum model according to an embodiment of the present application.
As shown in fig. 3, the robot 301 may be equivalent to one rigid rod. Wherein, the equivalent link length of the rigid rod may be denoted as l, and the equivalent link length may be the distance between the centroid and the sole of the robot 301. The mass of the robot 301 may be marked m 1 . The mass of balance car 302 may be labeled M. The wheel radius of balance car 302 may be labeled R. The moment of inertia of the rigid body bar may be marked as I 1 The moment of inertia of the wheels of balance car 302 may be labeled I 2 . The location of the wheel center in the world coordinate system may be marked (x, y). The torque of the wheel may be denoted as τ. The angle of the robot 301 to the vertical (i.e., pitch angle) may be denoted as θ. f (f) x And f y May be an external force to be observed acting on the robot 301, f x Can be an external force f applied to the robot 301 in the horizontal direction y May be an external force to which the robot 301 is subjected in the vertical direction.
It should be understood that, neglecting the effect of ground friction, the kinetic equation of the wheel type inverted pendulum model in the absence of external force disturbance may be:
Wherein,is the first derivative of θ (i.e. the angular velocity of the robot), is +.>Is the second derivative of θ (i.e. the angular acceleration of the robot), is +.>Is the first derivative of x (i.e. the speed of the wheel in the x direction),/>Is the second derivative of x (i.e., the acceleration of the wheel in the x direction), g is the gravitational acceleration.
It can be understood that based on the dynamics equation of the wheel type inverted pendulum model, the acceleration of the wheel in the x direction is achieved in the absence of external force disturbanceDynamic expression of (2) and angular acceleration of the robot +.>The dynamic expression of (2) may be as follows:
optionally, after external force disturbance is added to the dynamics equation of the wheel type inverted pendulum model, acceleration of the wheel in the x directionDynamic expression of (2) and angular acceleration of the robot +.>The dynamic expression of (2) may be as follows:
s202, carrying out dynamics analysis on the wheel type inverted pendulum model, and determining a dynamics model expression corresponding to the robot.
For example, after determining the dynamic equation of the wheel type inverted pendulum model, the state quantity and the observed quantity may be determined according to the observed requirement of the external force disturbance, so as to determine the dynamic model expression corresponding to the robot. Alternatively, the dynamic model expression may include a state expression corresponding to the state quantity and an observation expression corresponding to the observed quantity.
Alternatively, the state quantityI.e. the state quantity X may comprise the speed of the wheel in the X-direction +.>Pitch angle θ of robot to vertical direction, angular velocity of robot +.>External force f applied to robot in horizontal direction x And an external force f applied to the robot in the vertical direction y 。
Alternatively, the state expression corresponding to the state quantity may be as follows:
alternatively, observed quantityI.e. the observed quantity Y may comprise the speed of the wheel in the x-direction +.>Acceleration of the wheel in the x-direction +.>Angular velocity +.>Angular acceleration of the robot>
Alternatively, the observation expression corresponding to the observed quantity may be as follows:
s203, predicting the external force applied to the robot when riding the balance car according to the dynamic model expression and the extended Kalman filtering.
In the embodiment of the application, after the state expression corresponding to the state quantity and the observation expression corresponding to the observed quantity are determined, the robot can determine the discretization state expression corresponding to the robot according to the state expression, and can determine the discretization observation expression corresponding to the robot according to the observation expression, so as to predict the external force applied to the robot when riding the balance car according to the discretization state expression, the discretization observation expression and the extended kalman filter. That is, a predicted state quantity of the robot at the first time (hereinafter, may be referred to as a first predicted state quantity) is predicted. The first predicted state quantity may include an external force applied to the robot at a first time, that is, the first predicted state quantity may include an external force f applied to the robot in a horizontal direction at the first time x And an external force f applied to the robot in the vertical direction at the first moment y 。
Alternatively, the discretized state expression may be:
X(k+1)＝X(k)+X(k)·dt＝X(k)+F(X(k))·dt＝Fk(X(k))。
alternatively, the discretized observation expression may be:
Y(k+1)＝H(X(k+1))。
where dt represents a discrete sampling period, X (k) is a state quantity at time k, X (k+1) is a state quantity at time k+1, and Y (k+1) is an observed quantity at time k+1.
In one example, after determining the discretized state expression and the discretized observation expression, the robot may acquire a predicted state quantity of the robot at a second time (hereinafter may be referred to as a second predicted state quantity), which may be a time before the first time, and may determine a candidate state quantity of the robot at the first time from the second predicted state quantity and the discretized state expression. Subsequently, the robot can acquire the actual observed quantity of the robot at the first moment and according to the first momentCorrecting the candidate state quantity to obtain a first predicted state quantity of the robot at the first moment so as to enable the external force f in the first predicted state quantity to be calculated x And f y External force applied to the robot riding balance car at the first moment is determined.
Hereinafter, the first time is k+1, and the second time is k.
For example, the robot may predict the candidate state quantity at the first time based on the second predicted state quantity at the second time.
Alternatively, the robot may determine the candidate state quantity of the robot at the first moment according to the following formula:
X(k+1|k)＝Fk(X(k|k))；
where X (k|k) is an optimal estimate of the state quantity at time k (i.e., the second predicted state quantity at time k), and X (k+ 1|k) is a state quantity at time k+1 (i.e., the candidate state quantity at time k+1) predicted from the optimal estimate of the state quantity at time k (i.e., the first time), which may also be referred to as an a priori estimate.
For example, when determining the candidate state quantity of the robot at the first time based on the second predicted state quantity and the discretized state expression, the robot may further acquire a covariance matrix of an error of the second predicted state quantity (hereinafter may be referred to as a first error covariance matrix) and may acquire a covariance matrix of process noise at the second time. Subsequently, the robot may determine a covariance matrix of the error of the candidate state quantity (hereinafter may be referred to as a second error covariance matrix) from the discretized state expression, the first error covariance matrix, and the covariance matrix of the process noise.
Optionally, the robot may determine a second error covariance matrix corresponding to the candidate state quantity according to the following formula:
P(k+1|k)＝f(k)P(k|k)f T (k)+Q k ；
wherein P (k+ 1|k) is the second error covariance matrix corresponding to the candidate state quantity, and P (k|k) is the first error covariance matrixFirst error covariance matrix corresponding to two prediction state quantities, f (k) is jacobian matrix of Fk (X (k|k)), Q k Is the covariance matrix of the system process noise at the second moment.
Optionally, after determining the second error covariance matrix corresponding to the candidate state quantity, the robot may determine a covariance matrix corresponding to the measurement noise at the first moment, and may determine a kalman gain matrix corresponding to the first moment according to the second error covariance matrix, the discretized observation expression, and the covariance matrix corresponding to the measurement noise, so as to determine the first prediction state quantity at the first moment according to the kalman gain matrix corresponding to the first moment. I.e. determining an optimal estimate of the state quantity at the first moment, may also be referred to as a posterior estimate.
Optionally, the robot may determine the kalman gain matrix corresponding to the first time according to the following formula:
K(k+1)＝P(k+1|k)h T (k)[h(k)P(k+1|k)h T (k)+R k+1 ]-1；
wherein K (k+1) is a Kalman gain matrix corresponding to a first moment, P (k+ 1|k) is a second error covariance matrix corresponding to a candidate state quantity, H (K) is a jacobian matrix of H (X (k|k)), H (X (k|k)) is a prediction observed quantity of K moment predicted according to the optimal estimation of the state quantity at K moment (i.e., a second predicted state quantity at K moment), R k+1 The covariance matrix corresponding to the measurement noise at the first moment.
For example, the robot may correct the candidate state quantity at the first moment according to the following formula, to obtain a first predicted state quantity of the robot at the first moment:
X(k+1|k+1)＝X(k+1|k)+K(k+1)[Y(k+1)-H(X(k+1|k))]；
wherein X (k+1|k+1) is a first predicted state quantity of the robot at the first time, X (k+ 1|k) is a candidate state quantity of the robot at the first time, K (k+1) is a kalman gain matrix corresponding to the first time, Y (k+1) is an actual observed quantity of the robot at the first time, and H (X (k+ 1|k)) is a predicted observed quantity at the first time predicted from the candidate state quantity at the first time.
In one possible implementation, after determining the kalman gain matrix corresponding to the first time, the robot may determine a covariance matrix of the error of the first predicted state quantity (hereinafter may be referred to as a third error covariance matrix) according to the second error covariance matrix corresponding to the candidate state quantity, the discretized observation expression, and the kalman gain matrix corresponding to the first time.
Optionally, the robot may determine a third error covariance matrix corresponding to the first predicted state quantity according to the following formula:
P(k+1|k+1)＝[I-K(k+1)H(X(k|k))]P(k+1|k)；
wherein, P (k+1|k+1) is a third error covariance matrix corresponding to the first predicted state quantity, I is an identity matrix, P (k+ 1|k) is a second error covariance matrix corresponding to the candidate state quantity, and K (k+1) is a kalman gain matrix corresponding to the first moment.
From the above description, the extended kalman filter can predict the state quantity (i.e. a priori estimated value) at the current time according to the optimal estimation of the state quantity at the previous time (i.e. the a priori estimated value at the previous time). Then, the kalman gain matrix can be updated according to the covariance matrix of the error of the prior estimated value, and the optimal estimation (i.e. the posterior estimated value) of the state quantity at the current moment can be determined according to the actual observed quantity at the current moment and the updated kalman gain matrix. That is, the extended kalman filter is insensitive to the initial value of the state quantity, so that the state quantity at the current moment can be rapidly estimated, and the external force applied to the robot at the current moment can be rapidly determined.
The following describes the observability of the robot to predict the external forces experienced by the robot while riding a balance based on the dynamics model expression.
It will be appreciated that if the robot predicts that the external forces experienced by the robot while riding a balance are considerable based on the dynamics model expression, the robot may estimate the total state quantity based on the dynamics model expression. If not, the robot cannot estimate the state quantity.
Specifically, the observation expression corresponding to the observed quantity may be solved according to the following formula Partial differentiation of H (X) to state quantity X
Wherein H is 1 、H 2 、H 3 And H 4 For each row of H (X), X 1 、X 2 、X 3 、X 4 And X 5 Each row of X.
Then, can let L 0 Fh=h (X), calculate L 1 FH：
σ 4 ＝(I 1 +lm 1 )σ 1 -lm 1 2 cos(θ) 2 ；
σ 6 ＝m 1 l 2 +I 1 +I 2 ；
Similarly, L can be calculated 2 FH、L 3 FH and L 4 FH. Wherein,and
it is to be understood that L 2 FH、L 3 FH and L 4 FH solving process and L 1 The solution process for FH is similar and will not be described in detail here for the sake of brevity.
Subsequently, the matrix g= [ L may be made 0 FH L 1 FH L 2 FH L 3 FH L 4 FH] T The observability dG of the wheel type inverted pendulum model can be obtained by solving:
finally, it can be determined that the rank of the observability matrix dG of the wheel type inverted pendulum model is 5, which is equal to the dimension of the state quantity X. That is, it is considerable that the robot predicts an external force that the robot receives while riding the balance, based on the dynamic model expression. Therefore, the robot can predict the external force f applied to each moment of riding the balance car by the robot based on the dynamic model expression x And f y 。
It should be noted that, in the embodiment of the present application, the robot and the balance car are simplified into the wheel type inverted pendulum model, which is only exemplary to be explained, and should not be construed as limiting the embodiment of the present application. In the embodiment of the present application, the robot may be equivalent to a multi-rigid-body structure, for example, the leg may be equivalent to one link, the upper body and the arm may be equivalent to another link, so as to perform external force prediction through the two-stage wheel type inverted pendulum model, and so on.
In the embodiment of the application, when external force applied to the robot when riding the balance car is required to be predicted, the wheel type inverted pendulum model of the robot when riding the balance car can be established, dynamic analysis can be carried out on the wheel type inverted pendulum model, and a dynamic model expression corresponding to the robot is determined. And then, the external force born by the robot during the balance riding process can be predicted according to the dynamic model expression and the extended Kalman filtering so as to accurately predict the external force born by the robot during the balance riding process, so that the external force can be compensated in the control process so as to maintain the stability of the robot during the balance riding process.
It should be understood that the sequence number of each step in the foregoing embodiment does not mean that the execution sequence of each process should be determined by the function and the internal logic of each process, and should not limit the implementation process of the embodiment of the present application in any way.
Corresponding to the external force prediction method during robot riding balance vehicle described in the above embodiments, fig. 4 shows a block diagram of the external force prediction device during robot riding balance vehicle provided in the embodiment of the present application, and for convenience of explanation, only the parts related to the embodiment of the present application are shown.
As shown in fig. 4, the apparatus may include:
An inverted pendulum model building module 401, configured to build a wheel type inverted pendulum model when the robot rides a balance car;
the dynamics analysis module 402 is configured to perform dynamics analysis on the wheel type inverted pendulum model, and determine a dynamics model expression corresponding to the robot;
and the external force prediction module 403 is used for predicting the external force applied to the robot when riding the balance car according to the dynamic model expression and the extended Kalman filtering.
In one possible implementation, the kinetic model expression includes a state expression and an observation expression.
The external force prediction module 403 is specifically configured to determine a discretization state expression corresponding to the robot according to the state expression, and determine a discretization observation expression corresponding to the robot according to the observation expression; and predicting a first predicted state quantity of the robot at a first moment according to the discretization state expression, the discretization observation expression and the extended Kalman filtering, wherein the first predicted state quantity comprises an external force applied to the robot at the first moment.
In one example, the external force prediction module 403 is further configured to obtain a second predicted state quantity of the robot at a second time, where the second time is a time before the first time; determining a candidate state quantity of the robot at the first moment according to the second predicted state quantity and the discretization state expression; acquiring the actual observed quantity of the robot at the first moment; and correcting the candidate state quantity according to the actual observed quantity of the robot at the first moment, the discretization observed expression and the Kalman gain matrix corresponding to the first moment to obtain a first predicted state quantity of the robot at the first moment.
Optionally, the external force prediction module 403 is further configured to determine a first predicted state quantity of the robot at the first moment according to the following formula:
X(k+1|k+1)＝X(k+1|k)+K(k+1)[Y(k+1)-H(X(k+1|k))]；
wherein X (k+1|k+1) is a first predicted state quantity of the robot at the first time, X (k+ 1|k) is a candidate state quantity of the robot at the first time, K (k+1) is a kalman gain matrix corresponding to the first time, Y (k+1) is an actual observed quantity of the robot at the first time, and H (X (k+ 1|k)) is a predicted observed quantity at the first time predicted from the candidate state quantity at the first time.
In one possible implementation, the apparatus may further include:
the first error covariance matrix determination module is used for determining a first error covariance matrix corresponding to the second prediction state quantity;
a second error covariance matrix determination module, configured to determine a second error covariance matrix corresponding to the candidate state quantity according to the discretized state expression and the first error covariance matrix;
the covariance matrix determining module is used for determining a covariance matrix corresponding to the measurement noise at the first moment;
And the Kalman gain matrix determining module is used for determining the Kalman gain matrix corresponding to the first moment according to the second error covariance matrix, the discretization observation expression and the covariance matrix corresponding to the measurement noise.
The kalman gain matrix determining module is specifically configured to determine the kalman gain matrix corresponding to the first time according to the following formula:
K(k+1)＝P(k+1|k)h T (k)[h(k)P(k+1|k)h T (k)+R k+1 ]-1；
wherein K (k+1) is a kalman gain matrix corresponding to the first time, P (k+ 1|k) is a second error covariance matrix corresponding to the candidate state quantity, H (K) is a jacobian matrix of H (X (k|k)), H (X (k|k)) is a prediction observed quantity at the second time predicted from a second prediction state quantity at the second time, R k+1 And the covariance matrix is corresponding to the measurement noise at the first moment.
In another possible implementation manner, the apparatus may further include:
and the third error covariance matrix determination module is used for determining a third error covariance matrix corresponding to the first prediction state quantity according to the second error covariance matrix corresponding to the candidate state quantity, the discretization observation expression and the Kalman gain matrix corresponding to the first moment.
It should be noted that, because the content of information interaction and execution process between the above devices/units is based on the same concept as the method embodiment of the present application, specific functions and technical effects thereof may be referred to in the method embodiment section, and will not be described herein again.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of the functional units and modules is illustrated, and in practical application, the above-described functional distribution may be performed by different functional units and modules according to needs, i.e. the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-described functions. The functional units and modules in the embodiment may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit, where the integrated units may be implemented in a form of hardware or a form of a software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working process of the units and modules in the above system may refer to the corresponding process in the foregoing method embodiment, which is not described herein again.
Fig. 5 is a schematic structural diagram of a robot according to an embodiment of the present disclosure. As shown in fig. 5, the robot 5 of this embodiment includes: at least one processor 50 (only one is shown in fig. 5), a memory 51, and a computer program 52 stored in the memory 51 and executable on the at least one processor 50, the processor 50 implementing the steps in any of the various robotic riding balance vehicle external force prediction method embodiments described above when executing the computer program 52.
The robot 5 may include, but is not limited to, a processor 50, a memory 51. It will be appreciated by those skilled in the art that fig. 5 is merely an example of the robot 5 and is not meant to be limiting of the robot 5, and may include more or fewer components than shown, or may combine certain components, or may include different components, such as input-output devices, network access devices, etc.
The processor 50 may be a central processing unit (central processing unit, CPU), the processor 50 may also be other general purpose processors, digital signal processors (digital signal processor, DSP), application specific integrated circuits (application specific integrated circuit, ASIC), field-programmable gate arrays (field-programmable gate array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory 51 may in some embodiments be an internal storage unit of the robot 5, such as a hard disk or a memory of the robot 5. The memory 51 may in other embodiments also be an external storage device of the robot 5, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a flash card (flash card) or the like, which are provided on the robot 5. Further, the memory 51 may also include both an internal memory unit and an external memory device of the robot 5. The memory 51 is used for storing an operating system, application programs, boot loader (BootLoader), data, other programs, etc., such as program codes of the computer program. The memory 51 may also be used to temporarily store data that has been output or is to be output.
The embodiment of the application also provides a computer readable storage medium, wherein the computer readable storage medium stores a computer program, and the computer program can realize the steps in the external force prediction method embodiment of each robot riding balance car when being executed by a processor.
Embodiments of the present application provide a computer program product that, when executed on a robot, enables the robot to perform the steps in the embodiments of the external force prediction method described above for each robot riding balance car.
The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the present application implements all or part of the flow of the method of the above embodiments, and may be implemented by a computer program to instruct related hardware, where the computer program may be stored in a computer readable storage medium, where the computer program, when executed by a processor, may implement the steps of each of the method embodiments described above. Wherein the computer program comprises computer program code which may be in source code form, object code form, executable file or some intermediate form etc. The computer readable storage medium may include at least: any entity or device capable of carrying computer program code to an apparatus/robot, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (random access memory, RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Such as a U-disk, removable hard disk, magnetic or optical disk, etc. In some jurisdictions, computer-readable storage media may not be electrical carrier signals and telecommunications signals in accordance with legislation and patent practice.
In the foregoing embodiments, the descriptions of the embodiments are emphasized, and in part, not described or illustrated in any particular embodiment, reference is made to the related descriptions of other embodiments.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus/robot and method may be implemented in other ways. For example, the apparatus/robot embodiments described above are merely illustrative, e.g., the division of the modules or units is merely a logical function division, and there may be additional divisions in actual implementation, e.g., multiple units or components may be combined or integrated into another system, or some features may be omitted, or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection via interfaces, devices or units, which may be in electrical, mechanical or other forms.
The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
The above embodiments are only for illustrating the technical solution of the present application, and are not limiting; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present application, and are intended to be included in the scope of the present application.
Claims (10)
1. The external force prediction method for the robot riding balance car is characterized by comprising the following steps of:
establishing a wheel type inverted pendulum model when the robot rides a balance car;
carrying out dynamics analysis on the wheel type inverted pendulum model, and determining a dynamics model expression corresponding to the robot;
And predicting the external force applied to the robot when riding the balance car according to the dynamic model expression and the extended Kalman filtering.
2. The method of claim 1, wherein the dynamic model expression comprises a state expression and an observation expression, and wherein predicting the external force experienced by the robot while riding a balance based on the dynamic model expression and extended kalman filter comprises:
determining a discretization state expression corresponding to the robot according to the state expression, and determining a discretization observation expression corresponding to the robot according to the observation expression;
and predicting a first predicted state quantity of the robot at a first moment according to the discretization state expression, the discretization observation expression and the extended Kalman filtering, wherein the first predicted state quantity comprises an external force applied to the robot at the first moment.
3. The method of claim 2, wherein predicting a first predicted state quantity of the robot at a first time based on the discretized state expression, the discretized observation expression, and the extended kalman filter comprises:
Acquiring a second predicted state quantity of the robot at a second moment, wherein the second moment is a moment before the first moment;
determining a candidate state quantity of the robot at the first moment according to the second predicted state quantity and the discretization state expression;
acquiring the actual observed quantity of the robot at the first moment;
and correcting the candidate state quantity according to the actual observed quantity of the robot at the first moment, the discretization observed expression and the Kalman gain matrix corresponding to the first moment to obtain a first predicted state quantity of the robot at the first moment.
4. A method according to claim 3, wherein correcting the candidate state quantity according to the actual observed quantity of the robot at the first moment, the discretized observed expression and the kalman gain matrix corresponding to the first moment to obtain a first predicted state quantity of the robot at the first moment comprises:
determining a first predicted state quantity of the robot at the first moment according to the following formula:
X(k+1|k+1)＝X(k+1|k)+K(k+1)[Y(k+1)-H(X(k+1|k))]；
wherein X (k+1|k+1) is a first predicted state quantity of the robot at the first time, X (k+ 1|k) is a candidate state quantity of the robot at the first time, K (k+1) is a kalman gain matrix corresponding to the first time, Y (k+1) is an actual observed quantity of the robot at the first time, and H (X (k+ 1|k)) is a predicted observed quantity at the first time predicted from the candidate state quantity at the first time.
5. A method according to claim 3, wherein before correcting the candidate state quantity according to the actual observed quantity of the robot at the first time, the discretized observed expression, and the kalman gain matrix corresponding to the first time, to obtain a first predicted state quantity of the robot at the first time, the method further comprises:
determining a first error covariance matrix corresponding to the second prediction state quantity;
determining a second error covariance matrix corresponding to the candidate state quantity according to the discretization state expression and the first error covariance matrix;
determining a covariance matrix corresponding to the measurement noise at the first moment;
and determining a Kalman gain matrix corresponding to the first moment according to the second error covariance matrix, the discretization observation expression and the covariance matrix corresponding to the measurement noise.
6. The method of claim 5, wherein determining the kalman gain matrix corresponding to the first time based on the second error covariance matrix, the discretized observed expression, and the covariance matrix corresponding to the measurement noise comprises:
Determining a Kalman gain matrix corresponding to the first moment according to the following formula:
wherein K (k+1) is the Kalman gain matrix corresponding to the first moment, and P (k+ 1|k) is the second error covariance moment corresponding to the candidate state quantityMatrix, H (k) is jacobian matrix of H (X (k|k)) which is a prediction observation quantity at the second time predicted from a second prediction state quantity at the second time, R k+1 And the covariance matrix is corresponding to the measurement noise at the first moment.
7. The method according to claim 5 or 6, wherein after said determining the corresponding kalman gain matrix at the first time, the method further comprises:
and determining a third error covariance matrix corresponding to the first prediction state quantity according to the second error covariance matrix corresponding to the candidate state quantity, the discretization observation expression and the Kalman gain matrix corresponding to the first moment.
8. An external force prediction device when a robot rides a balance car, comprising:
the inverted pendulum model building module is used for building a wheel type inverted pendulum model when the robot rides the balance vehicle;
the dynamics analysis module is used for carrying out dynamics analysis on the wheel type inverted pendulum model and determining a dynamics model expression corresponding to the robot;
And the external force prediction module is used for predicting external force applied to the robot when the robot rides the balance car according to the dynamic model expression and the extended Kalman filtering.
9. A robot comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor, when executing the computer program, causes the robot to implement the method of predicting external forces when the robot is riding a balance car as claimed in any one of claims 1 to 7.
10. A computer-readable storage medium storing a computer program which, when executed by a robot, causes the robot to implement the external force prediction method of the robot riding balance vehicle according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311795319.3A CN117891167A (en) | 2023-12-22 | 2023-12-22 | External force prediction method and device for robot riding balance car and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311795319.3A CN117891167A (en) | 2023-12-22 | 2023-12-22 | External force prediction method and device for robot riding balance car and robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117891167A true CN117891167A (en) | 2024-04-16 |
Family
ID=90638567
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311795319.3A Pending CN117891167A (en) | 2023-12-22 | 2023-12-22 | External force prediction method and device for robot riding balance car and robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117891167A (en) |
-
2023
- 2023-12-22 CN CN202311795319.3A patent/CN117891167A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10012984B2 (en) | System and method for controlling autonomous vehicles | |
| CN106476728B (en) | Motion compensation for vehicle-mounted vehicle sensors | |
| Kosecka et al. | A comparative study of vision-based lateral control strategies for autonomous highway driving | |
| CN113064433B (en) | Robot stepping control method and device, robot control equipment and storage medium | |
| CN112720483B (en) | Method and device for acquiring combined mass center state, humanoid robot and readable storage medium | |
| US20230271656A1 (en) | Robot state estimation method, computer-readable storage medium, and legged robot | |
| Dar et al. | Slip estimation for small-scale robotic tracked vehicles | |
| US20200386782A1 (en) | Apparatus and method for calibrating inertial measuring unit | |
| CN112099378B (en) | Front vehicle lateral motion state real-time estimation method considering random measurement time lag | |
| CN113031579B (en) | Walking control method and device of biped robot and biped robot | |
| CN109177909B (en) | Vehicle rollover pre-judging method, device and equipment | |
| CN117891167A (en) | External force prediction method and device for robot riding balance car and robot | |
| CN113788022A (en) | Vehicle control method and device, computer-readable storage medium and electronic equipment | |
| CN109987097B (en) | Method and equipment for adjusting motion state of target vehicle | |
| CN112947428B (en) | Movement control method and device for four-legged robot | |
| CN112046491B (en) | Method and device for estimating cornering stiffness of wheel, vehicle and readable storage medium | |
| CN114777762A (en) | Inertial navigation method based on Bayesian NAS | |
| CN110308724B (en) | Automatic driving control method, automatic driving control device, vehicle, storage medium and electronic equipment | |
| CN114852051A (en) | Vehicle running control method and device, electronic equipment and storage medium | |
| CN112045687A (en) | Dynamics calculation method for operating mechanical arm and central control method and system for mechanical arm | |
| Yang et al. | Recurrent End-to-End Neural Network Design with Temporal Dependencies for Model-Free Lane Keeping Systems | |
| CN115303291B (en) | Trailer trajectory prediction method and device for towed vehicle, electronic device and storage medium | |
| CN112874315B (en) | Motion control method, device and equipment of balance car and storage medium | |
| Odry et al. | Adaptive Gradient-Descent Extended Kalman Filter for Pose Estimation of Mobile Robots with Sparse Reference Signals | |
| US20240140413A1 (en) | System and method for target behavior prediction using host prediction in advanced driving assist system (adas), autonomous driving (ad), or other applications |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination |