CN116358603A - Course angle correction method, self-mobile device and storage medium - Google Patents
Course angle correction method, self-mobile device and storage medium Download PDFInfo
- Publication number
- CN116358603A CN116358603A CN202310370107.4A CN202310370107A CN116358603A CN 116358603 A CN116358603 A CN 116358603A CN 202310370107 A CN202310370107 A CN 202310370107A CN 116358603 A CN116358603 A CN 116358603A
- Authority
- CN
- China
- Prior art keywords
- angle
- moment
- course angle
- current
- observation
- 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 56
- 238000012937 correction Methods 0.000 title claims abstract description 18
- 230000004927 fusion Effects 0.000 claims abstract description 61
- 238000005259 measurement Methods 0.000 claims abstract description 59
- 230000008859 change Effects 0.000 claims description 42
- 238000004364 calculation method Methods 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 9
- 230000006870 function Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 description 46
- 238000004891 communication Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 238000007499 fusion processing Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 235000012054 meals Nutrition 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/40—Correcting position, velocity or attitude
- G01S19/41—Differential correction, e.g. DGPS [differential GPS]
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
-
- 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
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application provides a course angle correction method, electronic equipment and a storage medium, which are applied to self-mobile equipment. The correction method comprises the following steps: acquiring an observation course angle at the current moment through a real-time differential positioning module and an inertial measurement unit; acquiring a fusion course angle at the previous moment and a fusion angular speed at the previous moment; determining a time interval between the last time and the current time; and calculating to obtain the predicted course angle at the current moment according to the fused course angle at the last moment, the fused angular speed at the last moment and the time interval between the last moment and the current moment. And the predicted course angle at the current moment and the observed course angle at the current moment can be fused according to the prediction reliability and the observation reliability at the current moment, and the obtained fused course angle can improve the accuracy of the obtained course angle and realize the accurate positioning of the self-mobile equipment.
Description
Technical Field
The present disclosure relates to the field of positioning technologies, and in particular, to a course angle correction method, a self-mobile device, and a computer readable storage medium.
Background
In the operation process of the self-mobile device along the pre-planned path, the accurate course angle of the self-mobile device needs to be acquired in real time so as to control the self-mobile device in real time to better track the pre-planned path.
At present, if an angle obtained by integrating the angular velocity of an IMU (Inertial Measurement Unit, an inertial measurement unit) is used as a course angle of the self-mobile device, a steady-state drift problem exists in long-time operation, and after the condition of temperature drift, zero drift, vibration and the like of a gyroscope in the IMU occurs, the steady-state error of the course angle of the self-mobile device obtained by measurement is large. If the RTK (Real time kinematic, real-time differential positioning) technique is used alone to calculate the heading angle of the self-mobile device from the positioning coordinates, the angle jump in short-time operation is large. It can be seen that no matter the self-mobile device is heading-positioned by the IMU or RTK technology, the deviation exists, and the accuracy is not enough.
Disclosure of Invention
The main objective of the embodiments of the present application is to provide a course angle correction method, a self-mobile device and a computer readable storage medium. The method aims at combining the prediction reliability and the observation reliability of the current moment, and fusing the predicted course angle and the observation course angle of the self-mobile device, so that the course angle of the self-mobile device can be corrected according to the fused course angle, and the accurate positioning of the self-mobile device is realized.
To achieve the above object, a first aspect of the embodiments of the present application proposes a heading angle correction method, which is applied to a self-mobile device, and the method includes:
in the moving process of the self-mobile equipment, acquiring an observation course angle at the current moment through a real-time differential positioning module and an inertial measurement unit;
acquiring a fusion course angle at the previous moment and a fusion angular speed at the previous moment;
determining a time interval between the previous time and the current time;
calculating to obtain a predicted course angle at the current moment according to the time interval, the fused course angle at the last moment and the fused angular speed at the last moment;
determining prediction credibility and observation credibility of the current moment, wherein the observation credibility is adjusted according to the positioning precision of the real-time differential positioning module;
according to the prediction credibility and the observation credibility of the current moment, fusing the observation course angle of the current moment and the prediction course angle of the current moment to obtain a fused course angle of the current moment;
and correcting the observed course angle at the current moment based on the fused course angle at the current moment.
According to the method, the real-time differential positioning module and the inertial measurement unit are used for acquiring the observation course angle at the current moment, and the predicted course angle at the current moment can be calculated according to the fusion course angle at the last moment, the fusion angular speed at the last moment and the time interval between the last moment and the current moment because of certain deviation of the observation course angle. And the predicted course angle at the current moment and the observed course angle at the current moment can be fused according to the prediction reliability and the observation reliability at the current moment to obtain a fused course angle. The method and the device can improve the accuracy of the course angle, and the reliability of the predicted course angle at the current moment and the reliability of the observed course angle at the current moment can be determined by adjusting the prediction reliability and the observation reliability of the current moment through the positioning accuracy of the real-time differential positioning module, so that the predicted course angle at the current moment and the observed course angle at the current moment are fused according to the prediction reliability and the observation reliability at the current moment, the obtained fused course angle is more accurate, and the accurate positioning of the self-mobile equipment is realized.
To achieve the above object, a second aspect of the embodiments of the present application proposes a self-mobile device, which includes a memory and a processor, where the memory stores a computer program, and the processor implements the method of the first aspect of the embodiments of the present application when executing the computer program.
To achieve the above object, a third aspect of the embodiments of the present application proposes a computer-readable storage medium storing a computer program, which when executed by a processor, implements the method of the first aspect of the embodiments of the present application.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
FIG. 1 is a flow chart of a heading angle correction method provided by an embodiment of the present application;
fig. 2 is a flowchart of steps for obtaining an observation course angle at a current moment through a real-time differential positioning module and an inertial measurement unit according to an embodiment of the present application;
FIG. 3 is a flowchart of a step of obtaining an observation course angle at a current moment according to first positioning information, second positioning information and an angle variation provided in an embodiment of the present application;
fig. 4 is a flowchart of determining a movement state of a self-mobile device according to an angle change amount according to an embodiment of the present application;
FIG. 5 is a flowchart of steps for obtaining a fused angular velocity at a previous time according to an embodiment of the present application;
FIG. 6 is a flowchart illustrating a step of acquiring a fused heading angle at a previous time according to an embodiment of the present application;
FIG. 7 is a flowchart of steps performed after obtaining an observed heading angle at a current time and a predicted heading angle at the current time provided by an embodiment of the present application;
FIG. 8 is another flow chart of a heading angle correction method provided by an embodiment of the present application;
fig. 9 is a schematic structural diagram of a self-mobile device according to an embodiment of the present application.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. However, the exemplary embodiments may be embodied in many forms and should not be construed as limited to the examples set forth herein; rather, these example embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of the example embodiments to those skilled in the art. The drawings are merely schematic illustrations of the present application and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus a repetitive description thereof will be omitted.
For self-moving devices, such as lawnmowers, meal delivery robots, etc., it is often necessary to move along a pre-planned path during the work. At this time, it is necessary to acquire an accurate heading angle of the self-mobile device in real time, so as to control the self-mobile device to better track the pre-planned path in real time.
In order to obtain the course angle of the self-mobile device, an IMU is adopted for measurement in the related art, and the angle obtained by integrating the angular velocity is taken as the course angle of the self-mobile device. Alternatively, the related art also adopts an RTK technique, and the course angle of the self-mobile device is obtained through positioning coordinate calculation.
However, when the IMU is used for measurement, the steady-state drift problem exists when the self-mobile device runs for a long time, and after the gyroscope in the IMU has the conditions of temperature drift, zero drift, vibration and the like, the steady-state error of the course angle of the self-mobile device obtained by measurement is large. With the RTK technique, the angle jump is large in a short time operation from the mobile device. It can be seen that, whether the self-mobile device is heading-located by IMU or RTK technology, there is a bias and insufficient accuracy.
Based on the above, the embodiment of the application provides a course angle correction method, which is applied to a self-mobile device, and can fuse the obtained predicted course angle and the observed course angle of the self-mobile device, so that the course angle of the self-mobile device can be corrected according to the fused course angle, the accuracy of the obtained course angle can be improved, and the accurate positioning of the self-mobile device can be realized.
Referring to fig. 1, fig. 1 is a flowchart of a heading angle correction method provided in an embodiment of the present application. The method shown in fig. 1 is applied to a self-mobile device, and the course angle correction method includes, but is not limited to, steps S110 to S170.
Step S110, in the moving process of the self-mobile equipment, the observation course angle at the current moment is obtained through the real-time differential positioning module and the inertia measurement unit.
In the embodiment of the present application, the self-mobile device generally moves according to a pre-planned path when performing the operation. The real-time position and heading angle of the self-mobile device can be acquired through a real-time differential positioning module and an inertial measurement unit which are installed on the self-mobile device, so that whether the self-mobile device moves according to a pre-planned path is determined. The real-time differential positioning module can be used for measuring position information, speed information, gesture information and the like of the self-mobile device in the moving process. The inertial measurement unit can be used for measuring information such as acceleration information, angular velocity information, angle change and the like of the self-mobile device in the moving process.
According to the embodiment of the application, according to the output results of the real-time differential positioning module and the inertia measurement unit at each moment, the observation course angle corresponding to each moment of the self-mobile device can be calculated. Naturally, according to the output results of the real-time differential positioning module and the inertial measurement unit at the current moment, the observation course angle corresponding to the current moment of the self-mobile device can be calculated.
In the embodiment of the application, because the real-time differential positioning module generates larger deviation in the measurement result when the self-moving equipment moves for a long time, the inertial measurement unit also generates larger deviation in the measurement result when the self-moving equipment moves for a short time. Therefore, in the whole moving process of the self-moving equipment, according to the output results of the real-time differential positioning module and the inertia measuring unit at all times, the calculated observation course angle corresponding to all times of the self-moving equipment is not accurate enough. That is, the self-mobile device cannot be precisely positioned directly by using the observation course angle obtained through the real-time differential positioning module and the inertial measurement unit. Therefore, the observed course angle needs to be corrected through the fusion course angle so as to realize the accurate positioning of the self-mobile device.
In one embodiment of the present application, referring to fig. 2, fig. 2 is a flowchart of steps for obtaining an observed heading angle at a current moment through a real-time differential positioning module and an inertial measurement unit according to an embodiment of the present application. As shown in fig. 2, the acquisition of the observation course angle at the current time by the real-time differential positioning module and the inertial measurement unit includes, but is not limited to, steps S210 to S230.
Step S210, acquiring first positioning information at the previous moment and second positioning information at the current moment through a real-time differential positioning module.
In the embodiment of the application, the real-time differential positioning module can measure the position information of the self-mobile equipment. Therefore, the first positioning information at the moment and the second positioning information at the current moment on the mobile equipment can be obtained according to the position information measured by the real-time differential positioning module.
For example, the position coordinates from time k-1 of the mobile device may be measured by a real-time differential positioning module as
Position coordinate at time k->
Step S220, acquiring the angle variation between the previous time and the current time through the inertial measurement unit.
For example, the angle change between the time k and the time k-1 measured by the inertial measurement unit is denoted as Δθ.
Step S230, according to the first positioning information, the second positioning information and the angle change amount, the observation course angle at the current moment is obtained.
In this embodiment of the present application, after the first positioning information from the last time of the mobile device, the second positioning information at the current time, and the angle variation between the last time and the current time are obtained, the observation course angle at the current time of the mobile device may be further calculated according to the first positioning information, the second positioning information, and the angle variation.
In an embodiment of the present application, referring to fig. 3, fig. 3 is a flowchart of a step of obtaining an observation course angle at a current moment according to first positioning information, second positioning information and an angle variation provided in an embodiment of the present application. As shown in fig. 3, obtaining the observed heading angle at the current time according to the first positioning information, the second positioning information and the angle variation includes, but is not limited to, steps S310 to S330.
Step S310, determining a first coordinate variation corresponding to the first coordinate axis and a second coordinate variation corresponding to the second coordinate axis according to the first positioning information and the second positioning information, respectively.
Step S320, according to the first coordinate variation, the second coordinate variation, the angle variation and a preset inverse trigonometric function formula, the observed course angle at the previous moment is obtained.
Step S330, according to the observed course angle and the angle change quantity of the previous moment, the observed course angle of the current moment is obtained.
The first coordinate axis may be an x-axis, the second coordinate axis may be a y-axis, and the first positioning information and the second positioning information may be represented by x-axis coordinates and y-axis coordinates. The first coordinate variation corresponding to the x-axis and the second coordinate variation corresponding to the y-axis can be determined by the first positioning information and the second positioning information. And then according to the first coordinate variation, the second coordinate variation, the angle variation and a preset inverse trigonometric function formula, the observed course angle at the moment on the mobile device can be calculated. And then the observed course angle at the current moment can be obtained according to the observed course angle and the angle variation quantity at the previous moment.
Illustratively, the position coordinates obtained from the time of the mobile device k-1 (i.e., the last time) by the real-time differential positioning module are
Position coordinate at time k (i.e. the current time)>
At this time, considering that the real-time differential positioning module outputs the position coordinates under the RTK coordinate system, when the real-time differential positioning moduleWhen the position installed on the self-mobile device is not coincident with the movement center point of the self-mobile device, coordinate system transformation needs to be performed first, namely, the position coordinate under the RTK coordinate system is transformed into the position coordinate under the ODOM coordinate system. The RTK coordinate system is a coordinate system taking a position point of the real-time differential positioning module as an origin. The ODOM coordinate system is a coordinate system with a center point of motion from the mobile device as an origin. Specifically, if the position coordinate from the time of mobile device k-1 in the ODOM coordinate system is set to +.>
The position coordinate at time k is set to +.>
Then there is formula (1) as follows:
in the formula (1), M is the distance between the position point of the real-time differential positioning module and the motion center point of the mobile device, and θ is the included angle between the X axis in the RTK coordinate system and the X axis in the ODOM coordinate system.
The position coordinates of the self-mobile equipment k-1 moment measured by the real-time differential positioning module are obtained 
And the position coordinate at time k>
Then, according to the formula (1), the position coordinate +_in the ODOM coordinate system from the time of the mobile device k-1 can be determined>
And the position coordinate at time k>
After the coordinate system conversion, the position coordinate of the converted k-1 moment can be used for +.>
And the position coordinate at time k>
The first coordinate variation deltax and the second coordinate variation deltay from the moment of the mobile device k are calculated. Wherein the first coordinate variation Δx and the second coordinate variation Δy satisfy the formula (2), and the formula (2) is as follows:
at this time, if the moving state of the self-moving device is a straight traveling state, that is, the angle change amount Δθ at the k time and the k-1 time measured by the inertial measurement unit is regarded as 0, the first coordinate change amount Δx and the second coordinate change amount Δy satisfy the formula (3), and the formula (3) is as follows:
in the formula (3),
then according to formula (3) the +.>
Thus, the observed course angle at the moment k-1 of the mobile device can be obtained by calculation according to the formula (4), wherein the formula (4) is as follows: θ k-1 =atan2 (Δy, Δx) (4). At this time, since the angle change amount Δθ at the time k and the time k-1 is regarded as 0, the observed heading angle θ at the time k from the mobile device can be calculated k ＝θ k-1 。
If the moving state of the self-moving device is not a straight traveling state, such as a curve, the angle change amount between the k time and the k-1 time measured by the inertial measurement unit is Δθ. The first coordinate variation Δx and the second coordinate variation and the angle variation Δθ satisfy the formula (5). The formula (5) is:
In the formula (5) of the present invention,
then, the formula (5) is transformed, and the following formula (6) can be obtained:
from equation (6), equation (7) can be derived, equation (7) being as follows:
thereby, according to the formula (7), the observed course angle theta of the mobile device at the moment k-1 can be calculated k-1 =atan2 (sin Δθ×Δy+ (cos Δθ—1) ×Δx, sin Δθ×Δx+ (1-cos Δθ) ×Δy). Since the angle change between the moment k and the moment k-1 measured by the inertial measurement unit is delta theta, the observed course angle theta at the moment k of the mobile device can be calculated k ＝θ k-1 +Δθ。
According to the embodiment of the application, the real-time differential positioning module is used for measuring the position coordinate of the last moment of output and the position coordinate of the current moment, and the inertial measurement unit is used for measuring the angle change quantity between the last moment of output and the current moment, so that the observed course angle of the last moment of the mobile equipment can be calculated. And then according to the observed course angle at the last moment and the angle change quantity between the last moment and the current moment, the observed course angle at the current moment of the self-mobile equipment can be calculated. It can be seen that the observed course angle from the current time of the mobile device is calculated based on the observed course angle of the previous time.
In one embodiment of the present application, referring to fig. 4, fig. 4 is a flowchart of determining a moving state of a self-mobile device according to an angle change amount provided in an embodiment of the present application. As shown in fig. 4, determining the movement state of the self-mobile device according to the angle change amount includes steps S410 to S440.
In step S410, the angle change between the previous time and the current time is acquired by the inertial measurement unit.
Step S420, judging whether the angle change amount is larger than a preset angle threshold.
In step S430, if the angle variation is greater than the preset angle threshold, it is determined that the moving state of the self-mobile device is the turning driving state.
In step S440, if the angle variation is not greater than the preset angle threshold, it is determined that the moving state of the self-mobile device is a straight running state.
Since the heading angle reflects the direction of the self-moving device during movement, that is, when the direction of the self-moving device is not changed, the direction of movement of the self-moving device is indicated to be a straight direction, that is, the moving state of the self-moving device may be a straight running state at this time. When the direction of the self-moving device is continuously changed, the moving direction of the automatic device is a curve direction, namely, the moving state of the self-moving device can be a turning running state. According to the embodiment of the application, after the angle change delta theta between the previous moment and the current moment is obtained through the inertia measurement unit, the angle change delta theta and the preset angle threshold value can be further judged. If the angle change delta theta is larger than the preset angle threshold value, the moving state of the self-moving equipment can be determined to be a turning driving state. If the angle change delta theta is not greater than the preset angle threshold value, the moving state of the self-moving equipment can be determined to be a straight running state.
It will be appreciated that the self-moving device generally cannot travel straight completely during long travel periods, taking into account the effects of other factors such as air resistance, frictional resistance, etc. I.e. the angle change delta theta between the last moment and the current moment is generally difficult to be 0. In the embodiment of the present application, a preset angle threshold is set, and when the angle variation Δθ is not greater than the preset angle threshold, the moving state of the self-mobile device is regarded as a straight running state.
It can be understood that the preset angle threshold can be set and adjusted according to practical situations, and the preset angle threshold is a smaller angle value.
In the embodiment of the application, the movement state of the self-mobile device can be determined by comparing the angle change amount between the last moment and the current moment acquired by the inertia measurement unit with the preset angle threshold value. Therefore, the formula for calculating the observed course angle at the current moment of the self-mobile device can be determined according to the moving state of the self-mobile device. Such as determining that the movement state of the self-moving device is a straight traveling state. The observation course angle at the current moment can be obtained directly according to the first positioning information at the last moment and the second positioning information at the current moment which are measured and output by the real-time differential positioning module, the calculation of parameters such as angle variation and the like can be reduced, the operation efficiency is improved, and the positioning efficiency is finally improved.
Step S120, the fused course angle at the previous moment and the fused angular velocity at the previous moment are obtained.
In the embodiment of the application, after the observation course angle at the current moment is obtained according to the real-time differential positioning module and the inertial measurement unit. The obtained observation course angle at the current moment is not accurate enough, and accurate positioning of the self-mobile equipment cannot be performed. Therefore, the predicted heading angle of the current time needs to be further acquired, so that the predicted heading angle of the current time and the observed heading angle of the current time acquired in step S110 are fused, and then the observed heading angle of the current time can be corrected according to the fusion result. In order to obtain the predicted course angle at the current moment, the fused course angle at the previous moment and the fused angular velocity at the previous moment need to be obtained first.
In one embodiment of the present application, referring to fig. 5, fig. 5 is a flowchart of the steps for obtaining a fusion angular velocity at a previous time provided in the embodiment of the present application. As shown in fig. 5, acquiring the fusion angular velocity at the previous time includes, but is not limited to, step S510 to step S540.
In step S510, the observed angular velocity at the previous time is obtained by the inertial measurement unit.
Step S520, determining the predicted angular velocity at the previous time, where the predicted angular velocity at the previous time is the fusion angular velocity corresponding to the previous time at the previous time.
In step S530, the angular velocity prediction reliability and the angular velocity observation reliability at the previous time are determined.
Step S540, according to the angular velocity prediction reliability and the angular velocity observation reliability of the previous moment, the predicted angular velocity of the previous moment and the observed angular velocity of the previous moment are fused to obtain the fused angular velocity of the previous moment.
In the embodiment of the application, the observed angular velocity obtained from the mobile device at the moment through the inertial measurement unit is w k-1 . The fusion angular velocity corresponding to the previous moment of the previous moment
As the predicted angular velocity at the last moment, i.e. +.>
Then determining the angular velocity prediction reliability of the previous time as +.>
Determining the angular velocity observation reliability of the last time as +.>
So that the degree of reliability can be predicted from the angular velocity at the previous moment>
And angular velocity observation reliability at the last time +.>
Predicted angular velocity at the previous moment +.>
And the observed angular velocity w at the previous time k-1 Fusing to obtain fused angular velocity at the previous moment
The angular velocity fusion can be performed by the formula (8). Equation (8) is: />
Wherein (1)>
That is, the sum of the angular velocity prediction reliability at the previous time and the angular velocity observation reliability at the previous time is 1.
It should be noted that the angular velocity prediction reliability and the angular velocity observation reliability may be determined by parameter adjustment according to actual situations during the measurement of the inertial measurement unit. The prediction reliability of the angular velocity corresponding to each moment is not the same, and the observation reliability of the angular velocity corresponding to each moment is not the same.
In one embodiment of the present application, the method shown in fig. 1 further includes:
when the inertial measurement unit is detected not to output the angle change quantity, taking the product result of the fusion angular speed and the time interval at the last moment as the angle change quantity.
In this embodiment of the present application, since the inertial measurement unit may further measure the observed angular velocity of the self-mobile device, the fused angular velocity at the previous time may be obtained according to the steps shown in fig. 5. Therefore, when the inertial measurement unit does not directly output the angle change amount, the product of the fusion angular velocity and the time interval at the last moment can be used as the angle change amount. Namely, the angle change amount between the last time and the current time can be calculated by the following formula (9):
wherein delta theta is the angle change amount between the previous moment and the current moment,
for the fusion angular velocity at the previous time, Δt is the time interval between the previous time and the current time.
According to the method and the device for obtaining the angle change quantity, when the inertial measurement unit does not output the angle change quantity, the obtained angle change quantity can be ensured by taking the product result of the fusion angular speed and the time interval at the previous moment as the angle change quantity, so that the observation course angle can be ensured to be obtained according to the angle change quantity, and finally the positioning of the self-mobile equipment is ensured to be realized.
In one embodiment of the present application, referring to fig. 6, fig. 6 is a flowchart of the steps for obtaining a fused heading angle at a previous time provided in the embodiment of the present application. As shown in fig. 6, the acquisition of the fusion heading angle at the previous time includes, but is not limited to, steps S610 to S640.
Step S610, the predicted course angle of the previous moment is obtained.
Step S620, the observed course angle at the previous moment is obtained through the real-time differential positioning module and the inertial measurement unit.
In step S630, the angle prediction reliability at the previous time and the angle observation reliability at the previous time are determined.
Step S640, according to the angle prediction reliability of the previous moment and the angle observation reliability of the previous moment, carrying out fusion calculation on the predicted course angle of the previous moment and the observed course angle of the previous moment to obtain the fusion course angle of the previous moment.
The embodiment of the application firstly obtains the predicted course angle at the last moment
Then the real-time differential positioning module and the inertial measurement unit are used for obtaining the observed course angle theta at the previous moment k-1 The method comprises the steps of measuring first positioning information at the previous moment and second positioning information at the current moment according to the real-time differential positioning module, acquiring the angle change between the previous moment and the current moment according to the inertial measurement unit, and calculating the observed course angle theta at the previous moment according to the first positioning information, the second positioning information and the angle change k-1 . Then confirmThe angle prediction reliability at the previous time is +.>
And the angle observation reliability at the last time is +.>
Thus, the degree of reliability can be predicted from the angle of the last moment>
And the angle observation reliability of the last moment +.>
Predicted heading angle of the last moment +.>
And the observed heading angle theta at the previous moment k-1 Fusion calculation is carried out to obtain the fusion course angle +.>
The fused course angle at the last moment can be calculated by the following formula (10):
wherein,,
that is, the sum of the angle prediction reliability of the previous time and the angle observation reliability of the previous time is 1.
It should be noted that, the predicted heading angle at the previous time
It is calculated by equation (11). The formula (11) is: />
I.e. predicted heading angle at time k-1 +.>
It is calculated from the fusion heading angle at time k-2, the fusion angular velocity at time k-2, and the time interval between time k-1 and time k-2.
The angle prediction credibility corresponding to each time is not the same, and the angle observation credibility corresponding to each time is not the same. When the positioning accuracy of the real-time differential positioning module is poor, the angle observation reliability is reduced, and the predicted course angle can be more depended at the moment, namely the angle prediction reliability is improved.
Step S130, determining a time interval between the previous time and the current time.
In the embodiment of the present application, in order to obtain the predicted course angle at the current time, after the fused course angle at the previous time and the fused angular velocity at the previous time are obtained, the time interval between the previous time and the current time is also required to be obtained, so that the predicted course angle at the current time can be obtained by calculation according to the fused course angle at the previous time, the fused angular velocity at the previous time and the time interval.
Step S140, calculating to obtain the predicted course angle at the current moment according to the time interval, the fused course angle at the last moment and the fused angular velocity at the last moment.
According to the embodiment of the application, the predicted course angle at the current moment is calculated through the formula (12), and the formula (12) is as follows:
in the formula (12) of the present invention,
representing the predicted heading angle at the current time, +.>
Indicating the last momentFused course angle->
Indicating the fusion angular velocity at the previous moment and Δt the time interval between the previous moment and the current moment.
And step S150, determining the prediction reliability and the observation reliability of the current moment, wherein the observation reliability is adjusted according to the positioning precision of the real-time differential positioning module.
In the embodiment of the present application, after the observation course angle at the current time and the predicted course angle at the current time are obtained, the prediction reliability and the observation reliability at the current time need to be further determined. The observation course angle is obtained by calculation through the measurement result of the real-time differential positioning module, so that the corresponding observation reliability can be adjusted according to the positioning accuracy of the real-time differential positioning module. For example, it is detected that the observed course angle calculated by the real-time differential positioning module is greater than two angular jumps
And when the positioning accuracy of the real-time differential positioning module is poor, the positioning accuracy of the real-time differential positioning module can be determined. At this time, the observation reliability can be reduced, that is, the predicted course angle is more relied, so that the final fused course angle is more biased to the result of the predicted course angle, and the precision of the fused course angle can be improved.
Illustratively, when the positioning accuracy of the real-time differential positioning module is low, the observation reliability is adjusted to 0.1. At this time, since the sum of the prediction reliability and the observation reliability is 1, it is determined that the prediction reliability is 0.9. Therefore, after the observation reliability is adjusted according to the positioning accuracy of the real-time differential positioning module, the weights occupied by the predicted course angle and the observation course angle in the fusion process are actually determined.
According to the embodiment of the application, the observation reliability is adjusted according to the positioning accuracy of the real-time differential positioning module, and the accuracy of the observation reliability can be improved, so that the accuracy of the fused course angle is improved.
Step S160, according to the prediction reliability and the observation reliability of the current moment, the observation course angle of the current moment and the prediction course angle of the current moment are fused, and the fusion course angle of the current moment is obtained.
In the embodiment of the application, the fused course angle at the current moment is calculated by the formula (13), and the formula (13) is as follows:
in the formula (13) of the present invention,
fused course angle representing the current time, +.>
Representing the predicted heading angle at the current time, +.>
Representing the prediction reliability of the current moment, theta k Represents the current observation course angle, +.>
Indicating the observation confidence level at the current time.
According to the method and the device, according to the prediction reliability and the observation reliability of the current moment, the observation course angle of the current moment and the prediction course angle of the current moment are fused, and in fact, the measurement result of the real-time differential positioning module and the measurement result of the inertial measurement unit are combined, so that the problem that the real-time differential positioning module is deviated when the self-mobile device runs for a long time is solved, and the problem that the angle jump of the inertial measurement unit is large when the self-mobile device runs for a short time is solved.
In one embodiment of the present application, referring to fig. 7, fig. 7 is a flowchart of steps performed after obtaining an observed heading angle at a current time and a predicted heading angle at the current time provided by an embodiment of the present application. As shown in fig. 7, after the observed heading angle at the current time and the predicted heading angle at the current time are obtained, the heading angle correction method further includes, but is not limited to, steps S710 to S730.
Step S710, determining an angle difference value between the observed course angle at the current moment and the predicted course angle at the current moment;
step S720, if the angle difference value is larger than a preset angle threshold value, adding the preset angle value and the current observation course angle to obtain a target course angle;
step S730, taking the target course angle as the new observation course angle at the current moment, and executing the steps of fusing the observation course angle at the current moment and the prediction course angle at the current moment according to the prediction reliability and the observation reliability at the current moment to obtain the fused course angle at the current moment.
In this embodiment of the present application, after the observed course angle at the current time is obtained through step S110, and the predicted course angle at the current time is obtained through steps S120 to S140, the angle difference between the observed course angle at the current time and the predicted course angle at the current time may be further determined. If the angle difference value is larger than the preset angle threshold value, adding the preset angle value and the current observation course angle to obtain the target course angle. And then, after taking the target course angle as the new current observation course angle, executing step S160. That is, the method is implemented according to the prediction reliability and the observation reliability of the current moment, and the new observation course angle of the current moment and the prediction course angle of the current moment are fused to obtain the fused course angle of the current moment.
Illustratively, when the angle difference between the observed heading angle at the current time and the predicted heading angle at the current time is greater than pi, the observed heading angle at the current time is added with 2pi, or added with-2pi, to obtain the target heading angle. And taking the target course angle as a new current observation course angle to execute a subsequent fusion calculation process.
The embodiment of the application considers that the problem of angle jump occurs when the angle difference between the observed course angle at the current moment and the predicted course angle at the current moment is greater than a preset angle threshold, such as greater than pi. At this time, the direction of the observed course angle and the predicted course angle can be ensured to be a uniform direction by adding the preset angle value and the observed course angle at the current moment, for example, adding 2 pi or-2 pi to the observed course angle at the current moment.
Step S170, based on the fusion course angle of the current moment, correcting the observation course angle of the current moment.
In the embodiment of the present application, after the fused course angle at the current time is obtained, the observed course angle at the current time may be corrected according to the fused course angle at the current time. Because the fused course angle is obtained by fusing the observed course angle and the predicted course angle based on the observed reliability and the predicted reliability, the fused course angle is more accurate, and the accurate positioning of the self-mobile equipment can be realized.
In one embodiment of the present application, referring to fig. 8, fig. 8 is another flowchart of a heading angle correction method provided in an embodiment of the present application, which is applied to a self-mobile device. As shown in fig. 8, the course angle correction method includes, but is not limited to, steps S810 to S890.
Step S810, acquiring an observation course angle at the current moment through a real-time differential positioning module and an inertial measurement unit in the moving process of the self-mobile equipment;
step S820, obtaining the observation angular velocity at the current moment through an inertia measurement unit;
step S830, obtaining the fused course angle at the previous moment and the fused angular velocity at the previous moment;
step S840, determining the time interval between the previous time and the current time;
step S850, calculating to obtain a predicted course angle at the current moment according to the time interval, the fused course angle at the last moment and the fused angular velocity at the last moment;
step S860, taking the fusion angular velocity at the previous moment as the predicted angular velocity at the current moment;
step S870, determining the prediction reliability and the observation reliability at the current time; the observation reliability is adjusted according to the positioning precision of the real-time differential positioning module;
step S880, obtaining a fused course angle at the current moment according to the observed course angle at the current moment, the observed angular velocity at the current moment, the predicted course angle at the current moment, the predicted angular velocity at the current moment, the predicted reliability at the current moment and the observed reliability;
Step S890, based on the fused course angle at the current time, corrects the observed course angle at the current time.
In the moving process of the self-mobile equipment, the embodiment of the application can acquire the observed course angle theta at the current moment through the real-time differential positioning module and the inertial measurement unit k The observation angular velocity w at the current moment can be obtained by the inertial measurement unit k . The observation course angle theta at the current moment k And the observed angular velocity w at the current time k Constructing and obtaining an observed quantity matrix Y at the current moment k ，
The embodiment of the application acquires the fused course angle at the last moment
And the fusion angular velocity at the last moment +.>
And determines the time interval deltat between the last moment and the current moment. So that the fused course angle at the previous moment can be used according to the time interval deltat>
Fusion angular velocity at the last moment +.>
Calculating to obtain the predicted course angle at the current moment>
I.e. by the formula
The predicted course angle of the current moment can be calculated>
Then the fusion angular velocity at the last moment
Predicted angular velocity as the current time>
I.e. < ->
Predicted heading angle at the present time +.>
And the predicted angular velocity at the current moment +.>
Constructing a prediction quantity matrix of the current moment>
Thus, the observed quantity matrix Y at the current moment is constructed k And a prediction quantity matrix
Thereafter, the fusion is performed by substituting the fusion value into the formula (14), and the formula (14) is as follows:
in the formula (14) of the present invention,
and the fusion quantity matrix at the time k is represented and comprises a fusion course angle at the time k and a fusion angular velocity at the time k. Y is Y k And the observation matrix at the time k is represented, and comprises an observation course angle at the time k and an observation angular velocity at the time k. />
The prediction quantity matrix of the k moment is represented, and comprises a prediction course angle of the k moment and a prediction angular velocity of the k moment. K (K) k Representing the k moment prediction matrix->
And observed quantity matrix Y k A weighting coefficient matrix between them.
Wherein the weighting coefficient matrix K k The calculation can be performed by the formula (15):
K k ＝p k-1 (R k +p k-1 ) -1 (15)。
in formula (15), R k Reliability matrix representing observation course angle and observation angular velocity at k moment, p k-1 And the credibility covariance matrix of the course angle and the angular velocity at the moment k-1 is represented. Credibility covariance matrix p at time k-1 k-1 The calculation can be performed by the formula (16):
in the formula (16), I represents an identity matrix, K k-1 A weighting coefficient matrix representing the time of k-1, p k-2 A credibility covariance matrix representing heading angle and angular velocity at time k-2, R k-1 And the reliability matrix of the observation course angle and the observation angular velocity at the time k-1 is represented. Thereby obtaining the credibility covariance matrix of the course angle and the angular velocity at the moment k
From this, it can be seen from the equation (15) and the equation (16) that the prediction amount matrix at the k time is
And observed quantity matrix Y k A weighting coefficient matrix K between k The reliability covariance matrix p of the course angle and the angular velocity at the moment k-1 k-1 And a reliability matrix R of the observation course angle and the observation angular velocity at the time k k Determining the reliability covariance matrix of the heading angle and the angular velocity at the moment k-1, and utilizing the reliability covariance matrix p of the heading angle and the angular velocity at the moment k-2 k-2 Prediction matrix at time k-1 +.>
And observed quantity matrix Y k A weighting coefficient matrix K between k-1 And a reliability matrix R of the observation course angle and the observation angular velocity at the time k-1 k-1 And (5) determining. That is, in order to obtain the prediction quantity matrix +.>
And observed quantity matrix Y k A weighting coefficient matrix K between k The reliability covariance matrix p of the course angle and the angular velocity at the moment k-2 is needed to be obtained firstly k-2 Prediction matrix at time k-1 +.>
And observed quantity matrix Y k A weighting coefficient matrix K between k-1 Reliability matrix R of observation course angle and observation angular velocity at k-1 time k-1 And a reliability matrix R of the observation course angle and the observation angular velocity at the time k k Can only be according to p k-2 、K k-1 、R k-1 And R is k The 4 calculation results in K k 。
In the embodiment of the present application, since the predicted heading angle at the current time needs to be calculated by using the fused heading angle and the fused angular velocity at the previous time, the predicted angular velocity at the current time needs to be obtained by using the fused angular velocity at the previous time, thereby obtaining the predicted heading angle by using the formula
For observed quantity matrix Y k (including the observation course angle and the observation angular velocity) and the prediction quantity matrix +>
The fusion performed (including the predicted heading angle and the predicted angular velocity) is actually a constantly iterative fusion process. The previous fusion result is used for calculating the predicted quantity in the current fusion, the current fusion result is obtained by calculating the current predicted quantity obtained by calculation, the predicted quantity required by the next fusion is needed to be calculated by using the current fusion result, and the iteration is performed, so that the fusion result corresponding to each moment, namely a fusion quantity matrix (comprising a fusion course angle and a fusion angular velocity) can be output. The output fusion course angle can correct the observation course angle at the corresponding moment so as to accurately position the self-mobile equipment.
It should be noted that, since the current fusion amount needs to be calculated by the current predicted amount, and the current predicted amount needs to be calculated by the previous fusion amount, it is necessary to give an initial value to the fusion amount or the predicted amount, so that the subsequent continuous iteration process can be performed. Therefore, when the fusion starts, the embodiment of the application assigns an initial matrix to the fusion quantity matrix according to the calculated observed quantity matrix, so that the predicted quantity matrix can be calculated according to the initial matrix, and then the fusion step of continuous iteration can be executed.
According to the method and the device, the observation course angle of the current moment obtained through the real-time differential positioning module and the inertia measurement unit and the observation course angle of the current moment obtained through the inertia measurement unit are not accurate enough, so that the predicted course angle and the predicted course angle of the current moment are required to be further obtained, and then the obtained observation course angle, the obtained predicted course angle and the obtained predicted course angle are fused according to the prediction reliability and the obtained observation reliability, and the fused course angle with high accuracy can be obtained. Therefore, the observed course angle is corrected through fusing the course angle, and accurate positioning of the self-mobile device can be achieved.
Referring to fig. 9, fig. 9 is a schematic structural diagram of a self-mobile device according to an embodiment of the present application, where the self-mobile device includes:
the processor 901 may be implemented by a general purpose CPU (Central Processing Unit ), a microprocessor, an ASIC (Application Specific Integrated Circuit ), or one or more integrated circuits, etc. for executing relevant programs to implement the technical solutions provided by the embodiments of the present application;
the Memory 902 may be implemented in the form of a ROM (Read Only Memory), a static storage device, a dynamic storage device, or a RAM (Random Access Memory ). The memory 902 may store an operating system and other application programs, and when the technical solutions provided in the embodiments of the present application are implemented by software or firmware, relevant program codes are stored in the memory 902, and the processor 901 is used to invoke the course angle correction method to execute the embodiments of the present application;
An input/output interface 903 for inputting and outputting information;
the communication interface 904 is configured to implement communication interaction between the device and other devices, and may implement communication in a wired manner (e.g. USB, network cable, etc.), or may implement communication in a wireless manner (e.g. mobile network, WIFI, bluetooth, etc.);
a bus 905 that transfers information between the various components of the device (e.g., the processor 901, the memory 902, the input/output interface 903, and the communication interface 904);
wherein the processor 901, the memory 902, the input/output interface 903 and the communication interface 904 are communicatively coupled to each other within the device via a bus 905.
According to the self-moving device, the course angle can be automatically corrected, so that the self-moving device can effectively and accurately move, and the self-moving device can move according to a pre-planned path.
The embodiment of the application also provides a storage medium, which is a computer readable storage medium, and the storage medium stores a computer program, and the computer program realizes the course angle correction method when being executed by a processor.
The memory, as a non-transitory computer readable storage medium, may be used to store non-transitory software programs as well as non-transitory computer executable programs. In addition, the memory may include high-speed random access memory, and may also include non-transitory memory, such as at least one magnetic disk storage device, flash memory device, or other non-transitory solid state storage device. In some embodiments, the memory optionally includes memory remotely located relative to the processor, the remote memory being connectable to the processor through a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
The embodiments described in the embodiments of the present application are for more clearly describing the technical solutions of the embodiments of the present application, and do not constitute a limitation on the technical solutions provided by the embodiments of the present application, and as those skilled in the art can know that, with the evolution of technology and the appearance of new application scenarios, the technical solutions provided by the embodiments of the present application are equally applicable to similar technical problems.
Claims (10)
1. A heading angle correction method, characterized by being applied to a self-mobile device, the method comprising:
in the moving process of the self-mobile equipment, acquiring an observation course angle at the current moment through a real-time differential positioning module and an inertial measurement unit;
acquiring a fusion course angle at the previous moment and a fusion angular speed at the previous moment;
determining a time interval between the previous time and the current time;
calculating to obtain a predicted course angle at the current moment according to the time interval, the fused course angle at the last moment and the fused angular speed at the last moment;
determining prediction credibility and observation credibility of the current moment, wherein the observation credibility is adjusted according to the positioning precision of the real-time differential positioning module;
According to the prediction credibility and the observation credibility of the current moment, fusing the observation course angle of the current moment and the prediction course angle of the current moment to obtain a fused course angle of the current moment;
and correcting the observed course angle at the current moment based on the fused course angle at the current moment.
2. The method of claim 1, wherein obtaining the last instant of fused heading angle comprises:
acquiring a predicted course angle of the previous moment;
acquiring an observed course angle at the previous moment through the real-time differential positioning module and the inertial measurement unit;
determining the angle prediction credibility of the last moment and the angle observation credibility of the last moment;
and carrying out fusion calculation on the predicted course angle at the previous moment and the observed course angle at the previous moment according to the angle prediction reliability at the previous moment and the angle observation reliability at the previous moment to obtain the fusion course angle at the previous moment.
3. The method of claim 1, wherein obtaining the last instant of fusion angular velocity comprises:
acquiring the observation angular velocity at the previous moment through the inertial measurement unit;
Determining the predicted angular velocity of the previous moment, wherein the predicted angular velocity of the previous moment is the fusion angular velocity corresponding to the previous moment of the previous moment;
determining the angular velocity prediction credibility and the angular velocity observation credibility of the last moment;
and according to the angular velocity prediction reliability and the angular velocity observation reliability of the previous moment, fusing the predicted angular velocity of the previous moment and the observed angular velocity of the previous moment to obtain the fused angular velocity of the previous moment.
4. The method according to claim 1, wherein the obtaining, by the real-time differential positioning module and the inertial measurement unit, the observed heading angle at the current time includes:
acquiring the first positioning information of the previous moment and the second positioning information of the current moment through the real-time differential positioning module;
acquiring the angle change quantity between the previous moment and the current moment through the inertia measurement unit;
and obtaining the observed course angle at the current moment according to the first positioning information, the second positioning information and the angle variation.
5. The method of claim 4, wherein the obtaining the current observed heading angle according to the first positioning information, the second positioning information, and the angle change amount comprises:
According to the first positioning information and the second positioning information, respectively determining a first coordinate variation corresponding to a first coordinate axis and a second coordinate variation corresponding to a second coordinate axis;
obtaining an observed course angle at the previous moment according to the first coordinate variation, the second coordinate variation, the angle variation and a preset inverse trigonometric function formula;
and obtaining the observed course angle at the current moment according to the observed course angle at the previous moment and the angle variation.
6. The method of claim 5, wherein the method further comprises:
when the angle change amount is smaller than or equal to a preset angle threshold value, determining that the moving state of the self-moving equipment is a straight running state;
and when the angle change amount is larger than a preset angle threshold value, determining that the moving state of the self-moving equipment is a turning driving state.
7. The method according to any one of claims 1-6, further comprising:
and when the inertial measurement unit is detected to not output the angle change quantity, taking the product result of the fusion angular speed at the last moment and the time interval as the angle change quantity.
8. The method according to any one of claims 1-6, further comprising:
determining an angle difference value between the observed course angle at the current moment and the predicted course angle at the current moment;
if the angle difference value is larger than a preset angle threshold value, adding the preset angle value and the current observation course angle to obtain a target course angle;
and taking the target course angle as a new observation course angle at the current moment, executing the step of fusing the observation course angle at the current moment and the prediction course angle at the current moment according to the prediction reliability and the observation reliability at the current moment to obtain a fused course angle at the current moment.
9. A self-mobile device, characterized in that it comprises a memory storing a computer program and a processor implementing the method according to any of claims 1 to 8 when executing the computer program.
10. A computer readable storage medium storing a computer program, characterized in that the computer program, when executed by a processor, implements the method of any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310370107.4A CN116358603A (en) | 2023-03-31 | 2023-03-31 | Course angle correction method, self-mobile device and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310370107.4A CN116358603A (en) | 2023-03-31 | 2023-03-31 | Course angle correction method, self-mobile device and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116358603A true CN116358603A (en) | 2023-06-30 |
Family
ID=86904624
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310370107.4A Pending CN116358603A (en) | 2023-03-31 | 2023-03-31 | Course angle correction method, self-mobile device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116358603A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117053803A (en) * | 2023-10-10 | 2023-11-14 | 腾讯科技（深圳）有限公司 | Course information processing method, device, equipment and computer readable storage medium |
| CN117818754A (en) * | 2024-03-04 | 2024-04-05 | 辅易航智能科技(苏州)有限公司 | Course angle correction method and device, storage medium and electronic equipment |
-
2023
- 2023-03-31 CN CN202310370107.4A patent/CN116358603A/en active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117053803A (en) * | 2023-10-10 | 2023-11-14 | 腾讯科技（深圳）有限公司 | Course information processing method, device, equipment and computer readable storage medium |
| CN117053803B (en) * | 2023-10-10 | 2024-01-26 | 腾讯科技（深圳）有限公司 | Course information processing method, device, equipment and computer readable storage medium |
| CN117818754A (en) * | 2024-03-04 | 2024-04-05 | 辅易航智能科技(苏州)有限公司 | Course angle correction method and device, storage medium and electronic equipment |
| CN117818754B (en) * | 2024-03-04 | 2024-05-28 | 辅易航智能科技(苏州)有限公司 | Course angle correction method and device, storage medium and electronic equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116358603A (en) | Course angle correction method, self-mobile device and storage medium | |
| US10240931B2 (en) | System and method for navigation by applying corrected bias values to gyroscopic data | |
| KR20210072738A (en) | Vehicle positioning method, device, electronic equipment and computer storage medium | |
| CN105509738B (en) | Vehicle positioning orientation method based on inertial navigation/Doppler radar combination | |
| KR20200119920A (en) | Appratus and method for estimating the position of an automated valet parking system | |
| KR102086270B1 (en) | Control method and traveling control device of the traveling control device | |
| KR101704634B1 (en) | Apparatus and method for generating driving route of autonomous vehicle and method for controlling driving of autonomous vehicle | |
| US11118913B2 (en) | Vehicle positioning correction method and mobile device | |
| CN113155124B (en) | Multi-source auxiliary navigation method and device | |
| US20140358434A1 (en) | Peer-Assisted Dead Reckoning | |
| JP2014048228A (en) | Moving state calculation method and moving state calculation device | |
| KR20220024791A (en) | Method and apparatus for determining the trajectory of a vehicle | |
| CN105737853B (en) | A kind of drift calibration method of robot inertial navigation system | |
| KR102274502B1 (en) | Method for updating Navigation Map | |
| CN110361003B (en) | Information fusion method, apparatus, computer device and computer readable storage medium | |
| KR102109953B1 (en) | Apparatus and method for estimating position of vehicle using yawrate sensor | |
| KR101177374B1 (en) | Method for estimating position of vehicle using Interacting Multiple Model filter | |
| CN115290071A (en) | Relative positioning fusion method, device, equipment and storage medium | |
| KR20200119092A (en) | Vehicle and localization method thereof | |
| CN112859139B (en) | Gesture measurement method and device and electronic equipment | |
| US20230302904A1 (en) | Display device and display method | |
| JP7400922B2 (en) | Positioning device, positioning method and positioning program | |
| CN114509087B (en) | Positioning method, electronic device and computer storage medium | |
| TWI635302B (en) | Real-time precise positioning system of vehicle | |
| WO2017168588A1 (en) | Measurement device, measurement method, and program |
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 |