Carrier positioning and attitude determination method and device based on multi-sensor fusion

Abstract

The embodiment of the present application provides a method for positioning and determining the attitude of a carrier with multi-sensor fusion, the method comprising: obtaining a first factor and a second factor through a GNSS / INS combined navigation system; obtaining a third factor through a visual system; when there is no abnormality in the GNSS system and the visual system, executing the first mode: obtaining the current state information and target position of the carrier through a factor graph optimization algorithm based on a priori factors, the first factor, the second factor and the third factor; when there is an abnormality in the GNSS system or the visual system, executing the second mode: obtaining the current state information and target position of the carrier through a factor graph optimization algorithm based on a priori factors, the corresponding factors of the normal system and the second factor; feeding back the current state information of the carrier to the INS system to obtain the target attitude and target speed. The technical solution of the embodiment of the present application can improve the accuracy, robustness and stability of positioning and determination and its applicability in complex scenes.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of navigation and positioning technology, and in particular, to a method, device, computer equipment, and computer-readable storage medium for carrier positioning and attitude determination using multi-sensor fusion. Background Art

[0002] Positioning and attitude determination technology can be used to determine the position and orientation of an object. In unmanned driving, the vehicle can determine its own position and orientation through positioning and attitude determination technology, thereby achieving route planning, obstacle avoidance and autonomous driving. Positioning and attitude determination technology can be achieved through sensors such as GNSS (Global Navigation Satellite System) receivers, IMU (Inertial Measurement Unit), cameras, lidar, wheel speed meters, etc.

[0003] However, sensors such as GNSS receivers, IMUs, visual cameras, and lidars all have inherent defects and are prone to failure due to interference from terrain, obstacles, bad weather, etc., resulting in low positioning accuracy, poor stability, and limited application in complex scenarios.

[0004] It should be noted that the above content is not necessarily prior art, nor is it intended to limit the scope of patent protection of this application. Summary of the invention

[0005] The embodiments of the present application provide a multi-sensor fusion carrier positioning and attitude determination method, apparatus, computer equipment, and computer-readable storage medium to solve or alleviate one or more of the technical problems raised above.

[0006] One aspect of an embodiment of the present application provides a carrier positioning and attitude determination method using multi-sensor fusion, the method comprising:

[0007] Acquire a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is acquired through a GNSS system, and the second factor is acquired through an INS system;

[0008] Acquire the third factor through the visual system;

[0009] When there is no abnormality in the GNSS system and the visual system, the first mode is executed: according to the prior factor, the first factor, the second factor and the third factor, the current state information and the target position of the carrier are obtained through the factor graph optimization algorithm;

[0010] In the event of an abnormality in the GNSS system or the visual system, the second mode is executed: the current state information and target position of the carrier are obtained through a factor graph optimization algorithm based on the prior factors, the corresponding factors of the normal system and the second factors;

[0011] Feedback the current status information of the carrier to the INS system to obtain the target attitude and target speed;

[0012] The a priori factor includes the state information of the carrier at the last moment.

[0013] Optionally, the first factor includes a GNSS-RTK factor and a GNSS observation factor;

[0014] Correspondingly, the first factor and the second factor are obtained through the GNSS / INS integrated navigation system, including:

[0015] Acquiring GNSS observation factors through a GNSS receiver, wherein the GNSS observation factors include Doppler frequency shift, pseudorange and / or carrier phase;

[0016] According to the GNSS observation factor, a GNSS-RTK factor is obtained through a GNSS-RTK algorithm, where the GNSS-RTK factor includes the position of the carrier.

[0017] Optionally, the multi-sensor fusion carrier positioning and attitude determination method further includes pre-initializing the operation of the GNSS / INS integrated navigation system:

[0018] Acquire satellite measurement information of the carrier according to the GNSS observation factor, wherein the satellite measurement information includes the position, attitude and speed of the carrier;

[0019] Acquire inertial measurement information of the vehicle through the IMU, wherein the inertial measurement information includes acceleration and angular velocity of the carrier;

[0020] The GNSS / INS integrated navigation system is initialized according to the satellite measurement information and the inertial measurement information to obtain estimated system parameters and initial state information of the carrier.

[0021] Optionally, the multi-sensor fusion carrier positioning and attitude determination method further includes an operation of pre-estimating an installation angle of the INS relative to the carrier:

[0022] Acquire a dead reckoning result and a positioning result of the carrier according to the satellite measurement information and the inertial measurement information;

[0023] The dead reckoning result and the positioning result are fused by Kalman filtering to estimate the installation angle of the INS relative to the carrier.

[0024] Optionally, the second factor comprises an IMU pre-integration factor;

[0025] Correspondingly, obtaining the first factor and the second factor through the GNSS / INS integrated navigation system also includes:

[0026] Initializing an inertial navigation mechanical arrangement algorithm according to the estimated system parameters, the initial state information, and the installation angle of the INS relative to the carrier;

[0027] The IMU pre-integration factor is obtained through the initialized inertial navigation mechanical arrangement algorithm.

[0028] Optionally, the third factor comprises a reprojection factor;

[0029] Correspondingly, the third factor is obtained through the visual system, including:

[0030] Acquire the target image through the visual system;

[0031] Acquire multiple feature points from the target image, and perform tracking on the multiple feature points;

[0032] Perform triangulation on multiple feature points to obtain multiple visual landmark points;

[0033] Obtain a reprojection factor according to multiple feature points and multiple visual landmark points.

[0034] Optionally, acquiring a plurality of feature points from the target image and performing tracking on the plurality of feature points includes:

[0035] Extracting multiple feature points from the target image using a Shi-Tomasi corner detection algorithm;

[0036] Tracking of multiple feature points is performed using the Lukas-Kanade optical flow algorithm.

[0037] Optionally, tracking is performed on multiple feature points, including:

[0038] Obtaining light change information of the target image through a histogram detection algorithm;

[0039] According to the light change information, adjusting the light intensity of the target image according to a preset rule;

[0040] Based on the adjusted target image, tracking is performed on a plurality of feature points.

[0041] Optionally, obtaining a plurality of feature points from the target image includes:

[0042] ORB feature points are extracted from the target image through a preset low-frequency process.

[0043] Optionally, triangulation is performed on multiple feature points, including:

[0044] Adjust the error threshold based on historical reprojection;

[0045] Eliminate some feature points from the plurality of feature points according to the adjusted error threshold;

[0046] Perform triangulation on the remaining feature points to obtain the corresponding visual landmark points.

[0047] Optionally, the multi-sensor fusion carrier positioning and attitude determination method further includes:

[0048] Acquiring prior attitude information of the carrier through the GNSS / INS integrated navigation system;

[0049] According to the prior posture information of the carrier, multiple feature points are acquired and tracked through the visual system, and triangulation is performed.

[0050] Optionally, the multi-sensor fusion carrier positioning and attitude determination method further includes:

[0051] Feedback the current status information of the carrier to the inertial navigation mechanical arrangement algorithm and GNSS-RTK algorithm for algorithm optimization;

[0052] Feedback the current status information of the carrier to the visual system to update the visual landmarks.

[0053] Another aspect of the embodiment of the present application provides a multi-sensor fusion carrier positioning and attitude determination device, the device comprising:

[0054] A first acquisition module is used to acquire a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is acquired through a GNSS system, and the second factor is acquired through an INS system;

[0055] A second acquisition module is used to acquire the third factor through a visual system;

[0056] The first execution module is used to execute the first mode when there is no abnormality in the GNSS system and the visual system: obtaining the current state information and the target position of the carrier through the factor graph optimization algorithm according to the prior factor, the first factor, the second factor and the third factor;

[0057] The second execution module is used to execute the second mode when an abnormality occurs in the GNSS system or the visual system: obtaining the current state information and the target position of the carrier through a factor graph optimization algorithm according to the prior factor, the corresponding factor of the normal system and the second factor;

[0058] Feedback module, used to feed back the current status information of the carrier to the INS system to obtain the target attitude and target speed;

[0059] The a priori factor includes the state information of the carrier at the last moment.

[0060] Another aspect of an embodiment of the present application provides a computer device, including:

[0061] at least one processor; and a memory in communication with the at least one processor;

[0062] Wherein: the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the method as described above.

[0063] Another aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and when the computer instructions are executed by a processor, the method described above is implemented.

[0064] The above technical solution adopted in the embodiment of the present application may have the following advantages:

[0065] The first factor, the second factor and the third factor are obtained by the GNSS / INS combined navigation system and the visual system. When the GNSS system and the visual system are both in normal state, the optimal estimate of the carrier state is obtained by the factor graph optimization algorithm according to the above factors and the prior factors (the result of the last optimization), wherein the carrier state includes the target position. When an abnormality occurs in the GNSS system or the visual system, the optimal estimate of the carrier state is obtained by the factor graph optimization algorithm according to the corresponding factors, the second factors and the prior factors of the normal system. The estimated carrier state is fed back to the INS system to obtain the target attitude and the target speed. It can be seen that the embodiment of the present application can realize the elastic fusion of multiple sensors through the switching algorithm, so that the abnormal system will not affect the solution of the normal system, improve the robustness and stability of positioning and attitude, and the applicability in complex scenes. The information of multiple sensors is jointly optimized by the factor graph optimization algorithm, and the accuracy of positioning and attitude can be improved. Feeding back the optimization results to the INS system can help the INS recursive results to be more accurate, thereby further improving the accuracy of positioning and attitude. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] The accompanying drawings exemplarily illustrate the embodiments and constitute a part of the specification, and together with the text description of the specification, are used to explain the exemplary implementation of the embodiments. The embodiments shown are for illustrative purposes only and do not limit the scope of the claims. In all drawings, the same reference numerals refer to similar but not necessarily identical elements.

[0067] Figure 1 A flowchart of a carrier positioning and attitude determination method using multi-sensor fusion according to Embodiment 1 of the present application is schematically shown;

[0068] Figure 2 Schematically shows Figure 1Flow chart of sub-steps of step S100;

[0069] Figure 3 A newly added flow chart of the carrier positioning and attitude determination method using multi-sensor fusion according to the first embodiment of the present application is schematically shown;

[0070] Figure 4 Another newly added flow chart of the carrier positioning and attitude determination method using multi-sensor fusion according to the first embodiment of the present application is schematically shown;

[0071] Figure 5 Schematically shows Figure 1 Flow chart of sub-steps of step S100;

[0072] Figure 6 Schematically shows Figure 1 Flow chart of sub-steps of step S102;

[0073] Figure 7 A diagram schematically shows an application example of the carrier positioning and attitude determination method using multi-sensor fusion according to the first embodiment of the present application;

[0074] Figure 8 A block diagram of a carrier positioning and attitude determination device using multi-sensor fusion according to the second embodiment of the present application is schematically shown; and

[0075] Fig. 9 The hardware architecture diagram of the computer device according to the third embodiment of the present application is schematically shown. DETAILED DESCRIPTION

[0076] In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0077] It should be noted that the descriptions involving "first", "second", etc. in the embodiments of the present application are only for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0078] In the description of the present application, it should be understood that the numerical labels before the steps do not indicate the order in which the steps are executed, but are only used to facilitate the description of the present application and to distinguish each step, and therefore should not be understood as a limitation on the present application.

[0079] First, the following terms are explained:

[0080] GNSS (Global Navigation Satellite System): A navigation system that provides global position and time information through a group of satellites operating in orbit and transmitting signals.

[0081] GNSS receiver: It is a device that works with the GNSS system to receive satellite signals, including ephemeris and measurement data. Ephemeris is used to calculate the satellite position, and the measurement data includes the distance between the GNSS receiver and the satellite. When the GNSS receiver receives satellite signals from at least 4 satellites at the same time and the satellite positions are known, its own spatial coordinate position can be calculated through the space intersection algorithm. A carrier equipped with a GNSS receiver can obtain accurate positioning information with the help of the RTK (Real-Time Kinematic) algorithm. When the carrier is equipped with a dual-antenna GNSS receiver, the carrier's attitude and heading changes can also be perceived through changes in the baseline vector.

[0082] INS (Inertial Navigation System): A navigation system that estimates the position, velocity, and attitude of a carrier by measuring its acceleration and angular velocity.

[0083] IMU (Inertial Measurement Unit): It is the core component of the INS system. It has multiple inertial sensors, such as accelerometers and gyroscopes, built in to measure the acceleration and angular velocity of the carrier. The position change of the carrier can be obtained by double integration of the carrier's acceleration, and the attitude change of the carrier can be obtained by integrating the carrier's angular velocity.

[0084] Visual camera: A sensor that estimates the motion state of the carrier and perceives the environment by analyzing image changes. It is low-cost and suitable for new energy vehicles.

[0085] LiDAR: By comparing the scanned point cloud data, the changes in the LiDAR position and posture are estimated. LiDAR is not affected by changes in lighting and scene texture information, and its distance measurement is more accurate.

[0086] Wheel speed meter: Uses the Hall effect and magnetic induction principle to sense the wheel speed and then calculate the vehicle's trajectory. Specifically: Install a magnet next to the wheel. When the wheel rotates, the magnet will rotate with the wheel. At the same time, the Hall element inside the sensor will generate an electromagnetic field. When the magnet passes through the Hall element, the electromagnetic field will change, thereby generating an electrical signal. The rotation speed of the wheel per unit time can be calculated based on the number of pulses per unit time or the time difference of the unit pulse.

[0087] Factor graph optimization algorithm: an algorithm used to solve combinatorial optimization problems. A factor graph is a graph composed of variables and factors, where factors represent constraints or error models between variables. The core goal of the factor graph optimization algorithm is to obtain the optimal solution to the problem by adjusting the values ​​of the variables to maximize the satisfaction of all factors in the factor graph.

[0088] Sliding Window Optimizer: It is an optimization method for attitude and trajectory estimation in real-time visual or inertial navigation systems. The goal of this optimization method is to estimate the attitude and trajectory of the carrier by minimizing the error in a continuous time window so that it can track the moving or localized target as accurately as possible.

[0089] Secondly, in order to facilitate those skilled in the art to understand the technical solutions provided in the embodiments of the present application, the relevant technologies are described below:

[0090] Positioning and attitude determination technology can be used to determine the position and orientation of an object. In the field of unmanned driving, vehicles can use positioning and attitude determination technology to determine their own position and orientation, thereby achieving route planning, obstacle avoidance and autonomous driving. Positioning and attitude determination technology can be achieved through sensors such as GNSS (Global Navigation Satellite System) receivers, IMU (Inertial Measurement Unit), cameras, lidar, wheel speed meters, etc.

[0091] The applicant understands that GNSS receivers, IMUs, visual cameras, lidars and other sensors all have inherent defects and are prone to failure under interference from terrain, obstacles, bad weather, etc., resulting in low positioning accuracy, poor stability, and limited application in complex scenarios. Specifically:

[0092] GNSS receiver: GNSS is a wireless navigation technology, but in complex environments such as high-rise buildings, forests and tunnels, the signal is easily interfered with, and the cycle slips and gross errors in the GNSS observations increase, resulting in a decrease in positioning accuracy.

[0093] IMU: Inertial devices have errors such as zero bias, and there are integral operations in the solution process, which will lead to cumulative errors. As time goes by, the error will become larger and larger, which is not conducive to the perception of vehicle position and posture over a longer time scale. In addition, high-precision inertial navigation devices are expensive and their application scale is limited.

[0094] Visual cameras: In environments with insufficient lighting, unclear texture features, or high-speed motion, problems such as tracking loss may occur, resulting in performance degradation, making them unsuitable for real-time applications that require stable and continuous positioning.

[0095] LiDAR: Performance degradation occurs in bad weather such as rain and snow or in scenes with simple geometric structures. LiDAR is also expensive and its application scale is limited.

[0096] Wheel speed meter: Low measurement accuracy, poor performance on complex and uneven terrain and slippery surfaces due to wheel slippage.

[0097] In view of the shortcomings of the above methods, the embodiments of the present application provide a multi-sensor fusion carrier positioning and attitude determination technical solution. In this technical solution: (1) the carrier can be positioned and determined continuously, stably and reliably in complex scenes, and the forward speed of the carrier can be accurately perceived; (2) the position and attitude of the carrier can be accurately estimated for various complex scenes, such as elevated roads, tunnels, boulevards, etc., with wide scene coverage; (3) the measurement values ​​of multiple sensors are jointly optimized to achieve maximum a posteriori estimation and improve perception accuracy; (4) the visual system is applied, which does not rely on high-precision inertial navigation systems, effectively reducing costs; (5) the GNSS / INS combined navigation system and the visual system can operate independently by adopting elastic fusion and switching algorithms. When both are in normal state, they are fused through the factor graph framework. When there is a problem in one of the systems, the remaining systems can be solved independently without being affected, thereby improving the robustness and stability of positioning and attitude determination. See below for details.

[0098] The technical solutions of the present application are described below through multiple embodiments. It should be noted that these embodiments can be implemented in a variety of different forms and should not be construed as being limited to the embodiments described here.

[0099] Embodiment 1

[0100] Figure 1 The flowchart of the carrier positioning and attitude determination method of multi-sensor fusion according to the first embodiment of the present application is schematically shown. Figure 1 As shown, the multi-sensor fusion carrier positioning and attitude determination method may include steps S100 to S108, wherein:

[0101] Step S100: acquiring a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is acquired through a GNSS system, and the second factor is acquired through an INS system.

[0102] Step S102, obtaining a third factor through a visual system.

[0103] Step S104, when there is no abnormality in the GNSS system and the visual system, the first mode is executed: according to the a priori factor, the first factor, the second factor and the third factor, the current state information of the carrier and the target position are obtained by the factor graph optimization algorithm. The a priori factor includes the state information of the carrier at the last moment.

[0104] Step S106, when an abnormality occurs in the GNSS system or the visual system, the second mode is executed: the current state information and target position of the carrier are obtained through a factor graph optimization algorithm according to the prior factors, the corresponding factors of the normal system and the second factors.

[0105] Step S108, feeding back the current state information of the carrier to the INS system to obtain the target attitude and target speed.

[0106] The carrier positioning and attitude determination method of multi-sensor fusion provided in this embodiment: the first factor, the second factor and the third factor are obtained correspondingly through the GNSS / INS combined navigation system and the visual system. When the GNSS system and the visual system are in normal state, the optimal estimate of the carrier state is obtained through the factor graph optimization algorithm according to the above factors and the prior factors (the result of the last optimization), wherein the carrier state includes the target position. When either the GNSS system or the visual system is abnormal, the optimal estimate of the carrier state is obtained through the factor graph optimization algorithm according to the corresponding factors, the second factor and the prior factors of the normal system. The estimated carrier state is fed back to the INS system to obtain the target attitude and target speed. It can be seen that this embodiment can realize the elastic fusion of multiple sensors through the switching algorithm, so that the abnormal system will not affect the solution of the normal system, improve the robustness and stability of positioning and attitude determination, and the applicability in complex scenes. The accuracy of positioning and attitude determination can be improved by jointly optimizing the information of multiple sensors through the factor graph optimization algorithm. Feeding back the optimization results to the INS system can help the INS recursive results to be more accurate, thereby further improving the accuracy of positioning and attitude determination.

[0107] The following combination Figure 1 , each step in steps S100 to S108 and other optional steps are described in detail.

[0108] Step S100, obtaining a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is obtained through a GNSS system, and the second factor is obtained through an INS system.

[0109] The GNSS / INS integrated navigation system includes GNSS (Global Navigation Satellite System) and INS (Inertial Navigation System). The combination of GNSS and INS can make up for the disadvantage of GNSS being susceptible to interference by using INS and make periodic corrections to INS by using GNSS, thereby synergistically improving navigation performance.

[0110] Factors are used in factor graphs to represent constraints or error models of variables. In an embodiment of the present application, the factor graph optimization algorithm uses error minimization as the optimization goal to obtain the optimal estimate of the carrier state. Correspondingly, the variables may include parameters such as the position, attitude, and speed of the carrier. The carrier may be a device equipped with a GNSS / INS integrated navigation system, including but not limited to vehicles, ships, and aircraft.

[0111] The first factor and the second factor are obtained by the GNSS / INS integrated navigation system, the first factor may be the measurement data and / or solution result of the GNSS system, and the second factor may be the measurement data and / or solution result of the INS system. Adding the above factors to the factor graph can provide more information to help and improve the optimal estimation of the carrier state, thereby providing a more accurate carrier state estimation.

[0112] The first factor and the second factor, as well as their respective acquisition processes, are exemplarily introduced below.

[0113] In an optional embodiment, the first factor includes a GNSS-RTK factor and a GNSS observation factor. Figure 2 As shown, step S100 may include:

[0114] Step S200: acquiring GNSS observation factors through a GNSS receiver, wherein the GNSS observation factors include Doppler frequency shift, pseudorange and / or carrier phase.

[0115] Step S202: acquiring a GNSS-RTK factor through a GNSS-RTK algorithm according to the GNSS observation factor, wherein the GNSS-RTK factor includes a position of a carrier.

[0116] The GNSS observation factors are obtained through the GNSS receiver. The GNSS observation factors may be raw measurement data, including Doppler frequency shift, pseudorange and / or carrier phase, and may also include signal strength, correction data or satellite status, etc. It should be noted that different types and combinations of raw measurement data may be selected according to different application requirements to meet specific positioning and navigation requirements.

[0117] Based on the GNSS observation factor, the GNSS-RTK algorithm can be used to calculate the position of the carrier in real time (GNSS-RTK factor), effectively improving the solution accuracy of the GNSS system.

[0118] In this embodiment, GNSS observation factors and GNSS-RTK factors are obtained through a GNSS receiver and a GNSS-RTK algorithm, so that the original observation data and the position information estimated by the GNSS system can be used as constraints to further improve the accuracy and robustness of the carrier state estimation.

[0119] In an optional embodiment, if Figure 3 As shown, the multi-sensor fusion carrier positioning and attitude determination method also includes the operation of pre-initializing the GNSS / INS integrated navigation system:

[0120] Step S300: acquiring satellite measurement information of the carrier according to the GNSS observation factors, wherein the satellite measurement information includes the position, attitude and speed of the carrier.

[0121] Step S302: Acquire inertial measurement information of the vehicle through the IMU, where the inertial measurement information includes acceleration and angular velocity of the carrier.

[0122] Step S304: Initialize the GNSS / INS integrated navigation system according to the satellite measurement information and the inertial measurement information to obtain estimated system parameters and initial state information of the carrier.

[0123] According to the GNSS observation factors, the position, attitude and speed of the carrier, that is, the satellite measurement information, can be estimated by combining various methods such as carrier zero speed, GNSS Doppler velocity measurement, carrier phase three-difference velocity measurement, GNSS positioning, and dual-antenna GNSS attitude determination. For example, the speed of the carrier can be estimated by analyzing the Doppler frequency shift. The attitude of the carrier can be estimated by analyzing the change of the baseline vector in the dual-antenna GNSS. The acceleration and angular velocity of the carrier, that is, the inertial measurement information, can be measured by the accelerometer and gyroscope in the IMU (inertial measurement unit). The GNSS / INS integrated navigation system is initialized by integrating satellite measurement information and inertial measurement information to obtain estimated system parameters and initial state information of the carrier. Among them, the estimated system parameters include IMU zero bias and scale factor. IMU zero bias can include accelerometer zero bias and gyroscope zero bias. The scale factor is the proportional relationship between the IMU value-added and the output value, which can be used to calibrate the IMU to improve the measurement accuracy. The initial state information includes the initial position, initial attitude and initial velocity.

[0124] In this embodiment, the GNSS / INS integrated navigation system is initialized by integrating methods such as carrier zero speed, GNSS Doppler velocity measurement, carrier phase triple difference velocity measurement, GNSS positioning, and dual-antenna GNSS attitude determination, which can improve the success rate and accuracy of initialization, reduce cumulative errors, and improve system stability.

[0125] In an optional embodiment, if Figure 4 As shown, the multi-sensor fusion carrier positioning and attitude determination method also includes an operation of pre-estimating the installation angle of INS relative to the carrier:

[0126] Step S400: acquiring a dead reckoning result and a positioning result of a carrier according to the satellite measurement information and the inertial measurement information.

[0127] Step S402: The dead reckoning result and the positioning result are fused by Kalman filtering to estimate the installation angle of the INS relative to the carrier.

[0128] The installation angle of INS relative to the carrier, also known as installation deflection or installation error, is the angle between the actual installation direction of INS relative to the vehicle and the vehicle coordinate system. The existence of installation deflection may affect the performance and accuracy of INS, resulting in errors in the estimation of the carrier's direction, rotation angle, position, etc. Therefore, it is necessary to pre-calculate the installation deflection in order to subsequently eliminate or control its impact on INS. For example, the dead reckoning result is obtained by performing dead reckoning based on the carrier's attitude and travel distance obtained by the GNSS / INS integrated navigation system. The dead reckoning result and the positioning result (three-dimensional position information) of the GNSS / INS integrated navigation system are fused through Kalman filtering to estimate the installation angle of INS relative to the carrier.

[0129] In this embodiment, by pre-estimating the installation angle of the INS relative to the carrier, the error can be reduced and the performance of the INS can be improved.

[0130] In an optional embodiment, the second factor includes an IMU pre-integration factor. Figure 5 As shown, the step S100 may further include:

[0131] Step S500, initializing an inertial navigation mechanical arrangement algorithm according to the estimated system parameters, the initial state information and the installation angle of the INS relative to the carrier.

[0132] Step S502, obtaining the IMU pre-integration factor through the initialized inertial navigation mechanical arrangement algorithm.

[0133] Based on the known initial state information, installation deflection angle and estimated system parameters, a more accurate starting point can be provided for the inertial navigation mechanical arrangement algorithm, thereby improving the accuracy of the INS recursive result, reducing the drift and error of the INS, accelerating convergence and improving robustness. According to the initialized inertial navigation mechanical arrangement algorithm, the IMU pre-integration factor can be obtained. The IMU pre-integration factor can include the position, attitude and velocity of the carrier, as well as the accelerometer bias and gyroscope bias.

[0134] In this embodiment, the IMU pre-integration factor is obtained through the initialized inertial navigation mechanical arrangement algorithm, so that the position, attitude and velocity of the carrier estimated by the INS system, as well as the accelerometer bias and gyroscope bias information can be used as constraints, further improving the accuracy and robustness of the carrier state estimation and reducing error jumps and drifts.

[0135] In the GNSS / INS integrated navigation system, the position, attitude and speed of the carrier can be updated in real time by combining the inertial navigation arrangement algorithm and the GNSS-RTK algorithm.

[0136] The above-mentioned multiple embodiments exemplarily introduce the scheme of obtaining the first factor and the second factor by the GNSS / INS integrated navigation system. The third factor and how the visual system obtains the third factor will be exemplarily introduced through multiple embodiments below.

[0137] Step S102 , and obtain the third factor through the visual system.

[0138] The visual system can acquire, analyze and understand visible information in the environment through cameras and image processing technology to help determine the carrier state. The third factor can be the solution result and / or measurement information of the visual system. Adding the third factor to the factor graph can provide more information to help and improve the optimal estimate of the carrier state, thereby providing a more accurate carrier state estimate.

[0139] An exemplary acquisition scheme of multiple third factors will be provided below.

[0140] In an optional embodiment, the third factor includes a reprojection factor. Figure 6 As shown, step S102 may include:

[0141] Step S600, acquiring a target image through a visual system.

[0142] Step S602: Acquire multiple feature points from the target image, and perform tracking on the multiple feature points.

[0143] Step S604: performing triangulation on the multiple feature points to obtain multiple visual landmark points.

[0144] Step S606, obtaining a reprojection factor according to the multiple feature points and the multiple visual landmark points.

[0145] The visual system can use a camera to capture a series of target images / frames of an external series. After acquiring the target image, a preset feature extraction detection algorithm is used to detect multiple feature points in the image, such as corners, edges, spots, etc. Among them, the feature point can be a two-dimensional projection of a landmark point in the three-dimensional world. The detected feature points are tracked by a preset feature tracking algorithm, that is, the same feature points are continuously identified and tracked between different target images to ensure the stability, reliability and consistency of the feature points involved in the triangulation. Triangulation is performed on multiple feature points to obtain visual landmark points, that is, to determine the three-dimensional position of the landmark points. The three-dimensional position of the landmark point is projected onto a two-dimensional plane to obtain two-dimensional coordinates, and compared with the coordinates of the feature point in the target image, the reprojection error, that is, the reprojection factor, can be obtained. In some embodiments, based on multiple feature points and multiple visual landmark points, a visual odometer can also be obtained, that is, the position, posture and speed of the carrier are estimated.

[0146] In this embodiment, the reprojection factor is obtained through the visual system so that the reprojection error can be used as a constraint condition, thereby further improving the accuracy and robustness of the carrier state estimation.

[0147] In an optional embodiment, multiple feature points are extracted from the target image by using a Shi-Tomasi corner detection algorithm, and multiple feature points are tracked by using a Lukas-Kanade optical flow algorithm.

[0148] The preset feature extraction algorithm may include the Shi-Tomasi corner detection algorithm. The algorithm can be used to identify corners in an image. Corner points are structural points with high contrast and distinctiveness, which are helpful for motion estimation and environment modeling of the carrier and improve efficiency. In some embodiments, the feature extraction algorithm may also include the Harris corner detection algorithm, the FAST corner detection algorithm, the AKAZE feature extraction algorithm, etc., which are not limited to this.

[0149] The preset feature tracking algorithm may include a Lukas-Kanade optical flow algorithm, which can obtain accurate motion estimation of multiple feature points based on brightness information of different frames, thereby realizing feature point tracking. In some embodiments, the feature tracking algorithm may also include a Farneback optical flow algorithm, a sparse optical flow algorithm, etc., which is not limited to this.

[0150] In an optional embodiment, step S602 may include: obtaining light change information of the target image through a histogram detection algorithm, adjusting the light intensity of the target image according to a preset rule based on the light change information, and tracking multiple feature points based on the adjusted target image.

[0151] The optical flow method performs feature point tracking based on the premise that the brightness of the same target (such as a feature point) does not change when it moves between different frames. However, in practical applications, the premise for implementing the optical flow method is difficult to meet, resulting in poor tracking results.

[0152] In this embodiment, in order to alleviate the above problem, histogram detection can be enabled to monitor the light of the target image in real time to obtain the light change information of the target image. According to the light change information, the light intensity of the target image can be adaptively adjusted to improve the stability of feature point tracking.

[0153] In an optional embodiment, step S602 may further include: extracting ORB feature points from the target image through a preset low-frequency process.

[0154] In this embodiment, a low-frequency process can be set under low-speed scenes / post-processing conditions to extract ORB (Oriented FAST and Rotated BRIEF) feature points in the target image, thereby increasing the feature points used for triangulation and improving redundancy and computational efficiency. Since ORB feature points are robust to a certain degree of light changes, the stability of feature point tracking can be improved to a certain extent.

[0155] In an optional embodiment, step S604 may include: adjusting an error threshold according to historical reprojection; removing some feature points from the multiple feature points according to the adjusted error threshold; and performing triangulation on the remaining feature points to obtain corresponding visual landmark points.

[0156] In this embodiment, the error threshold is dynamically adjusted by statistically analyzing historical reprojection errors, and feature points greater than the error threshold are deleted. This allows for dynamic screening of feature points involved in triangulation, thereby improving the robustness of the visual odometer.

[0157] In an optional embodiment, the multi-sensor fusion carrier positioning and attitude determination method may also include: acquiring prior attitude information of the carrier through the GNSS / INS combined navigation system; acquiring and tracking multiple feature points through a visual system based on the prior attitude information of the carrier, and performing triangulation.

[0158] Prior attitude information may include the heading, roll, pitch, etc. of the carrier. Prior attitude information can be obtained using a GNSS / INS combined navigation system and can be used to assist visual processes including feature detection, tracking, and landmark point triangulation. The corresponding motion transformation of the carrier can be known based on the prior attitude information, and the position (pixel coordinates) of the feature point in the next frame can be predicted through the motion change. Prior attitude information can be used to limit the possible landmark point locations, thereby reducing the search space for matching. This can improve the accuracy of matching, thereby facilitating more accurate triangulation.

[0159] In this embodiment, the positioning and attitude determination accuracy and efficiency can be improved by using the GNSS / INS combined navigation system to assist the visual system.

[0160] The above-mentioned embodiments exemplarily introduce the process of obtaining multiple factors through multiple sensors. The following will introduce an exemplary fusion process of multiple sensors.

[0161] Step S104 , when there is no abnormality in the GNSS system and the visual system, the first mode is executed: according to the prior factor, the first factor, the second factor and the third factor, the current state information and the target position of the carrier are obtained through the factor graph optimization algorithm.

[0162] The prior factor is the optimization result of the previous moment, which can provide constraints and initial values ​​for the next optimization. Searching near the initial value can alleviate the problem of falling into the local optimal solution and the multi-valued nature of the optimized solution.

[0163] When no abnormality occurs in any system, the sliding window optimizer is used to integrate the prior factor, the first factor, the second factor, and the third factor in the factor graph framework for optimization, and the optimal estimate of the current state information of the carrier can be obtained, where the state includes the position, posture, and speed of the carrier. The position information in the current state information of the carrier is output as the target position.

[0164] In this embodiment, the information of multiple sensors is jointly optimized by a factor graph optimization algorithm, so that the accuracy of positioning and attitude determination can be improved.

[0165] Step S106 ,When an abnormality occurs in the GNSS system or the vision system, the second mode is executed: the current state information and the target position of the carrier are obtained through the factor graph optimization algorithm based on the prior factors, the corresponding factors of the normal system and the second factors.

[0166] Since the GNSS / INS integrated navigation system and the visual system can operate independently, when the GNSS system is abnormal, it can be converted to a combined system of INS and vision. By optimizing the factor graph based on the prior factor, the second factor, and the third factor, the optimal estimate of the current state information of the carrier can still be obtained. When the visual system is abnormal, the GNSS / INS integrated navigation system can operate independently, and the optimal estimate of the current state information of the carrier can also be obtained based on the prior factor, the first factor, and the second factor.

[0167] In this embodiment, the flexible fusion and switching algorithm is adopted, and the GNSS system, INS system and vision system can all achieve independent positioning and attitude determination. When one of the systems has a problem, the remaining systems can be solved independently without being affected, which improves the robustness and stability of positioning and attitude determination.

[0168] Step S108 , and feeds back the current state information of the carrier to the INS system to obtain the target attitude and target speed.

[0169] Feeding the optimization results of the factor graph back to the factors in the INS system can help the INS recursive results to be more accurate, thereby updating the inertial navigation state (attitude and velocity) and IMU pre-integration. The INS system can obtain and output the target attitude and target velocity of the carrier, realizing accurate attitude determination and velocity perception of the carrier.

[0170] In an optional embodiment, the multi-sensor fusion carrier positioning and attitude determination method can feed back the carrier's current state information to the inertial navigation mechanical arrangement algorithm and the GNSS-RTK algorithm for algorithm optimization, and feed back the carrier's current state information to the visual system to update the visual landmark points.

[0171] Feeding the optimization results of the factor graph back to the GNSS-RTK algorithm can help GNSS detect cycle slips and fix ambiguities, improve RTK positioning accuracy, and correct or compensate for observation factors (raw observation data) to optimize the overall system. Feeding the optimization results of the factor graph back to the inertial navigation mechanical arrangement algorithm can help INS recursive results be more accurate. Feeding the optimization results of the factor graph back to the visual system can update the visual landmark points, optimize the visual system, and improve the accuracy of the visual odometer.

[0172] In some embodiments, virtual observations can be constructed to participate in the data collection and solution process of each system. For example, when the carrier is a vehicle, due to the particularity of vehicle movement, the vehicle rarely experiences side slips, drifts, bounces, etc., that is, the lateral and vertical speeds of the vehicle are 0, and the elevation does not jump when the vehicle is driving slowly. Therefore, the virtual observation can be to assume that the lateral and elevation speeds of the vehicle are approximately 0. Constructing virtual observations to participate in the solution can improve the positioning and attitude accuracy of the vehicle.

[0173] In order to make this application easier to understand, the following Figure 7 An exemplary application is provided.

[0174] S11, observe Doppler frequency shift, pseudorange and carrier phase (GNSS observation factor) through the GNSS receiver.

[0175] S12, estimating the position, attitude and speed of the vehicle by using a preset algorithm (such as GNSS-RTK).

[0176] S13, collecting the specific force (acceleration) and angular velocity of the vehicle through the IMU.

[0177] S14, initializing the GNSS / INS integrated navigation system according to the vehicle position, attitude and velocity estimated by the GNSS system and the vehicle acceleration and angular velocity collected by the IMU, so as to obtain the estimated system parameters and the initial state information of the vehicle, and estimate the installation angle of the INS relative to the vehicle.

[0178] S15, initializing the inertial navigation mechanical arrangement algorithm according to the estimated system parameters, the initial state information of the vehicle and the installation angle of the INS relative to the vehicle.

[0179] S16, obtaining a GNSS-RTK factor through a GNSS-RTK algorithm.

[0180] S17, obtaining the IMU pre-integration factor through the inertial navigation mechanical arrangement algorithm.

[0181] S18, takes a series of target images / frames through the visual system, and performs feature detection and tracking on the frames with the assistance of the GNSS / INS integrated navigation system.

[0182] S19, performing triangulation on multiple feature points with the assistance of a GNSS / INS integrated navigation system to obtain visual landmark points (three-dimensional positions of landmark points).

[0183] S20, obtaining a reprojection factor according to the visual landmark point. If there is no abnormality in the GNSS system and the visual system, S21 is executed; if there is an abnormality in the GNSS system or the visual system, S22 is executed.

[0184] S21, the prior factor, GNSS observation factor and GNSS-RTK factor (first factor), IMU pre-integration factor (second factor) and reprojection factor (third factor) are integrated in the framework of the factor graph for joint optimization and outlier elimination to obtain the optimal estimate of the vehicle state, from which the position information is extracted as the target position output.

[0185] S22, the prior factor, GNSS observation factor and GNSS-RTK factor (first factor) or reprojection factor (third factor), and IMU pre-integration factor (second factor) are integrated in the framework of the factor graph for joint optimization and outlier elimination, so as to obtain the optimal estimate of the vehicle state, from which the position information is extracted as the target position output.

[0186] S23, feeding back the optimization result to the inertial navigation mechanical arrangement algorithm, optimizing the algorithm and acquiring and outputting the target attitude and target speed.

[0187] S24, feeding back the optimization result to the GNSS-RTK algorithm to optimize the algorithm.

[0188] S25, feeding back the optimization result to the visual system to update the visual landmark points.

[0189] In this exemplary application: (1) the carrier can be positioned and determined continuously, stably and reliably in complex scenes, and the forward speed of the carrier can be accurately perceived; (2) the position and attitude of the carrier can be accurately estimated for various complex scenes, such as elevated roads, tunnels, and boulevards, with wide scene coverage; (3) the measurement values ​​of multiple sensors are jointly optimized to achieve maximum a posteriori estimation, thereby improving perception accuracy; (4) the visual system is applied, which does not rely on the high-precision inertial navigation system, effectively reducing costs; (5) the GNSS / INS combined navigation system and the visual system can operate independently by adopting a flexible fusion and switching algorithm. When both are in normal conditions, they are fused through a factor graph framework. When one of the systems has a problem, the remaining system can be solved independently without being affected, thereby improving the robustness and stability of positioning and attitude determination; (6) the GNSS, INS, vision and other modules can achieve independent positioning and attitude determination, and the original observation values ​​and solution results of each sensor can be flexibly fused and switched, thereby ensuring the stability of the system.

[0190] Embodiment 2

[0191] Figure 8The block diagram of the multi-sensor fusion carrier positioning and attitude determination device according to the second embodiment of the present application is schematically shown. The device can be divided into one or more program modules, one or more program modules are stored in a storage medium, and are executed by one or more processors to complete the embodiment of the present application. The program module referred to in the embodiment of the present application refers to a series of computer program instruction segments that can complete specific functions. The following description will specifically introduce the functions of each program module in this embodiment. Figure 8 As shown, the device 1000 may include: a first acquisition module 1100, a second acquisition module 1200, a first execution module 1300, a second execution module 1400, and a feedback module 1500, wherein:

[0192] A first acquisition module 1100 is configured to acquire a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is acquired through a GNSS system and the second factor is acquired through an INS system;

[0193] A second acquisition module 1200 is used to acquire a third factor through a visual system;

[0194] The first execution module 1300 is used to execute the first mode when there is no abnormality in the GNSS system and the visual system: obtaining the current state information and the target position of the carrier through the factor graph optimization algorithm according to the prior factor, the first factor, the second factor and the third factor;

[0195] The second execution module 1400 is used to execute the second mode when an abnormality occurs in the GNSS system or the visual system: obtaining the current state information and the target position of the carrier through a factor graph optimization algorithm according to the prior factor, the corresponding factor of the normal system and the second factor;

[0196] Feedback module 1500, used to feed back the current state information of the carrier to the INS system to obtain the target attitude and target speed;

[0197] The a priori factor includes the state information of the carrier at the last moment.

[0198] As an optional embodiment, the first factor includes a GNSS-RTK factor and a GNSS observation factor;

[0199] Correspondingly, the first acquisition module 1100 is further used for:

[0200] Acquiring GNSS observation factors through a GNSS receiver, wherein the GNSS observation factors include Doppler frequency shift, pseudorange and / or carrier phase;

[0201] According to the GNSS observation factor, a GNSS-RTK factor is obtained through a GNSS-RTK algorithm, where the GNSS-RTK factor includes the position of the carrier.

[0202] As an optional embodiment, the device 1000 is further used for:

[0203] Acquire satellite measurement information of the carrier according to the GNSS observation factor, wherein the satellite measurement information includes the position, attitude and speed of the carrier;

[0204] Acquire inertial measurement information of the vehicle through the IMU, wherein the inertial measurement information includes acceleration and angular velocity of the carrier;

[0205] The GNSS / INS integrated navigation system is initialized according to the satellite measurement information and the inertial measurement information to obtain estimated system parameters and initial state information of the carrier.

[0206] As an optional embodiment, the device 1000 is further used to: obtain a dead reckoning result and a positioning result of the carrier according to the satellite measurement information and the inertial measurement information;

[0207] The dead reckoning result and the positioning result are fused by Kalman filtering to estimate the installation angle of the INS relative to the carrier.

[0208] As an optional embodiment, the second factor includes an IMU pre-integration factor;

[0209] Correspondingly, the first acquisition module 1100 is further used for:

[0210] Initializing an inertial navigation mechanical arrangement algorithm according to the estimated system parameters, the initial state information, and the installation angle of the INS relative to the carrier;

[0211] The IMU pre-integration factor is obtained through the initialized inertial navigation mechanical arrangement algorithm.

[0212] As an optional embodiment, the third factor includes a reprojection factor;

[0213] Correspondingly, the second acquisition module 1200 is further used for:

[0214] Acquire the target image through the visual system;

[0215] Acquire multiple feature points from the target image, and perform tracking on the multiple feature points;

[0216] Perform triangulation on multiple feature points to obtain multiple visual landmark points;

[0217] Obtain a reprojection factor according to multiple feature points and multiple visual landmark points.

[0218] As an optional embodiment, the second obtaining module 1200 is further configured to:

[0219] Extracting multiple feature points from the target image using a Shi-Tomasi corner detection algorithm;

[0220] Tracking of multiple feature points is performed using the Lukas-Kanade optical flow algorithm.

[0221] As an optional embodiment, the second obtaining module 1200 is further configured to:

[0222] Obtaining light change information of the target image through a histogram detection algorithm;

[0223] According to the light change information, adjusting the light intensity of the target image according to a preset rule;

[0224] Based on the adjusted target image, tracking is performed on a plurality of feature points.

[0225] As an optional embodiment, the second obtaining module 1200 is further configured to:

[0226] ORB feature points are extracted from the target image through a preset low-frequency process.

[0227] As an optional embodiment, the second obtaining module 1200 is further configured to:

[0228] Adjust the error threshold based on historical reprojection;

[0229] Eliminate some feature points from the plurality of feature points according to the adjusted error threshold;

[0230] Perform triangulation on the remaining feature points to obtain the corresponding visual landmark points.

[0231] As an optional embodiment, the apparatus 1000 is further used for:

[0232] Acquiring prior attitude information of the carrier through the GNSS / INS integrated navigation system;

[0233] According to the prior posture information of the carrier, multiple feature points are acquired and tracked through the visual system, and triangulation is performed.

[0234] As an optional embodiment, the apparatus 1000 is further used for:

[0235] Feedback the current status information of the carrier to the inertial navigation mechanical arrangement algorithm and GNSS-RTK algorithm for algorithm optimization;

[0236] Feedback the current status information of the carrier to the visual system to update the visual landmarks.

[0237] Embodiment 3

[0238] Fig. 9 The hardware architecture diagram of the computer device 10000 suitable for implementing the carrier positioning and attitude determination method of multi-sensor fusion according to the third embodiment of the present application is schematically shown. In some embodiments, the computer device 10000 can be a terminal device such as a smart phone, a wearable device, a tablet computer, a personal computer, a vehicle terminal, a virtual device, a workbench, a robot, etc. In other embodiments, the computer device 10000 can be a rack server, a blade server, a tower server or a cabinet server (including an independent server, or a server cluster composed of multiple servers), etc. Fig. 9 As shown, the computer device 10000 includes but is not limited to: a memory 10010, a processor 10020, and a network interface 10030 that can communicate with each other through a system bus. Among them:

[0239] The memory 10010 includes at least one type of computer-readable storage medium, and the readable storage medium includes a flash memory, a hard disk, a multimedia card, a card-type memory (such as an SD or DX memory), a random access memory (RAM), a static random access memory (SRAM), a read-only memory (ROM), an electrically erasable programmable read-only memory (EEPROM), a programmable read-only memory (PROM), a magnetic memory, a magnetic disk, an optical disk, etc. In some embodiments, the memory 10010 may be an internal storage module of the computer device 10000, such as a hard disk or a memory of the computer device 10000. In other embodiments, the memory 10010 may also be an external storage device of the computer device 10000, such as a plug-in hard disk equipped on the computer device 10000, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. Of course, the memory 10010 may also include both the internal storage module of the computer device 10000 and its external storage device. In this embodiment, the memory 10010 is generally used to store the operating system and various application software installed in the computer device 10000, such as the program code of the carrier positioning and attitude determination method of multi-sensor fusion, etc. In addition, the memory 10010 can also be used to temporarily store various data that have been output or will be output.

[0240] In some embodiments, the processor 10020 may be a central processing unit (CPU), a controller, a microcontroller, a microprocessor, or other chips. The processor 10020 is generally used to control the overall operation of the computer device 10000, such as performing control and processing related to data interaction or communication with the computer device 10000. In this embodiment, the processor 10020 is used to run the program code stored in the memory 10010 or process data.

[0241] The network interface 10030 may include a wireless network interface or a wired network interface, and the network interface 10030 is generally used to establish a communication link between the computer device 10000 and other computer devices. For example, the network interface 10030 is used to connect the computer device 10000 to an external terminal through a network, and to establish a data transmission channel and a communication link between the computer device 10000 and the external terminal. The network may be a wireless or wired network such as an intranet, the Internet, the Global System of Mobile communication (GSM), Wideband Code Division Multiple Access (WCDMA), 4G network, 5G network, Bluetooth, Wi-Fi, etc.

[0242] It should be pointed out that Fig. 9 Only a computer device having components 10010 - 10030 is shown, but it should be understood that implementation of all of the components shown is not a requirement, and more or fewer components may alternatively be implemented.

[0243] In this embodiment, the multi-sensor fusion carrier positioning and attitude determination method stored in the memory 10010 can also be divided into one or more program modules and executed by one or more processors (such as processor 10020) to complete the embodiment of the present application.

[0244] Embodiment 4

[0245] An embodiment of the present application further provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of the carrier positioning and attitude determination method of multi-sensor fusion in the embodiment are implemented.

[0246] In this embodiment, the computer-readable storage medium includes flash memory, hard disk, multimedia card, card-type memory (for example, SD or DX memory, etc.), random access memory (RAM), static random access memory (SRAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEP ROM), programmable read-only memory (PROM), magnetic memory, disk, optical disk, etc. In some embodiments, the computer-readable storage medium can be an internal storage unit of a computer device, such as a hard disk or memory of the computer device. In other embodiments, the computer-readable storage medium can also be an external storage device of a computer device, such as a plug-in hard disk equipped on the computer device, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. Of course, the computer-readable storage medium can also include both the internal storage unit of the computer device and its external storage device. In this embodiment, the computer-readable storage medium is generally used to store the operating system and various application software installed on the computer device, such as the program code of the carrier positioning and attitude determination method of multi-sensor fusion in the embodiment. In addition, the computer-readable storage medium can also be used to temporarily store various types of data that have been output or are to be output.

[0247] Obviously, those skilled in the art should understand that the modules or steps of the above-mentioned embodiments of the present application can be implemented by general-purpose computer devices, they can be concentrated on a single computer device, or distributed on a network composed of multiple computer devices, optionally, they can be implemented by executable program codes of computer devices, so that they can be stored in a storage device and executed by the computer device, and in some cases, the steps shown or described can be executed in a different order from that herein, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. In this way, the embodiments of the present application are not limited to any specific combination of hardware and software.

[0248] It should be noted that the above are only preferred embodiments of the present application, and the patent protection scope of the present application is not limited thereto. Any equivalent structure or equivalent process transformation made using the contents of the specification and drawings of the present application, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A multi-sensor fusion carrier positioning and attitude determination method, characterized in that: The method comprises: Acquire a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is acquired through a GNSS system, and the second factor is acquired through an INS system; Acquire the third factor through the visual system; When there is no abnormality in the GNSS system and the visual system, the first mode is executed: according to the prior factor, the first factor, the second factor and the third factor, the current state information and the target position of the carrier are obtained through the factor graph optimization algorithm; In the event of an abnormality in the GNSS system or the visual system, the second mode is executed: the current state information and target position of the carrier are obtained through a factor graph optimization algorithm based on the prior factors, the corresponding factors of the normal system and the second factors; Feedback the current status information of the carrier to the INS system to obtain the target attitude and target speed; Wherein, the a priori factor includes the state information of the carrier at a previous moment, and the third factor includes the reprojection factor; Correspondingly, the third factor is obtained through the visual system, including: Acquiring prior attitude information of the carrier through the GNSS / INS integrated navigation system; Acquire the target image through the visual system; According to the prior posture information of the carrier, a plurality of feature points are obtained from the target image through a visual system, and the plurality of feature points are tracked; Perform triangulation on multiple feature points to obtain multiple visual landmark points; Obtain a reprojection factor according to multiple feature points and multiple visual landmark points.

2. The method according to claim 1, characterized in that The first factor includes a GNSS-RTK factor and a GNSS observation factor; Correspondingly, the first factor and the second factor are obtained through the GNSS / INS integrated navigation system, including: Acquiring GNSS observation factors through a GNSS receiver, wherein the GNSS observation factors include Doppler frequency shift, pseudorange and / or carrier phase; According to the GNSS observation factor, a GNSS-RTK factor is obtained through a GNSS-RTK algorithm, where the GNSS-RTK factor includes the position of the carrier.

3. The method according to claim 2, characterized in that It also includes the operation of pre-initializing the GNSS / INS integrated navigation system: Acquire satellite measurement information of the carrier according to the GNSS observation factor, wherein the satellite measurement information includes the position, attitude and speed of the carrier; Acquire inertial measurement information of the vehicle through the IMU, wherein the inertial measurement information includes acceleration and angular velocity of the carrier; The GNSS / INS integrated navigation system is initialized according to the satellite measurement information and the inertial measurement information to obtain estimated system parameters and initial state information of the carrier.

4. The method according to claim 3, characterized in that It also includes the operation of pre-estimating the installation angle of INS relative to the vehicle: Acquire a dead reckoning result and a positioning result of the carrier according to the satellite measurement information and the inertial measurement information; The dead reckoning result and the positioning result are fused by Kalman filtering to estimate the installation angle of the INS relative to the carrier.

5. The method according to claim 4, characterized in that The second factor includes an IMU pre-integration factor; Correspondingly, obtaining the first factor and the second factor through the GNSS / INS integrated navigation system also includes: Initializing an inertial navigation mechanical arrangement algorithm according to the estimated system parameters, the initial state information, and the installation angle of the INS relative to the carrier; The IMU pre-integration factor is obtained through the initialized inertial navigation mechanical arrangement algorithm.

6. The method according to claim 1, characterized in that Acquiring a plurality of feature points from the target image and performing tracking on the plurality of feature points, including: Extracting multiple feature points from the target image using a Shi-Tomasi corner detection algorithm; Tracking of multiple feature points is performed using the Lukas-Kanade optical flow algorithm.

7. The method according to claim 1, characterized in that Perform tracking on multiple feature points, including: Obtaining light change information of the target image through a histogram detection algorithm; According to the light change information, adjusting the light intensity of the target image according to a preset rule; Based on the adjusted target image, tracking is performed on a plurality of feature points.

8. The method according to claim 1, characterized in that Acquire multiple feature points from the target image, including: ORB feature points are extracted from the target image through a preset low-frequency process.

9. The method according to claim 1, characterized in that: Perform triangulation on multiple feature points, including: Adjust the error threshold based on historical reprojection; Eliminate some feature points from the plurality of feature points according to the adjusted error threshold; Perform triangulation on the remaining feature points to obtain the corresponding visual landmark points.

10. The method according to any one of claims 1 to 9, characterized in that: Also includes: Feedback the current status information of the carrier to the inertial navigation mechanical arrangement algorithm and GNSS-RTK algorithm for algorithm optimization; Feedback the current status information of the carrier to the visual system to update the visual landmarks.

11. A multi-sensor fusion carrier positioning and attitude determination device, characterized in that: The device comprises: A first acquisition module is used to acquire a first factor and a second factor through a GNSS / INS integrated navigation system, wherein the first factor is acquired through a GNSS system, and the second factor is acquired through an INS system; A second acquisition module is used to acquire the third factor through a visual system; The first execution module is used to execute the first mode when there is no abnormality in the GNSS system and the visual system: obtaining the current state information and the target position of the carrier through the factor graph optimization algorithm according to the prior factor, the first factor, the second factor and the third factor; The second execution module is used to execute the second mode when an abnormality occurs in the GNSS system or the visual system: obtaining the current state information and the target position of the carrier through a factor graph optimization algorithm according to the prior factor, the corresponding factor of the normal system and the second factor; Feedback module, used to feed back the current status information of the carrier to the INS system to obtain the target attitude and target speed; Wherein, the a priori factor includes the state information of the carrier at a previous moment, and the third factor includes the reprojection factor; Correspondingly, the third factor is obtained through the visual system, including: Acquiring prior attitude information of the carrier through the GNSS / INS integrated navigation system; Acquire the target image through the visual system; According to the prior posture information of the carrier, a plurality of feature points are obtained from the target image through a visual system, and the plurality of feature points are tracked; Perform triangulation on multiple feature points to obtain multiple visual landmark points; Obtain a reprojection factor according to multiple feature points and multiple visual landmark points.

12. A computer device, characterized in that: include: at least one processor; and a memory communicatively coupled to the at least one processor; wherein: The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method according to any one of claims 1 to 10.

13. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and when the computer instructions are executed by a processor, the method according to any one of claims 1 to 10 is implemented.

Images