CN104501829B - Error correction method of inertial navigation system - Google Patents
Error correction method of inertial navigation system Download PDFInfo
- Publication number
- CN104501829B CN104501829B CN201410677883.XA CN201410677883A CN104501829B CN 104501829 B CN104501829 B CN 104501829B CN 201410677883 A CN201410677883 A CN 201410677883A CN 104501829 B CN104501829 B CN 104501829B
- Authority
- CN
- China
- Prior art keywords
- straight line
- robot
- point
- coordinate system
- laser
- 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
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
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention discloses an error correction method of an inertial navigation system. The method is used on a robot which at least has a motion control system, an inertial navigation system and a laser scanning sensor. The method comprises the following steps: 1) an overall coordinate system is arranged; 2) the robot continuously scans environmental information from the starting point; 3) filtering processing is carried out on scanning data points; 4) the scanning data points which have undergone filtering are divided into different categories; 5) each category undergoes linear feature identification by an IEP algorithm; 6) by least squares criterion, centralized fitting of straight line is realized from data points with linear features, and feature parameters are calculated; 7) two groups of linear feature parameters are extracted from laser scanning data obtained from the above step; 8) the two groups of linear feature parameters are transformed to the overall coordinate system, and whether two straight lines are from a same object is determined; and 9) when L1 and L2 are a same straight line in the above step, posture is corrected by the utilization of posture variation of the robot.
Description
Technical field
The invention belongs to localization for Mobile Robot technical field, more particularly to a kind of error correction side of inertial navigation system
Method.
Background technology
In the application of mobile robot, navigator fix is the key function of robot system.Navigation refers to mobile machine
People obtains environmental information and itself position and posture by sensor, realizes that autonomic movement is to destination in the environment of barrier.
The key of navigation and on condition that positioning, positioning are referred to and determine in real time robot itself position in the environment and attitude.
Environmental information of the robot localization technology relatively conventional at present mainly according to priori, with reference to current robot position
The information that confidence ceases and sensor is obtained determines robot position and posture.Location technology is broadly divided into absolute fix and relatively fixed
Position, relative localization change to determine current position and posture by the pose calculated relative to initial pose, common are mileage
Meter, inertial navigation system.Speedometer, inertial navigation system etc. long-time or large scale under cumulative error it is larger, positioning precision compared with
Difference, it is impossible to accurately, effortlessly provide posture information for robot.
The content of the invention
The invention aims to solve the above problems, there is provided a kind of that the boat position error of inertial navigation system can be entered
Row correction, reduces the error calibration method of error.
For this purpose, the technical scheme is that：A kind of error calibration method of inertial navigation system, at least has motion
Use in control system, inertial navigation system, the robot of scanning laser sensor；It is characterized in that：It comprises the following steps：
1）First under new circumstances not known, arrange starting point pose Z0, and as benchmark arrange global coordinate system, i.e., with
Start position is coordinate origin, is oriented y-axis forward direction with starting point motion, determines that x-axis is positive by right-hand rule；
2）Robot is moved from the off, and the contained laser sensor of robot continually scans for environmental information and obtains with laser
Sensor for origin angle, apart from polar coordinate point data；Often motion certain distance will process a laser scanning for robot
Data；The current boat position information of the continuous output device people of inertial navigation system；
3）Filtering Processing is carried out to mixed and disorderly irregular laser scanning data point, is removed outside effective computed range scope
Useless point；
4）The continuation property clustering processing that filtered data point is distributed according to which, is divided into different classifications；Method is
Adjacent two laser scanning point is calculated apart from nd_laser, is judged to if the distance is less than or equal to given threshold thr_cate same
Classification, same category represent continuous surface of these data points from barrier；Suitable threshold value thr_cate is set simultaneously, will
The less classification of data point is rejected to accelerate data processing speed；
5）Linear feature identification is carried out to each classification using IEP algorithms, composition straight line is found out in each category special
The data point set levied；Rational threshold value thr_cate is set, data point less straight line point set is rejected；
6）Straight line is fitted from the data point set of linear feature using criterion of least squares, and calculates its characteristic parameter；
Coordinate system centered on robot is overlapped with the coordinate system centered on laser sensor, and the parameter is in laser
Under sensor coordinate system, the vertical line and polar axis shaft angle theta, rectilinear end of straight line to initial point distance d, straight line and zero
Point coordinates p1 (x, y), p2 (x, y) and linearly dependent coefficient r；Laser sensor coordinate system is rectangular coordinate system, by what is collected
Polar coordinate system information d, theta is transformed under rectangular coordinate system；Wherein d, theta are linear equation parameter, using line segment end points
P1, p2 calculate line segment length and matching line segments；By the use of linearly dependent coefficient r as its linear feature similarity degree is differentiated, preferentially
Location Calculation is carried out using the high linear feature of linear coefficient；
7）Two groups of linear feature parameters are extracted in the laser scanning data obtained from above-mentioned steps；Extract laser scanning number
According to a certain straight line parameter L1 (r1, theta1) at middle T1 moment, this moment robot pose coordinate is Z1 (X1, Y1, THETA1)；
Extract laser scanning data in the T2 moment a certain straight line parameter L2 (r2, theta2), this moment robot pose be Z2 (X1,
Y1, THETA1)；
8）By step 7）Two groups of linear feature parameter transformations of middle extraction are determined according to discrimination standard under global coordinate system
Whether two straight lines are from same object；
The discrimination standard for adopting for：Two air line distances difference absolute value is less than given threshold lthr_d, and two straight line differential seat angles
Absolute value is less than given threshold lthr_theta, and two line correspondences end-point distances are less than given threshold lthr_pd；
9）L1 in above-mentioned steps, L2 be same straight line when, then the T1 moment to T2 moment robots pose variable quantity be
DeltaZ, corrects pose Z2=Z1+deltaZ using the pose variable quantity.
, based on the output pose of inertial navigation, the straight line obtained using laser scanning in natural environment is special for the present invention
Levy, by, on the basis of the characteristic parameter that the laser scanning data twice by same straight line is extracted, Extrapolation coordinate system transformation is closed
System, under laser scanner coordinate system, as between laser scanning, robot is the next in global coordinate system twice for the change of characteristic straight line pose
Appearance variable quantity, corrects inertial navigation error with this, creates high-precision environmental characteristic map.
Description of the drawings
It is described in further details below in conjunction with accompanying drawing and embodiments of the present invention.
The extraction of straight line flow chart of Fig. 1 present invention.
The error correction flow chart of Fig. 2 present invention.
Specific embodiment
Referring to accompanying drawing.The present embodiment at least has kinetic control system, inertial navigation system, scanning laser sensor
Use in robot；It comprises the following steps：
1）First under new circumstances not known, arrange starting point pose Z0, and as benchmark arrange global coordinate system, i.e., with
Start position is coordinate origin, is oriented y-axis forward direction with starting point motion, determines that x-axis is positive by right-hand rule；
2）Robot is moved from the off, and the contained laser sensor of robot continually scans for environmental information and obtains with laser
Sensor for origin angle, apart from polar coordinate point data；Often motion certain distance will process a laser scanning for robot
Data；
The current boat position information of the continuous output device people of inertial navigation system, constantly calculates current point using the boat position information
With last scan data processing position apart from delta_d, when the distance is more than or equal to threshold value delta_ of setting
Then carry out following laser scanning data to process during dthreshod, and by delta_d clear 0, restart to calculate from current point
delta_d；
3）It is as shown in Figure 1 that linear feature process is extracted from laser scanning data：To mixed and disorderly irregular laser scanning number
Strong point is filtered process, removes the useless point outside effective computed range scope；
4）The continuation property clustering processing that filtered data point is distributed according to which, is divided into different classifications；Method is
Adjacent two laser scanning point is calculated apart from nd_laser, is judged to if the distance is less than or equal to given threshold thr_cate same
Classification, same category represent continuous surface of these data points from barrier；Suitable threshold value thr_cate is set simultaneously, will
The less classification of data point is rejected to accelerate data processing speed；
5）Linear feature identification is carried out to each classification using IEP algorithms, composition straight line is found out in each category special
The data point set levied；Rational threshold value thr_cate is set, data point less straight line point set is rejected；
6）Straight line is fitted from the data point set of linear feature using criterion of least squares, and calculates its characteristic parameter；
Assume that coordinate system centered on robot is overlapped with the coordinate system centered on laser sensor, the parameter be
It is under laser sensor coordinate system, the vertical line and polar axis shaft angle theta of straight line to initial point distance d, straight line and zero, straight
Line end point coordinates p1 (x, y), p2 (x, y) and linearly dependent coefficient r；Laser sensor coordinate system is rectangular coordinate system, will collection
To polar coordinate system information d, theta be transformed under rectangular coordinate system；Wherein d, theta are linear equation parameter, using line segment
End points p1, p2 calculate line segment length and matching line segments；By the use of linearly dependent coefficient r as its linear feature similarity degree of differentiation,
The preferential linear feature high using linear coefficient carries out location Calculation；
7）Two groups of linear feature parameters are extracted in the laser scanning data obtained from above-mentioned steps；Extract laser scanning number
According to a certain straight line parameter L1 (r1, theta1) at middle T1 moment, this moment robot pose coordinate is Z1 (X1, Y1, THETA1)；
Extract laser scanning data in the T2 moment a certain straight line parameter L2 (r2, theta2), this moment robot pose be Z2 (X1,
Y1, THETA1)；
8）By step 7）Two groups of linear feature parameter transformations of middle extraction are determined according to discrimination standard under global coordinate system
Whether two straight lines are from same object；
The discrimination standard for adopting for：Two air line distances difference absolute value is less than given threshold lthr_d, and two straight line differential seat angles
Absolute value is less than given threshold lthr_theta, and two line correspondences end-point distances are less than given threshold lthr_pd；
9）L1 in above-mentioned steps, L2 be same straight line when, then the T1 moment to T2 moment robots pose variable quantity be
DeltaZ, corrects pose Z2=Z1+deltaZ using the pose variable quantity.
Error recovery procedure is illustrated in figure 2, wherein L1 represents the characteristic straight line of extraction under laser sensor coordinate system
Parameter, { L1 ... } represent characteristic straight line collection, and { L1 ' ... } is represented and straight line collection { L1 ... } transformed under global coordinate system,
{（L1’’,L2’’）... } represent by the line correspondence of { L1 ... } to { L2 ... } to mapping ensemblen；
In laser scanning twice, under identical characteristic straight line conversion global coordinate system, its characteristic parameter can have differences, main
Want reason and there is error, i.e. coordinate conversion relation for current pose caused by inertial navigation there is error, thus result in environment
Same straight line has different parameters under world coordinatess；Thus the characteristic parameter extracted with the laser scanning data twice of same straight line
On the basis of, Extrapolation coordinate system transformation relation, characteristic straight line pose change under laser scanner coordinate system are laser twice
Scanning room robot pose variable quantity under global coordinate system, corrects the position and posture of inertial navigation output with this；Wherein adopt
Whether nearest neighbor method is same straight line in differentiating different scanning data twice.
Claims (1)
1. a kind of error calibration method of inertial navigation system, at least has kinetic control system, inertial navigation system, laser
Use in the robot of scanning sensor；It is characterized in that：It comprises the following steps：
1）First under new circumstances not known, starting point pose Z0 is set, and global coordinate system is set as benchmark, i.e., with starting point
Position is coordinate origin, is oriented y-axis forward direction with starting point motion, determines that x-axis is positive by right-hand rule；
2）Robot is moved from the off, and the contained laser sensor of robot continually scans for environmental information and obtains with laser sensing
Device for origin angle, apart from polar coordinate point data；Often motion certain distance will process a laser scanning data for robot；
The current boat position information of the continuous output device people of inertial navigation system；
3）Filtering Processing is carried out to mixed and disorderly irregular laser scanning data point, the nothing outside effective computed range scope is removed
With point；
4）The continuation property clustering processing that filtered data point is distributed according to which, is divided into different classifications；Method is calculating
Adjacent two laser scanning point is judged to same class apart from nd_laser if the distance is less than or equal to given threshold thr_cate
Not, same category represents continuous surface of these data points from barrier；Suitable threshold value thr_cate is set simultaneously, by number
The less classification in strong point is rejected to accelerate data processing speed；
5）Linear feature identification is carried out to each classification using IEP algorithms, is found out in each category and is constituted linear feature
Data point set；Rational threshold value thr_cate is set, data point less straight line point set is rejected；
6）Straight line is fitted from the data point set of linear feature using criterion of least squares, and calculates its characteristic parameter；
Coordinate system centered on robot is overlapped with the coordinate system centered on laser sensor, and the parameter is in laser sensing
Under device coordinate system, the vertical line and polar axis shaft angle theta of straight line to initial point distance d, straight line and zero, straight line end points are sat
Mark p1 (x, y), p2 (x, y) and linearly dependent coefficient r；Laser sensor coordinate system is rectangular coordinate system, and the pole for collecting is sat
Mark system information d, theta is transformed under rectangular coordinate system；Wherein d, theta are linear equation parameter, using line segment end points p1, p2
Calculate line segment length and matching line segments；By the use of linearly dependent coefficient r as its linear feature similarity degree is differentiated, preferentially using line
The high linear feature of property coefficient carries out location Calculation；
7）Two groups of linear feature parameters are extracted in the laser scanning data obtained from above-mentioned steps；Extract in laser scanning data
A certain straight line parameter L1 (r1, theta1) at T1 moment, this moment robot pose coordinate are Z1 (X1, Y1, THETA1)；Extract
A certain straight line parameter L2 (r2, theta2) at T2 moment in laser scanning data, this moment robot pose be Z2 (X1, Y1,
THETA1)；
8）By step 7）According to discrimination standard, two groups of linear feature parameter transformations of middle extraction determine that two is straight under global coordinate system
Whether line is from same object；
The discrimination standard for adopting for：Two air line distances difference absolute value is less than given threshold lthr_d, and two straight line differential seat angles are absolute
Value is less than given threshold lthr_theta, and two line correspondences end-point distances are less than given threshold lthr_pd；
9）When L1 in above-mentioned steps, L2 are same straight line, then the T1 moment to T2 moment robots poses variable quantity be deltaZ, it is sharp
Pose Z2=Z1+deltaZ is corrected with the pose variable quantity.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201410677883.XA CN104501829B (en) | 2014-11-24 | 2014-11-24 | Error correction method of inertial navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201410677883.XA CN104501829B (en) | 2014-11-24 | 2014-11-24 | Error correction method of inertial navigation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104501829A CN104501829A (en) | 2015-04-08 |
| CN104501829B true CN104501829B (en) | 2017-04-12 |
Family
ID=52943255
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201410677883.XA Active CN104501829B (en) | 2014-11-24 | 2014-11-24 | Error correction method of inertial navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104501829B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106020188B (en) * | 2016-05-17 | 2018-10-30 | 杭州申昊科技股份有限公司 | A kind of Intelligent Mobile Robot recharging method based on laser navigation |
| CN107527382B (en) * | 2017-08-16 | 2020-11-03 | 北京京东尚科信息技术有限公司 | Data processing method and device |
| CN110895408B (en) * | 2018-08-22 | 2023-05-02 | 杭州海康机器人股份有限公司 | Autonomous positioning method and device and mobile robot |
| CN112256007A (en) * | 2019-07-03 | 2021-01-22 | 东元电机股份有限公司 | Auxiliary positioning system for reflective sticker |
| CN112215301B (en) * | 2020-10-29 | 2023-04-07 | 上海勘察设计研究院(集团)有限公司 | Image straight line detection method based on convolutional neural network |
| CN112444247B (en) * | 2020-11-19 | 2023-09-05 | 贵州北斗空间信息技术有限公司 | Indoor positioning method and system based on matrix transformation |
| CN112964251A (en) * | 2021-02-09 | 2021-06-15 | 中国计量大学 | Positioning method of inspection robot |
| CN114166219B (en) * | 2021-12-01 | 2024-06-14 | 珠海一微半导体股份有限公司 | Inertial navigation error correction method, chip and robot |
| CN115167392A (en) * | 2022-06-16 | 2022-10-11 | 珠海一微半导体股份有限公司 | Robot mapping method, chip and robot |
| CN116156445A (en) * | 2023-01-04 | 2023-05-23 | 西安电子科技大学 | Error correction method for multi-sensor network system under general sense integrated background |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101609149A (en) * | 2009-07-13 | 2009-12-23 | 北京航空航天大学 | A kind of method that improves attitude determination precision of airborne laser radar |
| JP2011164069A (en) * | 2010-02-15 | 2011-08-25 | Mitsubishi Electric Corp | Position correction system |
| CN103808316A (en) * | 2012-11-12 | 2014-05-21 | 哈尔滨恒誉名翔科技有限公司 | Indoor-flying intelligent body inertial system and laser range finder combination type navigation improving method |
-
2014
- 2014-11-24 CN CN201410677883.XA patent/CN104501829B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101609149A (en) * | 2009-07-13 | 2009-12-23 | 北京航空航天大学 | A kind of method that improves attitude determination precision of airborne laser radar |
| JP2011164069A (en) * | 2010-02-15 | 2011-08-25 | Mitsubishi Electric Corp | Position correction system |
| CN103808316A (en) * | 2012-11-12 | 2014-05-21 | 哈尔滨恒誉名翔科技有限公司 | Indoor-flying intelligent body inertial system and laser range finder combination type navigation improving method |
Non-Patent Citations (1)
| Title |
|---|
| 基于扫描匹配预处理的即时定位与地图创建;温安邦等;《计算机工程与应用》;20091231;第45卷(第33期);245-248 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104501829A (en) | 2015-04-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104501829B (en) | Error correction method of inertial navigation system | |
| CN104503449A (en) | Positioning method based on environment line features | |
| CN113436260B (en) | Mobile robot pose estimation method and system based on multi-sensor tight coupling | |
| CN102062587B (en) | Laser sensor-based method for determining poses of multiple mobile robots | |
| KR101782057B1 (en) | Apparatus for building map and method thereof | |
| CN104501811A (en) | Map matching method based on environmental linear features | |
| KR101739996B1 (en) | Moving robot and simultaneous localization and map-buliding method thereof | |
| CN109000649B (en) | Omni-directional mobile robot pose calibration method based on right-angle bend characteristics | |
| US9797981B2 (en) | Moving-object position/attitude estimation apparatus and moving-object position/attitude estimation method | |
| CN101441769A (en) | Real time vision positioning method of monocular camera | |
| CN105973265B (en) | A kind of mileage estimation method based on scanning laser sensor | |
| CN107923758B (en) | Vehicle location estimating device, vehicle location estimate method | |
| CN108226938A (en) | A kind of alignment system and method for AGV trolleies | |
| CN108647646A (en) | The optimizing detection method and device of low obstructions based on low harness radar | |
| CN107065887A (en) | Backward air navigation aid in omni-directional mobile robots passage | |
| KR20170088228A (en) | Map building system and its method based on multi-robot localization | |
| CN110243380A (en) | A kind of map-matching method based on multi-sensor data and angle character identification | |
| CN110827353B (en) | Robot positioning method based on monocular camera assistance | |
| CN1811644A (en) | Automatic positioning method for intelligent robot under complex environment | |
| CN108828518A (en) | A kind of boxcar inside carrier-and-stacker localization method | |
| CN112882000A (en) | Automatic calibration method of laser radar | |
| CN113188557B (en) | Visual inertial integrated navigation method integrating semantic features | |
| CN104501794A (en) | Map building method based on environmental linear features | |
| CN110370287B (en) | Subway train inspection robot path planning system and method based on visual guidance | |
| Qimin et al. | A methodology of vehicle speed estimation based on optical flow |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |