Fusion positioning method and device based on multi-source sensor and storage medium

Abstract

The invention discloses a fusion positioning method and device based on a multi-source sensor, and a storage medium. The method comprises the steps of obtaining first absolute position information through GNSS positioning data; carrying out position course calculation on the distance measurement data of the multi-label UWB to obtain three-dimensional position information of a vehicle body; iMU data of an inertial measurement unit of the unmanned vehicle are obtained and calculated to obtain inclination angle information; performing pose initialization based on the absolute position information, the three-dimensional position information and the inclination angle information to obtain initial pose information; acquiring laser radar point cloud data of the unmanned vehicle, and matching the laser radar point cloud data with an electronic map to obtain map pose information of the vehicle body; obtaining wheel speed mileage data of the unmanned vehicle and obtaining vehicle body longitudinal speed information according to the wheel speed mileage data; and inputting the absolute position information, the inclination angle information, the initial pose information, the vehicle body longitudinal speed information, the distance measurement data and the map pose information into an extended Kalman filter, and generating second absolute position information as state estimation output.

Description

Technical Field

[0001] The present invention relates to the field of sensor positioning technology, and in particular to a fusion positioning method, device and storage medium based on multi-source sensors. Background Art

[0002] UWB positioning has the characteristics of high positioning accuracy, low power consumption, high security, and strong signal penetration. It is very suitable for warehouse environments without GNSS signals. Ultra-wideband ranging uses time-of-flight ranging, which calculates the transmission time of radio electromagnetic waves and then converts the transmission time into distance. The ranging process involves two devices, a base station and a tag. The tag sends information to the base station at a certain moment, and the signal reaches the base station after a period of propagation. After the base station receives the information from the tag, it sends feedback information to the tag. After the tag receives the feedback information from the base station, it records the propagation time of the round-trip signal to accurately obtain the distance between the tag and the base station. However, when UWB is applied in warehouses, there are also non-line-of-sight phenomena such as signal obstruction and reflection, which may cause the UWB positioning error to increase or the positioning to fail. Therefore, it is necessary to adopt multi-sensor fusion methods to obtain accurate and stable positioning information in the warehouse environment. Summary of the invention

[0003] In order to overcome the shortcomings of the above-mentioned background technology that UWB is applied in warehouses and also has non-line-of-sight phenomena such as signal blocking and reflection, which may cause the UWB positioning error to increase or positioning failure, it is necessary to adopt multi-sensor fusion means to obtain accurate and stable positioning information in the warehouse environment. The purpose of the present invention is to provide a fusion positioning method based on multi-source sensors.

[0004] In order to achieve the above purpose, the present invention adopts the following technical solution:

[0005] A first aspect of the present invention provides a fusion positioning method based on multi-source sensors, comprising the following steps:

[0006] Acquire GNSS positioning data of the unmanned vehicle, and obtain first absolute position information through the GNSS positioning data;

[0007] Obtaining the distance measurement data of the multi-tag UWB of the unmanned vehicle, and obtaining the three-dimensional position information of the vehicle body by performing position and heading calculation on the distance measurement data;

[0008] Acquire IMU data of the inertial measurement unit of the unmanned vehicle, and obtain inclination information by solving the IMU data;

[0009] Performing posture initialization based on the absolute position information, the three-dimensional position information and the inclination information to obtain initial posture information;

[0010] Acquire laser radar point cloud data of the unmanned vehicle, match the laser radar point cloud data with the electronic map, and obtain map position information of the vehicle body;

[0011] Obtaining wheel speed mileage data of the unmanned vehicle, and obtaining vehicle longitudinal speed information according to the wheel speed mileage data;

[0012] The absolute position information, inclination information, initial posture information, vehicle longitudinal velocity information, distance measurement data and map posture information are input into an extended Kalman filter to generate second absolute position information as a state estimation output.

[0013] Compared with the prior art, the beneficial effects of the present invention are as follows: the multi-sensor fusion positioning method based on GNSS, UWB, IMU and 3D laser radar can adapt to navigation and positioning in both open-air and warehouse environments, realize the judgment of non-line-of-sight phenomena such as UWB signal occlusion and reflection, and obtain accurate navigation and positioning data of vehicles in two different working environments. Through the fusion positioning and attitude determination composed of GNSS, multi-tag UWB, IMU, laser radar and other sensors, the UWB non-line-of-sight situation can be judged, and the dual-tag UWB can solve the vehicle heading, so that the system has complete attitude measurement information.

[0014] In some possible implementations, the tilt angle information includes a pitch angle, a roll angle, and a yaw angle.

[0015] In some possible implementations, the three-dimensional position information is obtained by wirelessly receiving UWB signals to obtain the base station ID number and the distance between the tag and the base station. When more than four UWB base station data information is received, the three-dimensional position of the tag is solved to determine the three-dimensional position information of the vehicle.

[0016] In some possible implementations, the IMU installation error is compensated to process the arm effect of the GNSS antenna and the UWB tag, as follows:

[0017] Assume that the installation position of the GNSS antenna / UWB tag in the vehicle coordinate system with the IMU as the origin is l b ;

[0018] Assume that the installation error angles of the IMU are the error pitch angle δα, the error roll angle δγ and the error azimuth angle δβ. Then the arm of the GNSS antenna / UWB tag in the IMU coordinate system is:

[0019] l i =(I-μ×)l b

[0020]

[0021] The position of the UWB tag in the navigation coordinate system is:

[0022]

[0023] P is the vehicle position.

[0024] A second aspect of the present invention provides a fusion positioning device based on a multi-source sensor, comprising:

[0025] A first absolute position information acquisition module: used to acquire GNSS positioning data of the unmanned vehicle, and obtain first absolute position information through the GNSS positioning data;

[0026] Three-dimensional position information calculation module: used to obtain the distance measurement data of the multi-tag UWB of the unmanned vehicle, and obtain the three-dimensional position information of the vehicle body by performing position and heading calculation on the distance measurement data;

[0027] Inclination information calculation module: used to obtain the IMU data of the inertial measurement unit of the unmanned vehicle, and obtain the inclination information by calculating the IMU data;

[0028] An initial posture information acquisition module is used to initialize the posture based on the first absolute position information, the three-dimensional position information and the inclination information to obtain the initial posture information;

[0029] Map position information acquisition module: used to obtain the laser radar point cloud data of the unmanned vehicle, match the laser radar point cloud data with the electronic map, and obtain the map position information of the vehicle body;

[0030] Longitudinal speed information acquisition module: used to acquire wheel speed mileage data of the unmanned vehicle, and obtain the longitudinal speed information of the vehicle body according to the wheel speed mileage data;

[0031] State estimation output module: used to input the absolute position information, inclination information, initial posture information, vehicle longitudinal speed information, distance measurement data and map posture information into the extended Kalman filter to generate second absolute position information as state estimation output.

[0032] In some possible implementations, the tilt angle information includes a pitch angle, a roll angle, and a yaw angle.

[0033] In some possible implementations, the three-dimensional position information is obtained by wirelessly receiving UWB signals to obtain the base station ID number and the distance between the tag and the base station. When more than four UWB base station data information is received, the three-dimensional position of the tag is solved to determine the three-dimensional position information of the vehicle.

[0034] In some possible implementations, the IMU installation error is compensated to process the arm effect of the GNSS antenna and the UWB tag, as follows:

[0035] Assume that the installation position of the GNSS antenna / UWB tag in the vehicle coordinate system with the IMU as the origin is l b ;

[0036] Assume that the installation error angles of the IMU are the error pitch angle δα, the error roll angle δγ and the error azimuth angle δβ. Then the arm of the GNSS antenna / UWB tag in the IMU coordinate system is:

[0037] l i =(I-μ×)l b

[0038]

[0039] The position of the UWB tag in the navigation coordinate system is:

[0040]

[0041] P is the vehicle position.

[0042] According to a third aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned multi-source sensor-based fusion positioning method are performed. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 The present invention is a flowchart of the overall steps of the fusion positioning method based on multi-source sensors according to an embodiment of the present invention;

[0044] Figure 2 A flowchart of the detailed steps of a fusion positioning method based on multi-source sensors according to an embodiment of the present invention;

[0045] Figure 3 It is a schematic diagram of the structure of a fusion positioning device based on multi-source sensors according to an embodiment of the present invention. DETAILED DESCRIPTION

[0046] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby making a clearer and more definite definition of the protection scope of the present invention.

[0047] See attached Figure 1 and 2 As shown, the fusion positioning method based on multi-source sensors in this embodiment includes the following steps:

[0048] S1: Obtain GNSS positioning data of the unmanned vehicle, and obtain first absolute position information through the GNSS positioning data. The first absolute position information includes first two-dimensional position information, first speed information and first heading information.

[0049] S2: Obtain the distance measurement data of the multi-tag UWB of the unmanned vehicle, and obtain the three-dimensional position information of the vehicle body by performing position and heading calculation on the distance measurement data. Specifically, the three-dimensional position information obtains the base station ID number and the distance between the tag and the base station by wirelessly receiving the UWB signal. When receiving data information from more than four UWB base stations, the three-dimensional position of the tag is calculated to determine the three-dimensional position information of the vehicle body.

[0050] S3: Obtain the IMU data of the unmanned vehicle's inertial measurement unit, and obtain the inclination information by solving the IMU data. Specifically, the IMU data includes data such as angular velocity and acceleration, and the pitch angle, roll angle and yaw angle of the vehicle body are solved. For an IMU that can accurately solve the Euler angle by itself, the Euler angle can be directly applied to the filter fusion.

[0051] S4: Initialize the posture based on the absolute position information, three-dimensional position information and inclination information to obtain initial posture information.

[0052] S5: Obtain the laser radar point cloud data of the unmanned vehicle, match the laser radar point cloud data with the electronic map, and obtain the map pose information of the vehicle body. The map pose information is the position and pose of the vehicle body in the map coordinate system. Using this pose information, the transformation relationship between the odometer coordinate system and the map coordinate system can be solved.

[0053] S6: Obtain wheel speed mileage data of the unmanned vehicle, and obtain the longitudinal speed information of the vehicle body according to the wheel speed mileage data. Specifically, the encoder pulse signal is released by the integral odometer, and the vehicle movement track is calculated according to the wheel speed mileage data, and the longitudinal speed information of the vehicle body is obtained as the input of the extended Kalman filter.

[0054] S7: Input the absolute position information, inclination information, initial posture information, vehicle longitudinal velocity information, distance measurement data and map posture information into an extended Kalman filter to generate second absolute position information as a state estimation output. The second absolute position information includes second two-dimensional position information, second velocity information and second heading information.

[0055] The multi-sensor fusion positioning method of GNSS, UWB, IMU and 3D LiDAR can adapt to navigation and positioning in both open-air and warehouse environments, realize the judgment of non-line-of-sight phenomena such as UWB signal occlusion and reflection, and obtain accurate navigation and positioning data of the vehicle in two different working environments. Through the fusion positioning and attitude determination composed of GNSS, multi-tag UWB, IMU, LiDAR and other sensors, it can judge the UWB non-line-of-sight situation, and the dual-tag UWB can solve the vehicle heading, so that the system has complete attitude measurement information.

[0056] In some implementations, the IMU installation error is compensated to handle the arm effect of the GNSS antenna and the UWB tag, as follows:

[0057] Assume that the installation position of the GNSS antenna / UWB tag in the vehicle coordinate system with the IMU as the origin is l b ;

[0058] Assume that the installation error angles of the IMU are the error pitch angle δα, the error roll angle δγ and the error azimuth angle δβ. Then the arm of the GNSS antenna / UWB tag in the IMU coordinate system is:

[0059] l i =(I-μ×)l b

[0060]

[0061] The position of the UWB tag in the navigation coordinate system is:

[0062]

[0063] P is the vehicle position.

[0064] According to the IMU estimated state, judge and handle the UWB non-line-of-sight situation: Since there may be obstacles blocking the UWB measurement process, there will be non-line-of-sight errors NLOS in the ranging information. These errors need to be judged before the filter fusion. In order to make full use of the UWB ranging information, considering that each UWB base station is independent of each other, a measurement update method with dynamic change of the measurement equation dimension is adopted. The ranging residual can be expressed as:

[0065] in is the IMU estimated value, Measurement value for UWB

[0066] Based on the above residual value, it can be judged whether NLOS is abnormal, as follows:

[0067]

[0068] Among them, δρ max is the non-line-of-sight empirical threshold. When the ranging residual is greater than the threshold, the ranging information is discarded.

[0069] This embodiment also provides a fusion positioning device based on multi-source sensors, and applies the above fusion positioning method. Figure 3 As shown, the fusion positioning device includes

[0070] A first absolute position information acquisition module: used to acquire GNSS positioning data of the unmanned vehicle, and obtain first absolute position information through the GNSS positioning data;

[0071] Three-dimensional position information calculation module: used to obtain the distance measurement data of the multi-tag UWB of the unmanned vehicle, and obtain the three-dimensional position information of the vehicle body by performing position and heading calculation on the distance measurement data;

[0072] Inclination information calculation module: used to obtain the IMU data of the inertial measurement unit of the unmanned vehicle, and obtain the inclination information by calculating the IMU data;

[0073] An initial posture information acquisition module is used to initialize the posture based on the first absolute position information, the three-dimensional position information and the inclination information to obtain the initial posture information;

[0074] Map pose information acquisition module: used to obtain the laser radar point cloud data of the unmanned vehicle, match the laser radar point cloud data with the electronic map, and obtain the map pose information of the vehicle body;

[0075] Longitudinal speed information acquisition module: used to acquire wheel speed mileage data of the unmanned vehicle, and obtain the longitudinal speed information of the vehicle body according to the wheel speed mileage data;

[0076] State estimation output module: used to input the absolute position information, inclination information, initial posture information, vehicle longitudinal speed information, distance measurement data and map posture information into the extended Kalman filter to generate second absolute position information as state estimation output.

[0077] In some embodiments, the tilt information includes a pitch angle, a roll angle, and a yaw angle.

[0078] In some embodiments, the three-dimensional position information is obtained by wirelessly receiving UWB signals to obtain the base station ID number and the distance between the tag and the base station. When more than four UWB base station data information is received, the three-dimensional position of the tag is solved to determine the three-dimensional position information of the vehicle.

[0079] In some implementations, the IMU installation error is compensated to handle the arm effect of the GNSS antenna and the UWB tag, as follows:

[0080] Assume that the installation position of the GNSS antenna / UWB tag in the vehicle coordinate system with the IMU as the origin is l b ;

[0081] Assume that the installation error angles of the IMU are the error pitch angle δα, the error roll angle δγ and the error azimuth angle δβ. Then the arm of the GNSS antenna / UWB tag in the IMU coordinate system is:

[0082] l i =(I-μ×)l b

[0083]

[0084] The position of the UWB tag in the navigation coordinate system is:

[0085]

[0086] P is the vehicle position.

[0087] This embodiment further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned fusion positioning method based on multi-source sensors are implemented.

[0088] The storage medium stores program instructions that can implement all the above methods, wherein the program instructions can be stored in the above storage medium in the form of a software product, including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, or a terminal device such as a computer, a server, a mobile phone, or a tablet.

[0089] The processor may also be referred to as a CPU (Central Processing Unit). The processor may be an integrated circuit chip with signal processing capabilities. The processor may also be:

[0090] DSP (Digital Signal Processor, digital signal processor, digital signal processor is a processor composed of large-scale or ultra-large-scale integrated circuit chips to complete certain signal processing tasks. It has gradually developed to meet the needs of high-speed real-time signal processing tasks. With the development of integrated circuit technology and digital signal processing algorithms, the implementation methods of digital signal processors are constantly changing, and the processing functions are constantly improving and expanding.)

[0091] ASIC (Application Specific Integrated Circuit) is an integrated circuit designed and manufactured to meet the needs of specific users and specific electronic systems.

[0092] FPGA (Field Programmable Gate Array) is a product further developed on the basis of programmable devices such as PAL (Programmable Array Logic) and GAL (generic array logic). It emerged as a semi-custom circuit in the field of application-specific integrated circuits (ASICs), which not only solves the shortcomings of custom circuits, but also overcomes the shortcomings of the limited number of gate circuits of the original programmable devices.

[0093] A general-purpose processor may be a microprocessor or any conventional processor.

[0094] The above implementation modes are only for illustrating the technical concept and features of the present invention, and their purpose is to enable people familiar with this technology to understand the content of the present invention and implement it, and they cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made according to the spirit of the present invention should be included in the protection scope of the present invention.

Claims

1. A fusion positioning method based on multi-source sensors, characterized in that: The steps include: Acquire GNSS positioning data of the unmanned vehicle, and obtain first absolute position information through the GNSS positioning data, where the first absolute position information includes first two-dimensional position information, first speed information, and first heading information; Obtaining the distance measurement data of the multi-tag UWB of the unmanned vehicle, and obtaining the three-dimensional position information of the vehicle body by performing position and heading calculation on the distance measurement data; Acquire IMU data of the inertial measurement unit of the unmanned vehicle, and obtain inclination information by solving the IMU data; Performing posture initialization based on the absolute position information, the three-dimensional position information and the inclination information to obtain initial posture information; Acquire laser radar point cloud data of the unmanned vehicle, match the laser radar point cloud data with the electronic map, and obtain map position information of the vehicle body; Obtaining wheel speed mileage data of the unmanned vehicle, and obtaining vehicle longitudinal speed information according to the wheel speed mileage data; The absolute position information, inclination information, initial posture information, vehicle longitudinal velocity information, distance measurement data and map posture information are input into an extended Kalman filter to generate second absolute position information as a state estimation output.

2. The fusion positioning method based on multi-source sensors according to claim 1 is characterized in that: The tilt angle information includes a pitch angle, a roll angle, and a yaw angle.

3. The fusion positioning method based on multi-source sensors according to claim 1 is characterized in that: The three-dimensional position information is obtained by wirelessly receiving UWB signals to obtain the base station ID number and the distance between the tag and the base station. When more than four UWB base station data information is received, the three-dimensional position of the tag is solved to determine the three-dimensional position information of the vehicle.

4. The fusion positioning method based on multi-source sensors according to claim 1 is characterized in that: The IMU installation error is compensated to handle the arm effect of the GNSS antenna and the UWB tag, as follows: Assume that the installation position of the GNSS antenna / UWB tag in the vehicle coordinate system with the IMU as the origin is l b ; Assume that the installation error angles of the IMU are the error pitch angle δα, the error roll angle δγ and the error azimuth angle δβ. Then the arm of the GNSS antenna / UWB tag in the IMU coordinate system is: l i =(I-μ×)l b The position of the UWB tag in the navigation coordinate system is: P is the vehicle position.

5. A fusion positioning device based on multi-source sensors, characterized in that: include A first absolute position information acquisition module: used to acquire GNSS positioning data of the unmanned vehicle, and obtain first absolute position information through the GNSS positioning data, wherein the first absolute position information includes first two-dimensional position information, first speed information and first heading information; Three-dimensional position information calculation module: used to obtain the distance measurement data of the multi-tag UWB of the unmanned vehicle, and obtain the three-dimensional position information of the vehicle body by performing position and heading calculation on the distance measurement data; Inclination information calculation module: used to obtain the IMU data of the inertial measurement unit of the unmanned vehicle, and obtain the inclination information by calculating the IMU data; An initial posture information acquisition module is used to initialize the posture based on the absolute position information, the three-dimensional position information and the inclination information to obtain the initial posture information; Map pose information acquisition module: used to obtain the laser radar point cloud data of the unmanned vehicle, match the laser radar point cloud data with the electronic map, and obtain the map pose information of the vehicle body; Longitudinal speed information acquisition module: used to acquire wheel speed mileage data of the unmanned vehicle, and obtain the longitudinal speed information of the vehicle body according to the wheel speed mileage data; State estimation output module: used to input the absolute position information, inclination information, initial posture information, vehicle longitudinal speed information, distance measurement data and map posture information into the extended Kalman filter to generate second absolute position information as state estimation output.

6. The fusion positioning device based on multi-source sensors according to claim 5 is characterized in that: The tilt angle information includes a pitch angle, a roll angle, and a yaw angle.

7. The fusion positioning device based on multi-source sensors according to claim 5 is characterized in that: The three-dimensional position information is obtained by wirelessly receiving UWB signals to obtain the base station ID number and the distance between the tag and the base station. When more than four UWB base station data information is received, the three-dimensional position of the tag is solved to determine the three-dimensional position information of the vehicle.

8. The fusion positioning device based on multi-source sensors according to claim 5 is characterized in that: The IMU installation error is compensated to handle the arm effect of the GNSS antenna and the UWB tag, as follows: Assume that the installation position of the GNSS antenna / UWB tag in the vehicle coordinate system with the IMU as the origin is l b ; Assume that the installation error angles of the IMU are the error pitch angle δα, the error roll angle δγ and the error azimuth angle δβ. Then the arm of the GNSS antenna / UWB tag in the IMU coordinate system is: l i =(I-μ×)l b The position of the UWB tag in the navigation coordinate system is: P is the vehicle position.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the multi-source sensor-based fusion positioning method according to any one of claims 1 to 4 are implemented.

Images