CN113063441A - Data source correction method and device for accumulated calculation error of odometer - Google Patents
Data source correction method and device for accumulated calculation error of odometer Download PDFInfo
- Publication number
- CN113063441A CN113063441A CN202110282290.3A CN202110282290A CN113063441A CN 113063441 A CN113063441 A CN 113063441A CN 202110282290 A CN202110282290 A CN 202110282290A CN 113063441 A CN113063441 A CN 113063441A
- Authority
- CN
- China
- Prior art keywords
- robot
- information
- measured
- gnss
- mileage
- 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.)
- Granted
Links
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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Abstract
The invention discloses a data source correction method for accumulated calculation errors of an odometer, which comprises the following steps: acquiring first attitude information output by the odometer; acquiring second position and attitude information by adopting a second positioning system; performing confidence judgment on the second posture information; when the confidence coefficient of the second posture information meets the set confidence coefficient judgment criterion, judging the error level of the first posture information; and when the first position posture information has an error level, correcting the second position posture information as a data source. The invention also discloses a device of the data source correction method based on the accumulated calculation error of the odometer, which comprises an odometer adjusting unit and a second positioning system. The method utilizes the second positioning system to carry out auxiliary positioning, corrects the data source of the accumulated calculation error of the odometer, efficiently and quickly provides an accurate data source, and enables the odometer to more accurately measure the mileage passed by the mobile carrier.
Description
Technical Field
The invention particularly relates to a data source correction method and device for accumulated calculation errors of an odometer.
Background
The development of wheeled robots is now more and more automated, with higher and higher demands on the accuracy of position and mileage measurements. Nowadays, mobile carriers such as wheeled mobile robots and autonomous vehicles are generally equipped with a plurality of wheel speed sensors, wherein each wheel speed sensor is capable of performing wheel mileage estimation to obtain wheel mileage information. The wheeled mileage information includes an accumulated pose of the mobile carrier from the departure position and current carrier speed information. However, the odometer has a phenomenon of accumulating mileage errors due to wheel slip, changes in wheel diameter with wheel wear, and the like. If an angular error of the odometer occurs once, the angular error value causes the position deviation of the odometer to increase along with the increase of the moving distance along with the movement of the moving carrier, namely, the position error increases gradually in the direction of the angular error. In the prior art, algorithms such as extended Kalman filtering, unscented Kalman filtering and the like are generally adopted to perform fusion-related work on the wheel-type odometer and a plurality of inertia elements to obtain fused mileage, but the used mileage value is still an inaccurate value after the wheel-type odometer contains errors and is accumulated. The correlation algorithm of the traditional method adopts a multi-sensing fusion mileage algorithm of a factor graph optimization framework, and reduces the accumulated error of the pose of the mobile carrier by using a closed-loop detection and back-end optimization mode.
Therefore, in the prior art, when the wheel-type odometer is fused with the inertial element by using a filtering method and a multi-sensing data fusion algorithm based on factor graph optimization and the like, the correction of the accumulated error of the wheel-type odometer on a data source is lacked. The operation is complex, and an accurate data source cannot be provided.
Disclosure of Invention
One of the objectives of the present invention is to provide a data source correction method for an accumulated error of an odometer, which corrects the data source of the accumulated error of the odometer to accurately calculate the original movement mileage of a robot to be measured.
The invention also aims to provide a device of the data source correction method based on the accumulated estimation error of the odometer.
The invention provides a data source correction method for accumulated calculation errors of an odometer, which comprises the following steps:
s1, acquiring first attitude information output by a speedometer;
s2, acquiring second position and posture information by adopting a second positioning system;
s3, judging the confidence of the second position information;
s4, when the confidence coefficient of the second position posture information meets the set confidence coefficient judgment criterion, judging the error level of the first position posture information;
and S5, when the first position information accords with the error grade judgment, correcting the data source by taking the second position information as the data source.
The first position information of step S1 includes first position information, first angle information, and first velocity information.
Step S2, specifically, providing position output for the second positioning system, and obtaining second position and attitude information through angle conversion with the mileage coordinate system; the second position information comprises second position information, or the second position information comprises second angle information and second speed information; when the second posture information comprises second angle information and second speed information, directly outputting the second posture information; and when the second position information does not comprise the second angle information or the confidence coefficient of the second position information is lower than a set value, calculating the second angle information of the current mobile carrier according to the current second position information, a plurality of groups of second position information historical values in a set time window and interval time.
Calculating second angle information of the current mobile carrier, specifically, calculating the middle position of the GNSS antenna installed at the rear part of the robot to be tested at tk-1First GNSS position and t of timekAcquiring a GNSS positioning signal from a second GNSS position at the moment; at t, the robot to be measuredk-1The robot to be tested is located at a starting position at all times, the position of the robot to be tested in a mileage coordinate system is set to be (0,0), and the mileage coordinate system adopts a coordinate system of a universal transverse ink card grid supporting system; will tkThe direction angle of the robot to be measured at the moment in the mileage coordinate system is marked as psikWill tkThe center position of the robot to be measured in the mileage coordinate system at that moment is recorded as (x)k,yk) (ii) a Setting the directions of a coordinate system compatible with the robot to be tested to be forward in the x axis and left in the y axis, and enabling the robot to be tested to move on a horizontal plane; antenna of robot to be measured at tk-1Time t andkthe positions of the universal transverse ink card grid system obtained by the GNSS longitude and latitude value conversion of the moment are recorded as utm respectivelyk-1And utmkWherein t iskThe universal lateral ink card grid system position at that moment includes a north component utmNkAnd east component utmEk(ii) a The position of the universal horizontal ink card grid supporting system is converted by a GNSS longitude and latitude numerical value; thus, according to the antenna of the robot under test at tk-1Time t andkthe position of the moment in the coordinate system of the universal transverse ink card grid supporting system is calculated by adopting an inverse tangent method to calculate tk-1Time tkThe angle of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction at any moment is recorded as gamma:
γ＝atan2(utmNk-utmNk-1,utmEk-utmEk-1)
will be alphatIs set to tkThe center of the robot to be measured at the moment is opposite to tk-1Direction angle increment of the center of the robot to be measured at the moment:
αt＝atan2(yk-yk-1,xk-xk-1)
Dais tkAntenna of robot to be measured at time tk-1Displacement of the center of the robot to be measured at any moment:
wherein D iscFor the center of the robot to be measured at tk-1Time and tkMileage shift, L, occurring between momentsaThe distance between the center of the robot to be tested and the installation position of the antenna of the robot to be tested;
delta is DnWith the robot to be measured at tk-1Angle between the center and the antenna at the time:
wherein D isnFor the antenna of the robot to be measured at tk-1Time and tkMileage shift, L, occurring between momentsaIs the distance D between the center of the robot to be measured and the mounting position of the antenna of the robotaIs tkAntenna of robot to be measured at time tk-1And the displacement of the center of the robot to be measured at any moment.
Step S4, specifically, when the time sequence elements of the second position information in a time sliding window are judged to be in accordance with the set confidence after being denoised, comparing the first position information with the second position information, and judging whether an error level exists; judging whether the error grade exists or not, wherein when the distance difference between the first position information and the second position information and the difference between the two horizontal coordinate components are larger than a preset threshold value, judging that the first position information has the position error grade; and when the difference between the first angle information and the second angle information is larger than a preset threshold value, judging that the first position information has an angle error level.
The invention also discloses a device of the data source correction method based on the accumulated calculation error of the odometer, which comprises an odometer adjusting unit and a second positioning system; the mileage adjusting unit is connected with a second positioning system, and the second positioning system is used for outputting second position and posture information; the mileage adjusting unit receives the first position and posture information and the second position and posture information output by the external odometer by adopting a data source correction method based on the accumulated calculation error of the odometer and corrects the first position and posture information.
The second positioning system comprises WIFI indoor positioning based on location fingerprints, GNSS (global satellite positioning) information or other global positioning systems.
GNSS (global satellite positioning) information, specifically, a region capable of receiving a plurality of positioning satellites is selected in advance in a driving range of a mobile carrier; the mobile carrier starts to run from the area, when the mobile carrier runs into the area for the first time, the mobile carrier pauses for a period of time to read a first GNSS position and state sequence of the satellite receiving antenna, then the mobile carrier runs for a plurality of distances to the front of the mobile carrier in the area, and pauses again to read a second GNSS position and state sequence of the satellite receiving antenna; and converting the position and the state sequence of the universal transverse ink card gridding system obtained by converting the first GNSS position and state sequence and the second GNSS position and state sequence by combining the installation position parameters of the satellite receiving antenna on the mobile carrier and the mileage postures of the mobile carrier corresponding to the first GNSS position and the second GNSS position to obtain the direction angle of the mobile carrier corresponding to the antenna positioned on the first GNSS position in a mileage coordinate system, and calculating the GNSS conversion wheel type mileage value under the mileage coordinate system corresponding to the GNSS position.
Calculating a GNSS conversion wheel type mileage numerical value under a mileage coordinate system corresponding to the GNSS position, specifically, installing a GNSS antenna at the middle position of the rear part of the robot to be tested; at tk-1At a time and any time t thereaftersAt the moment, the position of the universal transverse ink card grid supporting system of the center of the robot to be tested is obtained by using the position of the universal transverse ink card grid supporting system of the antenna of the robot to be tested;
utmNG＝utmNA+sin(λ+ψk)*La
utmEG＝utmEA+sin(λ+ψt)*La
wherein (utmN)G,utmEG) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measuredA,utmEA) Is the calculated position, L of the universal transverse ink card grid supporting system of the antenna of the robot to be measuredaThe distance between the center of the robot to be measured and the installation position of the antenna of the robot and the included angle between the robot to be measured and the east direction when the lambda is the first GNSS position are shown, wherein the lambda is gamma-delta and psikIs tkDirection angle psi of robot to be measured in mileage coordinate system at momenttIs tsThe direction angle of the robot to be measured in the mileage coordinate system at the moment;
is (utmN)Gk-1,utmEGk-1) Is tk-1First GNSS position of timeThe position of the universal transverse ink card grid supporting system at the center of the robot to be tested is set, tsAnd obtaining the mileage position under a mileage coordinate system by the universal horizontal ink card grid supporting system position of the center of the robot to be tested at any moment:
px＝(utmEG-utmEGk-1)*cosλ+(utmNG-utmEGk-1)*sinλ
py＝(utmEG-utmEGk-1)*cosλ-(utmNG-utmEGk-1)*sinλ
wherein (utmN)G,utmEG) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measuredA,utmEA) Is the calculated position of the universal transverse ink card grid system of the antenna of the robot to be measured, and the included angle between the robot to be measured and the east direction when the lambda is the first GNSS position, wherein the lambda is gamma-delta and the gamma is tk-1Time tkThe angle delta of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction is DnWith the robot to be measured at tk-1The angle between the center and the direction of the antenna at the moment.
The binocular vision inertia module is used for acquiring vision inertia mileage poses, specifically comprises closed loop detection and pose optimization, and performs road surface detection to determine the width of a travelable road surface; according to the obtained external reference calibration of the vision inertia module and the mobile carrier chassis, converting the vision inertia mileage pose into a vision inertia pose in a mileage coordinate system; when the GNSS signal is shielded, the visual inertial pose is used as second pose information to provide second position information and second angle information.
According to the data source correction method and device for the accumulated calculation error of the odometer, the second positioning system is used for auxiliary positioning, the data source of the accumulated calculation error of the odometer is corrected, an accurate data source is efficiently and quickly provided, and the odometer can more accurately measure the mileage of a mobile carrier.
Drawings
FIG. 1 is a schematic flow diagram of the process of the present invention.
FIG. 2 is a schematic illustration of the location of the process of the present invention.
FIG. 3 is a logic diagram of the method of the present invention.
FIG. 4 is a functional block diagram of a hardware device according to the present invention.
Detailed Description
FIG. 1 is a schematic flow chart of the method of the present invention: the invention provides the system, which comprises the following steps:
s1, acquiring first attitude information output by a speedometer; the first position information comprises first position information, first angle information and first speed information. In this embodiment, a wheel odometer is preferred.
S2, acquiring second position and posture information by adopting a second positioning system; specifically, UWB, WIFI indoor positioning, GNSS/INS, GNSS-RTK or GNSS-PPP can be used as a second positioning system to provide position output, and angle conversion is carried out on the position output and a mileage coordinate system to obtain second position and attitude information; the second position information comprises second position information, or the second position information comprises second angle information and second speed information; when the second position and posture information comprises second angle information, directly outputting the second position and posture information; and when the second posture information does not comprise the second angle information or the second angle information has errors due to limited confidence of the second position information, calculating the second angle information of the current moving carrier according to the current reliable second posture information, the previous groups of second posture information with close time and the interval time.
Referring to fig. 2, which is a schematic position diagram of the method of the present invention, a GNSS (global navigation satellite system) antenna is installed at an intermediate position of the rear portion of a robot under test at tk-1First GNSS position and t of timekThe second GNSS position of the time instant obtains an accurate GNSS positioning signal. As shown in FIG. 2, Ak-1Is tk-1Antenna position of robot to be measured at time, AkIs tkAntenna position G of robot to be measured at any timek-1Is tk-1Center position of robot to be measured at time GkIs tkCenter position of robot to be measured at time, DaIs tkAntenna of robot to be measured at time tk-1Displacement of center of robot to be measured at any time, DcCentering the robot at tk-1Time and tkMileage shift, L, occurring between momentsaIs the distance D between the center of the robot to be measured and the mounting position of the antenna of the robotnFor the antenna of the robot to be measured at tk-1Time and tkThe mileage shift between moments, gamma, is DnThe distance of the antenna movement of the robot to be tested forms an angle with the east. At t, the robot to be measuredk-1The robot to be tested is located at a starting position at the moment, the position of the robot to be tested in a mileage coordinate system is determined as (0,0), and the mileage coordinate system adopts a universal transverse ink card grid supporting system (UTM) coordinate system.
Will tkThe direction angle of the robot to be measured at the moment in the mileage coordinate system is marked as psikWill tkThe center position of the robot to be measured in the mileage coordinate system at that moment is recorded as (x)k,yk). And setting the directions of the coordinate system compatible with the robot to be tested to be forward in the x axis and left in the y axis, wherein the robot to be tested moves on the horizontal plane. Antenna of robot to be measured at tk-1Time t andkthe positions of the universal transverse ink card grid system obtained by the GNSS longitude and latitude value conversion of the moment are recorded as utm respectivelyk-1And utmkWherein t iskThe universal lateral ink card grid system position at that moment includes a north component utmNkAnd east component utmEk(ii) a The position of the universal transverse ink card grid supporting system is converted by the GNSS longitude and latitude numerical value.
Thus, according to the antenna of the robot under test at tk-1Time t andkthe position of the moment in the coordinate system of the universal transverse ink card grid supporting system is calculated by adopting an inverse tangent method to calculate tk-1Time tkAnd recording the angle of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction as gamma.
γ＝atan2(utmNk-utmNk-1,utmEk-utmEk-1)
If the angle of the mileage displacement of the antenna of the robot to be measured relative to the true north direction is calculated, the method comprises the steps of
γ＝atan2(utmNk-utmNk-1,utmEk-utmEk-1)-π/2
Will be alphatIs set to tkThe center of the robot to be measured at the moment is opposite to tk-1Direction angle increment of the center of the robot to be measured at the moment:
αt＝atan2(yk-yk-1,xk-xk-1)
Dais tkAntenna of robot to be measured at time tk-1Displacement of the center of the robot to be measured at any moment:
wherein D iscFor the center of the robot to be measured at tk-1Time and tkMileage shift, L, occurring between momentsaThe distance between the center of the robot to be tested and the installation position of the antenna of the robot to be tested.
Delta is DnWith the robot to be measured at tk-1Angle between the center and the antenna at the time:
wherein D isnFor the antenna of the robot to be measured at tk-1Time and tkMileage shift, L, occurring between momentsaIs the distance D between the center of the robot to be measured and the mounting position of the antenna of the robotaIs tkAntenna of robot to be measured at time tk-1And the displacement of the center of the robot to be measured at any moment.
And calculating a GNSS conversion wheel type mileage value of a mileage coordinate system corresponding to the GNSS position by recording an included angle between the robot to be measured and the east direction as lambda when the first GNSS position is passed, wherein the lambda is gamma-delta.
S3, judging the confidence of the second position information; and calculating the confidence coefficient of the second position information, including the GNSS observation satellite number standard and the GNSS covariance standard or the GNSS fixed solution state standard in an outdoor occasion, and setting a judgment criterion for determining that the second position information is reliable enough.
S4, when the confidence coefficient of the second position posture information meets the set confidence coefficient judgment criterion, judging the error level of the first position posture information; when the time sequence elements of the second position information in a time sliding window all accord with the set confidence degree, comparing the first position information with the second position information, and judging whether an error level exists; judging whether the error grade exists or not, wherein when the distance difference between the first position information and the second position information and the difference between the two horizontal coordinate components are larger than a preset threshold value, judging that the first position information has the position error grade; and when the difference between the first angle information and the second angle information is larger than a preset threshold value, judging that the first position information has an angle error level. The time sliding window is set according to the running environment of the mobile carrier, for example, the time sliding window of the GNSS sensor data in an open environment can be larger; aiming at the condition that the high-rise buildings in the town block satellite signals, a plurality of places which are relatively far away from the high-rise buildings and have enough visible satellites are pre-selected in the running environment of the mobile carrier, and second position information of the corresponding places is pre-obtained. And deducing and calculating second angle information according to the second position information with similar time to easily generate a certain angle error, so that a time-series denoising method is used, confidence judgment of each second position information is combined, second position information elements with less obvious confidence are removed, and the confidence of the second angle information is provided.
And S5, when the error grade exists in the first position information, correcting by adopting the second position information. And when the first position information is judged to have obvious errors compared with the second position information meeting the confidence coefficient judgment criterion, the mileage adjusting unit provides the second position information to the wheel type odometer unit, the wheel type odometer unit corrects the mileage data source, namely, the second position information is used as the current position value of the wheel type odometer, the accumulated position data of the original wheel type odometer is replaced, and the subsequent wheel type odometer dead reckoning is carried out on the basis of the accumulated position data. When the wheel-type odometer unit corrects the mileage data source, if the second angle information in the second position information meets the confidence degree judgment, the first angle information in the wheel-type odometer data source is corrected according to the error value amplitude of the first angle information compared with the second angle information. The first position information is compared with the position corrected by the previous wheeled odometer data source, and the mileage adjusting unit is allowed to correct the wheeled odometer data source according to the second position information and the second angle information only when the displacement between the first position information and the previous wheeled odometer data source is greater than a specified threshold.
FIG. 3 is a logic diagram of the method of the present invention. The method comprises the following steps: acquiring first attitude information output by a wheel type odometer unit; acquiring second position and attitude information by using a second positioning sensor; the mileage adjusting unit receives the first position and posture information and the second position and posture information; performing confidence judgment on the second posture information; when the confidence coefficient of the second position posture information meets the judgment criterion, judging the error amplitude of the first position posture information; and when the error amplitude exists in the first position information, using the second position information.
Fig. 4 is a functional block diagram of a hardware device according to the present invention. The device of the data source correction method based on the odometer accumulated calculation error comprises an odometer adjusting unit and a second positioning system; the mileage adjusting unit is connected with a second positioning system, and the second positioning system is used for outputting second position and posture information; the mileage adjusting unit receives the first position and posture information and the second position and posture information output by the external odometer by adopting a data source correction method based on the accumulated calculation error of the odometer and corrects the first position and posture information.
The second positioning system may provide second position information based on the WIFI indoor positioning of the location fingerprint, and derive second angle information.
The second positioning system can adopt GNSS information, and specifically, a region which can receive more satellites and has reliable GNSS information is selected in advance in the driving range of the mobile carrier; the mobile carrier starts to run from the area, when the mobile carrier runs into the area for the first time, the mobile carrier pauses for a period of time to read a first GNSS position and state sequence of the satellite receiving antenna, then the mobile carrier runs for a distance to the front of the mobile carrier in the area, and pauses again to read a second GNSS position and state sequence of the satellite receiving antenna; converting the position and the state sequence of the universal transverse ink card gridding system obtained by converting the first GNSS position and state sequence and the second GNSS position and state sequence, combining the installation position parameters of the satellite receiving antenna on the mobile carrier, and converting the mileage postures of the mobile carrier corresponding to the first GNSS position and the second GNSS position to obtain the direction angle of the corresponding mobile carrier in a mileage coordinate system when the antenna is positioned on the first GNSS position; the travel distance may be no more than thirty meters. The optimal running distance is more than two meters so as to ensure that the conversion error of the GNSS measurement error of the antenna to the direction angle is less than a certain range; when the mobile carrier does not receive GNSS positioning information at the initial position, then when the mobile carrier first travels into the area where the GNSS information is reliable, the wheeled mileage data source of the mobile carrier should ensure the accuracy of the first position information and the first angle information.
Specifically, a GNSS antenna is arranged in the middle of the rear part of a robot to be tested; at tk-1At any time t after the momentsAnd at the moment, obtaining the position of the universal transverse ink card grid supporting system in the center of the robot to be tested by using the position of the universal transverse ink card grid supporting system of the antenna of the robot to be tested.
utmNG＝utmNA+sin(λ+ψk)*La
utmEG＝utmEA+sin(λ+ψt)*La
Wherein (utmN)G,utmEG) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measuredA,utmEA) Is the calculated position, L of the universal transverse ink card grid supporting system of the antenna of the robot to be measuredaThe distance between the center of the robot to be measured and the installation position of the antenna of the robot is defined, and the lambda is the first GNSS positionAngle with respect to the east direction, wherein λ - γ - δ, ψkIs tkDirection angle psi of robot to be measured in mileage coordinate system at momenttIs tsThe direction angle of the robot to be measured in the mileage coordinate system at the moment;
is (utmN)Gk-1,utmEGk-1) Is tk-1Universal transverse ink card grid system position at the center of the robot under test at the first GNSS position of time, then tsAnd obtaining the mileage position under a mileage coordinate system by the universal horizontal ink card grid supporting system position of the center of the robot to be tested at any moment:
px＝(utmEG-utmEGk-1)*cosλ+(utmNG-utmEGk-1)*sinλ
py＝(utmEG-utmEGk-1)*cosλ-(utmNG-utmEGk-1)*sinλ
wherein (utmN)G,utmEG) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measuredA,utmEA) Is the calculated position of the universal transverse ink card grid system of the antenna of the robot to be measured, and the included angle between the robot to be measured and the east direction when the lambda is the first GNSS position, wherein the lambda is gamma-delta and the gamma is tk-1Time tkThe angle delta of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction is DnWith the robot to be measured at tk-1The angle between the center and the direction of the antenna at the moment.
The second positioning system may further comprise a binocular vision inertial module; the binocular vision inertia module is used for acquiring vision inertia mileage poses, including closed loop detection and pose optimization, and performing road surface detection to determine the width of a travelable road surface; according to the obtained external reference calibration of the vision inertia module and the mobile carrier chassis, converting the vision inertia mileage pose into a vision inertia pose in a mileage coordinate system; when the GNSS signal is blocked by a nearby high-rise building and is unreliable, if the visual inertial pose in the operation environment of the mobile carrier is credible, the visual inertial pose serves as second pose information and provides second position information and second angle information.
In another embodiment, when a mounting position where magnetic field interference is stable can be found on the mobile carrier, the direction of the mobile carrier in the ENU (northeast sky) coordinates can be obtained using a magnetic compass, and further, the coordinate transformation method in navsat _ transform _ node can be used. The GNSS longitude and latitude coordinates are converted into numerical values in a mileage coordinate system through the coordinates of the universal transverse ink card grid supporting system.
The device of the data source correction method based on the odometer accumulated calculation error corrects the position data source of the wheel type odometer unit under the abnormal condition based on the second position information.
Claims (10)
1. A data source correction method for an accumulated calculation error of an odometer comprises the following steps:
s1, acquiring first attitude information output by a speedometer;
s2, acquiring second position and posture information by adopting a second positioning system;
s3, judging the confidence of the second position information;
s4, when the confidence coefficient of the second position posture information meets the set confidence coefficient judgment criterion, judging the error level of the first position posture information;
and S5, when the first position information accords with the error grade judgment, correcting the data source by taking the second position information as the data source.
2. The method for correcting data source of cumulative estimation error of odometer according to claim 1, wherein the first position information of step S1 includes first position information, first angle information and first velocity information.
3. The method for correcting the data source of the accumulated calculation error of the odometer according to claim 2, wherein in step S2, a position output is provided for the second positioning system, and the second position information is obtained by performing angle conversion with the odometer coordinate system; the second position information comprises second position information, or the second position information comprises second angle information and second speed information; when the second posture information comprises second angle information and second speed information, directly outputting the second posture information; and when the second position information does not comprise the second angle information or the confidence coefficient of the second position information is lower than a set value, calculating the second angle information of the current mobile carrier according to the current second position information, a plurality of groups of second position information historical values in a set time window and interval time.
4. The method for correcting the data source of the accumulated calculation error of the odometer according to claim 3, wherein the second angle information of the current mobile carrier is calculated, specifically, the GNSS antenna is installed at the middle position of the rear part of the robot to be tested, and the robot to be tested is at tk-1First GNSS position and t of timekAcquiring a GNSS positioning signal from a second GNSS position at the moment; at t, the robot to be measuredk-1The robot to be tested is located at a starting position at all times, the position of the robot to be tested in a mileage coordinate system is set to be (0,0), and the mileage coordinate system adopts a coordinate system of a universal transverse ink card grid supporting system; will tkThe direction angle of the robot to be measured at the moment in the mileage coordinate system is marked as psikWill tkThe center position of the robot to be measured in the mileage coordinate system at that moment is recorded as (x)k,yk) (ii) a Setting the directions of a coordinate system compatible with the robot to be tested to be forward in the x axis and left in the y axis, and enabling the robot to be tested to move on a horizontal plane; antenna of robot to be measured at tk-1Time t andkthe positions of the universal transverse ink card grid system obtained by the GNSS longitude and latitude value conversion of the moment are recorded as utm respectivelyk-1And utmkWherein t iskThe universal lateral ink card grid system position at that moment includes a north component utmNkAnd east component utmEk(ii) a The position of the universal horizontal ink card grid supporting system is converted by a GNSS longitude and latitude numerical value; thus, according to the antenna of the robot under test at tk-1Time t andkthe position of the moment in the coordinate system of the universal transverse ink card grid supporting system is calculated by adopting an inverse tangent method to calculate tk-1Time tkThe angle of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction at any moment is recorded as gamma:
γ＝atan2(utmNk-utmNk-1,utmEk-utmEk-1)
will be alphatIs set to tkThe center of the robot to be measured at the moment is opposite to tk-1Direction angle increment of the center of the robot to be measured at the moment:
αt＝atan2(yk-yk-1,xk-xk-1)
Dais tkAntenna of robot to be measured at time tk-1Displacement of the center of the robot to be measured at any moment:
wherein D iscFor the center of the robot to be measured at tk-1Time and tkMileage shift, L, occurring between momentsaThe distance between the center of the robot to be tested and the installation position of the antenna of the robot to be tested;
delta is DnWith the robot to be measured at tk-1Angle between the center and the antenna at the time:
wherein D isnFor the antenna of the robot to be measured at tk-1Time and tkMileage shift, L, occurring between momentsaIs the distance D between the center of the robot to be measured and the mounting position of the antenna of the robotaIs tkAntenna of robot to be measured at time tk-1And the displacement of the center of the robot to be measured at any moment.
5. The method for correcting the data source of the accumulated calculation error of the odometer according to claim 4, wherein in step S4, when the time series elements of the second pose information in a time sliding window are denoised, the time series elements are determined to be in accordance with the set confidence level, and the first pose information is compared with the second pose information to determine whether an error level exists; judging whether the error grade exists or not, wherein when the distance difference between the first position information and the second position information and the difference between the two horizontal coordinate components are larger than a preset threshold value, judging that the first position information has the position error grade; and when the difference between the first angle information and the second angle information is larger than a preset threshold value, judging that the first position information has an angle error level.
6. An apparatus for correcting a data source based on the odometer accumulated dead reckoning error according to any one of claims 1 to 5, comprising an odometer adjusting unit and a second positioning system; the mileage adjusting unit is connected with a second positioning system, and the second positioning system is used for outputting second position and posture information; the mileage adjusting unit receives the first position and posture information and the second position and posture information output by the external odometer by adopting a data source correction method based on the accumulated calculation error of the odometer and corrects the first position and posture information.
7. The apparatus of claim 6, wherein the second positioning system comprises a WIFI indoor positioning based on location fingerprint and a GNSS global satellite positioning system.
8. The apparatus of claim 7, wherein the GNSS global satellite positioning system information is obtained by pre-selecting a region capable of receiving a plurality of positioning satellites in a driving range of the mobile carrier; the mobile carrier starts to run from the area, when the mobile carrier runs into the area for the first time, the mobile carrier pauses for a period of time to read a first GNSS position and state sequence of the satellite receiving antenna, then the mobile carrier runs for a plurality of distances to the front of the mobile carrier in the area, and pauses again to read a second GNSS position and state sequence of the satellite receiving antenna; and converting the position and the state sequence of the universal transverse ink card gridding system obtained by converting the first GNSS position and state sequence and the second GNSS position and state sequence by combining the installation position parameters of the satellite receiving antenna on the mobile carrier and the mileage postures of the mobile carrier corresponding to the first GNSS position and the second GNSS position to obtain the direction angle of the mobile carrier corresponding to the antenna positioned on the first GNSS position in a mileage coordinate system, and calculating the GNSS conversion wheel type mileage value under the mileage coordinate system corresponding to the GNSS position.
9. The device according to claim 8, wherein the GNSS converted wheel mileage value under the mileage coordinate system corresponding to the GNSS position is calculated, specifically, a GNSS antenna is installed at an intermediate position of the rear portion of the robot to be tested; at tk-1At a time and any time t thereaftersAt the moment, the position of the universal transverse ink card grid supporting system of the center of the robot to be tested is obtained by using the position of the universal transverse ink card grid supporting system of the antenna of the robot to be tested;
utmNG＝utmNA+sin(λ+ψk)*La
utmEG＝utmEA+sin(λ+ψt)*La
wherein (utmN)G,utmEG) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measuredA,utmEA) Is the calculated position, L of the universal transverse ink card grid supporting system of the antenna of the robot to be measuredaThe distance between the center of the robot to be measured and the installation position of the antenna of the robot and the included angle between the robot to be measured and the east direction when the lambda is the first GNSS position are shown, wherein the lambda is gamma-delta and psikIs tkDirection angle psi of robot to be measured in mileage coordinate system at momenttIs tsThe direction angle of the robot to be measured in the mileage coordinate system at the moment;
is (utmN)Gk-1,utmEGk-1) Is tk-1Universal transverse ink card grid system position at the center of the robot under test at the first GNSS position of time, then tsAnd obtaining the mileage position under a mileage coordinate system by the universal horizontal ink card grid supporting system position of the center of the robot to be tested at any moment:
px＝(utmEG-utmEGk-1)*cosλ+(utmNG-utmEGk-1)*sinλ
py＝(utmEG-utmEGk-1)*cosλ-(utmNG-utmEGk-1)*sinλ
wherein (utmN)G,utmEG) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measuredA,utmEA) Is the calculated position of the universal transverse ink card grid system of the antenna of the robot to be measured, and the included angle between the robot to be measured and the east direction when the lambda is the first GNSS position, wherein the lambda is gamma-delta and the gamma is tk-1Time tkThe angle delta of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction is DnWith the robot to be measured at tk-1The angle between the center and the direction of the antenna at the moment.
10. The device of claim 9, wherein the binocular vision inertial module is configured to obtain a vision inertial mileage pose, specifically including closed loop detection and pose optimization, and perform road surface detection to determine a width of a drivable road surface; according to the obtained external reference calibration of the vision inertia module and the mobile carrier chassis, converting the vision inertia mileage pose into a vision inertia pose in a mileage coordinate system; when the GNSS signal is shielded, the visual inertial pose is used as second pose information to provide second position information and second angle information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110282290.3A CN113063441B (en) | 2021-03-16 | 2021-03-16 | Data source correction method and device for accumulated calculation error of odometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110282290.3A CN113063441B (en) | 2021-03-16 | 2021-03-16 | Data source correction method and device for accumulated calculation error of odometer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113063441A true CN113063441A (en) | 2021-07-02 |
| CN113063441B CN113063441B (en) | 2022-11-18 |
Family
ID=76560673
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110282290.3A Active CN113063441B (en) | 2021-03-16 | 2021-03-16 | Data source correction method and device for accumulated calculation error of odometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113063441B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113884006A (en) * | 2021-09-27 | 2022-01-04 | 视辰信息科技(上海)有限公司 | Space positioning method, system, equipment and computer readable storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105607071A (en) * | 2015-12-24 | 2016-05-25 | 百度在线网络技术（北京）有限公司 | Indoor positioning method and device |
| WO2018090323A1 (en) * | 2016-11-18 | 2018-05-24 | 深圳配天智能技术研究院有限公司 | Method, system, and device for calibrating coordinate system |
| CN108873001A (en) * | 2018-09-17 | 2018-11-23 | 江苏金智科技股份有限公司 | A kind of accurate method for judging robot localization precision |
| CN108942942A (en) * | 2018-08-16 | 2018-12-07 | 居鹤华 | A kind of multi-axis robot Inverse Kinematics and calculation method based on axis invariant |
| CN111637877A (en) * | 2020-05-29 | 2020-09-08 | 拉扎斯网络科技（上海）有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
| CN111982099A (en) * | 2019-05-21 | 2020-11-24 | 顺丰科技有限公司 | Robot hybrid positioning method, device, equipment and computer readable medium |
| CN112450820A (en) * | 2020-11-23 | 2021-03-09 | 深圳市银星智能科技股份有限公司 | Pose optimization method, mobile robot and storage medium |
-
2021
- 2021-03-16 CN CN202110282290.3A patent/CN113063441B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105607071A (en) * | 2015-12-24 | 2016-05-25 | 百度在线网络技术（北京）有限公司 | Indoor positioning method and device |
| WO2018090323A1 (en) * | 2016-11-18 | 2018-05-24 | 深圳配天智能技术研究院有限公司 | Method, system, and device for calibrating coordinate system |
| CN108942942A (en) * | 2018-08-16 | 2018-12-07 | 居鹤华 | A kind of multi-axis robot Inverse Kinematics and calculation method based on axis invariant |
| CN108873001A (en) * | 2018-09-17 | 2018-11-23 | 江苏金智科技股份有限公司 | A kind of accurate method for judging robot localization precision |
| CN111982099A (en) * | 2019-05-21 | 2020-11-24 | 顺丰科技有限公司 | Robot hybrid positioning method, device, equipment and computer readable medium |
| CN111637877A (en) * | 2020-05-29 | 2020-09-08 | 拉扎斯网络科技（上海）有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
| CN112450820A (en) * | 2020-11-23 | 2021-03-09 | 深圳市银星智能科技股份有限公司 | Pose optimization method, mobile robot and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 闫铭 等: "基于虚拟特征点的视觉里程计运动物体干扰消除方法", 《计算机与数字工程》 * |
| 陈庆武等: "基于改进扩展卡尔曼滤波算法的移动机器人定位方法研究", 《测试技术学报》 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113884006A (en) * | 2021-09-27 | 2022-01-04 | 视辰信息科技(上海)有限公司 | Space positioning method, system, equipment and computer readable storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113063441B (en) | 2022-11-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4964047B2 (en) | Position detection apparatus and position detection method | |
| CN110779521A (en) | Multi-source fusion high-precision positioning method and device | |
| US10234292B2 (en) | Positioning apparatus and global navigation satellite system, method of detecting satellite signals | |
| US8775063B2 (en) | System and method of lane path estimation using sensor fusion | |
| EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
| US7286933B2 (en) | GPS/dead-reckoning combination system and operating method thereof | |
| CN100533066C (en) | Inertia compensation method used for earth-based vehicle GPS navigation | |
| CN101285686B (en) | Agricultural machines navigation hierarchical positioning process and system | |
| CN105652306A (en) | Dead reckoning-based low-cost Big Dipper and MEMS tight-coupling positioning system and method | |
| CN102508278A (en) | Adaptive filtering method based on observation noise covariance matrix estimation | |
| CN107247275B (en) | Urban GNSS vulnerability monitoring system and method based on bus | |
| CN111156994A (en) | INS/DR & GNSS loose integrated navigation method based on MEMS inertial component | |
| CN108345021B (en) | Doppler radar assisted GPS/INS vehicle speed measurement method | |
| CN111025366B (en) | Grid SLAM navigation system and method based on INS and GNSS | |
| US10132915B2 (en) | System and method for integrated navigation with wireless dynamic online models | |
| Mu et al. | A GNSS/INS-integrated system for an arbitrarily mounted land vehicle navigation device | |
| KR100948089B1 (en) | Method for deciding car position by pseudo dead reckoning and car navigation system using the same | |
| CN115060257B (en) | Vehicle lane change detection method based on civil-grade inertia measurement unit | |
| Meguro et al. | Low-cost lane-level positioning in urban area using optimized long time series GNSS and IMU data | |
| CN113063441B (en) | Data source correction method and device for accumulated calculation error of odometer | |
| CN113670334B (en) | Initial alignment method and device for aerocar | |
| KR101141984B1 (en) | DR/GPS Data Fusion Method | |
| KR20160056083A (en) | System and method for positioning | |
| KR20210009688A (en) | An integrated navigation system combining ins/gps/ultrasonic speedometer to overcome gps-denied area | |
| US10469982B2 (en) | System and method for enhanced integrated navigation with wireless angle of arrival |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |