CN112904382A - Laser odometer-assisted rapid optimization satellite selection method under urban canyon environment - Google Patents
Laser odometer-assisted rapid optimization satellite selection method under urban canyon environment Download PDFInfo
- Publication number
- CN112904382A CN112904382A CN202110099311.8A CN202110099311A CN112904382A CN 112904382 A CN112904382 A CN 112904382A CN 202110099311 A CN202110099311 A CN 202110099311A CN 112904382 A CN112904382 A CN 112904382A
- Authority
- CN
- China
- Prior art keywords
- satellite
- intelligent vehicle
- angle
- satellites
- positioning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/24—Acquisition or tracking or demodulation of signals transmitted by the system
- G01S19/28—Satellite selection
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
Abstract
The invention discloses a laser odometer-assisted rapid optimization satellite selection method under an urban canyon environment. The method comprises the steps of accurately calculating the prior absolute position of the intelligent vehicle by using a laser odometer, accurately calculating the altitude angle and the azimuth angle of a satellite by depending on accurate position information of the intelligent vehicle, calculating the self-adaptive cut-off altitude angle, rapidly optimizing and selecting the satellite by using a fuzzy rule, and finally implementing a multi-mode positioning strategy on the intelligent vehicle according to the number of the selected satellites. The method for rapidly optimizing and selecting the satellite disclosed by the invention overcomes the problems of inaccurate calculation of the altitude angle and the azimuth angle of the satellite, low adaptability, low satellite selection efficiency, large positioning error and the like in the conventional method, and ensures the high-precision positioning of the intelligent vehicle in the urban canyon environment.
Description
Technical Field
The invention relates to the field of vehicle navigation and positioning, in particular to a laser odometer-assisted rapid optimization satellite selection method in an urban canyon environment.
Background
In recent years, with the continuous maturity of technologies such as sensors, electronic information, deep learning and the like and the gradual improvement of relevant policy and regulations, intelligent driving technologies such as Adaptive Cruise (ACC), Automatic Emergency Braking (AEB), Lane Keeping (LKA) and the like have advanced sufficiently, and the intelligent vehicle industry is promoted to develop rapidly. The intelligent driving technology is taken as the leading edge of a new technological revolution, the intelligent degree of the vehicle is improved, and the efficiency and the safety of the trip are also improved. The existing intelligent driving technology depends on accurate intelligent vehicle position information. When the position information of the intelligent vehicle has larger errors or even errors, misleading can be brought to subsequent intelligent driving strategies such as acceleration and deceleration, direction control and the like, and even serious safety problems can be caused in serious conditions, so that the social public safety and the life and property safety of people are endangered. Currently, the positioning method of the smart car mainly depends on a Global Navigation Satellite System (GNSS). The system receives at least four satellite signals through the vehicle-mounted receiver to perform space positioning on the intelligent vehicle, and can realize high-precision positioning of the intelligent vehicle under most conditions.
Cities are a major area of human life and smart car operation. With the acceleration of the urbanization process, dense and high-rise buildings often appear on the two sides of the urban road, and an urban canyon environment similar to a natural canyon is formed. The satellite positioning in the environment has two problems of non-line-of-sight effect and multipath effect. The non-line-of-sight effect means that the satellite signals are blocked by high buildings and the like, so that the signals cannot reach a vehicle-mounted receiver; multipath effects refer to the possibility of satellite signals traveling multiple paths to the vehicle receiver, thereby creating signal aliasing. The urban canyon environment has great people/vehicle flow, but the non-line-of-sight effect or the multi-path effect can seriously reduce the positioning precision of the satellite on the intelligent vehicle, influence the intelligent driving control strategy and bring great hidden dangers to social public safety and people life and property safety. Therefore, high-precision and reliable positioning of the intelligent vehicle in the urban canyon environment and the key thereof are realized.
At present, the solution of intelligent vehicle high-precision positioning in urban canyon environment is satellite/inertial navigation-based optimized satellite selection and combined navigation. Selecting satellites in a sight distance range and less influenced by multipath effect by depending on inertial navigation auxiliary satellite selection, and fusing inertial navigation data by utilizing the satellite data for positioning; when the number of satellites is less than four, the position is determined by inertial navigation. However, the existing methods have certain problems: (1) satellite elevation and azimuth calculations are inaccurate. The altitude angle and the azimuth angle are the basis of satellite selection and need to be calculated through the mathematical relationship between the intelligent vehicle and the satellite. Although the position of the satellite is precisely known, the position information of the vehicle in the urban canyon environment is not accurate because it is accompanied by noise due to non-line of sight and multipath and accumulated errors of inertial navigation. Therefore, the calculation of the altitude angle and the azimuth angle of the satellite is inaccurate, and the satellite selection result is not necessarily optimal every time; (2) the star selection algorithm is not very applicable. In the existing satellite selection algorithm, a cut-off altitude angle is usually a fixed value, is difficult to adapt to various scenes, and is easy to select a non-line-of-sight satellite or discard part of line-of-sight satellites; (3) the star selection algorithm has a large calculation amount. The existing star selection algorithm has a large amount of matrix multiplication and matrix inverse operation, the time consumed by the algorithm is about more than half of the time of single position calculation, the positioning output frequency is influenced, and the positioning requirement of an intelligent vehicle under an urban environment is difficult to meet; (4) and (4) accumulated error of independent positioning of inertial navigation. When the number of satellites is less than four, the accumulated error can be caused by depending on inertial navigation positioning, and the vehicle-mounted inertial navigation is not enough to support high-precision positioning of the intelligent vehicle under the condition of satellite signal loss.
Disclosure of Invention
The invention provides a laser odometer assisted rapid optimization satellite selection method, which realizes high-precision positioning of an intelligent vehicle in an urban canyon environment and is mainly characterized in that: (1) and (4) accurate calculation of the elevation angle and the azimuth angle. The vehicle-mounted laser radar sensor is equipped, the increment information of the vehicle position is obtained by a radar odometer method depending on the high-precision measurement result of the laser radar, and the accurate vehicle prior position is provided for optimizing satellite selection. The radar odometer obtains incremental information of the vehicle position by calculating rotation and translation matrixes of front and rear frame point clouds of a radar and matches with the nearest absolute position of the intelligent vehicle on a time sequence, so that accurate prior absolute position information of the intelligent vehicle is obtained, and the accuracy of the satellite altitude angle and the satellite azimuth angle is ensured; (2) and self-adapting the cut-off height angle. The self-adaptive adjustment of the cut-off height angle can be realized according to the actual situation, so that the missing of a line-of-sight satellite or the selection of a non-line-of-sight satellite is avoided; (3) and (5) rapidly optimizing a satellite selection method. And based on the accurate satellite altitude angle and azimuth angle, an improved fuzzy star selection algorithm is adopted to perform optimized satellite selection on the satellite. The algorithm does not relate to a large number of matrix multiplications and matrix inverse operations, and the satellite selection efficiency is improved; (4) a multi-mode positioning strategy. When the number of the selected satellites is more than or equal to four, positioning the vehicle by fusing longitude and latitude height data after satellite selection with a laser odometer; when the number of the selected satellites is equal to three, fusing longitude and latitude information calculated by the satellites and laser odometer information; when the number of satellites is less than three, the laser odometer with higher precision than the inertial navigation is used for recursion, and the vehicle position is resolved.
The optimization satellite selection method provided by the invention solves the problem that the calculation of the satellite altitude angle and the satellite azimuth angle is not accurate at present, improves the satellite selection speed and quality, overcomes the influence of inertial navigation accumulated errors, lays a solid foundation for the development of an intelligent driving technology, provides a sufficient guarantee for the high-precision positioning and safety of an intelligent vehicle in an urban canyon environment, and has positive and important significance for the development of the intelligent vehicle industry.
The idea of the invention is further explained below:
the method determines the prior absolute position of the intelligent vehicle according to the laser odometer under the urban canyon environment, further calculates the accurate satellite altitude angle and azimuth angle, effectively reduces the interference caused by non-line-of-sight effect and multipath effect by combining with an optimized satellite selection algorithm, overcomes the influence of inertial navigation accumulated error, selects different positioning strategies according to the satellite selection result, and realizes the accurate and reliable positioning of the intelligent vehicle under the urban canyon environment, and specifically comprises the following steps:
the method comprises the following steps: method for calculating prior absolute position of intelligent vehicle by using radar odometer
The laser radar sensor has high ranging precision and good robustness, and has relatively wide application in the intelligent vehicle industry. The laser radar can acquire a rotation and translation matrix between the front frame and the rear frame through matching the point clouds of the front frame and the rear frame, and further obtain the incrementAnd (4) quantity information. This function of lidar is similar to that of automotive odometers and is therefore also known as laser odometers, the detailed principles of which are described in the literature references (JiZhang, san jivsingh. low-drift and real-time radar equation and mapping J]Autonomous Robots, 2017.). The result of positioning the latitude, longitude and altitude of the intelligent vehicle under the geocentric geodetic coordinates at the moment k-1 is recorded as
Converting the coordinate system into a rectangular coordinate system of the geocentric space by the following formula of (x)k-1,yk-1,zk-1)：
In the above formula, e is the eccentricity of the earth ellipsoid, and N is the curvature radius of the earth reference ellipsoid. The position increment of the intelligent vehicle obtained by recording the k moment through a laser odometer method under a geocentric space rectangular coordinate system is (delta x)k,Δyk,Δzk) Thus obtaining the position (x) of the intelligent vehicle at the moment k under the rectangular coordinate system of the earth center spacek,yk,zk)：
Then the position of the intelligent vehicle is turned to a geocentric geodetic coordinate system from a geocentric space rectangular coordinate system:
in the formula (I), the compound is shown in the specification,
respectively representing the prior longitude, latitude and altitude of the intelligent vehicle at the moment k, consisting ofThus, prior absolute position information of the intelligent vehicle is obtained;
step two: satellite ephemeris data are received through a vehicle-mounted GNSS receiver, and position coordinates of each satellite are obtained
Acquiring coordinate information of each satellite at the moment k through a satellite ephemeris, and recording the coordinate information as
Where k denotes the current time and n denotes the nth satellite.
Step three: based on accurate vehicle prior position information, the altitude angle and the azimuth angle of the satellite are accurately calculated
The altitude angle and the azimuth angle of the satellite need to be calculated through a mathematical relation between the intelligent vehicle and the satellite, and the specific formula is as follows:
in the formula (I), the compound is shown in the specification,
representing the altitude of the nth satellite at time k,
the azimuth of the nth satellite at the moment k is shown, R is the earth radius, and R is the satellite orbit radius.
Step four: fast optimizing star selection
The first substep: screening according to signal-to-noise ratio
Firstly, removing satellites with signal-to-noise ratios smaller than 30 db/Hz;
and a second substep: calculating an adaptive cut-off elevation angle
Calculating the self-adaptive cutoff altitude angle of the satellite in the urban canyon environment:
wherein the content of the first and second substances,
the method comprises the steps that a cutoff altitude angle at the moment k is shown, n is a satellite sequence, when the altitude angle of a satellite is larger than the cutoff altitude angle, the satellite is a line-of-sight satellite, H is the heights of buildings on two sides, d is the distance from an intelligent vehicle to a right side building, the smaller d is, the larger the angle is, and the distance from the intelligent vehicle to the right side building is definitely smaller than the distance from the intelligent vehicle to the left side building, so that only the cutoff altitude angle on the right side is calculated, and the average height of buildings around urban canyons is set to be 100m according to the general rule of civil building design (GB50352-2005) in China; according to urban road intersection planning specifications (GB50647), urban road intersection design regulations (CJJ152), urban road engineering design specifications (CJJ37), urban expressway design specifications (CJJ129) and the like, the average distance between an intelligent vehicle and the edge of a road is taken as the width of a half lane, namely 5.25m, and meanwhile, the average cut-off height angle at the moment is calculated
Recording the time interval of each frame as delta k, and if two thirds of satellite height angles appear at 5 times delta k continuously and are all larger than the average cut-off height angle, modifying the average height of the building and the average distance from the intelligent vehicle to the right side building as follows:
if two thirds of satellite height angles appear at 5 times Δ k continuously and are all smaller than the average cut-off height angle, modifying the average height of the building and the average distance from the intelligent vehicle to the right side building as follows:
when the cut-off height angle at the next moment is calculated, the modified parameters are used for realizing the self-adaptive change of the cut-off height angle;
and a third substep: satellite with rejecting altitude angle smaller than self-adaptive cut-off altitude angle
According to the satellite altitude angle obtained through calculation, removing satellites smaller than the self-adaptive cut-off altitude angle;
and a fourth substep: fuzzy satellite selection according to the number of the remaining satellites
And (3) selecting the remaining satellites by using the fuzzy satellite selection idea for optimization:
(1) if the number of the remaining satellites is less than or equal to five, all the satellites are selected;
(2) if the number of the remaining satellites is more than five, selecting three satellites with the largest altitude angle, the second largest altitude angle and the smallest altitude angle, recording the number of the remaining satellites as m, sequencing the remaining satellites from small to large according to the azimuth angles, and recording the sequence as
Corresponding to an elevation angle of
Calculating the mean value of the remaining satellite azimuths
To azimuth angle at
And
q satellites in between construct a fuzzy vector:
constructing a fuzzy relation between the two:
Z＝[Z1 T Z2 T]T (10)
improved adaptive weights:
P＝[p1 p2] (11)
in the above formula, there are:
and finally, carrying out fuzzy transformation:
Q＝P·Z (13)
the fourth satellite, the smallest element in Q. Then, the azimuth angle is at
And
and performing fuzzy satellite selection again on the satellites in the middle to select the fifth satellite.
Step five: positioning intelligent vehicle based on selected satellite and laser odometer
The first substep: reading the number of selected satellites
And adopting different positioning strategies according to the number of the selected satellites.
And a second substep: multi-mode positioning strategy
(1) If the number of the selected satellites is more than or equal to four, the longitude and latitude heights of the satellites are used as observed quantities, recursive data of the laser odometer are used as state quantities, a Kalman filtering equation is constructed, and a positioning result is obtained, wherein a specific Kalman filtering method is described in a reference (Qinyuan, Zhang flood tomahawk, Wang, Tertiary, Kalman filtering and combined navigation [ M ]. the northwest university of industry publishers, 2012);
(2) if the number of the selected satellites is equal to three, the longitude and latitude information of the intelligent vehicle can be calculated, but the height information cannot be acquired. Using the longitude and latitude calculated by the satellite as an observed quantity, using the recursive data of the laser odometer as a state quantity, constructing a Kalman filtering equation, and obtaining a positioning result;
(3) and if the number of the selected satellites is less than three, using the recursion data of the laser odometer as a positioning result.
By selecting different modes, the positioning result of the intelligent vehicle at the moment k under the geocentric geodetic coordinate is finally obtained
Drawings
FIG. 1 is a schematic view of the present invention;
fig. 2 is a flow chart of the star selection method of the present invention.
Detailed Description
In recent years, with the continuous maturity of technologies such as sensors, electronic information, deep learning and the like and the gradual improvement of relevant policy and regulations, intelligent driving technologies such as Adaptive Cruise (ACC), Automatic Emergency Braking (AEB), Lane Keeping (LKA) and the like have advanced sufficiently, and the intelligent vehicle industry is promoted to develop rapidly. The intelligent driving technology is taken as the leading edge of a new technological revolution, the intelligent degree of the vehicle is improved, and the efficiency and the safety of the trip are also improved. The existing intelligent driving technology depends on accurate intelligent vehicle position information, when the intelligent vehicle position information has large errors or even errors, misleading can be brought to subsequent intelligent driving strategies such as acceleration and deceleration, direction control and the like, and even serious safety problems can be caused in serious conditions, so that the social public safety and the life and property safety of people are endangered. Currently, the positioning method of the smart car mainly depends on a Global Navigation Satellite System (GNSS). The system receives at least four satellite signals through the vehicle-mounted receiver to perform space positioning on the intelligent vehicle, and can realize high-precision positioning of the intelligent vehicle under most conditions.
Cities are a major area of human life and smart car operation. With the acceleration of the urbanization process, dense and high-rise buildings often appear on the two sides of the urban road, and an urban canyon environment similar to a natural canyon is formed. The satellite positioning in the environment has two problems of non-line-of-sight effect and multipath effect. The non-line-of-sight effect means that the satellite signals are blocked by high buildings and the like, so that the signals cannot reach a vehicle-mounted receiver; multipath effects refer to the possibility of satellite signals traveling multiple paths to the vehicle receiver, thereby creating signal aliasing. The urban canyon environment has great people/vehicle flow, but the non-line-of-sight effect or the multi-path effect can seriously reduce the positioning precision of the satellite on the intelligent vehicle, influence the intelligent driving control strategy and bring great hidden dangers to social public safety and people life and property safety. Therefore, high-precision and reliable positioning of the intelligent vehicle in the urban canyon environment and the key thereof are realized.
At present, the solution of intelligent vehicle high-precision positioning in urban canyon environment is satellite/inertial navigation-based optimized satellite selection and combined navigation. Selecting satellites in a sight distance range and less influenced by multipath effect by depending on inertial navigation auxiliary satellite selection, and fusing inertial navigation data by utilizing the satellite data for positioning; when the number of satellites is less than four, the position is determined by inertial navigation. However, the existing methods have certain problems: (1) satellite elevation and azimuth calculations are inaccurate. The altitude angle and the azimuth angle are the basis of satellite selection and need to be calculated through the mathematical relationship between the intelligent vehicle and the satellite. Although the position of the satellite is precisely known, the position information of the vehicle in the urban canyon environment is not accurate because it is accompanied by noise due to non-line of sight and multipath and accumulated errors of inertial navigation. Therefore, the calculation of the altitude angle and the azimuth angle of the satellite is inaccurate, and the satellite selection result is not necessarily optimal every time; (2) the star selection algorithm is not very applicable. In the existing satellite selection algorithm, a cut-off altitude angle is usually a fixed value, is difficult to adapt to various scenes, and is easy to select a non-line-of-sight satellite or discard part of line-of-sight satellites; (3) the star selection algorithm has a large calculation amount. The existing star selection algorithm has a large amount of matrix multiplication and matrix inverse operation, the time consumed by the algorithm is about more than half of the time of single position calculation, the positioning output frequency is influenced, and the positioning requirement of an intelligent vehicle under an urban environment is difficult to meet; (4) and (4) accumulated error of independent positioning of inertial navigation. When the number of satellites is less than four, the accumulated error can be caused by depending on inertial navigation positioning, and the vehicle-mounted inertial navigation is not enough to support high-precision positioning of the intelligent vehicle under the condition of satellite signal loss.
Aiming at the existing problems, the invention provides a laser odometer auxiliary rapid optimization satellite selection method, which realizes high-precision positioning of an intelligent vehicle in an urban canyon environment and is mainly characterized in that: (1) and (4) accurate calculation of the elevation angle and the azimuth angle. The vehicle-mounted laser radar sensor is equipped, the increment information of the vehicle position is obtained by a radar odometer method depending on the high-precision measurement result of the laser radar, and the accurate vehicle prior position is provided for optimizing satellite selection. The radar odometer obtains incremental information of the vehicle position by calculating rotation and translation matrixes of front and rear frame point clouds of a radar and matches with the nearest absolute position of the intelligent vehicle on a time sequence, so that accurate prior absolute position information of the intelligent vehicle is obtained, and the accuracy of the satellite altitude angle and the satellite azimuth angle is ensured; (2) and self-adapting the cut-off height angle. The self-adaptive adjustment of the cut-off height angle can be realized according to the actual situation, so that the missing of a line-of-sight satellite or the selection of a non-line-of-sight satellite is avoided; (3) and (5) rapidly optimizing a satellite selection method. And based on the accurate satellite altitude angle and azimuth angle, an improved fuzzy star selection algorithm is adopted to perform optimized satellite selection on the satellite. The algorithm does not relate to a large number of matrix multiplications and matrix inverse operations, and the satellite selection efficiency is improved; (4) a multi-mode positioning strategy. When the number of the selected satellites is more than or equal to four, positioning the vehicle by fusing longitude and latitude height data after satellite selection with a laser odometer; when the number of the selected satellites is equal to three, fusing longitude and latitude information calculated by the satellites and laser odometer information; when the number of satellites is less than three, the laser odometer with higher precision than the inertial navigation is used for recursion, and the vehicle position is resolved.
The optimization satellite selection method provided by the invention solves the problem that the calculation of the satellite altitude angle and the satellite azimuth angle is not accurate at present, improves the satellite selection quality and speed, overcomes the influence of inertial navigation accumulated errors, lays a solid foundation for the development of an intelligent driving technology, provides a sufficient guarantee for the high-precision positioning and safety of an intelligent vehicle in an urban canyon environment, and has positive and important significance for the development of the intelligent vehicle industry.
The idea of the invention is further explained below:
the method comprises the following steps: method for calculating prior absolute position of intelligent vehicle by using radar odometer
The laser radar sensor has high ranging precision and good robustness, and is used in an intelligent vehicleThe method has wide application in the industry. The laser radar can acquire a rotation matrix and a translation matrix between the front frame and the rear frame through matching the point clouds of the front frame and the rear frame, and then incremental information is obtained. This function of lidar is similar to that of automotive odometers and is therefore also known as laser odometers, the detailed principles of which are described in the literature references (JiZhang, san jivsingh. low-drift and real-time radar equation and mapping J]Autonomous Robots, 2017.). The result of positioning the latitude, longitude and altitude of the intelligent vehicle under the geocentric geodetic coordinates at the moment k-1 is recorded as
Converting the coordinate system into a rectangular coordinate system of the geocentric space by the following formula of (x)k-1,yk-1,zk-1)：
In the above formula, e is the eccentricity of the earth ellipsoid, and N is the curvature radius of the earth reference ellipsoid. The position increment of the intelligent vehicle obtained by recording the k moment through a laser odometer method under a geocentric space rectangular coordinate system is (delta x)k,Δyk,Δzk) Thus obtaining the position (x) of the intelligent vehicle at the moment k under the rectangular coordinate system of the earth center spacek,yk,zk)：
Then the position of the intelligent vehicle is turned to a geocentric geodetic coordinate system from a geocentric space rectangular coordinate system:
in the formula (I), the compound is shown in the specification,
respectively representing the prior longitude, the latitude and the height of the intelligent vehicle at the moment k, so as to obtain the prior absolute position information of the intelligent vehicle;
step two: satellite ephemeris data are received through a vehicle-mounted GNSS receiver, and position coordinates of each satellite are obtained
Acquiring coordinate information of each satellite at the moment k through a satellite ephemeris, and recording the coordinate information as
Where k denotes the current time and n denotes the nth satellite.
Step three: based on accurate vehicle prior position information, the altitude angle and the azimuth angle of the satellite are accurately calculated
The altitude angle and the azimuth angle of the satellite need to be calculated through a mathematical relation between the intelligent vehicle and the satellite, and the specific formula is as follows:
in the formula (I), the compound is shown in the specification,
representing the altitude of the nth satellite at time k,
the azimuth of the nth satellite at the moment k is shown, R is the earth radius, and R is the satellite orbit radius.
Step four: fast optimizing star selection
The first substep: screening according to signal-to-noise ratio
Firstly, removing satellites with signal-to-noise ratios smaller than 30 db/Hz;
and a second substep: calculating an adaptive cut-off elevation angle
Calculating the self-adaptive cutoff altitude angle of the satellite in the urban canyon environment:
wherein the content of the first and second substances,
the method comprises the steps that a cutoff altitude angle at the moment k is shown, n is a satellite sequence, when the altitude angle of a satellite is larger than the cutoff altitude angle, the satellite is a line-of-sight satellite, H is the heights of buildings on two sides, d is the distance from an intelligent vehicle to a right side building, the smaller d is, the larger the angle is, and the distance from the intelligent vehicle to the right side building is definitely smaller than the distance from the intelligent vehicle to the left side building, so that only the cutoff altitude angle on the right side is calculated, and the average height of buildings around urban canyons is set to be 100m according to the general rule of civil building design (GB50352-2005) in China; according to urban road intersection planning specifications (GB50647), urban road intersection design regulations (CJJ152), urban road engineering design specifications (CJJ37), urban expressway design specifications (CJJ129) and the like, the average distance between an intelligent vehicle and the edge of a road is taken as the width of a half lane, namely 5.25m, and meanwhile, the average cut-off height angle at the moment is calculated
Recording the time interval of each frame as delta k, and if two thirds of satellite height angles appear at 5 times delta k continuously and are all larger than the average cut-off height angle, modifying the average height of the building and the average distance from the intelligent vehicle to the right side building as follows:
if two thirds of satellite height angles appear at 5 times Δ k continuously and are all smaller than the average cut-off height angle, modifying the average height of the building and the average distance from the intelligent vehicle to the right side building as follows:
when the cut-off height angle at the next moment is calculated, the modified parameters are used for realizing the self-adaptive change of the cut-off height angle;
and a third substep: satellite with rejecting altitude angle smaller than self-adaptive cut-off altitude angle
According to the satellite altitude angle obtained through calculation, removing satellites smaller than the self-adaptive cut-off altitude angle;
and a fourth substep: fuzzy satellite selection according to the number of the remaining satellites
And (3) selecting the remaining satellites by using the fuzzy satellite selection idea for optimization:
(1) if the number of the remaining satellites is less than or equal to five, all the satellites are selected;
(2) if the number of the remaining satellites is more than five, selecting three satellites with the largest altitude angle, the second largest altitude angle and the smallest altitude angle, recording the number of the remaining satellites as m, sequencing the remaining satellites from small to large according to the azimuth angles, and recording the sequence as
Corresponding to an elevation angle of
Calculating the mean value of the remaining satellite azimuths
To azimuth angle at
And
q satellites in between construct a fuzzy vector:
constructing a fuzzy relation between the two:
Z＝[Z1 T Z2 T]T (23)
improved adaptive weights:
P＝[p1 p2] (24)
in the above formula, there are:
and finally, carrying out fuzzy transformation:
Q＝P·Z (26)
the fourth satellite, the smallest element in Q. Then, the azimuth angle is at
And
and performing fuzzy satellite selection again on the satellites in the middle to select the fifth satellite.
Step five: positioning intelligent vehicle based on selected satellite and laser odometer
The first substep: reading the number of selected satellites
And adopting different positioning strategies according to the number of the selected satellites.
And a second substep: multi-mode positioning strategy
(1) If the number of the selected satellites is more than or equal to four, the longitude and latitude heights of the satellites are used as observed quantities, recursive data of the laser odometer are used as state quantities, a Kalman filtering equation is constructed, and a positioning result is obtained, wherein a specific Kalman filtering method is described in a reference (Qinyuan, Zhang flood tomahawk, Wang, Tertiary, Kalman filtering and combined navigation [ M ]. the northwest university of industry publishers, 2012);
(2) if the number of the selected satellites is equal to three, the longitude and latitude information of the intelligent vehicle can be calculated, but the height information cannot be acquired. Using the longitude and latitude calculated by the satellite as an observed quantity, using the recursive data of the laser odometer as a state quantity, constructing a Kalman filtering equation, and obtaining a positioning result;
(3) and if the number of the selected satellites is less than three, using the recursion data of the laser odometer as a positioning result.
By selecting different modes, the positioning result of the intelligent vehicle at the moment k under the geocentric geodetic coordinate is finally obtained
Claims (1)
1. A laser odometer assisted rapid optimization satellite selection method in an urban canyon environment is characterized in that in the urban canyon environment, a priori absolute position of an intelligent vehicle is determined according to the laser odometer, an accurate satellite altitude angle and an accurate satellite azimuth angle are further calculated, an optimization satellite selection algorithm is combined, different positioning strategies are selected according to satellite selection results, and accurate and reliable positioning of the intelligent vehicle in the urban canyon environment is achieved, and the method comprises the following steps:
the method comprises the following steps: method for calculating prior absolute position of intelligent vehicle by using radar odometer
The result of positioning the latitude, longitude and altitude of the intelligent vehicle under the geocentric geodetic coordinates at the moment k-1 is recorded as
Converting the coordinate system into a rectangular coordinate system of the geocentric space by the following formula of (x)k-1,yk-1,zk-1)：
In the above formula, e is the eccentricity of the earth ellipsoid, N is the curvature radius of the earth reference ellipsoid, and the increment of the position of the intelligent vehicle obtained by recording the k time through a laser odometer method in a rectangular coordinate system of earth center space is (delta x)k,Δyk,Δzk) Thus obtaining the position (x) of the intelligent vehicle at the moment k under the rectangular coordinate system of the earth center spacek,yk,zk)：
Then the position of the intelligent vehicle is turned to a geocentric geodetic coordinate system from a geocentric space rectangular coordinate system:
in the formula (I), the compound is shown in the specification,
respectively representing the prior longitude, the latitude and the height of the intelligent vehicle at the moment k, so as to obtain the prior absolute position information of the intelligent vehicle;
step two: satellite ephemeris data are received through a vehicle-mounted GNSS receiver, and position coordinates of each satellite are obtained
Acquiring coordinate information of each satellite at the moment k through a satellite ephemeris, and recording the coordinate information as
Wherein k represents the current time, and n represents the nth satellite;
step three: based on accurate vehicle prior position information, the altitude angle and the azimuth angle of the satellite are accurately calculated
The altitude angle and the azimuth angle of the satellite need to be calculated through a mathematical relation between the intelligent vehicle and the satellite, and the specific formula is as follows:
in the formula (I), the compound is shown in the specification,
representing the altitude of the nth satellite at time k,
the azimuth angle of the nth satellite at the moment k is shown, R is the earth radius, and R is the satellite orbit radius;
step four: fast optimizing star selection
The first substep: screening according to signal-to-noise ratio
Firstly, removing satellites with signal-to-noise ratios smaller than 30 db/Hz;
and a second substep: calculating an adaptive cut-off elevation angle
Calculating the self-adaptive cutoff altitude angle of the satellite in the urban canyon environment:
wherein the content of the first and second substances,
the method comprises the steps that a cutoff altitude angle at the moment k is represented, n is a satellite sequence, when the altitude angle of a satellite is larger than the cutoff altitude angle, the satellite is a line-of-sight satellite, H is the heights of buildings on two sides, d is the distance from an intelligent vehicle to a right side building, the smaller d is, the larger the angle is, the distance from the intelligent vehicle to the right side building is certainly smaller than the distance from the intelligent vehicle to the left side building, therefore, only the cutoff altitude angle on the right side is calculated, and the average height of buildings around urban canyons is set to be 100 m; taking the average distance between the intelligent vehicle and the road edge as the width of a half lane, namely 5.25m, and meanwhile, calculating the average cut-off height angle at the moment
Recording the time interval of each frame as delta k, and if two thirds of satellite height angles appear at 5 times delta k continuously and are all larger than the average cut-off height angle, modifying the average height of the building and the average distance from the intelligent vehicle to the right side building as follows:
if two thirds of satellite height angles appear at 5 times Δ k continuously and are all smaller than the average cut-off height angle, modifying the average height of the building and the average distance from the intelligent vehicle to the right side building as follows:
when the cut-off height angle at the next moment is calculated, the modified parameters are used for realizing the self-adaptive change of the cut-off height angle;
and a third substep: satellite with rejecting altitude angle smaller than self-adaptive cut-off altitude angle
According to the satellite altitude angle obtained through calculation, removing satellites smaller than the self-adaptive cut-off altitude angle;
and a fourth substep: fuzzy satellite selection according to the number of the remaining satellites
And (3) selecting the remaining satellites by using the fuzzy satellite selection idea for optimization:
(1) if the number of the remaining satellites is less than or equal to five, all the satellites are selected;
(2) if the number of the remaining satellites is more than five, selecting three satellites with the largest altitude angle, the second largest altitude angle and the smallest altitude angle, recording the number of the remaining satellites as m, sequencing the remaining satellites from small to large according to the azimuth angles, and recording the sequence as
Corresponding to an elevation angle of
Calculating the mean value of the remaining satellite azimuths
To azimuth angle at
And
q satellites in between construct a fuzzy vector:
constructing a fuzzy relation between the two:
Z＝[Z1 T Z2 T]T (10)
improved adaptive weights:
P＝[p1 p2] (11)
in the above formula, there are:
and finally, carrying out fuzzy transformation:
Q＝P·Z (13)
the fourth satellite, the smallest element in Q, then has an azimuth angle of
And
the satellite between the satellite selection and the satellite selection are subjected to fuzzy satellite selection again to select a fifth satellite;
step five: positioning intelligent vehicle based on selected satellite and laser odometer
The first substep: reading the number of selected satellites
Adopting different positioning strategies according to the number of the selected satellites;
and a second substep: multi-mode positioning strategy
(1) If the number of the selected satellites is more than or equal to four, the longitude and latitude heights of the satellites are used as observed quantities, recursive data of the laser odometer are used as state quantities, a Kalman filtering equation is constructed, and a positioning result is obtained;
(2) if the number of the selected satellites is equal to three, the longitude and latitude information of the intelligent vehicle can be calculated, but the height information cannot be acquired, the longitude and latitude calculated by the satellites are used as observed quantities, the recursive data of the laser odometer is used as state quantities, a Kalman filtering equation is constructed, and a positioning result is acquired;
(3) if the number of the selected satellites is less than three, using the recursive data of the laser odometer as a positioning result;
by selecting different modes, the positioning result of the intelligent vehicle at the moment k under the geocentric geodetic coordinate is finally obtained
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110099311.8A CN112904382B (en) | 2021-01-25 | 2021-01-25 | Laser odometer-assisted rapid optimization satellite selection method under urban canyon environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110099311.8A CN112904382B (en) | 2021-01-25 | 2021-01-25 | Laser odometer-assisted rapid optimization satellite selection method under urban canyon environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112904382A true CN112904382A (en) | 2021-06-04 |
| CN112904382B CN112904382B (en) | 2022-05-13 |
Family
ID=76120240
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110099311.8A Active CN112904382B (en) | 2021-01-25 | 2021-01-25 | Laser odometer-assisted rapid optimization satellite selection method under urban canyon environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112904382B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116755126A (en) * | 2023-08-15 | 2023-09-15 | 北京航空航天大学 | Beidou real-time accurate positioning method based on three-dimensional model mapping matching |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104931981A (en) * | 2015-05-11 | 2015-09-23 | 中国科学院光电研究院 | GNSS anti-multipath satellite selecting method based on signal to noise ratio fluctuation information |
| CN105807301A (en) * | 2016-03-03 | 2016-07-27 | 东南大学 | Enhanced digital map based vehicle optimization oriented satellite selection positioning method |
| CN107064974A (en) * | 2017-02-28 | 2017-08-18 | 广东工业大学 | A kind of localization method and system for suppressing urban canyons multipath satellite-signal |
| CN107132551A (en) * | 2017-06-12 | 2017-09-05 | 广州市纳微卫星导航技术有限公司 | Multisystem GNSS integrated positioning selecting-star algorithms |
| CN107390238A (en) * | 2017-07-23 | 2017-11-24 | 天津博创金成技术开发有限公司 | A kind of Beidou navigation constellation quick satellite selection method |
| CN110749909A (en) * | 2019-07-25 | 2020-02-04 | 中国民用航空中南地区空中交通管理局 | Aircraft position high-precision positioning method based on multi-constellation network post difference |
-
2021
- 2021-01-25 CN CN202110099311.8A patent/CN112904382B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104931981A (en) * | 2015-05-11 | 2015-09-23 | 中国科学院光电研究院 | GNSS anti-multipath satellite selecting method based on signal to noise ratio fluctuation information |
| CN105807301A (en) * | 2016-03-03 | 2016-07-27 | 东南大学 | Enhanced digital map based vehicle optimization oriented satellite selection positioning method |
| CN107064974A (en) * | 2017-02-28 | 2017-08-18 | 广东工业大学 | A kind of localization method and system for suppressing urban canyons multipath satellite-signal |
| CN107132551A (en) * | 2017-06-12 | 2017-09-05 | 广州市纳微卫星导航技术有限公司 | Multisystem GNSS integrated positioning selecting-star algorithms |
| CN107390238A (en) * | 2017-07-23 | 2017-11-24 | 天津博创金成技术开发有限公司 | A kind of Beidou navigation constellation quick satellite selection method |
| CN110749909A (en) * | 2019-07-25 | 2020-02-04 | 中国民用航空中南地区空中交通管理局 | Aircraft position high-precision positioning method based on multi-constellation network post difference |
Non-Patent Citations (1)
| Title |
|---|
| 伍劭实等: "北斗高精度相对定位选星方法研究", 《重庆邮电大学学报(自然科学版)》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116755126A (en) * | 2023-08-15 | 2023-09-15 | 北京航空航天大学 | Beidou real-time accurate positioning method based on three-dimensional model mapping matching |
| CN116755126B (en) * | 2023-08-15 | 2023-11-14 | 北京航空航天大学 | Beidou real-time accurate positioning method based on three-dimensional model mapping matching |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112904382B (en) | 2022-05-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110631593B (en) | Multi-sensor fusion positioning method for automatic driving scene | |
| CN108226980B (en) | Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit | |
| Schreiber et al. | Vehicle localization with tightly coupled GNSS and visual odometry | |
| Obst et al. | Urban multipath detection and mitigation with dynamic 3D maps for reliable land vehicle localization | |
| Obst et al. | Multipath detection with 3D digital maps for robust multi-constellation GNSS/INS vehicle localization in urban areas | |
| EP2149056B1 (en) | Positioning device, method and program with absolute positioning and relative positioning modes | |
| JP5673071B2 (en) | Position estimation apparatus and program | |
| EP1918677B1 (en) | Systems and methods for a terrain contour matching navigation system | |
| AU2009200190B2 (en) | Methods and systems for underwater navigation | |
| EP2656109B1 (en) | Methods, devices, and uses for calculating a position using a global navigation satellite system | |
| US5883595A (en) | Method and apparatus for mitigating multipath effects and smoothing groundtracks in a GPS receiver | |
| CN110779521A (en) | Multi-source fusion high-precision positioning method and device | |
| EP2597485B1 (en) | Rapid lidar image correlation for ground navigation | |
| CN109343095B (en) | Vehicle-mounted navigation vehicle combined positioning device and combined positioning method thereof | |
| CN107015259B (en) | Method for calculating pseudorange/pseudorange rate by using Doppler velocimeter | |
| CN102426018A (en) | Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering | |
| JP2001183439A (en) | General-purpose positioning system based on use of statistical filter | |
| US20200150279A1 (en) | Positioning device | |
| Suzuki | First place award winner of the smartphone decimeter challenge: Global optimization of position and velocity by factor graph optimization | |
| Hide et al. | GPS and low cost INS integration for positioning in the urban environment | |
| CN114562992A (en) | Multi-path environment combined navigation method based on factor graph and scene constraint | |
| CN112904382B (en) | Laser odometer-assisted rapid optimization satellite selection method under urban canyon environment | |
| Dawson et al. | Radar-based multisensor fusion for uninterrupted reliable positioning in GNSS-denied environments | |
| Li et al. | A tightly coupled positioning solution for land vehicles in urban canyons | |
| Rahman et al. | Earth-centered earth-fixed (ecef) vehicle state estimation performance |
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 |