CN115032672A - Fusion positioning method and system based on positioning subsystem - Google Patents

Abstract

The invention relates to a fusion positioning method and system based on a positioning subsystem. The method includes: acquiring real-time first position information of a target according to a satellite positioning system; Second position information; real-time calculation of third position information of the target based on a plurality of second position information and dead reckoning system; In a mutual intersection relationship, the first position information, the second position information and the third position information are fused. Through the fusion of multiple positioning systems, the invention can improve the positioning accuracy and robustness of a single positioning system. Since no parameter setting is required in the fusion process, the invention is simple and convenient, and has high execution efficiency. high question.

Description

A fusion positioning method and system based on positioning subsystem

technical field

The invention belongs to the technical field of positioning, and in particular relates to a fusion positioning method and system based on a positioning subsystem.

Background technique

Multi-sensor fusion positioning or multi-sensing system usually refers to a positioning system including visual sensors (such as cameras), inertial measurement units (Inertial Measurement Unit, IMU, "Inertial Navigation") and Global Navigation Satellite System (Global Navigation Satellite System, GNSS) positioning, etc. , the system has the advantages of high positioning accuracy and low cost, and has become the main positioning solution for application scenarios that require high-precision (centimeter-level) positioning results. In the process of obtaining high-precision positioning results through sensor fusion positioning, it is necessary to determine the parameters such as the position and attitude of each sensor, and determine the covariance of these parameters, so as to estimate the positioning accuracy radius of fusion positioning and evaluate the correlation.

The prior art generally uses the Hessian matrix inversion method for covariance estimation, but the parameters of multi-sensor fusion positioning have a large number of dimensions, and the direct matrix inversion consumes a lot of computing power, which leads to an increase in system costs. , the approximation will affect the accuracy of the estimated covariance. Therefore, new covariance determination schemes need to be provided to meet the requirements of cost and accuracy.

SUMMARY OF THE INVENTION

In order to solve the problems of large computing power consumption and high cost in the process of fusion of multiple positioning systems, a first aspect of the present invention provides a fusion positioning method based on a positioning subsystem, including: obtaining a real-time first number of targets according to a satellite positioning system. a position information; based on the first position information, use the matching positioning system to calculate the second position information of the target in real time; based on a plurality of second position information and the dead reckoning system to calculate the third position information of the target in real time; A mutual intersection of the location areas covered by the respective covariances of the location information, the second location information, and the third location information, and the first location information, the second location information, and the third location information are fused.

In some embodiments of the present invention, calculating the second position information of the target in real time by using a matching positioning system based on the first position information includes: acquiring real-time first position information O ₁ (x ₁ of the target according to a satellite positioning system) , y ₁ )～(0, r ₁ ); extract the local high-precision map within the preset range of the first position information O ₁ (x ₁ , y ₁ )～(0, r ₁ ), and extract the map through visual recognition and match it with the first position information O ₁ (x ₁ , y ₁ )～(0, r ₁ ) to obtain the position information O ₂ (x ₂ , y ₂ )～(0, r ₂ ).

Further, the described real-time calculation of the third position information of the target based on multiple second position information and the dead reckoning system includes: based on multiple frames of the first position information or the second position information, utilizing dead reckoning to output the position information. ₂ (x ₂ , y ₂ ) to (0, r ₂ ) are solved to obtain position information O ₃ (x ₃ , y ₃ ) to (0, r ₃ ).

In some embodiments of the present invention, the first location information, the third The fusion of the second position information and the third position information includes: using the coordinates in the first position information, the second position information and the third position information as the center of the circle, and the covariance of each position information as the radius, constructing a Covered location area; according to the mutual intersection relationship between the covered location area of each location information and the covered location areas of the remaining two location information, and fuse them by the least square method.

Further, according to the mutual intersection relationship between the location area covered by each location information and the location areas covered by the remaining two location information, and the fusion thereof by the least squares method includes: if the coverage of the three location information is If there is one intersection point in the location area, the corresponding location information of the intersection point is used as the fused location information; if there are at least two intersection points in the location area covered by the three location information, the position information corresponding to the center of the circle jointly determined by the intersection points is used. as the fused position information.

In the above-mentioned embodiment, the method further includes: if the distance between the position determined by the second position information and the position determined by the first position information is lower than a threshold, setting the third position information to be equal to the second position information.

In a second aspect of the present invention, a fusion positioning system based on a positioning subsystem is provided, comprising: a determination module for acquiring real-time first position information of a target according to a satellite positioning system; a calculation module for obtaining real-time first position information of a target based on the first position information, the second position information of the target is calculated in real time by the matching positioning system; the third position information of the target is calculated in real time based on a plurality of second position information and the dead reckoning system; the fusion module is used for according to the first position information, The mutual intersection relationship of the location areas covered by the respective covariances of the second location information and the third location information, and the first location information, the second location information, and the third location information are fused.

A third aspect of the present invention provides an electronic device, comprising: one or more processors; a storage device for storing one or more programs, when the one or more programs are stored by the one or more programs The processor executes, so that the one or more processors implement the fusion positioning method based on the positioning subsystem provided in the first aspect of the present invention.

A fourth aspect of the present invention provides a computer-readable medium on which a computer program is stored, wherein, when the computer program is executed by a processor, the fusion positioning based on the positioning subsystem provided in the first aspect of the present invention is implemented method.

The beneficial effects of the present invention are:

1. There will be cumulative errors based on traditional inertial navigation, and visual or high-precision maps are affected by the accuracy of the map itself and sensors. In some scenarios such as under overpasses and parking lots, satellite positioning systems cannot accurately locate and calculate. The force requirement is high; the present invention improves the positioning accuracy and robustness of a single positioning system through the fusion of multiple positioning systems;

2. Data fusion based on the covariance of the positioning subsystem has better real-time performance because it does not require parameter settings; through the fusion of position information of different precision, the costs and calculations of the inertial navigation system, visual matching system and satellite positioning system are integrated. Therefore, it solves the problems of large computing power consumption and high cost in the process of multi-positioning system fusion.

Description of drawings

FIG. 1 is a basic flowchart of a fusion positioning method based on a positioning subsystem in some embodiments of the present invention;

2 is a schematic diagram of positioning information fusion when there is an intersection point in the location regions covered by the respective covariances of the first location information, the second location information, and the third location information in some embodiments of the present invention;

3 is a schematic diagram of positioning information fusion when there are multiple intersection points in the location regions covered by the respective covariances of the first location information, the second location information, and the third location information in some embodiments of the present invention;

4 is a schematic structural diagram of a fusion positioning system based on a positioning subsystem in some embodiments of the present invention;

FIG. 5 is a schematic structural diagram of an electronic device in some embodiments of the present invention.

Detailed ways

The principles and features of the present invention will be described below with reference to the accompanying drawings. The examples are only used to explain the present invention, but not to limit the scope of the present invention.

1 and 2, in a first aspect of the present invention, a fusion positioning method based on a positioning subsystem is provided, including: S100. Acquire real-time first position information of a target according to a satellite positioning system; S200. Based on the The first position information, the second position information of the target is calculated in real time by the matching positioning system; the third position information of the target is calculated in real time based on the plurality of second position information and the dead reckoning system; S300. According to the first position information, the third position information The mutual intersection relationship of the location regions covered by the respective covariances of the second location information and the third location information, and the first location information, the second location information, and the third location information are fused.

It can be understood that the positioning subsystem in the embodiment of the present invention usually includes a visual sensor, an inertial navigation system and a global navigation satellite system. Using multi-sensor positioning needs to determine the sensor parameters through iterative optimization according to the measurement data of the multi-sensor, that is, the positioning process. The positioning parameters related to the sensor that need to be optimized in the system can also be referred to as positioning parameters or position information for short.

Specifically, the sensor parameters may be direct pose parameters, or may be other parameters to be optimized in the process of determining the pose parameters. For example, the sensor parameters to be iteratively optimized during initialization include: the pose of the visual sensor (each frame position and attitude relative to the first frame), the scale of the vision sensor, the speed of the vision sensor, the external rotation parameters between the camera coordinate system and the world coordinate system, and the gyroscope bias, etc. The sensor parameters iteratively optimized during the positioning process include: The pose in the relative world coordinate system, the pose in the absolute world coordinate system, the rotation external parameter between the relative world coordinate system and the absolute world coordinate system, and the relative position external parameter between the antenna of the global positioning system and the inertial navigation system, etc. In the iterative optimization process, it is necessary to establish multiple residual factors including the measurement data of each sensor, that is, the observation data and the sensor parameters to be optimized. Each residual factor may include part of the observation data and part of the sensor parameters.

Referring to FIG. ₂ and FIG. ₃ , the position information can be expressed by the coordinates and the accuracy ( _error ) obtained after the transformation _of the positioning parameters. The coordinates of the acquired first position information are (x ₁ , y ₁ ), the position area covered by the circle with the covariance r ₁ as the center; O ₂ (x ₂ , y ₂ )～(0, r ₂ ) or O The location area represented by ₃ (x ₃ , y ₃ )～(0, r ₃ ) is analogous; the location area is usually flat, and in some special scenes, it can contain three-dimensional information. According to the GPS_RTK positioning system, the position information O ₁ (x ₁ , y ₁ )～(0, r ₁ ) can be directly given.

In step S200 of some embodiments of the present invention, calculating the second position information of the target in real time by using a matching positioning system based on the first position information includes: S201. Acquiring real-time first position information of the target according to a satellite positioning system O ₁ (x ₁ , y ₁ )～(0, r ₁ ); S202. Extract the local high-precision first position information O ₁ (x ₁ , y ₁ )～(0, r ₁ ) within a preset range Map, extract the fixed elements of the map through visual recognition and match them with the first position information O ₁ (x ₁ , y ₁ )～(0, r ₁ ) to obtain the position information O ₂ (x ₂ , y ₂ )～ (0, r ₂ ).

Specifically, the position information O ₁ ( _{x 1} , _{y 1} )～(0, r ₁ ) can be directly given according to the _{GPS_RTK} positioning system _; The local high-precision map of the location is matched with information such as lane lines and road signs extracted by visual methods (visual recognition or semantic recognition), and the matching output location information O ₂ (x ₂ , y ₂ )～(0, r ₂ ) ); for example, the visual sensor (camera or lidar) obtains or identifies an image of a road sign or landmark building within the visible range of the target’s current location, which records the specific location information of the current location, After matching with the high-precision map, position information O ₂ (x ₂ , y ₂ )～(0, r ₂ ) is obtained.

In some embodiments, since the dead reckoning has short-term high accuracy, multiple frames of historical position information are required for trajectory estimation. Therefore, based on the above steps S201 and S202, the real-time dead reckoning system based on multiple second position information and Calculating the third position information of the target includes: S203. Based on the multi-frame first position information or the second position information, use dead track reckoning to perform the output position information O ₂ (x ₂ , y ₂ )～(0, r ₂ ) The solution is to obtain the position information O ₃ (x ₃ , y ₃ ) to (0, r ₃ ).

Specifically, there is no fusion positioning in the first frame of data, and the dead reckoning is performed according to the position information O ₂ (x ₂ , y ₂ )～(0, r ₂ ) output by map matching, and the position information O ₃ (x ₃ , y ₃ )～(0, r ₃ ), the dead reckoning has short-term high precision, so set r ₃ =r ₂ ; after the first frame, the dead reckoning is solved according to the fusion positioning information, and the position information O ₃ ( x ₃ , y ₃ )～(0, r ₃ ), at this time, the dead reckoning accuracy r ₃ =r.

In S300 of some embodiments of the present invention, according to the mutual intersection of the location areas covered by the respective covariances of the first location information, the second location information, and the third location information, the first location information is The fusion of the second position information and the third position information includes: S301. Taking the coordinates in the first position information, the second position information and the third position information as the center of the circle, the covariance of each position information is the radius, and constructing each The location areas covered by each location information; S302. According to the mutual intersection relationship between the location areas covered by each location information and the location areas covered by the remaining two location information, fuse them by the least square method.

2 and 3, it can be understood that, taking the positioning point as the center of the circle and the covariance of the positioning point as the radius, according to the spatial position information of the three subsystems in step S100, the given position information can be abstracted as the intersection of three circles. The problem of intersection of three circles is mainly divided or processed according to the intersection point, therefore: the mutual intersection relationship between the location area covered by each location information and the location area covered by the remaining two location information, and The fusion of the least squares method includes: S3021. If there is an intersection in the position area covered by the three position information, then the corresponding position information of the intersection is used as the fused position information; S3022. If the three position information covers If there are at least two intersection points in the position area, the position information corresponding to the center of the circle jointly determined by the intersection points is used as the fused position information.

Specifically, Fig. 2 shows the situation where the same point (intersection point) exists for three circles. If the coordinates of the intersection point are O(x, y) ~ (0, r), the point satisfies the following equations:

Among them, (x _i , y _i ) represents the coordinates of the _ith subsystem, (0, ri ) represents that the subsystem coordinates satisfy the mean value of 0, and the covariance is Gaussian distribution of ri _i . Since the true value exists in a circle with the coordinates of each subsystem as the center and the covariance as the radius.

Figure 3 shows the situation where three circles have the same intersection (multiple intersections), and the three points in the intersection area are set as a ₁ , b ₁ , and c ₁ respectively, and the circle formed by the coordinates of the three points is set as O(x, y), the radius is r. Then the intersection points a ₁ , b1 and c ₁ satisfy the equation:

Point a1 satisfies the following equation:

Point b1 satisfies the following equation:

Point c1 satisfies the following equation:

In other cases, use equation (6) to solve the minimum covariance in the positioning subsystem to determine the final fusion positioning information:

It can be understood that, for the equations in step 200, the least squares or Gauss-Newton method can be used to solve the equation system: Equation (1) can be expanded into Equation (7):

The equation is sorted out:

which is:

The above formula is the classical AX=B problem, let

Among them, the A and B matrices are known matrices, which can be solved by least squares

X=(A ^T *A)*A ^T *B (13);

Output fusion positioning position information (x, y)～(0, r=0).

In particular, the above solution method is rewritten as the following equation according to the "three-circle intersection problem":

Using least squares to solve X=(A ^T *A)*A ^T *B, (x,y)～(0,r=R) can be obtained.

According to the minimum covariance of the three subsystems solved by the above steps, the fusion positioning position information is determined, as follows:

In some embodiments, due to the short-term high precision of dead reckoning, multiple frames of historical position information (given by the first positioning information or the second positioning information) are required for trajectory reckoning, that is, in the first position information or the second position When the information positioning is inaccurate, or the error exceeds the threshold, the historical position information is invalid or the target remains stationary. At this time, the third position information needs to be corrected, that is, in the above-mentioned embodiment, it also includes: S400. If the second position information If the distance between the determined location and the location determined by the first location information is lower than the threshold, the third location information is set to be equal to the second location information.

Example 2

Referring to FIG. 4 , a second aspect of the present invention provides a fusion positioning system 1 based on a positioning subsystem, comprising: a determination module 11 for acquiring real-time first position information of a target according to a satellite positioning system; a calculation module 12, for calculating the second position information of the target in real time based on the first position information by using the matching positioning system; calculating the third position information of the target in real time based on multiple second position information and the dead reckoning system; the fusion module 13, for The first position information, the second position information and the third position information are fused according to the mutual intersection of the position areas covered by the respective covariances of the first position information, the second position information and the third position information.

Further, the calculation module 12 includes: a first calculation unit for calculating the second position information of the target in real time by using a matching positioning system based on the first position information; a second calculation unit for based on a plurality of second position information The position information and dead reckoning system calculates the third position information of the target in real time.

Example 3

5, a third aspect of the present invention provides an electronic device, comprising: one or more processors; a storage device for storing one or more programs, when the one or more programs are described One or more processors execute such that the one or more processors implement the method of the first aspect of the invention.

Electronic device 500 may include processing means (eg, central processing unit, graphics processor, etc.) 501 that may be loaded into random access memory (RAM) 503 according to a program stored in read only memory (ROM) 502 or from storage means 508 program to perform various appropriate actions and processes. In the RAM 503, various programs and data necessary for the operation of the electronic device 500 are also stored. The processing device 501 , the ROM 502 , and the RAM 503 are connected to each other through a bus 504 . An input/output (I/O) interface 505 is also connected to bus 504 .

Typically the following devices can be connected to the I/O interface 505: input devices 506 including, for example, a touch screen, touchpad, keyboard, mouse, camera, microphone, accelerometer, gyroscope, etc.; including, for example, a liquid crystal display (LCD), speakers, vibrators output device 507 , etc.; storage device 508 including, for example, a hard disk; and communication device 509 . Communication means 509 may allow electronic device 500 to communicate wirelessly or by wire with other devices to exchange data. While FIG. 5 shows electronic device 500 having various means, it should be understood that not all of the illustrated means are required to be implemented or provided. More or fewer devices may alternatively be implemented or provided. Each block shown in FIG. 5 can represent one device, and can also represent multiple devices as required.

In particular, according to embodiments of the present disclosure, the processes described above with reference to the flowcharts may be implemented as computer software programs. For example, embodiments of the present disclosure include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the method illustrated in the flowchart. In such an embodiment, the computer program may be downloaded and installed from the network via the communication device 509 , or from the storage device 508 , or from the ROM 502 . When the computer program is executed by the processing device 501, the above-described functions defined in the methods of the embodiments of the present disclosure are performed. It should be noted that the computer-readable medium described in the embodiments of the present disclosure may be a computer-readable signal medium or a computer-readable storage medium, or any combination of the above two. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus or device, or a combination of any of the above. More specific examples of computer readable storage media may include, but are not limited to, electrical connections with one or more wires, portable computer disks, hard disks, random access memory (RAM), read only memory (ROM), erasable Programmable read only memory (EPROM or flash memory), optical fiber, portable compact disk read only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above. In embodiments of the present disclosure, a computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. Rather, in embodiments of the present disclosure, a computer-readable signal medium may include a data signal in baseband or propagated as part of a carrier wave, carrying computer-readable program code therein. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the foregoing. A computer-readable signal medium can also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device . Program code embodied on a computer readable medium may be transmitted using any suitable medium including, but not limited to, electrical wire, optical fiber cable, RF (radio frequency), etc., or any suitable combination of the foregoing.

The above-mentioned computer-readable medium may be included in the above-mentioned electronic device; or may exist alone without being assembled into the electronic device. The above-mentioned computer-readable medium carries one or more computer programs, and when the above-mentioned one or more programs are executed by the electronic device, the electronic device:

Computer program code for carrying out operations of embodiments of the present disclosure may be written in one or more programming languages, including object-oriented programming languages—such as Java, Smalltalk, C++, Python, or a combination thereof, Also included are conventional procedural programming languages - such as the "C" language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer, or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (eg, using an Internet service provider to via Internet connection).

The flowchart and block diagrams in the Figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more logical functions for implementing the specified functions executable instructions. It should also be noted that, in some alternative implementations, the functions noted in the blocks may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It is noted that each block of the block diagrams and/or flowchart illustrations, and combinations of blocks in the block diagrams and/or flowchart illustrations, can be implemented by dedicated hardware-based systems that perform the specified functions or operations, Alternatively, it may be implemented in a combination of dedicated hardware and computer instructions.

The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection of the present invention. within the range.

Claims (10)

1. a fusion positioning method based on positioning subsystem, is characterized in that, comprises: Obtain the real-time first position information of the target according to the satellite positioning system; Based on the first position information, the second position information of the target is calculated in real time by using the matching positioning system; the third position information of the target is calculated in real time based on the plurality of second position information and the dead reckoning system; The first position information, the second position information and the third position information are fused according to the mutual intersection of the position areas covered by the respective covariances of the first position information, the second position information and the third position information. 2. The fusion positioning method based on a positioning subsystem according to claim 1, wherein, based on the first position information, using the matching positioning system to calculate the second position information of the target in real time comprises: Obtain real-time first position information O ₁ (x ₁ , y ₁ )～(0, r ₁ ) of the target according to the satellite positioning system; Extract the local high-precision map within the preset range of the first position information O ₁ (x ₁ , y ₁ ) ~ (0, r ₁ ), extract the fixed elements of the map through visual recognition and combine them with the first position information O ₁ (x ₁ , y ₁ ) to (0, r ₁ ) are matched to obtain position information O ₂ (x ₂ , y ₂ ) to (0, r ₂ ). 3. The fusion positioning method based on a positioning subsystem according to claim 2, wherein the third position information of the real-time calculation target based on a plurality of second position information and the dead reckoning system comprises: Based on the first position information or the second position information of multiple frames, the output position information O ₂ (x ₂ ,y ₂ )～(0,r ₂ ) is solved by dead reckoning, and the position information O ₃ (x ₃ ) is solved. ,y ₃ )～(0,r ₃ ). 4. The fusion positioning method based on positioning subsystem according to claim 1, characterized in that, according to the mutual covariance of the position areas covered by the respective covariances of the first position information, the second position information and the third position information The intersection and union relationship, the fusion of the first position information, the second position information and the third position information includes: Taking the coordinates in the first position information, the second position information and the third position information as the center of the circle, and the covariance of each position information as the radius, construct the location area covered by each position information; According to the mutual intersection relationship between the location area covered by each location information and the location areas covered by the remaining two location information, they are fused by the least square method. 5 . The fusion positioning method based on positioning subsystem according to claim 4 , wherein the mutual intersection relationship between the position area covered by each position information and the position areas covered by the remaining two position information , and fused by the least squares method including: If there is an intersection in the location area covered by the three location information, the corresponding location information of the intersection is used as the fused location information; If there are at least two intersection points in the position areas covered by the three position information, the position information corresponding to the center of the circle jointly determined by the intersection points is used as the fused position information. 6. The fusion positioning method based on the positioning subsystem according to any one of claims 1 to 5, further comprising: if the distance between the position determined by the second position information and the position determined by the first position information is lower than a threshold , the third position information is set to be equal to the second position information. 7. A fusion positioning system based on a positioning subsystem, comprising: A determination module, used for acquiring real-time first position information of the target according to the satellite positioning system; a calculation module for calculating the second position information of the target in real time based on the first position information using the matching positioning system; calculating the third position information of the target in real time based on a plurality of second position information and the dead reckoning system; The fusion module is configured to analyze the first location information, the second location information and the third location information is fused. 8. The fusion positioning system based on the positioning subsystem according to claim 7, the computing module comprises: a first calculation unit, configured to calculate the second position information of the target in real time by using a matching positioning system based on the first position information; The second calculation unit is configured to calculate the third position information of the target in real time based on the plurality of second position information and the dead reckoning system. 9. An electronic device comprising: one or more processors; a storage device for storing one or more programs that, when executed by the one or more processors, cause the one or more programs to The one or more processors implement the fusion positioning method based on the positioning subsystem according to any one of claims 1 to 6. 10. A computer-readable medium on which a computer program is stored, wherein the computer program implements the fusion positioning method based on a positioning subsystem according to any one of claims 1 to 6 when the computer program is executed by a processor.

Images