CN108519615B - Mobile robot autonomous navigation method based on combined navigation and feature point matching - Google Patents

Mobile robot autonomous navigation method based on combined navigation and feature point matching Download PDF

Info

Publication number
CN108519615B
CN108519615B CN201810355206.4A CN201810355206A CN108519615B CN 108519615 B CN108519615 B CN 108519615B CN 201810355206 A CN201810355206 A CN 201810355206A CN 108519615 B CN108519615 B CN 108519615B
Authority
CN
China
Prior art keywords
mobile robot
information
equation
measurement
time
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
Application number
CN201810355206.4A
Other languages
Chinese (zh)
Other versions
CN108519615A (en
Inventor
蔡磊
秦晓晨
周广福
徐涛
张树静
刘艳昌
白林锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Henan Institute of Science and Technology
Original Assignee
Henan Institute of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Henan Institute of Science and Technology filed Critical Henan Institute of Science and Technology
Priority to CN201810355206.4A priority Critical patent/CN108519615B/en
Publication of CN108519615A publication Critical patent/CN108519615A/en
Application granted granted Critical
Publication of CN108519615B publication Critical patent/CN108519615B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

The invention discloses a mobile robot autonomous navigation method based on combined navigation and feature point matching, which comprises the following steps: s1, establishing a space map database; s2, planning a path; s3, acquiring the position information of the mobile robot; s4, performing Kalman filtering iteration; s5, the inertial navigation of the mobile robot is corrected. The autonomous navigation based on the combination navigation and the feature point matching is characterized in that scanning modeling of a laser radar is utilized to the surrounding environment, surrounding feature points are extracted, the position of the distance feature points is determined, the current position of the robot in a map is calculated according to the position of the feature points in the known map, and the position coordinates are determined so as to solve the problem that GPS signals of the inspection robot are shielded.

Description

Mobile robot autonomous navigation method based on combined navigation and feature point matching
Technical Field
The invention belongs to the field of self-positioning and navigation of robots, and particularly relates to an autonomous navigation method of a mobile robot based on combined navigation and feature point matching.
Background
The campus intelligent security patrol robot is a robot for safely patrolling in a campus specific environment, trees, tall buildings and the like exist due to the complex situation of the general campus, the current patrol robot adopts a GPS to navigate more, but the robot which mainly depends on the GPS to navigate is easy to block GPS signals, inaccurate in positioning and star loss, so that the robot cannot determine the position of the robot and cannot complete the navigation task.
There is a need to develop a method that can still locate its own position in the event of inaccurate and lost GPS signals. At present, the common solution is the fusion of GPS and inertial navigation technologies, which relies on inertial navigation to predict the position of the user in real time, and GPS can correct the error of inertial navigation. However, in the case where the GPS cannot accurately position for a long time, the error of the inertial navigation increases with time.
Disclosure of Invention
In view of the above-described deficiencies in the prior art, the present invention provides a mobile robot autonomous navigation method based on combined navigation and feature point matching.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows:
a mobile robot autonomous navigation method based on combined navigation and feature point matching comprises the following steps:
s1, establishing a space map database;
s2, planning a path;
obtaining a required map from a space map database, determining the specific position of a known characteristic point in the map, determining a starting point and an end point, and planning a path by using a Dstar algorithm;
and S3, acquiring the position information of the mobile robot:
s3.1, the mobile robot obtains longitude and latitude information of the GPS receiver;
the GPS receiver receives more than four satellite signals, and determines the longitude, latitude and height of the GPS receiver through the pseudo range from the GPS receiver to the satellite;
s3.2, converting the longitude and latitude information into local geographic coordinate system information, wherein the conversion formula is as follows:
Figure BDA0001634517100000021
Figure BDA0001634517100000022
e2＝2f-f2；
in the formula, L is the longitude of the current moment, B is the latitude of the current moment, H is the height of the current moment, X, Y, Z is the local geographic coordinate system of the current moment, N is the curvature radius of the prime circle of the target point, a is the long axis length of the earth, and f is the oblateness of the earth; e is the first eccentricity;
s3.3, acquiring pose information and position information of the mobile robot in a geographic coordinate system;
s3.3.1, obtaining pose information of the mobile robot;
the method comprises the steps that an accelerometer measures the motion acceleration of the mobile robot in an inertial reference system, time is integrated for the first time to obtain the instant speed information of the mobile robot relative to a local geographic coordinate system, then displacement information obtained after the instant speed information is integrated for the first time is converted into the local geographic coordinate system to obtain the position of the mobile robot in the local geographic coordinate system, and attitude information of the mobile robot is measured by combining a gyroscope;
s3.3.2, obtaining the position information of the mobile robot;
s3.3.2.1, the mobile robot scans the surrounding environment through the laser radar to obtain the distance rho between the scanning center of the laser radar and the nth target objectnAngle β of scanning direction of nth target objectnDistance ρnAnd angle betanForm a set of points (p)n，βn)；
S3.3.2.2, segmenting and aggregating the obtained point set to obtain measured characteristic point information;
s3.3.2.3, matching the extracted measured characteristic point information with the known characteristic point information in the map to obtain the position information (x) of the characteristic point in the mapp，yp)：
Figure BDA0001634517100000031
S3.3.2.4, obtaining the geographic coordinate system of the mobile robot according to the position information of the characteristic points, wherein the formula is as follows:
Figure BDA0001634517100000032
in the formula, xr，yr，θrRepresenting the position of the center of the mobile robot in a world coordinate system;
finishing to obtain:
Figure BDA0001634517100000033
s4, performing Kalman filtering iteration:
using position and speed state information measured by inertial navigation as state information of a Kalman filter, using an observed value of pose information obtained by a GPS (global positioning system) and an observed value of position information extracted by a characteristic point as measurement information of the Kalman filter, and performing iteration;
s4.1, constructing a system equation of the mobile robot;
the system equation comprises a state equation, a measurement equation and a prediction equation;
s4.1.1, constructing a state equation and a measurement equation,
the expression of the equation of state is:
dx(t)＝F(t)×x(t)+G(t)+W(t)；
wherein x (t) is an n-dimensional state vector of the mobile robot system; f (t) is the state transition matrix n multiplied by n; g (t) is a noise matrix; w (t) is a process noise vector;
the expression of the measurement equation is:
Z(t)＝H(t)×x(t)+V(t)；
wherein Z (t) is an m-dimensional measurement vector; h (t) is the m × n order of the measurement matrix; v (t) is a m-dimensional measurement noise vector;
s4.1.2, discretizing the state equation and the measurement equation respectively to obtain:
Figure BDA0001634517100000041
in the formula, XkAn n-dimensional state vector at a dimension k time; wkProcess noise at time k; zkThe measurement vector at the moment k is obtained; hkA measurement matrix at the time k; vk1A measurement noise vector received by the GPS; vk2Extracting a measurement noise vector for the feature points;
φk,k+1for the state transition matrix at the time from k to k +1, the calculation formula is:
Figure BDA0001634517100000042
Γxfor the process noise weighting matrix at time k, the calculation formula is:
Figure BDA0001634517100000043
in the formula, T is an iteration period;
s4.2, constructing a prediction equation
The prediction equation comprises a prediction value of a state vector
Figure BDA0001634517100000044
Covariance P of equation and state vector predictork,k+1The equation, the formula is as follows:
Figure BDA0001634517100000045
s4.3, constructing a correction equation
The correction equation includes an optimal estimate
Figure BDA0001634517100000046
Equation, filter gain Kk+1Equation and filtered value covariance Pk+1The equation is as follows:
Figure BDA0001634517100000051
and S4.4, iterating according to the steps S4.1-S4.3 to obtain accurate position information of the mobile robot.
S5, the inertial navigation of the mobile robot is corrected.
And setting a time range t according to the precision of the inertial navigation equipment, and performing zero clearing correction on the inertial navigation equipment by using the position information fused at the moment every t time periods, wherein the inertial navigation equipment takes the moment as a starting point and performs measurement again.
And the position information positioned by the GPS receiver and the position information obtained by matching and positioning the characteristic points may have outliers, and the outliers are processed.
Calculating a relatively real positioning point by adopting a polynomial fitting method for the acquired positioning point, substituting m continuous known normal positioning point coordinates Pi into a fitting polynomial, calculating a fitting coefficient according to a least square principle to obtain the fitting polynomial, extrapolating the positioning coordinate of the next point by using the polynomial, calculating a middle error sigma according to a fitted residual error, comparing the extrapolated positioning coordinate with an actual observation coordinate, and when the difference between the extrapolated positioning coordinate and the actual observation coordinate is less than 3 sigma, determining that the current observation value is not a outlier and is a normal observation value; when the difference between the two is more than or equal to 3 sigma, the current observation value is considered as a wild value to be removed, the extrapolated fitting value is adopted to replace the observation value of the current positioning point, one detection and correction are completed, then the earliest observation value is removed, the observation value of the next non-wild value is added, the process is repeated, and the positioning observation value is detected and corrected.
The autonomous navigation based on the combined navigation and the feature point matching utilizes the laser radar to scan and model the surrounding environment, extracts the surrounding feature points, determines the position of the distance feature points, and then deduces the current position of the robot in a map according to the position of the feature points in the known map so as to determine the position coordinates, thereby solving the problem that the GPS signal of the inspection robot is shielded. The invention adopts the combination of various positioning modes, can greatly improve the application range of the security patrol robot, and enables the patrol robot to carry out normal autonomous patrol in the environments of high buildings, trees and the like in schools, communities and the like which shield GPS signals. According to the invention, the positioning information is optimized by utilizing polynomial wild value elimination, some position information with large positioning errors is eliminated, and finally prediction is carried out by utilizing Kalman filtering, so that the positioning precision of the inspection robot is improved, the robot can safely work in regions with complex and changeable scenes, and the safety and reliability of the inspection robot are improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a flow chart of the present invention.
FIG. 2 is a flow chart of Kalman filtering in accordance with the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be obtained by a person skilled in the art without inventive effort based on the embodiments of the present invention, are within the scope of the present invention.
A mobile robot autonomous navigation method based on combined navigation and feature point matching is disclosed, as shown in FIG. 1, and comprises the following steps:
s1, establishing a space map database;
s2, planning a path;
obtaining a required map from a space map database, determining the specific position of a known characteristic point in the map, determining a starting point and an end point, and planning a path by using a Dstar algorithm;
and S3, acquiring the position information of the mobile robot:
s3.1, the mobile robot obtains longitude and latitude information of the GPS receiver;
the GPS receiver receives more than four satellite signals, and determines the longitude, latitude and height of the GPS receiver through the pseudo range from the GPS receiver to the satellite;
s3.2, converting the longitude and latitude information into local geographic coordinate system information, wherein the conversion formula is as follows:
Figure BDA0001634517100000071
Figure BDA0001634517100000072
e2＝2f-f2；
in the formula, L is the longitude of the current moment, B is the latitude of the current moment, H is the height of the current moment, X, Y, Z is the local geographic coordinate system of the current moment, N is the curvature radius of the prime circle of the target point, a is the long axis length of the earth, and f is the oblateness of the earth; e is the first eccentricity.
And (4) processing the outlier which possibly appears in the position information positioned by the GPS receiver.
Calculating a relatively real positioning point by adopting a polynomial fitting method for the acquired positioning point, substituting m continuous known normal positioning point coordinates Pi into a fitting polynomial, calculating a fitting coefficient according to a least square principle to obtain the fitting polynomial, extrapolating the positioning coordinate of the next point by using the polynomial, calculating a middle error sigma according to a fitted residual error, comparing the extrapolated positioning coordinate with an actual observation coordinate, and when the difference between the extrapolated positioning coordinate and the actual observation coordinate is less than 3 sigma, determining that the current observation value is not a outlier and is a normal observation value; when the difference between the two is more than or equal to 3 sigma, the current observation value is considered as a wild value to be removed, the extrapolated fitting value is adopted to replace the observation value of the current positioning point, one detection and correction are completed, then the earliest observation value is removed, the observation value of the next non-wild value is added, the process is repeated, and the positioning observation value is detected and corrected.
S3.3, acquiring pose information and position information of the mobile robot in a geographic coordinate system;
s3.3.1, obtaining pose information of the mobile robot;
the method comprises the steps that an accelerometer measures the motion acceleration of the mobile robot in an inertial reference system, time is integrated for the first time to obtain the instant speed information of the mobile robot relative to a local geographic coordinate system, then displacement information obtained after the instant speed information is integrated for the first time is converted into the local geographic coordinate system to obtain the position of the mobile robot in the local geographic coordinate system, and attitude information of the mobile robot is measured by combining a gyroscope;
s3.3.2, obtaining the position information of the mobile robot;
s3.3.2.1, the mobile robot scans the surrounding environment through the laser radar to obtain the distance rho between the scanning center of the laser radar and the nth target objectnAngle β of scanning direction of nth target objectnDistance ρnAnd angle betanForm a set of points (p)n，βn)；
S3.3.2.2, segmenting and aggregating the obtained point set to obtain measured characteristic point information;
s3.3.2.3, matching the extracted measured characteristic point information with the known characteristic point information in the map to obtain the position information (x) of the characteristic point in the mapp，yp)：
Figure BDA0001634517100000081
S3.3.2.4, obtaining the geographic coordinate system of the mobile robot according to the position information of the characteristic points, wherein the formula is as follows:
Figure BDA0001634517100000082
in the formula, xr，yr，θrRepresenting the position of the center of the mobile robot in a world coordinate system;
finishing to obtain:
Figure BDA0001634517100000083
and (4) the outlier possibly appears in the position information obtained by matching and positioning the characteristic points, and the outlier is processed. Calculating a relatively real positioning point by adopting a polynomial fitting method for the acquired positioning point, substituting m continuous known normal positioning point coordinates Pi into a fitting polynomial, calculating a fitting coefficient according to a least square principle to obtain the fitting polynomial, extrapolating the positioning coordinate of the next point by using the polynomial, calculating a middle error sigma according to a fitted residual error, comparing the extrapolated positioning coordinate with an actual observation coordinate, and when the difference between the extrapolated positioning coordinate and the actual observation coordinate is less than 3 sigma, determining that the current observation value is not a outlier and is a normal observation value; when the difference between the two is more than or equal to 3 sigma, the current observation value is considered as a wild value to be removed, the extrapolated fitting value is adopted to replace the observation value of the current positioning point, one detection and correction are completed, then the earliest observation value is removed, the observation value of the next non-wild value is added, the process is repeated, and the positioning observation value is detected and corrected.
S4, performing kalman filtering iteration, as shown in fig. 2;
using position and speed state information measured by inertial navigation as state information of a Kalman filter, using an observed value of pose information obtained by a GPS (global positioning system) and an observed value of position information extracted by a characteristic point as measurement information of the Kalman filter, and performing iteration;
s4.1, constructing a system equation of the mobile robot;
the system equation comprises a state equation, a measurement equation and a prediction equation;
s4.1.1, constructing a state equation and a measurement equation,
the expression of the equation of state is:
dx(t)＝F(t)×x(t)+G(t)+W(t)；
wherein x (t) is an n-dimensional state vector of the mobile robot system; f (t) is the state transition matrix n multiplied by n; g (t) is a noise matrix; w (t) is a process noise vector;
the expression of the measurement equation is:
Z(t)＝H(t)×x(t)+V(t)；
wherein Z (t) is an m-dimensional measurement vector; h (t) is the m × n order of the measurement matrix; v (t) is a m-dimensional measurement noise vector;
s4.1.2, discretizing the state equation and the measurement equation respectively to obtain:
Figure BDA0001634517100000091
in the formula, XkAn n-dimensional state vector at a dimension k time; wkProcess noise at time k; zkThe measurement vector at the moment k is obtained; hkA measurement matrix at the time k; vk1A measurement noise vector received by the GPS; vk2Extracting a measurement noise vector for the feature points;
φk,k+1for the state transition matrix at the time from k to k +1, the calculation formula is:
Figure BDA0001634517100000092
Γxfor the process noise weighting matrix at time k, the calculation formula is:
Figure BDA0001634517100000101
in the formula, T is an iteration period;
s4.2, constructing a prediction equation
The prediction equation comprises a prediction value of a state vector
Figure BDA0001634517100000102
Covariance P of equation and state vector predictork,k+1The equation, the formula is as follows:
Figure BDA0001634517100000103
s4.3, constructing a correction equation
The correction equation includes an optimal estimate
Figure BDA0001634517100000104
Equation, filter gain Kk+1Equation and filtered value covariance Pk+1The equation is as follows:
Figure BDA0001634517100000105
and S4.4, iterating according to the steps S4.1-S4.3 to obtain accurate position information of the mobile robot.
S5, the inertial navigation of the mobile robot is corrected.
And setting a time range t according to the precision of the inertial navigation equipment, and performing zero clearing correction on the inertial navigation equipment by using the position information fused at the moment every t time periods, wherein the inertial navigation equipment takes the moment as a starting point and performs measurement again.
The following explains the concept of the present invention
In a known environment, a starting point and an end point are determined, then a Dstar algorithm is used for planning a path, and autonomous navigation is carried out according to the planned path. When an obstacle is encountered in the operation process, the dynamic obstacle avoidance is carried out through the laser radar and the ultrasonic equipment carried by the robot, and the path planning is carried out again by taking the point as a starting point when the obstacle avoidance fails.
In the autonomous navigation process, the longitude and latitude information received by the GPS receiver is subjected to data processing and coordinate conversion and is converted into a geodetic coordinate system; fusing sensor equipment such as an accelerometer, a gyroscope and the like in the inertial navigation system to obtain position information and attitude information at the moment; and scanning the surrounding environment through a laser radar, extracting the characteristic points, matching with a spatial database, and determining the position information.
In the navigation process, the GPS and the feature point extraction have very large error fluctuation of measurement data when the GPS is interfered, and the feature point extraction fails to extract position information due to sensor noise or insufficient number of feature points, so that the positioning precision is influenced by preventing interference of related problems on a final positioning result, a threshold value is set, and the measurement information exceeding the threshold value is removed.
Respectively establishing a GPS, an INS, a system equation matched with the characteristic points, a state equation, a measurement equation and an error model, predicting the state of the robot at the next moment by using state information obtained by inertial navigation equipment, extracting the GPS and the characteristic points as measurement values to correct the inertial navigation prediction, inputting the measurement values and the GPS and the characteristic points into a Kalman filter, and obtaining accurate position information through iteration.
And correcting the inertial navigation by using the position information fused by filtering at regular time intervals according to the error range of the inertial navigation.
In the implementation, the required preliminary work is:
establishing a spatial map database: and according to the known map information, determining the specific position of the known characteristic point in the map, so as to calibrate the position information of the robot in the map.
And obtaining the longitude and latitude coordinates of the current position by using a GPS receiver, and then converting the longitude and latitude coordinates into a local geographic coordinate system. And giving the position information of the starting point and the end point, planning a path from the target point to the starting point by utilizing a Dstar backward search method, and finishing the path planning work.
After normal start-up, the required steps are:
the first step is as follows: GPS, inertial navigation and data preprocessing of feature point matching.
The GPS is a global positioning system, more than four satellite signals are received by a GPS receiver, and the longitude, the latitude and the height of the receiver are determined by the pseudo range from the receiver to the satellite. And converting the given longitude and latitude information into a local geographic coordinate system through coordinate system conversion.
Converting the coordinates of the GPS into longitude and latitude information and converting the longitude and latitude information into local geographic coordinate system information:
Figure BDA0001634517100000121
Figure BDA0001634517100000122
e2＝2f-f2；
l, B, H represents longitude, latitude, and altitude at that time. X, Y, Z is the local geographical coordinate system at that time. N is the curvature radius of the unitary-mortise ring of the target point; a long axis of the earth; f is the oblateness of the earth; e a first eccentricity.
2, a gyroscope and an accelerometer are arranged in the inertial navigation system, the motion acceleration of the carrier in an inertial reference system is measured through the accelerometer, the time is integrated for the first time to obtain the instant speed information of the carrier relative to the navigation coordinate system, then the time is integrated for the first time and converted into the navigation coordinate system, the position of the carrier in the navigation coordinate system can be obtained, and the attitude information of the carrier can be measured through the gyroscope.
And 3, scanning the surrounding environment by using the laser radar to scan the distance and the angle from the characteristic point, wherein the distance and the angle are expressed as (rho)n，βn)，ρnThe distance from the scanning center of the laser radar to the nth target object is obtained; beta is anIs the angle of the scanning direction of the nth target object, and n is the number of scanning points. Then, point set segmentation and aggregation are carried out, linear parameters are extracted, so that linear characteristics are extracted, characteristic point information is extracted, and the position (x) of the characteristic point in a robot coordinate system is obtainedp，yp). Then, the measured characteristic points are matched with the known characteristic point information in the map to determine the position information of the characteristic points in the map, and the position information (x) of the robot in the map is obtained through coordinate system conversionr,yr)。
In the characteristic point matching process, scanning the surrounding environment by using a laser radar, and scanning the distance and the angle from the characteristic point, wherein the distance and the angle are expressed as (rho)n，βn)，ρnThe distance from the scanning center of the laser radar to the nth target object is obtained; beta is anIs the angle of the scanning direction of the nth target object, and n is the number of scanning points. Then, point set segmentation and aggregation are carried out, linear parameters are extracted, so that linear features are extracted, environment information is extracted, then, feature point information in a map is matched, position information of feature points in a robot coordinate system is determined, and (x) is recordedp，yp) And then:
Figure BDA0001634517100000131
converting the robot coordinate system into a geographic coordinate system:
Figure BDA0001634517100000132
(xr,yr) And thetarRespectively expressed as the position of the robot center in the world coordinate system;
the position of the characteristic point in the geographic coordinate system is represented as:
Figure BDA0001634517100000133
the second step is that: outlier processing
For the outlier problem of the GPS receiver and the feature point extraction, a polynomial fitting method is adopted to calculate the position information. Substituting m continuous known normal positioning point coordinates (Pi, i is 1, 2, 3, …, 5) into a fitting polynomial, calculating a fitting coefficient according to the principle of least square to obtain the fitting polynomial, extrapolating the positioning coordinate of the next point by using the polynomial, comparing the extrapolated positioning coordinate with the actual observation coordinate according to the error sigma in residual calculation after fitting, and when the difference between the extrapolated positioning coordinate and the actual observation coordinate is less than 3 sigma, considering that the current observation value is not a wild value and is a normal observation value; and when the difference between the two is more than or equal to 3 sigma, the current observed value is considered as a wild value to be removed, and the extrapolated fitting value is adopted to replace the observed value of the current positioning point, so that one-time detection and correction are completed. Then removing the earliest observed value, adding the observed value of the next non-wild value, repeating the above process, and detecting and correcting the positioning observed value.
The third step: kalman filtering iteration
The inertial navigation determined position, and velocity state information are used as filtered state information. And (5) taking the observation values extracted by the GPS and the characteristic points as the measurement information of filtering, and performing iteration.
The Kalman filtering fusion process comprises the following steps:
in the known continuous system, the state equation and the measurement equation are as follows:
Figure BDA0001634517100000141
wherein x (t) is the n-dimensional state vector of the system; f (t) is the order of the state transition matrix (n × n); g (t) is a noise matrix; w (t) is a process noise vector; z (t) is an m-dimensional measurement vector; h (t) is the order of the measurement matrix (m × n); v (t) is an m-dimensional measurement noise vector.
Continuous system discretization:
discretizing the state equation and the measurement equation of the above formula to obtain:
Figure BDA0001634517100000142
in the formula, XkAn n-dimensional state vector at a dimension k time; phi is ak,k+1A state transition matrix at the moment from k to k + 1; gamma-shapedxA process noise weighting matrix at time k; wkProcess noise at time k; zkThe measurement vector at the moment k is obtained; hkA measurement matrix at the time k; vk1Vk2Measuring noise vectors extracted for the GPS and the characteristic points respectively;
φk,k+1and ΓkIs calculated by the following formula;
Figure BDA0001634517100000143
Figure BDA0001634517100000144
in the formula, T is an iteration period, and in actual calculation, the two formulas only take limited terms.
Time update (i.e. prediction equation)
The time update part comprises a prediction value of the state vector
Figure BDA0001634517100000145
Covariance P of equation and state vector predictork,k+1The equations, called prediction equations. It follows the dynamic change rule of the parameter X from the parameter value X at the moment kkPredicting parameter value X at time k +1k+1. Specifically, the formula is shown as follows:
Figure BDA0001634517100000146
measurement update (i.e. correction equation)
Figure BDA0001634517100000151
Through external observation information, the state vector can be predicted value
Figure BDA0001634517100000152
Obtaining an optimal filtered value of the vector based on
Figure BDA0001634517100000153
In the filter equation above, the optimal estimation
Figure BDA0001634517100000154
Equation, filter gain Kk+1Equation and filtered value covariance Pk+1The equations are collectively referred to as correction equations. It is based on observing certain parameters and values Zk+1And correcting the predicted value to give an optimal filtering value, and estimating a corresponding covariance matrix.
A fourth step of: correction for inertial navigation
And setting a time range t according to the precision x meters of the inertial navigation equipment, carrying out zero clearing correction by using the position information inertial navigation fused at the moment every t time periods, and carrying out measurement again by using the moment as a starting point by the inertial equipment. Therefore, the precision of inertial navigation is ensured, and the situation of error accumulation over time is prevented.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (3)

1. A mobile robot autonomous navigation method based on combined navigation and feature point matching is characterized by comprising the following steps:
s1, establishing a space map database;
s2, planning a path;
obtaining a required map from a space map database, determining the specific position of a known characteristic point in the map, determining a starting point and an end point, and planning a path by using a Dstar algorithm;
s3, acquiring the position information of the mobile robot;
in step S3, the specific steps are:
s3.1, the mobile robot obtains longitude and latitude information of the GPS receiver;
the GPS receiver receives more than four satellite signals, and determines the longitude, latitude and height of the GPS receiver through the pseudo range from the GPS receiver to the satellite;
s3.2, converting the longitude and latitude information into local geographic coordinate system information, wherein the conversion formula is as follows:
Figure FDA0003231400180000011
Figure FDA0003231400180000012
e2＝2f-f2；
in the formula, L is the longitude of the current moment, B is the latitude of the current moment, H is the height of the current moment, X, Y, Z is the local geographic coordinate system of the current moment, N is the curvature radius of the prime circle of the target point, a is the long axis length of the earth, and f is the oblateness of the earth; e is the first eccentricity;
s3.3, acquiring pose information and position information of the mobile robot in a geographic coordinate system;
s4, performing Kalman filtering iteration:
using position and speed state information measured by inertial navigation as state information of a Kalman filter, using an observed value of pose information obtained by a GPS (global positioning system) and an observed value of position information extracted by a characteristic point as measurement information of the Kalman filter, and performing iteration;
s5, correcting the inertial navigation of the mobile robot;
and setting a time range t according to the precision of the inertial navigation equipment, and performing zero clearing correction on the inertial navigation equipment by using the position information fused at the moment every t time periods, wherein the inertial navigation equipment takes the moment as a starting point and performs measurement again.
2. The mobile robot autonomous navigation method based on combined navigation and feature point matching according to claim 1, characterized in that in step S3.3, the specific steps are: s3.3.1, obtaining pose information of the mobile robot;
the method comprises the steps that an accelerometer measures the motion acceleration of the mobile robot in an inertial reference system, time is integrated for the first time to obtain the instant speed information of the mobile robot relative to a local geographic coordinate system, then displacement information obtained after the instant speed information is integrated for the first time is converted into the local geographic coordinate system to obtain the position of the mobile robot in the local geographic coordinate system, and attitude information of the mobile robot is measured by combining a gyroscope;
s3.3.2, obtaining the position information of the mobile robot;
s3.3.2.1, the mobile robot scans the surrounding environment through the laser radar to obtain the distance rho between the scanning center of the laser radar and the nth target objectnAngle β of scanning direction of nth target objectnDistance ρnAnd angle betanForm a set of points (p)n，βn)；
S3.3.2.2, segmenting and aggregating the obtained point set to obtain measured characteristic point information;
s3.3.2.3, matching the extracted measured characteristic point information with the known characteristic point information in the map to obtain the position information (x) of the characteristic point in the mapp,yp)：
Figure FDA0003231400180000021
S3.3.2.4, obtaining the geographic coordinate system of the mobile robot according to the position information of the characteristic points, wherein the formula is as follows:
Figure FDA0003231400180000022
in the formula, xr，yr，θrRepresenting the position of the center of the mobile robot in a world coordinate system;
finishing to obtain:
Figure FDA0003231400180000023
3. the mobile robot autonomous navigation method based on combined navigation and feature point matching according to claim 1, wherein in step S4, the specific steps are: s4.1, constructing a state equation and a measurement equation of the mobile robot,
the expression of the equation of state is:
dx(t)＝F(t)×x(t)+G(t)+W(t)；
wherein x (t) is an n-dimensional state vector of the mobile robot system; f (t) is the state transition matrix n multiplied by n; g (t) is a noise matrix; w (t) is a process noise vector;
the expression of the measurement equation is:
Z(t)＝H(t)×x(t)+V(t)；
wherein Z (t) is an m-dimensional measurement vector; h (t) is the m × n order of the measurement matrix; v (t) is a m-dimensional measurement noise vector;
s4.2, discretizing the state equation and the measurement equation respectively to obtain:
Figure FDA0003231400180000031
in the formula, XkAn n-dimensional state vector at a dimension k time; wkProcess noise at time k; zkThe measurement vector at the moment k is obtained; hkA measurement matrix at the time k; vk1A measurement noise vector received by the GPS; vk2Extracting a measurement noise vector for the feature points;
φk,k+1for the state transition matrix at the time from k to k +1, the calculation formula is:
Figure FDA0003231400180000032
Γxfor the process noise weighting matrix at time k, the calculation formula is:
Figure FDA0003231400180000033
in the formula, T is an iteration period;
s4.3, constructing a prediction equation
The prediction equation comprises a prediction value of a state vector
Figure FDA0003231400180000034
Covariance P of equation and state vector predictork,k+1The equation, the formula is as follows:
Figure FDA0003231400180000035
s4.4, constructing a correction equation
The correction equation includes an optimal estimate
Figure FDA0003231400180000036
Equation, filter gain Kk+1Equation and filtered value covariance Pk+1The equation is as follows:
Figure FDA0003231400180000037
and S4.5, iterating according to the steps S4.2-S4.4 to obtain accurate position information of the mobile robot.
CN201810355206.4A 2018-04-19 2018-04-19 Mobile robot autonomous navigation method based on combined navigation and feature point matching Active CN108519615B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810355206.4A CN108519615B (en) 2018-04-19 2018-04-19 Mobile robot autonomous navigation method based on combined navigation and feature point matching

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810355206.4A CN108519615B (en) 2018-04-19 2018-04-19 Mobile robot autonomous navigation method based on combined navigation and feature point matching

Publications (2)

Publication Number Publication Date
CN108519615A CN108519615A (en) 2018-09-11
CN108519615B true CN108519615B (en) 2021-11-26

Family

ID=63429725

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810355206.4A Active CN108519615B (en) 2018-04-19 2018-04-19 Mobile robot autonomous navigation method based on combined navigation and feature point matching

Country Status (1)

Country Link
CN (1) CN108519615B (en)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111137277A (en) * 2018-11-05 2020-05-12 陕西汽车集团有限责任公司 Method for setting automatic parking position of unmanned mining vehicle
CN109343013B (en) * 2018-11-20 2023-06-16 北京电子工程总体研究所 Spatial registration method and system based on restarting mechanism
CN109696908B (en) * 2019-01-18 2022-06-21 南方科技大学 Robot and flight path setting method and system thereof
CN109916431B (en) * 2019-04-12 2021-01-29 成都天富若博特科技有限责任公司 Wheel encoder calibration algorithm for four-wheel mobile robot
CN112179358B (en) * 2019-07-05 2022-12-20 东元电机股份有限公司 Map data comparison auxiliary positioning system and method thereof
CN112241016A (en) * 2019-07-19 2021-01-19 北京初速度科技有限公司 Method and device for determining geographic coordinates of parking map
CN110412987B (en) * 2019-08-21 2022-08-16 深圳市锐曼智能装备有限公司 Double-laser positioning navigation method and robot
CN110673148A (en) * 2019-10-25 2020-01-10 海鹰企业集团有限责任公司 Active sonar target real-time track resolving method
CN111123330B (en) * 2019-12-31 2022-03-15 上海摩勤智能技术有限公司 Positioning method and positioning system
CN111637894B (en) * 2020-04-28 2022-04-12 北京控制工程研究所 Navigation filtering method for constant coefficient landmark image
CN111679669B (en) * 2020-06-01 2023-08-08 陕西欧卡电子智能科技有限公司 Unmanned ship autonomous accurate berthing method and system
CN112083454A (en) * 2020-09-18 2020-12-15 北京卡路里信息技术有限公司 Trajectory deviation rectifying method, device, equipment and storage medium
CN112146660B (en) * 2020-09-25 2022-05-03 电子科技大学 Indoor map positioning method based on dynamic word vector
CN112965481A (en) * 2021-02-01 2021-06-15 成都信息工程大学 Orchard operation robot unmanned driving method based on point cloud map
CN113238247B (en) * 2021-03-30 2023-08-29 陈岳明 Laser radar-based robot positioning navigation method, device and equipment
CN113405560B (en) * 2021-05-28 2023-03-24 武汉理工大学 Unified modeling method for vehicle positioning and path planning
CN114237242B (en) * 2021-12-14 2024-02-23 北京云迹科技股份有限公司 Method and device for controlling robot based on optical encoder
CN114217622B (en) * 2021-12-16 2023-09-01 南京理工大学 BIM-based robot autonomous navigation method
CN114117113B (en) * 2022-01-28 2022-06-10 杭州宏景智驾科技有限公司 Multi-feature-point motor vehicle positioning method and device, electronic equipment and storage medium

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371840A (en) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 Method for combined navigation of inertia/visual odometer/laser radar
CN106840179A (en) * 2017-03-07 2017-06-13 中国科学院合肥物质科学研究院 A kind of intelligent vehicle localization method based on multi-sensor information fusion
EP3232221A1 (en) * 2016-04-12 2017-10-18 MAGNA STEYR Fahrzeugtechnik AG & Co KG Position determination system

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371840A (en) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 Method for combined navigation of inertia/visual odometer/laser radar
EP3232221A1 (en) * 2016-04-12 2017-10-18 MAGNA STEYR Fahrzeugtechnik AG & Co KG Position determination system
CN106840179A (en) * 2017-03-07 2017-06-13 中国科学院合肥物质科学研究院 A kind of intelligent vehicle localization method based on multi-sensor information fusion

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"激光雷达/微惯性室内自主建图与导航技术研究";廖自威 等;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20170315;全文 *

Also Published As

Publication number Publication date
CN108519615A (en) 2018-09-11

Similar Documents

Publication Publication Date Title
CN108519615B (en) Mobile robot autonomous navigation method based on combined navigation and feature point matching
Li et al. A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles
EP2133662B1 (en) Methods and system of navigation using terrain features
CN111795686B (en) Mobile robot positioning and mapping method
KR101663650B1 (en) Apparatus for reconizing location using range signal and method thereof
CN112639502A (en) Robot pose estimation
US11035915B2 (en) Method and system for magnetic fingerprinting
Ibrahim et al. Inertial measurement unit based indoor localization for construction applications
CN114166221B (en) Auxiliary transportation robot positioning method and system in dynamic complex mine environment
CN111947644B (en) Outdoor mobile robot positioning method and system and electronic equipment thereof
CN114526745A (en) Drawing establishing method and system for tightly-coupled laser radar and inertial odometer
Demim et al. Simultaneous localisation and mapping for autonomous underwater vehicle using a combined smooth variable structure filter and extended kalman filter
CN114396943A (en) Fusion positioning method and terminal
US11874116B2 (en) Range image aided inertial navigation system (INS) with map based localization
Verentsov et al. Bayesian localization for autonomous vehicle using sensor fusion and traffic signs
CN115930948A (en) Orchard robot fusion positioning method
CN116429090A (en) Synchronous positioning and mapping method and device based on line laser and mobile robot
CN112923934A (en) Laser SLAM technology suitable for combining inertial navigation in unstructured scene
Laftchiev et al. Terrain-based vehicle localization from real-time data using dynamical models
Emter et al. Stochastic cloning for robust fusion of multiple relative and absolute measurements
CN114915913A (en) UWB-IMU combined indoor positioning method based on sliding window factor graph
Emter et al. Stochastic cloning and smoothing for fusion of multiple relative and absolute measurements for localization and mapping
Lobo et al. Localization and tracking of indoor mobile robot with beacons and dead reckoning sensors
Fernández et al. Development of a simulation tool for collaborative navigation systems
Taghizadeh et al. Low-cost integrated INS/GNSS using adaptive H∞ Cubature Kalman Filter

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