CN113063441B - 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
- CN113063441B CN113063441B CN202110282290.3A CN202110282290A CN113063441B CN 113063441 B CN113063441 B CN 113063441B CN 202110282290 A CN202110282290 A CN 202110282290A CN 113063441 B CN113063441 B CN 113063441B
- Authority
- CN
- China
- Prior art keywords
- robot
- information
- mileage
- gnss
- antenna
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manufacturing & Machinery (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a data source correction method for an accumulated calculation error of an odometer, which comprises the following steps of: 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 an accumulated calculation error 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 frame, 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 an 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 the robot to be measured.
The invention also aims to provide a device of the data source correction method based on the accumulated calculation 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 coefficient of the second posture 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 attitude information accords with the error grade judgment, correcting the data source by taking the second attitude information as the data source.
The first position information in step S1 includes first position information, first angle information, and first velocity information.
Step S2 (in), position output is provided for the second positioning system, and second position and attitude information is obtained through angle conversion with a 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 attitude information does not comprise the second angle information or the second angle information is lower than a set value because the confidence coefficient of the second position information is lower than the set value, calculating the second angle information of the current mobile carrier according to the current second attitude information, a plurality of groups of second attitude 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 t k-1 First GNSS position and t of time k Acquiring a GNSS positioning signal from a second GNSS position at the moment; at t, the robot to be tested k-1 The 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 determined to be (0, 0), and the mileage coordinate system adopts a coordinate system of a universal transverse ink card grid supporting system; will t k The direction angle of the robot to be measured in the mileage coordinate system at that moment is marked as psi k Will t k The center position of the robot to be measured in the mileage coordinate system at that moment is recorded as (x) k ,y k ) (ii) a Setting the directions of a coordinate system compatible with the robot to be tested to be forward in an x axis and left in a y axis, wherein the robot to be tested moves on a horizontal plane; antenna of robot to be measured at t k-1 Time t and k the positions of the universal transverse ink card grid supporting system obtained by the GNSS latitude and longitude value conversion of the moment are respectively recorded as utm k-1 And utm k Wherein t is k The universal lateral ink card grid system position at that moment includes a north component utmN k And east component utmE k (ii) a The position of the universal transverse ink card grid supporting system is converted by GNSS latitude and longitude values; thus, according to the antenna of the robot under test at t k-1 Time and t k The 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 t k-1 Time t k And (3) recording the angle of the mileage displacement of the antenna of the robot to be tested relative to the east direction as gamma:
γ＝atan2(utmN k -utmN k-1 ,utmE k -utmE k-1 )
will be alpha t Is set to t k Of robots to be measured at any momentCenter relative to t k-1 The direction angle increment of the center of the robot to be tested at the moment is as follows:
α t ＝atan2(y k -y k-1 ,x k -x k-1 )
D a is t k Antenna of robot to be measured at time t k-1 Displacement of the center of the robot to be measured at any moment:
wherein D is c For the center of the robot to be measured at t k-1 Time and t k Mileage shift, L, occurring between moments a The 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 D n With the robot to be tested at t k-1 The angle between the center and the antenna at the time:
wherein D is n For the antenna of the robot to be measured at t k-1 Time and t k Mileage shift, L, occurring between moments a Is the distance D between the center of the robot to be measured and the mounting position of the antenna of the robot a Is t k Antenna of robot to be measured at time t k-1 And the displacement of the center of the robot to be measured at any moment.
S4, specifically, when time series elements of the second position information in a time sliding window are judged to be consistent with a set confidence coefficient after denoising, comparing the first position information with the second position information, and judging whether an error level exists; judging whether an error grade exists or not, wherein when the distance difference between the first position information and the second position information and the difference between two horizontal coordinate components are larger than a preset threshold value, judging that the first position information has a 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 the mobile carrier 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 t k-1 At a time and any time t thereafter s At any moment, 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 testedThe universal transverse ink card grid system position;
utmN G ＝utmN A +sin(λ+ψ k )*L a
utmE G ＝utmE A +sin(λ+ψ t )*L a
wherein (utmN) G ,utmE G ) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measured A ,utmE A ) Is the calculated position of the universal transverse ink card grid supporting system of the antenna of the robot to be tested, L a The distance between the center of the robot to be measured and the installation position of the antenna of the robot is determined, and the included angle between the robot to be measured and the east direction when the lambda is the first GNSS position is determined, wherein the lambda = gamma-delta and psi k Is t k Direction angle psi of robot to be measured in mileage coordinate system at moment t Is t s The direction angle of the robot to be measured in the mileage coordinate system at the moment;
is (utmN) Gk-1 ,utmE Gk-1 ) Is t k-1 Universal transverse ink card grid system position at the center of the robot under test at the first GNSS position of time, then t s And 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:
p x ＝(utmE G -utmE Gk-1 )*cosλ+(utmN G -utmE Gk-1 )*sinλ
p y ＝(utmE G -utmE Gk-1 )*cosλ-(utmN G -utmE Gk-1 )*sinλ
wherein (utmN) G ,utmE G ) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measured A ,utmE A ) Is the calculated position of the universal transverse ink card grid supporting 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 = gamma-delta and the gamma is t k-1 Time t k The angle delta of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction is D n With the robot to be measured at t k-1 The 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 signals are 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 position and 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 t k-1 First GNSS position and t of time k The second GNSS position of the time instant obtains an accurate GNSS positioning signal. As shown in FIG. 2, A k-1 Is t k-1 Antenna position of robot to be measured at time, A k Is t k Antenna position G of robot to be measured at any time k-1 Is t k-1 Center position of robot to be measured at time, G k Is t k Center position of robot to be measured at time, D a Is t k Antenna of robot to be measured at time t k-1 Displacement of center of robot to be measured at any time, D c Centering the robot at t k-1 Time and t k Mileage shift, L, occurring between moments a Is the distance between the center of the robot to be tested and the mounting position of the antenna of the robot, D n For the antenna of the robot to be measured at t k-1 Time and t k The mileage shift between moments, gamma, is D n The distance of the antenna movement of the robot to be tested forms an angle with the east. At t, the robot to be tested k-1 And (3) the 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 set to be (0, 0), and the mileage coordinate system adopts a universal transverse ink card grid supporting system (UTM) coordinate system.
Will t k The direction angle of the robot to be measured at the moment in the mileage coordinate system is marked as psi k Let t be k The central position of the robot to be measured in the odometer coordinate system at the moment is recorded as (x) k ,y k ). Setting the compatible coordinate system directions of the robot to be tested as that the x axis is forward and the y axis is leftAnd the robot to be tested moves on the horizontal plane. The antenna of the robot to be tested is at t k-1 Time t and k the positions of the universal transverse ink card grid system obtained by the GNSS longitude and latitude value conversion of the moment are respectively recorded as utm k-1 And utm k Wherein t is k The universal lateral ink card grid system position at that moment includes a north component utmN k And east component utmE k (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 t k-1 Time and t k The 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 t k-1 Time t k And 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(utmN k -utmN k-1 ,utmE k -utmE k-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(utmN k -utmN k-1 ,utmE k -utmE k-1 )-π/2
Will be alpha t Is set to t k The center of the robot to be measured at the moment is opposite to t k-1 Direction angle increment of the center of the robot to be measured at the moment:
α t ＝atan2(y k -y k-1 ,x k -x k-1 )
D a is t k Antenna of robot to be measured at time t k-1 Displacement of the center of the robot to be measured at any moment:
wherein D is c For the center of the robot to be measured at t k-1 Time and t k Mileage shift, L, occurring between moments a The 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 D n With the robot to be tested at t k-1 Angle between the center and the antenna at the time:
wherein D is n For the antenna of the robot to be tested at t k-1 Time and t k Mileage shift, L, occurring between moments a Is the distance D between the center of the robot to be measured and the mounting position of the antenna of the robot a Is t k Antenna of robot to be measured at time t k-1 And 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 lambda = 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 comparing the second position information with the second position information meeting the confidence coefficient judgment criterion, once the first position information is judged to have obvious errors, providing the second position information to the wheel-type odometer unit by the mileage regulating unit, correcting the mileage data source by the wheel-type odometer unit, namely replacing the accumulated position data of the original wheel-type odometer by using the second position information as the current position value of the wheel-type odometer, and performing subsequent wheel-type odometer dead reckoning 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 orientation 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; when the first position information has an error range, the second position information is used.
Fig. 4 is a functional block diagram of a hardware apparatus 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 state sequence of the universal transverse ink card gridding supporting 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 mobile carrier corresponding to the antenna positioned on the first GNSS position in a mileage coordinate system; 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 t k-1 At any time t s And at the moment, obtaining the position of the universal transverse ink card grid supporting system at 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.
utmN G ＝utmN A +sin(λ+ψ k )*L a
utmE G ＝utmE A +sin(λ+ψ t )*L a
Wherein (utmN) G ,utmE G ) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measured A ,utmE A ) Is the calculated position, L of the universal transverse ink card grid supporting system of the antenna of the robot to be measured a The distance between the center of the robot to be measured and the installation position of the antenna of the robot is determined, and the included angle between the robot to be measured and the east-ward direction when the lambda is at the first GNSS position is determined, wherein the lambda = gamma-delta and psi k Is t k Direction angle psi of robot to be measured in mileage coordinate system at moment t Is t s The direction angle of the robot to be measured in the mileage coordinate system at the moment;
is (utmN) Gk-1 ,utmE Gk-1 ) Is t k-1 A universal transverse ink card grid system position at the center of the robot to be tested at the first GNSS position of time, then t s And 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 time:
p x ＝(utmE G -utmE Gk-1 )*cosλ+(utmN G -utmE Gk-1 )*sinλ
p y ＝(utmE G -utmE Gk-1 )*cosλ-(utmN G -utmE Gk-1 )*sinλ
wherein (utmN) G ,utmE G ) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measured A ,utmE A ) Is thatCalculating the position of the universal transverse ink card grid supporting system of the antenna of the robot to be tested, and the included angle between the robot to be tested and the east direction when the lambda is the first GNSS position, wherein the lambda = gamma-delta and the gamma is t k-1 Time t k The angle delta of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction is D n With the robot to be measured at t k-1 The 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 (8)
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; specifically, position output is provided for a second positioning system, and second position and attitude information is obtained through angle conversion with a 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; when the second attitude information does not include the second angle information or the second angle information is lower than a set value due to the confidence coefficient of the second position information, calculating the second angle information of the current mobile carrier according to the current second attitude information, a plurality of groups of second attitude information historical values in a set time window and interval time;
second angle information of the current moving carrier is calculated, specifically, the GNSS antenna is arranged at the middle position of the rear part of the robot to be tested, and the robot to be tested is arranged at t k-1 First GNSS position and t of time k Acquiring a GNSS positioning signal from a second GNSS position at the moment; at t, the robot to be measured k-1 The 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 determined to be (0, 0), and the mileage coordinate system adopts a coordinate system of a universal transverse ink card grid supporting system; will t k The direction angle of the robot to be measured at the moment in the mileage coordinate system is marked as psi k Let t be k The center position of the robot to be measured in the mileage coordinate system at that moment is recorded as (x) k ,y k ) (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 t k-1 Time t and k the positions of the universal transverse ink card grid system obtained by the GNSS longitude and latitude value conversion of the moment are respectively recorded as utm k-1 And utm k Wherein t is k Universal lateral ink card grid system position at time of day including northbound component utmN k And east component utmE k (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 t k-1 Time t and k the 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 t k-1 Time t k And (3) recording the angle of the mileage displacement of the antenna of the robot to be tested relative to the east direction as gamma:
γ＝atan2(utmN k -utmN k-1 ,utmE k -utmE k-1 )
will be alpha t Is set to t k The center of the robot to be measured at the moment is opposite to t k-1 Direction angle increment of the center of the robot to be measured at the moment:
α t ＝atan2(y k -y k-1 ,x k -x k-1 )
D a is t k Antenna of robot to be measured at time t k-1 Displacement of the center of the robot to be measured at any moment:
wherein D is c For the center of the robot to be measured at t k-1 Time and t k Mileage shift, L, occurring between moments a The 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 D n With the robot to be tested at t k-1 Angle between the center and the antenna at the time:
wherein D is n For the antenna of the robot to be tested at t k-1 Time and t k Mileage shift, L, occurring between moments a Is the distance D between the center of the robot to be measured and the mounting position of the antenna of the robot a Is t k Antenna of robot to be measured at time t k-1 Displacement of the center of the robot to be tested at any moment;
s3, judging the confidence coefficient of the second posture 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 attitude information accords with the error grade judgment, correcting the data source by taking the second attitude information as the data source.
2. The method for correcting data source of accumulated dead reckoning errors of an odometer according to claim 1, wherein the first position information of step S1 comprises first position information, first angle information and first velocity information.
3. The data source correcting method for the accumulated calculation error of the odometer according to claim 2, wherein step S4 is specifically to compare the first attitude information with the second attitude information to determine whether an error level exists when time series elements of the second attitude information in a time sliding window are determined to be consistent with a set confidence after denoising; 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.
4. A device for data source correction based on the odometer accumulated estimation error according to one of claims 1 to 3, characterized by 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.
5. The apparatus of claim 4, wherein the second positioning system comprises a WIFI indoor positioning based on location fingerprint and a GNSS global satellite positioning system.
6. The device according to claim 5, wherein the information of the GNSS global satellite positioning system is selected in advance from a driving range of the mobile carrier, wherein the area capable of receiving a plurality of positioning satellites is selected; 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 the mobile carrier pauses again to read a second GNSS position and state sequence of the satellite receiving antenna; and converting the position and state sequence of the universal transverse ink card gridding supporting 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 in the mileage coordinate system corresponding to the GNSS position.
7. The device according to claim 6, wherein the GNSS converted wheel-type mileage value under the mileage coordinate system corresponding to the GNSS position is calculated, specifically, the GNSS antenna is installed at the middle position of the rear part of the robot to be tested; at t k-1 At a time and any time t thereafter s At 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;
utmN G ＝utmN A +sin(λ+ψ k )*L a
utmE G ＝utmE A +sin(λ+ψ t )*L a
wherein (utmN) G ,utmE G ) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measured A ,utmE A ) Is the calculated position of the universal transverse ink card grid supporting system of the antenna of the robot to be tested, L a The distance between the center of the robot to be measured and the installation position of the antenna of the robot is lambda is a first GNSThe included angle between the robot to be measured and the east-righting direction is formed when the robot is at the S position, wherein lambda = gamma-delta and psi k Is t k Direction angle psi of robot to be measured in mileage coordinate system at moment t Is t s The direction angle of the robot to be measured in the mileage coordinate system at the moment;
is (utmN) Gk-1 ,utmE Gk-1 ) Is t k-1 A universal transverse ink card grid system position at the center of the robot to be tested at the first GNSS position of time, then t s And 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:
p x ＝(utmE G -utmE Gk-1 )*cosλ+(utmN G -utmE Gk-1 )*sinλ
p y ＝(utmE G -utmE Gk-1 )*cosλ-(utmN G -utmE Gk-1 )*sinλ
wherein (utmN) G ,utmE G ) Is the calculated position of the universal transverse ink card grid system (utmN) of the center of the robot to be measured A ,utmE A ) Is the calculated position of the universal transverse ink card grid supporting 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 = gamma-delta and the gamma is t k-1 Time t k The angle delta of the mileage displacement of the antenna of the robot to be measured relative to the east-righting direction is D n With the robot to be tested at t k-1 The angle between the center and the direction of the antenna at the moment.
8. The device according to claim 7, wherein the binocular vision inertia module is used for acquiring vision inertia mileage pose, specifically comprising closed loop detection and pose optimization, and performing road surface detection to determine the width of the 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.
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 CN113063441A (en) | 2021-07-02 |
| CN113063441B true 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) |
Families Citing this family (2)
| 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 |
| CN115220068A (en) * | 2022-07-05 | 2022-10-21 | 合肥中科智驰科技有限公司 | Method for carrying out confidence degree analysis on GNSS (global navigation satellite system) signals |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108942942A (en) * | 2018-08-16 | 2018-12-07 | 居鹤华 | A kind of multi-axis robot Inverse Kinematics and calculation method based on axis invariant |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105607071B (en) * | 2015-12-24 | 2018-06-08 | 百度在线网络技术（北京）有限公司 | A kind of indoor orientation method and device |
| CN107995885B (en) * | 2016-11-18 | 2021-02-26 | 深圳配天智能技术研究院有限公司 | Coordinate system calibration method, system and device |
| CN108873001B (en) * | 2018-09-17 | 2022-09-09 | 江苏金智科技股份有限公司 | Method for accurately judging positioning accuracy of robot |
| CN111982099B (en) * | 2019-05-21 | 2022-09-16 | 顺丰科技有限公司 | Robot hybrid positioning method, device, equipment and computer readable medium |
| CN111637877B (en) * | 2020-05-29 | 2022-06-21 | 拉扎斯网络科技（上海）有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
| CN112450820B (en) * | 2020-11-23 | 2022-01-21 | 深圳市银星智能科技股份有限公司 | Pose optimization method, mobile robot and storage medium |
-
2021
- 2021-03-16 CN CN202110282290.3A patent/CN113063441B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108942942A (en) * | 2018-08-16 | 2018-12-07 | 居鹤华 | A kind of multi-axis robot Inverse Kinematics and calculation method based on axis invariant |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113063441A (en) | 2021-07-02 |
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 | |
| US8775063B2 (en) | System and method of lane path estimation using sensor fusion | |
| US10234292B2 (en) | Positioning apparatus and global navigation satellite system, method of detecting satellite signals | |
| EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
| US7286933B2 (en) | GPS/dead-reckoning combination system and operating method thereof | |
| CN102508278B (en) | Adaptive filtering method based on observation noise covariance matrix estimation | |
| US20170097642A1 (en) | Apparatus for correcting vehicle location | |
| 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 | |
| CN107247275B (en) | Urban GNSS vulnerability monitoring system and method based on bus | |
| CN113063441B (en) | Data source correction method and device for accumulated calculation error of odometer | |
| CN108345021B (en) | Doppler radar assisted GPS/INS vehicle speed measurement method | |
| CN111025366B (en) | Grid SLAM navigation system and method based on INS and GNSS | |
| CN113670334B (en) | Initial alignment method and device for aerocar | |
| US10132915B2 (en) | System and method for integrated navigation with wireless dynamic online models | |
| CN105556244A (en) | Method for determining a current position of a motor vehicle in a geodetic coordinate system and motor vehicle | |
| CN109507706B (en) | GPS signal loss prediction positioning method | |
| 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 | |
| KR101141984B1 (en) | DR/GPS Data Fusion Method | |
| CN115046547A (en) | Automatic driving automobile positioning system and method under complex urban environment | |
| WO2022029878A1 (en) | Vehicle control device | |
| 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 |