CN108415057B - Relative positioning method for cooperative work of unmanned fleet and road side unit - Google Patents

Relative positioning method for cooperative work of unmanned fleet and road side unit Download PDF

Info

Publication number
CN108415057B
CN108415057B CN201810072492.3A CN201810072492A CN108415057B CN 108415057 B CN108415057 B CN 108415057B CN 201810072492 A CN201810072492 A CN 201810072492A CN 108415057 B CN108415057 B CN 108415057B
Authority
CN
China
Prior art keywords
vehicle
unmanned
fleet
relative positioning
inertial navigation
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
CN201810072492.3A
Other languages
Chinese (zh)
Other versions
CN108415057A (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.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University 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 Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN201810072492.3A priority Critical patent/CN108415057B/en
Publication of CN108415057A publication Critical patent/CN108415057A/en
Application granted granted Critical
Publication of CN108415057B publication Critical patent/CN108415057B/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/51—Relative positioning
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

The invention discloses a relative positioning method for cooperative work of an unmanned fleet and a roadside unit. The method comprises the following steps: firstly, an unmanned fleet relative positioning system based on a vehicle-mounted unit and a road side unit is established, then information interaction content between adjacent vehicles of the unmanned fleet and between the vehicles and the road side unit is determined during relative positioning, then data of the vehicle-mounted inertial navigation system is transmitted to a vehicle-mounted data link, the distance between the vehicles in the unmanned fleet is measured by a radio distance measurement method, meanwhile, the vehicle-mounted data link is used for reporting inertial navigation data and distance measurement information, the distance measurement information and the inertial navigation data are transmitted to a relative positioning processor, finally, the error of the inertial navigation system of each unmanned vehicle is used as an amount to be estimated, the difference between the measured distance between the vehicles and the distance calculated by the inertial navigation information is measured as a quantity, and a mathematical model of the unmanned fleet relative positioning method is established. The invention improves the precision and reliability of the relative positioning of the unmanned motorcade and has strong robustness.

Description

Relative positioning method for cooperative work of unmanned fleet and road side unit
Technical Field
The invention relates to the technical field of navigation positioning, in particular to a relative positioning method for cooperative work of an unmanned fleet and a roadside unit.
Background
With the rapid development of economy, the number of automobiles is continuously increased, the problems of traffic jam, energy consumption, atmospheric pollution and the like are increasingly serious, and the specific application of the internet of vehicles as the technology of the internet of things in the field of intelligent transportation is more and more emphasized by people. The internet of vehicles is a comprehensive complex network system which integrates vehicles, travelers, roads and related infrastructure into an organic whole. The electronic equipment loaded on the vehicle can exchange data with other vehicles and road network system infrastructure through communication technologies such as radio frequency and the like, so that unmanned vehicles can coordinate to operate, and the fleet of vehicles can be uniformly managed. The application of the car networking technology can effectively reduce the probability of traffic accidents. In the research of the related technologies of the car networking, the navigation technology is the research core, and the accurate positioning problem is the most basic link of the navigation of the multiple unmanned vehicles, so that the most basic position information is provided for the application of the car networking technology. The unmanned vehicle can only safely travel by accurately knowing its own position, the position of the obstacle in the working space, and the movement condition of the obstacle. Therefore, the problem of self-positioning of the unmanned vehicle is particularly important, and is one of the most important aspects.
In recent years, the relative positioning of unmanned vehicles has received more and more attention and research, and is gradually becoming an active research field. Currently, absolute position information is mainly obtained by Global Navigation Satellite System (GNSS) technologies such as GPS and beidou. However, the civil GPS standard location service can only provide positioning accuracy of about 10m, and cannot meet the application requirement of accurate positioning of the internet of vehicles. In addition, in a tunnel, an underground/indoor parking lot, a forest, under a flyover, a dense building group and other multi-sheltered areas, a satellite signal is sheltered by a building to generate a large positioning error, so that positioning is not accurate or even cannot be performed. Therefore, the existing positioning technology cannot realize accurate positioning of multiple unmanned vehicles in areas with poor or unavailable GNSS signals.
Disclosure of Invention
The invention aims to provide a relative positioning method for cooperative work of an unmanned motorcade and a roadside unit, which has good stability and strong robustness, so that the precision and the reliability of the relative positioning of the unmanned motorcade are improved.
The technical solution for realizing the purpose of the invention is as follows: a relative positioning method for cooperative work of an unmanned fleet and a roadside unit comprises the following steps:
step 1, deploying two road side units in a road network of an unmanned vehicle driving area, using the two road side units as known fixed points, installing a vehicle-mounted unit on each vehicle of an unmanned vehicle fleet, and establishing an unmanned vehicle fleet relative positioning system based on a vehicle-mounted inertial navigation system, a vehicle-mounted data link and a relative positioning processor;
step 2, determining information interaction contents between adjacent vehicles of the unmanned fleet and between the vehicles and a roadside unit during relative positioning, transmitting data of the vehicle-mounted inertial navigation system to a vehicle-mounted data chain, measuring the distance between the vehicles in the unmanned fleet by using a radio distance measurement method through the vehicle-mounted data chain, simultaneously reporting inertial navigation data and distance measurement information in real time by using radio communication, and transmitting the distance measurement information and the inertial navigation data to a relative positioning processor;
step 3, taking the error of the inertial navigation system of each unmanned vehicle as the amount to be estimated, measuring the difference between the measured distance between the vehicles and the distance calculated by the inertial navigation information as the amount to be estimated, and establishing a mathematical model of the relative positioning method of the unmanned vehicle fleet under the condition of no GNSS; and then, solving the error estimation value of each inertial navigation system by using a nonlinear least square method and a Newton iteration method, and correcting the inertial navigation output position by using the feedback of the error estimation value of the vehicle inertial navigation position to finish vehicle positioning.
Further, the establishing of the unmanned fleet relative positioning system based on the vehicle-mounted inertial navigation system, the vehicle-mounted data link and the relative positioning processor in the step 1 is specifically as follows:
the method comprises the steps that two road side units are deployed in a road network of an unmanned vehicle driving area, an on-board unit is installed on each vehicle of an unmanned vehicle fleet, the on-board unit and the road side units are provided with wireless communication modules, a system network in the coverage range of the road side units has two basic communications, namely, the communication between the unmanned vehicles and the unmanned vehicles, and the communication between the unmanned vehicles and road side infrastructures such as the road side units, the vehicles in driving are communicated with each other by utilizing a wireless communication technology, and the road side units access an external network.
Further, the mathematical model for establishing the relative positioning method of the unmanned vehicle fleet without GNSS in step 3 is specifically as follows:
the number of vehicles in the unmanned vehicle fleet is set to n, [ x ]am,yam]Is the output position coordinate of the vehicle m inertial navigation system, [ x ]tm,ytm]As the true coordinates of vehicle m, xem,yemIs the inertial navigation position error of the vehicle m, m is 1, …, n, then [ xam,yam]And [ x ]tm,ytm]And [ x ]em,yem]The relationship of (1) is:
Figure BDA0001558542900000021
the mutual distance measurement value between the vehicle i and the vehicle j is li,jI, j e {1, …, n }, a calculated distance d calculated from the vehicle coordinates output from the inertial navigation systemi,jExpressed as:
Figure BDA0001558542900000031
substituting (1) into (2) to obtain:
Figure BDA0001558542900000032
order to
Figure BDA0001558542900000033
The distance between any two unmanned vehicles or road side units is measured at the same time, and the distance is measured
Figure BDA0001558542900000034
Group data, which together make up the following
Figure BDA0001558542900000035
The equation is a mathematical model of the relative positioning of the unmanned fleet:
Figure BDA0001558542900000036
setting solution x ═ xe1,ye1,xe2,ye2,…,…,xen,yen) Is initially of
Figure BDA0001558542900000037
Each equation in the formula is
Figure BDA0001558542900000038
Carrying out linearization; in the estimation of the parameter x, the equation sets on both sides have 2n unknown parameters, at least 2n non-linearly related measurement equations are needed according to the mathematical principle, and in the mathematical model of the relative positioning of the unmanned vehicle fleet, at most, there are
Figure BDA0001558542900000039
The independent measurement equations are adopted, so that at least 5 vehicle nodes are required in the unmanned fleet to realize relative positioning under the condition of no absolute positioning data; in the case of two fixed road side units with known positions, the two side equation sets have 2n-4 unknown parameters, at least 2n-4 non-linearly related measurement equations are needed according to the mathematical principle, and in the mathematical model of the relative positioning of the unmanned vehicle fleet, at most, the two unknown parameters are obtained
Figure BDA00015585429000000310
The independent measurement equations are adopted, so that the number of the unmanned vehicles is more than 1, namely at least two unmanned vehicles in the fleet can be accurately positioned;
the first in the system of equations
Figure BDA00015585429000000311
The taylor expansion of the equation is:
Figure BDA0001558542900000041
in the formula (I), the compound is shown in the specification,
Figure BDA0001558542900000042
is a function of
Figure BDA0001558542900000045
For xen-1Is partially guided in
Figure BDA0001558542900000043
A value of (i) i
Figure BDA0001558542900000044
The nonlinear equation set (4) is approximately converted into a linear equation set expressed in the form of a matrix:
G·Δx＝b (7)
in the formula (I), the compound is shown in the specification,
Figure BDA0001558542900000051
Figure BDA0001558542900000052
Figure BDA0001558542900000053
xk＝xk-1+Δx (11)
Δx＝(GTG)-1GTb (12)
in the formula, delta x is the nonlinear least square solution of the matrix equation (7), and the real coordinate of the target node is [ xam-xem,yam-yem]Thus completing the error correction of inertial navigation positioning.
Compared with the prior art, the invention has the following remarkable advantages: (1) inertial navigation positioning errors of all vehicles are estimated based on Newton iteration and a nonlinear least square method, and the relative positioning accuracy and reliability of the unmanned fleet are improved; (2) the relative positioning of the unmanned motorcade is realized under the condition of no satellite positioning, the positioning precision is improved, and the robustness is very strong; (3) the positions of all vehicles in the unmanned fleet are equal, and any vehicle in the fleet has an unexpected fault without influencing the continuation of the algorithm; (4) the position error of inertial navigation can be estimated by using inertial navigation data and mutual distance measurement information at the same moment, so that an estimated value of the real coordinate of the vehicle is obtained, the calculation is simple, the implementation is convenient, and the step of long-time filtering is not needed; (5) the vehicle inertial navigation positioning information and the relative distance measurement information between vehicles are fused, so that the relative error of the vehicle inertial navigation information can be corrected, and the accuracy and the stability of continuous relative positioning between unmanned vehicles in a given road network range and under the conditions of given road side unit quantity and position and no satellite positioning system are improved.
Drawings
Fig. 1 is a schematic diagram illustrating the principle of the relative positioning method for the cooperative work of the unmanned fleet and the roadside unit according to the present invention.
FIG. 2 is a general schematic diagram of the simulation of the present invention.
Fig. 3 is a block diagram of an unmanned fleet relative positioning system.
Fig. 4 is a diagram of unmanned fleet relative positioning topology.
FIG. 5 is a flow chart of simulation calculations in accordance with the present invention.
FIG. 6 shows an inertial navigation position error x of a vehicle A in a simulation resultea1And the position error x after iterative correctioneg1The relationship of (c) is compared to the graph.
FIG. 7 shows the inertial navigation position error y of the vehicle A in the simulation resultea1And the position error y after iterative correctioneg1The relationship of (c) is compared to the graph.
FIG. 8 shows an inertial navigation position error x of a vehicle B in a simulation resultea2And the position error x after iterative correctioneg2The relationship of (c) is compared to the graph.
FIG. 9 shows inertial navigation position error y of vehicle B in the simulation resultea2And the position error y after iterative correctioneg2The relationship of (c) is compared to the graph.
FIG. 10 shows the inertial navigation position error x of the vehicle C in the simulation resultea3And the position error x after iterative correctioneg3Is onCompare the graphs.
FIG. 11 shows the inertial navigation position error y of the vehicle C in the simulation resultea3And the position error y after iterative correctioneg3The relationship of (c) is compared to the graph.
FIG. 12 shows the inertial navigation position error x of the vehicle D in the simulation resultea4And the position error x after iterative correctioneg4The relationship of (c) is compared to the graph.
FIG. 13 shows the inertial navigation position error y of the vehicle D in the simulation resultea4And the position error y after iterative correctioneg4The relationship of (c) is compared to the graph.
Detailed Description
The following detailed description is provided to explain embodiments of the invention in conjunction with the drawings and to describe the technical solutions of the invention in detail.
With reference to fig. 1-5, the relative positioning method for cooperation of the unmanned fleet and the roadside unit includes the following steps:
step 1, deploying two road side units in a road network of an unmanned vehicle driving area, using the two road side units as known fixed points, installing a vehicle-mounted unit on each vehicle of an unmanned vehicle fleet, and establishing an unmanned vehicle fleet relative positioning system based on a vehicle-mounted inertial navigation system, a vehicle-mounted data link and a relative positioning processor;
step 2, determining information interaction contents between adjacent vehicles of the unmanned fleet and between the vehicles and a roadside unit during relative positioning, transmitting data of the vehicle-mounted inertial navigation system to a vehicle-mounted data chain, measuring the distance between the vehicles in the unmanned fleet by using a radio distance measurement method through the vehicle-mounted data chain, simultaneously reporting inertial navigation data and distance measurement information in real time by using radio communication, and transmitting the distance measurement information and the inertial navigation data to a relative positioning processor;
step 3, taking the error of the inertial navigation system of each unmanned vehicle as the amount to be estimated, measuring the difference between the measured distance between the vehicles and the distance calculated by the inertial navigation information as the amount to be estimated, and establishing a mathematical model of the relative positioning method of the unmanned vehicle fleet under the condition of no GNSS; and then solving the error estimation value of each inertial navigation system by using a nonlinear least square method and a Newton iteration method, and completing vehicle positioning by using the feedback of the error estimation value of the vehicle inertial navigation position.
The establishment of the unmanned fleet relative positioning system based on the vehicle-mounted inertial navigation system, the vehicle-mounted data link and the relative positioning processor in the step 1 is as follows:
the method comprises the steps that two road side units are deployed in a road network of an unmanned vehicle driving area, an on-board unit is installed on each vehicle of an unmanned vehicle fleet, the on-board unit and the road side units are provided with wireless communication modules, a system network in the coverage range of the road side units has two basic communications, namely, the communication between the unmanned vehicles and the unmanned vehicles, and the communication between the unmanned vehicles and road side infrastructures such as the road side units, the vehicles in driving are communicated with each other by utilizing a wireless communication technology, and the road side units access an external network.
The mathematical model for establishing the relative positioning method of the unmanned fleet under the condition of no GNSS in the step 3 is specifically as follows:
the ranging information refers to a mutual ranging value l between two vehiclesi,jBy the mutual distance measurement value l between vehiclesi,jAnd position information [ x ] provided by each vehicle-mounted inertial navigationam,yam](m-1, …, n), where n is the number of vehicles in the unmanned fleet, the calculated distance d being calculatedi,jThe difference is measured as a measure of the position error x of each vehicle's inertial navigationem,yemAnd (m is 1, …, n) is an amount to be estimated, and a mathematical model of the unmanned fleet relative positioning method is established.
The mathematical model is established by the following steps:
the number of vehicles in the unmanned fleet is n, [ x ]am,yam](m is 1, …, n) is the output position coordinate of the vehicle m inertial navigation system, [ x [ [ x ]tm,ytm](m is 1, …, n) is the real coordinate of vehicle m, xem,yem(m is 1, …, n) is the inertial navigation position error of vehicle m, then [ x ═ x { (x) } is the inertial navigation position error of vehicle mam,yam]And [ x ]tm,ytm]And [ x ]em,yem]The relationship of (1) is:
Figure BDA0001558542900000081
the mutual distance measurement value between the vehicle i and the vehicle j is li,jI, j ∈ {1, …, n }, and the calculated distance calculated from the vehicle coordinates output from the inertial navigation system is di,jCan be expressed as:
Figure BDA0001558542900000082
substituting (1) into (2), squaring both sides, can obtain:
Figure BDA0001558542900000083
order to
Figure BDA0001558542900000084
When the number of the unmanned vehicles in the unmanned vehicle fleet is n, the distance between any two points can be measured at the same time, and the distance is measured
Figure BDA0001558542900000085
Group data, which together make up the following
Figure BDA0001558542900000086
The equation is a mathematical model of the relative positioning of the unmanned fleet:
Figure BDA0001558542900000087
setting solution x ═ xe1,ye1,xe2,ye2,…,…,xen,yen) Is initially of
Figure BDA0001558542900000088
Each equation in the formula can be expressed in
Figure BDA0001558542900000089
Carrying out linearization; in the estimation of the parameter x, twoThe side equation set has 2n unknown parameters, at least 2n non-linearly related measurement equations are needed according to the mathematical principle, and at most, the side equation set has 2n unknown parameters in the mathematical model of the relative positioning of the unmanned vehicle fleet
Figure BDA0001558542900000091
The independent measurement equations are adopted, so that at least 5 vehicle nodes are required in the unmanned fleet to realize relative positioning under the condition of no absolute positioning data; in the case of two fixed road side units with known positions, the two side equation sets have 2n-4 unknown parameters, at least 2n-4 non-linearly related measurement equations are needed according to the mathematical principle, and in the mathematical model of the relative positioning of the unmanned vehicle fleet, at most, the two unknown parameters are obtained
Figure BDA0001558542900000092
The number of the unmanned vehicles is more than 1, namely at least two unmanned vehicles in the fleet can carry out accurate positioning.
The first in the system of equations
Figure BDA0001558542900000093
Taylor expansion of equation
Figure BDA0001558542900000094
In the formula (I), the compound is shown in the specification,
Figure BDA0001558542900000095
is a function of
Figure BDA0001558542900000098
For xen-1Is partially guided in
Figure BDA0001558542900000096
A value of (i) i
Figure BDA0001558542900000097
The nonlinear equation set (4) is approximately converted into a linear equation set expressed in the form of a matrix:
G·Δx＝b (7)
in the formula (I), the compound is shown in the specification,
Figure BDA0001558542900000101
Figure BDA0001558542900000102
Figure BDA0001558542900000103
xk＝xk-1+Δx (11)
Δx＝(GTG)-1GTb (12)
in the formula, delta x is the nonlinear least square solution of the matrix equation (7), and the real coordinate of the target node is [ xam-xem,yam-yem]And (m is 1, …, n), thereby completing the error correction of inertial navigation positioning.
In the invention, a vehicle-mounted data chain measures the distance between every two vehicles in an unmanned fleet by using a radio ranging method, simultaneously notifies inertial navigation data and ranging information in real time by using radio communication, and then transmits the ranging information and the inertial navigation data to a relative positioning processor; the vehicle-mounted inertial navigation system is an Inertial Navigation System (INS) and is an autonomous navigation system which does not depend on external information and does not emit energy; relative positioning processor refers to a processor system capable of performing relative positioning calculations.
Example 1
The results of the primary simulation performed according to the above steps in this embodiment are shown in table 1 and fig. 6 to 13, and the simulation conditions are set as follows: the number of the nodes of the networking is 6, and A, B, C, D total 4 unmanned vehicles exist in the unmanned vehicle fleet, wherein E, F is nodes with known positions in roads, and the coordinates of the nodes are E (0,0) and F (100,0), respectively. In the initial state, the coordinates of each unmanned vehicle are respectively as follows: a (200,0), B (0,100), C (300,200), D (400, 100). The vehicle a travels in the northeast direction at a speed of 25 m/s; the vehicle B runs in the north direction, and the running speed is 30 m/s; the vehicle C runs along the northeast direction, and the speed of the vehicle is 30 m/s; the vehicle D travels in the righteast direction at a speed of 20 m/s. The vehicle-mounted data chain outputs the ranging value at the same moment, a simulation calculation flow chart is shown in fig. 3, and the relative calculation period is 0.1 s.
Evaluating the networking positioning effect by using the standard deviation r and r' of the inertial navigation error before and after correction, as shown in formula (13),
Figure BDA0001558542900000111
in formula (13), xeaiAnd yeaiIs the inertial navigation positioning error, x, of the vehicle iegiAnd yegiAnd (4) iteratively correcting the inertial navigation position error of the vehicle i.
Table 1 simulation results of relative positioning
r/m r′/m
Inertial navigation of vehicle A 1.349 0.227
Inertial navigation of vehicle B 0.994 0.236
Inertial navigation of vehicle C 0.964 0.214
Inertial navigation of vehicle D 1.077 0.259
The above embodiments of the present invention are merely illustrative and not restrictive, and all modifications and substitutions which do not depart from the spirit and scope of the present invention should be embraced in the claims of the present invention.

Claims (2)

1. A relative positioning method for cooperative work of an unmanned fleet and a roadside unit is characterized by comprising the following steps:
step 1, deploying two road side units in a road network of an unmanned vehicle driving area, using the two road side units as known fixed points, installing a vehicle-mounted unit on each vehicle of an unmanned vehicle fleet, and establishing an unmanned vehicle fleet relative positioning system based on a vehicle-mounted inertial navigation system, a vehicle-mounted data link and a relative positioning processor;
step 2, determining information interaction contents between adjacent vehicles of the unmanned fleet and between the vehicles and a roadside unit during relative positioning, transmitting data of the vehicle-mounted inertial navigation system to a vehicle-mounted data chain, measuring the distance between the vehicles in the unmanned fleet by using a radio distance measurement method through the vehicle-mounted data chain, simultaneously reporting inertial navigation data and distance measurement information in real time by using radio communication, and transmitting the distance measurement information and the inertial navigation data to a relative positioning processor;
step 3, taking the error of the inertial navigation system of each unmanned vehicle as the amount to be estimated, measuring the difference between the measured distance between the vehicles and the distance calculated by the inertial navigation information as the amount to be estimated, and establishing a mathematical model of the relative positioning method of the unmanned vehicle fleet under the condition of no GNSS; then, solving the estimated value of each inertial navigation system error by using a nonlinear least square method and a Newton iteration method, and correcting the inertial navigation output position by using the feedback of the vehicle inertial navigation position error estimated value to complete vehicle positioning;
the mathematical model for establishing the relative positioning method of the unmanned fleet under the condition of no GNSS in the step 3 is specifically as follows:
the number of vehicles in the unmanned vehicle fleet is set to n, [ x ]am,yam]Is the output position coordinate of the vehicle m inertial navigation system, [ x ]tm,ytm]As the true coordinates of vehicle m, xem,yemIs the inertial navigation position error of the vehicle m, m is 1, …, n, then [ xam,yam]And [ x ]tm,ytm]And [ x ]em,yem]The relationship of (1) is:
Figure FDA0003454446730000011
the mutual distance measurement value between the vehicle i and the vehicle j is li,jI, j e {1, …, n }, a calculated distance d calculated from the vehicle coordinates output from the inertial navigation systemi,jExpressed as:
Figure FDA0003454446730000012
substituting (1) into (2) to obtain:
Figure FDA0003454446730000013
order to
Figure FDA0003454446730000021
The distance between any two unmanned vehicles or road side units is measured at the same time, and the distance is measured
Figure FDA0003454446730000022
Group data, which together make up the following
Figure FDA0003454446730000023
The equation is a mathematical model of the relative positioning of the unmanned fleet:
Figure FDA0003454446730000024
setting solution x ═ xe1,ye1,xe2,ye2,…,…,xen,yen) Is initially of
Figure FDA0003454446730000029
Each equation in the formula is
Figure FDA00034544467300000210
Carrying out linearization; in the estimation of the parameter x, the equation sets on both sides have 2n unknown parameters, at least 2n non-linearly related measurement equations are needed according to the mathematical principle, and in the mathematical model of the relative positioning of the unmanned vehicle fleet, at most
Figure FDA0003454446730000025
The independent measurement equations are adopted, so that at least 5 vehicle nodes are required in the unmanned fleet to realize relative positioning under the condition of no absolute positioning data; in the case of two fixed road side units with known positions, the two side equation sets have 2n-4 unknown parameters, at least 2n-4 non-linearly related measurement equations are needed according to the mathematical principle, and in the mathematical model of the relative positioning of the unmanned vehicle fleet, at most, the two unknown parameters are obtained
Figure FDA0003454446730000026
The independent measurement equations are adopted, so that the number of the unmanned vehicles is more than 1, namely at least two unmanned vehicles in the fleet can be accurately positioned;
the first in the system of equations
Figure FDA0003454446730000027
The taylor expansion of the equation is:
Figure FDA0003454446730000028
in the formula (I), the compound is shown in the specification,
Figure FDA0003454446730000031
is a function of
Figure FDA0003454446730000032
For xen-1Is partially guided in
Figure FDA0003454446730000033
A value of (i) i
Figure FDA0003454446730000034
The nonlinear equation set (4) is approximately converted into a linear equation set expressed in the form of a matrix:
G·Δx＝b (7)
in the formula (I), the compound is shown in the specification,
Figure FDA0003454446730000035
Figure FDA0003454446730000041
Figure FDA0003454446730000042
xk＝xk-1+Δx (11)
Δx＝(GTG)-1GTb (12)
in the formula, delta x is the nonlinear least square solution of the matrix equation (7), and the real coordinate of the target node is [ xam-xem,yam-yem]Thus completing the error correction of inertial navigation positioning.
2. The method for the relative positioning of the unmanned fleet of vehicles and the roadside unit in cooperation according to claim 1, wherein the step 1 of establishing the unmanned fleet of vehicles relative positioning system based on the vehicle-mounted inertial navigation system, the vehicle-mounted data link and the relative positioning processor comprises the following specific steps:
the method comprises the steps that two road side units are deployed in a road network of an unmanned vehicle driving area, an on-board unit is installed on each vehicle of an unmanned vehicle fleet, the on-board unit and the road side units are provided with wireless communication modules, a system network in the coverage range of the road side units has two basic communications, namely, the communication between the unmanned vehicles and the unmanned vehicles, and the communication between the unmanned vehicles and road side infrastructures such as the road side units, the vehicles in driving are communicated with each other by utilizing a wireless communication technology, and the road side units access an external network.
CN201810072492.3A 2018-01-25 2018-01-25 Relative positioning method for cooperative work of unmanned fleet and road side unit Active CN108415057B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810072492.3A CN108415057B (en) 2018-01-25 2018-01-25 Relative positioning method for cooperative work of unmanned fleet and road side unit

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810072492.3A CN108415057B (en) 2018-01-25 2018-01-25 Relative positioning method for cooperative work of unmanned fleet and road side unit

Publications (2)

Publication Number Publication Date
CN108415057A CN108415057A (en) 2018-08-17
CN108415057B true CN108415057B (en) 2022-04-01

Family

ID=63126200

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810072492.3A Active CN108415057B (en) 2018-01-25 2018-01-25 Relative positioning method for cooperative work of unmanned fleet and road side unit

Country Status (1)

Country Link
CN (1) CN108415057B (en)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111076716B (en) * 2018-10-18 2022-06-03 百度在线网络技术（北京）有限公司 Method, apparatus, device and computer-readable storage medium for vehicle localization
CN110207691B (en) * 2019-05-08 2021-01-15 南京航空航天大学 Multi-unmanned vehicle collaborative navigation method based on data link ranging
CN111123341B (en) * 2019-11-15 2023-03-10 西安电子科技大学 Three-dimensional co-location method for unmanned aerial vehicle group
CN111079079B (en) * 2019-11-29 2023-12-26 北京百度网讯科技有限公司 Data correction method, device, electronic equipment and computer readable storage medium
CN111708065B (en) * 2020-05-22 2023-07-14 广州杰赛科技股份有限公司 Positioning method, device and storage medium based on intelligent network-connected automobile
CN111707988B (en) * 2020-05-29 2023-06-20 江苏科技大学 Unmanned vehicle system positioning method based on unmanned vehicle-mounted UWB base station
CN113949984A (en) * 2020-06-30 2022-01-18 宝能汽车集团有限公司 Vehicle fleet communication method, vehicle fleet communication device and vehicle-mounted terminal
CN112629553B (en) * 2021-03-10 2021-06-15 华砺智行(武汉)科技有限公司 Vehicle co-location method, system and device under intelligent network connection environment
CN116929350B (en) * 2023-08-23 2023-11-21 南京理工大学 Rapid temporary reconstruction collaborative navigation system and method based on data link ranging
CN117880820B (en) * 2024-03-12 2024-05-17 南京纳特通信电子有限公司 Radio safety guarantee system and method for Internet of vehicles

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102353935A (en) * 2011-06-07 2012-02-15 北京万集科技股份有限公司 OBU (on board unit) positioning method, equipment and system based on time measurement
CN102565758A (en) * 2011-12-09 2012-07-11 北京握奇数据系统有限公司 Positioning device and method for vehicle-mounted unit in ETC (Electronic Toll Collection) system
CN104575039A (en) * 2015-01-19 2015-04-29 武汉理工大学 New emergency vehicle preferential pass method and system based on vehicle-road cooperation
CN104569962A (en) * 2014-09-11 2015-04-29 深圳市金溢科技股份有限公司 Road side unit as well as method and system for wirelessly positioning on-board unit
CN105206090A (en) * 2015-10-13 2015-12-30 厦门星辰天羽汽车设计有限公司 Vehicle positioning method
CN105467418A (en) * 2015-12-22 2016-04-06 中船航海科技有限责任公司 New relative positioning method for ship formation

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
ES2540866T3 (en) * 2011-12-27 2015-07-14 Kapsch Trafficcom Ag Procedure for the acquisition of traffic flow data in a road network

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102353935A (en) * 2011-06-07 2012-02-15 北京万集科技股份有限公司 OBU (on board unit) positioning method, equipment and system based on time measurement
CN102565758A (en) * 2011-12-09 2012-07-11 北京握奇数据系统有限公司 Positioning device and method for vehicle-mounted unit in ETC (Electronic Toll Collection) system
CN104569962A (en) * 2014-09-11 2015-04-29 深圳市金溢科技股份有限公司 Road side unit as well as method and system for wirelessly positioning on-board unit
CN104575039A (en) * 2015-01-19 2015-04-29 武汉理工大学 New emergency vehicle preferential pass method and system based on vehicle-road cooperation
CN105206090A (en) * 2015-10-13 2015-12-30 厦门星辰天羽汽车设计有限公司 Vehicle positioning method
CN105467418A (en) * 2015-12-22 2016-04-06 中船航海科技有限责任公司 New relative positioning method for ship formation

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A New Positioning Technique for RSS-Based Localization Based on a Weighted Least Squares Estimator;Paula Tarrío et al.;《2008 IEEE International Symposium on Wireless Communication Systems》;20081024;第633-637页 *
最小二乘算法在JTIDS相对导航中的应用;胡景疆 等;《通信技术》;20090410;第42卷(第4期);第212-217页 *
车联网定位与位置感知技术研究;王群 等;《南京师大学报（自然科学版）》;20150331;第38卷(第1期);第66-74页 *

Also Published As

Publication number Publication date
CN108415057A (en) 2018-08-17

Similar Documents

Publication Publication Date Title
CN108415057B (en) Relative positioning method for cooperative work of unmanned fleet and road side unit
CN105682222B (en) A kind of vehicle location positioning information fusion method based on vehicle self-organizing network
Adegoke et al. Infrastructure Wi-Fi for connected autonomous vehicle positioning: A review of the state-of-the-art
Parker et al. Vehicle localization in vehicular networks
CN106454722A (en) Dynamic differential positioning method based on map matching for mobile communication terminals
Elazab et al. Integrated cooperative localization for vehicular networks with partial GPS access in urban canyons
Ahammed et al. Vloci: Using distance measurements to improve the accuracy of location coordinates in gps-equipped vanets
CN102096702B (en) Process system of vehicle travel mileage and process method thereof
CN106710281A (en) Vehicle positioning data acquisition method and device
CN105792135A (en) Method and device for positioning lane where vehicle is
Yan et al. A grid-based on-road localization system in VANET with linear error propagation
CN111123334A (en) Multi-vehicle cooperative positioning platform and positioning method under limit working condition
CN105866731A (en) Vehicle location method and vehicle terminal device
Yan et al. GOT: Grid-based on-road localization through inter-vehicle collaboration
Baek et al. Accurate vehicle position estimation using a Kalman filter and neural network-based approach
Santos et al. A roadside unit-based localization scheme to improve positioning for vehicular networks
US20210270614A1 (en) Method of locating a vehicle
Ahammed et al. LICA: robust localization using cluster analysis to improve GPS coordinates
de Ponte Müller et al. Cooperative infrastructure-based vehicle positioning
Efatmaneshnik et al. A cooperative positioning algorithm for dsrc enabled vehicular networks
Huang et al. Cv2x-loca: Roadside unit-enabled cooperative localization framework for autonomous vehicles
Ahammed et al. Vloci2: Improving 2d location coordinates using distance measurements in gps-equipped vanets
CN113727434B (en) Vehicle-road cooperative auxiliary positioning system and method based on edge computing gateway
KR100555695B1 (en) A Method and System for caculating average speed of vehicle using DSRC and DGPS techniques in Traveler Information System
Shit et al. Localization for autonomous vehicle: Analysis of importance of IoT network localization for autonomous vehicle applications

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zhu Jianliang

Inventor after: Wang Wenqi

Inventor after: Bai Jingji

Inventor after: Chen Chen

Inventor after: Zhang Guoqing

Inventor before: Wang Wenqi

Inventor before: Zhu Jianliang

Inventor before: Bai Jingji

Inventor before: Chen Chen

Inventor before: Zhang Guoqing

GR01 Patent grant
GR01 Patent grant