Visual / inertial global positioning method and system in GNSS denial environment

Abstract

The invention relates to the technical field of unmanned platform navigation, in particular to a visual / inertial global positioning method and system in a GNSS denial environment, and the method comprises the steps: calibrating a starting position of an unmanned platform, obtaining an initial position # imgabs0 # and an initial course angle # imgabs1 #, and building a VIO coordinate system through the visual inertia joint initialization of the unmanned platform; the attitude # imgabs2 # and the position # imgabs3 # of the unmanned platform under the VIO coordinate system are obtained, the Z axis of the VIO coordinate system is aligned with the gravity direction through a gravity vector alignment method, a rotation matrix # imgabs4 # is obtained, and the VIO coordinate system subjected to gravity alignment is rotated according to the initial course angle # imgabs5 #; and obtaining a conversion matrix # imgabs6 # from the VIO coordinate system to a local geographic coordinate system, converting pose information under the VIO coordinate system to the local geographic coordinate system, obtaining # imgabs7 # and # imgabs8 #, and converting the pose information under the local geographic coordinate system to global coordinates # imgabs9 # under a geodetic coordinate system. The global positioning and navigation of the unmanned platform in the GNSS denial environment are realized without depending on real-time GNSS signals.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned platform navigation, and particularly relates to a visual / inertial global positioning method and system in a GNSS-denied environment, which is applicable to the autonomous navigation tasks of intelligent unmanned systems. Background Art

[0002] Intelligent unmanned systems are currently in the process of evolving from concepts to practical applications. Whether in civilian fields such as intelligent transportation and embodied intelligence, or in military fields such as reconnaissance and strikes on the military battlefield, the role of intelligent unmanned systems is becoming increasingly prominent. The prerequisite for unmanned platforms to perform these intelligent tasks is to achieve stable navigation. Although positioning and navigation methods such as scene matching based on map data or brain-like navigation based on learning can achieve navigation without providing the coordinates of the target but only relying on the image of the target, in a position environment without prior information, traditional navigation methods still require a known global coordinate system as the navigation target. GNSS can easily obtain the real-time global position information of the unmanned platform to complete the navigation task and has become the main method of civilian navigation. However, in environments such as urban canyons, valleys, and forests, GNSS signals are easily affected by occlusion, reflection, etc., resulting in signal weakening or loss; and as the electromagnetic environment becomes increasingly complex, the security of GNSS is vulnerable to jamming and spoofing interference, and its radio vulnerability is prominent. For GNSS-denied environments, vision and inertial combination can perform autonomous high-precision pose and position determination, but cameras and inertial measurement units (IMUs) cannot provide absolute measurement information. The pose obtained by the vision and inertial combination is a relative result relative to the initial state of the system and cannot be directly applied to the navigation task of the unmanned platform in the global coordinate system. VINS-Fusion resolves the transformation matrix between the VIO system and the GNSS system through the loose coupling of a continuous segment of VIO positions and GNSS positions within a sliding window, thereby obtaining the global visual, inertial, and GNSS combined pose result. However, this method requires stable and continuous GNSS positioning results and requires the platform to perform sufficient motion excitation within a certain time sliding window to perform accurate coordinate transformation, which has relatively high requirements for the environment in practical applications and relies on receiving stable GNSS signals. Summary of the Invention

[0003] The present invention overcomes the defect that the existing vision-inertial navigation system cannot directly obtain global absolute position information in a GNSS-denied environment, and proposes a visual / inertial global positioning method and system in a GNSS-denied environment. Through static calibration before the movement of the unmanned platform, stable and high-precision coordinate transformation is achieved under GNSS-denied conditions, thereby realizing visual / inertial positioning and navigation in the global coordinate system.

[0004] To achieve the above object, the technical solution adopted is as follows:

[0005] The present invention provides a visual / inertial global positioning method in a GNSS-denied environment, comprising the following steps:

[0006] Step 1: Calibrate the departure position of the unmanned platform to obtain the initial position and the initial heading angle

[0007] Step 2: Establish a VIO coordinate system through visual-inertial joint initialization of the unmanned platform, and obtain the attitude and position

[0008] Step 3: Align the Z-axis of the VIO coordinate system with the gravity direction by using the gravity vector alignment method to obtain the rotation matrix

[0009] Step 4: Rotate the gravity-aligned VIO coordinate system according to the initial heading angle to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system Convert the pose information in the VIO coordinate system to the local geodetic coordinate system to obtain and

[0010] Step 5: Convert the pose information in the local geodetic coordinate system to the global coordinates in the geodetic coordinate system

[0011] According to the visual / inertial global positioning method in a GNSS-denied environment of the present invention, further, the calibration of the departure position of the unmanned platform in Step 1 includes:

[0012] Obtain the geodetic coordinates of the departure position through RTK measurement as the initial position Obtain the initial azimuth angle through a dual-antenna RTK, magnetometer or geodetic reference orientation method as the initial heading angle of the unmanned platform in the local geodetic coordinate system

[0013] According to the visual / inertial global positioning method in a GNSS-denied environment of the present invention, further, Step 2 specifically includes:

[0014] Combined with the camera internal parameters and external parameters, triangulate the 2D feature points obtained by the binocular camera to solve the 3D coordinates of the feature points, and establish an initial VIO coordinate system with the optical center of the left camera as the origin;

[0015] Match the 2D feature points observed by the current binocular camera with the 3D map points obtained by triangulation, and use the PnP algorithm to inversely calculate the initial pose estimation of the unmanned platform in the VIO coordinate system as the initial value input for the backend optimization algorithm.

[0016] According to the visual / inertial global positioning method in the GNSS-denied environment of the present invention, further, step 3 specifically includes:

[0017] Collect the accelerometer output of the IMU in the stationary state, and the gravity vector is the average value of the accelerometer output;

[0018] Use the Rodrigues formula to construct the rotation matrix Rotate the VIO coordinate system around the rotation axis k by an angle θ so that the negative Z-axis of the rotated VIO coordinate system, i.e., the v' system, coincides exactly with the gravity direction.

[0019] According to the visual / inertial global positioning method in the GNSS-denied environment of the present invention, further, the rotation matrix is constructed by the formula:

[0020]

[0021] where θ is the rotation angle, k is the rotation axis, and [k] , , ,

[0022] ,

[0021] , , , , ,

[0026] , , ,

[0023] , ,

[0025] , , × , , ,

[0024] , ,

[0027] , ,

[0020] , <% , , is the skew-symmetric matrix and I is the identity matrix.

[0022] According to the visual / inertial global positioning method in the GNSS-denied environment of the present invention, further, in step 4, rotate the gravity-aligned VIO coordinate system according to the initial heading angle to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system, specifically including:

[0023] Rotate around the Z-axis of the v' system by the initial heading angle to align the X-axis of the v' system with the north direction, i.e., the Y-axis, of the local geodetic coordinate system;

[0024] Use the rotation matrix based on the initial heading angle and the rotation matrix to multiply and obtain the transformation matrix

[0025] According to the visual / inertial global positioning method in the GNSS-denied environment of the present invention, further, the formula of the rotation matrix is as follows:

[0026]

[0027] According to the visual / inertial global positioning method in a GNSS-denied environment of the present invention, further, in step 4, the and are converted through the following formula:

[0028]

[0029] wherein, is the attitude of the unmanned platform in the local geodetic coordinate system, is the position of the unmanned platform in the local geodetic coordinate system, is the relative translation amount between the VIO coordinate system and the local geodetic coordinate system.

[0030] According to the visual / inertial global positioning method in a GNSS-denied environment of the present invention, further, step 5 specifically includes:

[0031] Convert the initial position to the coordinates

[0032] in the geocentric earth coordinate system. Convert the position in the local geodetic coordinate system to the coordinates

[0033] in the geocentric earth coordinate system. Through iterative solution, convert to the global coordinates

[0034] in the geodetic coordinate system. Further, the present invention also provides a visual / inertial global positioning system in a GNSS-denied environment for implementing the above-mentioned visual / inertial global positioning method in a GNSS-denied environment, and the system includes:

[0035] A calibration module for calibrating the departure position of the unmanned platform to obtain the initial position and the initial heading angle

[0036] A visual-inertial combination module for establishing a VIO coordinate system through visual-inertial joint initialization of the unmanned platform and obtaining the attitude and the position

[0037] An alignment module for aligning the Z-axis of the VIO coordinate system with the gravity direction by using the gravity vector alignment method to obtain the rotation matrix

[0038] A coordinate transformation module 1 for rotating the gravity-aligned VIO coordinate system according to the initial heading angle to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system.Convert the pose information in the VIO coordinate system to the local geodetic coordinate system to obtain and

[0039] A coordinate conversion module 2 is used to convert the pose information in the local geodetic coordinate system into the global coordinates in the geodetic coordinate system

[0040] Adopting the above technical solution, the beneficial effects obtained are as follows:

[0041] 1. High-precision global positioning: Through the calibration of the departure position of the unmanned platform, the present invention provides high-precision initial azimuth and position information for the visual-inertial integrated system in the static initialization stage, and obtains the conversion matrix from the VIO coordinate system to the geodetic coordinate system through the conversion relationship between multiple coordinate systems, so as to accurately map the positioning information in the local VIO coordinate system obtained by the visual-inertial integrated algorithm to the global geodetic coordinate system, realizing the navigation and positioning of the visual-inertial navigation system in the global coordinate system under the GNSS denial environment, and providing global positioning guarantee for the unmanned platform to complete the navigation task and exert the efficiency of the intelligent unmanned system.

[0042] 2. Strong environmental adaptability: The present invention does not need to rely on continuous GNSS signals, and only needs a short calibration at the start to achieve stable navigation in complex environments (such as electromagnetic interference areas).

[0043] 3. Concealment and autonomy: In the calibration stage (before task execution), the present invention needs to use GNSS (such as RTK) to obtain the initial position and initial heading angle; however, in the task execution stage (under the GNSS denial environment), the unmanned platform is completely separated from the GNSS signal and can achieve global positioning only relying on visual / inertial sensors and calibration data. The present invention does not need to transmit and receive radio signals such as GNSS signals, increasing the autonomy and concealment of the unmanned platform.

[0044] This technology provides a reliable solution for the global positioning of unmanned platforms in GNSS failure scenarios, and has both theoretical innovation and engineering practical value. Brief Description of the Drawings

[0045] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings of the embodiments of the present invention will be briefly introduced below. Among them, the drawings are only used to show some embodiments of the present invention, rather than limiting all embodiments of the present invention thereto.

[0046] Figure 1 It is a schematic flow chart of the visual / inertial global positioning method under the GNSS denial environment of the embodiments of the present invention. Detailed Embodiments

[0047] In the following, the exemplary solutions of the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings of the specific embodiments of the present invention. Unless otherwise defined, the technical terms or scientific terms used in the present invention should have the ordinary meaning understood by those of ordinary skill in the art.

[0048] The conventional navigation task assigned to an intelligent autonomous unmanned platform is to perform information fusion processing on the navigation observation data of its own sensors to obtain information such as its own position and attitude, and to make autonomous decisions and path planning based on the target position coordinates provided by the task, so as to reach the target position. The two key points to complete this navigation task are the position coordinates of the target and the pose solution of the multi-source sensors of the platform itself. In actual situations, the target position that can be provided is generally the coordinate in the global coordinate system, such as the longitude and latitude of the target in the geodetic coordinate system. In the Global Navigation Satellite System (GNSS) denied environment, the position information obtained by fusing vision and inertia of the unmanned platform is usually the local coordinate system of the vehicle body. For example, the Visual-Inertial Odometry (VIO) establishes a local coordinate system based on the initial state of the platform, which is difficult to be directly used for the navigation of the unmanned platform. Therefore, the present invention aims to provide a global positioning vision / inertial integrated navigation method on the unmanned platform to provide global position information and attitude information for the unmanned platform, so that the unmanned platform can navigate to the target in the global coordinate system, which better meets the actual requirements of the navigation target.

[0049] First, the definition and description of the coordinate system are carried out.

[0050] ① Geodetic coordinate system (g system): The origin O coincides with the center of the earth mass. The spatial points in the coordinate system are represented by longitude b, latitude l, and elevation h, and its point coordinates are: P g (b, l, h). Among them, the differences caused by different reference ellipsoids are not within the scope of discussion of the present invention, and the definition of the global coordinate system is kept unified.

[0051] ② Earth-centered earth coordinate system (e system): The origin O coincides with the center of the earth mass. The Z-axis coincides with the earth's rotation axis and points to the north pole. The X-axis points to the intersection of the Greenwich meridian plane and the equatorial plane. The Y-axis is determined by the right-hand system. The e system is a global rectangular coordinate system, and its point coordinates are P e (x e , y e , z e ).

[0052] ③ Local geodetic coordinate system (l system): The origin O coincides with the starting point of the departure position. The X-axis points to the east, the Y-axis points to the north, and the Z-axis points to the sky, that is, the east-north-sky coordinate system. The l system is a local coordinate system relative to the starting point, and its point coordinates are P l(x l ,y l ,z l ).

[0053] ④ VIO coordinate system (v system): Taking VINS-Mono / VINS-Fusion as an example, the origin O of the established VIO coordinate system is the position of the camera optical center at the moment when the visual inertial system is initialized in the IMU system. Its axis is the same as that of the IMU coordinate system at the initialization moment, that is, the X-axis points forward, the Z-axis is perpendicular to the IMU installation plane and points upward, and the Y-axis is established by the right-hand system. The v system is the coordinate system where the visual inertial combination result is located. Its odometer attribute characterizes the relative motion of the system relative to the initial state. However, the v system is a local coordinate system independent of the earth parameters, and its point coordinates are P v (x v ,y v ,z v ).

[0054] ⑤ Vehicle body coordinate system (b system): The origin is the geometric center of the vehicle body. The X-axis points forward of the vehicle body, the Z-axis is perpendicular to the vehicle body plane and points upward, and the Y-axis is established by the right-hand system. The vehicle body coordinate system moves with the vehicle body and is used to express the motion state of the vehicle body. Since the relative relationship between the vehicle body and the sensor can be obtained through calibration, the present invention simplifies the vehicle body coordinate system to have the origin as the camera optical center, the X-axis pointing forward of the IMU, the Z-axis perpendicular to the IMU installation plane and pointing upward, and the Y-axis established by the right-hand system.

[0055] This embodiment discloses a visual / inertial global positioning method in a GNSS-denied environment, as Figure 1 shown, including the following steps:

[0056] Step S101, calibrate the departure position of the unmanned platform to obtain the initial position and the initial heading angle

[0057] Before the unmanned platform executes the task, it is necessary to calibrate the departure position of the unmanned platform. The calibration of the departure position of the unmanned platform mainly performs two tasks. One is to use the GNSS signal guarantee of the position to measure the geodetic coordinates of the measurement points at the departure position as the initial position through real-time kinematic (RTK) The other is to obtain the azimuth angle of the forward direction of the departure position relative to the geographic north through a dual-antenna RTK with a certain baseline length, a magnetometer, or an observation and orientation method using a geodetic datum, that is, the initial heading angle of the unmanned platform in the l system

[0058] During the calibration phase, the global coordinates and heading angle of the starting point are calibrated using high-precision GNSS (such as RTK) for a short time (only once). At this time, there is no signal rejection in the environment, and the GNSS signal is stable and reliable. After the platform enters the occlusion / interference area (GNSS signal rejection), the GNSS signal transceiver is completely cut off, and only the vision / inertial sensors and pre-calibrated initial parameters are relied on to maintain global positioning.

[0059] Step S102: Establish a VIO coordinate system through the vision-inertial joint initialization of the unmanned platform, and obtain the attitude and position

[0060] After the navigation task is issued, the unmanned platform is accurately placed at the indicated position of the departure position, and the vision / inertial joint initialization of the unmanned platform is carried out to establish a VIO coordinate system. This solution uses a combination of binocular vision and inertia. Compared with the monocular vision system that needs to estimate the scale during the movement process to complete the initialization, the binocular vision system can complete the initialization and establish a VIO coordinate system while stationary at the departure position.

[0061] During the initialization process, the vision / inertial system establishes a temporary VIO system with the optical center of the left eye camera as the origin. The binocular camera is used to take pictures of the map points to obtain the 2D coordinates of the feature points in different perspective camera systems, and the relative pose between the binoculars obtained by the internal and external parameter calibration is combined to solve the 3D position of the feature points, as shown in Equation (1).

[0062]

[0063] Among them, is the 2D coordinate of the feature point in the image coordinate system, is the 3D coordinate of the feature point in the VIO system, K is the internal parameter of the camera, and [R|t] is the external parameter of the binocular camera. By inputting enough feature points, the equation can be solved and the VIO system can be established. Then, the initial pose estimation of the unmanned platform's movement is completed through the matching 2D and 3D points by PnP (perspective-n-point) and input as the initial value of the backend optimization algorithm. During the movement of the unmanned platform, the initial pose estimation is optimized by the VINS-Fusion vision-inertial combination algorithm, and the stable attitude and position However, the platform pose obtained at this time is in the local coordinate system, which is only related to the initial state of the platform and has nothing to do with the earth parameters, and it is difficult to be applied to the navigation task under the global longitude and latitude coordinates.

[0064] Step S103: Align the Z axis of the VIO coordinate system with the gravity direction using the gravity vector alignment method to obtain the rotation matrix

[0065] Unmanned platforms need to navigate based on Earth-referenced orientations and thus must align sensor data to the horizontal plane and heading of a local geographic coordinate system.

[0066] To transform the pose in the VIO frame to the local geographic coordinate system, the gravity vector alignment process and the initial heading angle obtained from platform position calibration need to be combined. Since the Z-axis of the VIO frame is established perpendicular to the IMU plane, but in fact the plane where the IMU is placed cannot be guaranteed to be consistent with the horizontal plane, it is necessary to adjust the direction of its Z-axis so that the Z-axis direction is aligned with the gravity vector. For low-precision Micro-Electro-Mechanical Systems (MEMS) IMUs, although they cannot be directly aligned to the local geographic coordinate system through initial alignment, it can be considered that the accelerometer only senses the direction of gravity when stationary. When the unmanned platform is stationary on the departure platform during the initialization phase, the accelerometer of the IMU can continuously collect the acceleration output in a sliding window [a,…,b] from time a to time b. The gravity vector is the average accelerometer output:

[0067]

[0068] Normalize the gravity vector to obtain the gravity direction:

[0069]

[0070] Assume that the coordinate system after gravity vector alignment is the v′ frame. To rotate the v frame so that the negative Z-axis direction coincides with the gravity direction, a rotation matrix needs to be constructed through Rodrigues' formula.

[0071]

[0072] where I is the identity matrix, k is the axis of rotation, θ is the angle of rotation, and [k] × is the skew-symmetric matrix.

[0073] The axis of rotation k is perpendicular to the gravity vector direction and the Z-axis direction of the v frame and is obtained by cross multiplying the two vectors:

[0074]

[0075] Its skew-symmetric matrix [k] × is:

[0076]

[0077] The angle of rotation θ is the angle between the negative Z-axis and the gravity vector and satisfies:

[0078]

[0079] That is:

[0080]

[0081] By aligning the gravity vector, the Z-axis of the v' system is adjusted to be parallel to the Z-axis (skyward) of the local geographic coordinate system, thereby ensuring that the XY plane of the v' system is parallel to the east-north plane of the l system, eliminating the initial tilt error between the IMU installation plane and the horizontal plane, and providing a horizontal reference for subsequent heading angle alignment.

[0082] Step S104. According to the initial heading angle Rotate the gravity-aligned VIO coordinate system to obtain the transformation matrix from the VIO coordinate system to the local geographic coordinate system Convert the pose information in the VIO coordinate system to the local geographic coordinate system to obtain and

[0083] Gravity alignment (Equation 4) only ensures that the XY plane of the v' system is horizontal (the Z-axis is aligned with gravity), but the heading angle (orientation around the Z-axis) is still arbitrary and has nothing to do with the earth's orientation. Navigation requires the vehicle's attitude to be bound to the geographic directions (east, north, sky). For example, to determine whether the "forward direction points due north". Therefore, through the calibrated initial heading angle Rotate the v' system to be completely aligned with the l system (on the premise that the XY plane has been ensured to be horizontal).

[0084] After gravity alignment, the initial heading angle obtained by azimuth measurement methods such as RTK and magnetometer during the calibration of the departure platform is required Rotate the v' system around its Z-axis so that the X-axis (vehicle forward) of the v' system is aligned with the Y-axis (northward) of the local geographic coordinate system. The rotated coordinate system is completely aligned with the local geographic coordinate system.

[0085] Rotation matrix is as follows:

[0086]

[0087] Thus, the transformation matrix from the VIO system to the local geographic coordinate system is obtained

[0088]

[0089] Through the transformation matrix The pose output of the unmanned platform can be and Converted to the local geographic system associated with the earth information:

[0090]

[0091] Among them, is the attitude of the unmanned platform in the local geographic coordinate system, is the position of the unmanned platform in the local geographic coordinate system, is the relative translation between the v - system and the l - system, that is, the translation amount from the left - eye optical center of the camera to the measurement point of the departure position when the unmanned platform is stationary at the departure position, which can be obtained by measurement during the initialization process. At this time, the attitude of the vehicle in the local geographic coordinate system relative to the east - north - up direction can already globally represent the attitude of the platform relative to the earth, and is more conducive to platform navigation compared to the geocentric - earth coordinate system. Therefore, can be used as the global - system attitude result output to the unmanned platform.

[0092] Step S105: Convert the pose information in the local geographic coordinate system into the global coordinates in the geodetic coordinate system

[0093] Utilize the initial position obtained by RTK measurement during the calibration of the departure position Convert the east - north - up coordinates of the unmanned platform in the local geographic coordinate system into the global geodetic latitude - longitude - height coordinates in the geodetic coordinate system. First, convert the initial position to the geocentric - earth coordinate system:

[0094]

[0095] Among them, is the position of the departure position in the geocentric - earth coordinate system, N is the normal length of the reference ellipsoid at the position of the position, E is the eccentricity of the reference ellipsoid, and the reference ellipsoid is unified with the system used when measuring the departure position.

[0096] Next, convert the position of the unmanned platform in the local geographic coordinate system to the geocentric - earth coordinate system:

[0097]

[0098] Among them, S is the orthogonal transformation matrix from the local geographic coordinate system to the geocentric - earth coordinate system.

[0099] Finally, convert the position of the unmanned platform in the geocentric - earth coordinate system to the geodetic coordinate system:

[0100]

[0101] In the actual process of solving lat b , it is necessary to use Perform iterative solution with the initial value. Finally, under GNSS denial conditions, convert the pose of the local coordinate system obtained by visual-inertial integration into the global position information in the geodetic coordinate system and provide it to the unmanned platform, providing stable global pose information for the navigation task of the unmanned platform. The iterative solution process of formula (14) gradually approaches the true value of the carrier latitude lat b through the Newton-Raphson method. After the latitude converges, directly import the result into the third line of formula (14) to calculate the elevation.

[0102] Correspondingly, this embodiment also proposes a visual / inertial global positioning system in a GNSS denial environment, including:

[0103] A calibration module for calibrating the departure position of the unmanned platform to obtain the initial position and the initial heading angle

[0104] A visual-inertial integration module for establishing a VIO coordinate system through visual-inertial joint initialization of the unmanned platform and obtaining the attitude and position

[0105] An alignment module for aligning the Z axis of the VIO coordinate system with the gravity direction using the gravity vector alignment method to obtain the rotation matrix

[0106] A coordinate transformation module 1 for rotating the gravity-aligned VIO coordinate system according to the initial heading angle to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system Convert the pose information in the VIO coordinate system to the local geodetic coordinate system to obtain and

[0107] A coordinate transformation module 2 for converting the pose information in the local geodetic coordinate system into the global coordinates in the geodetic coordinate system

[0108] The present invention provides global reference information for the unmanned platform through the calibrated departure position of the unmanned platform, realizes the calculation of the accurate transformation matrix from the VIO coordinate system to the global coordinate system during the static initialization stage before the unmanned platform departs, and thus ensures that the unmanned platform can autonomously achieve global positioning during navigation. The present invention does not require radio signals such as GNSS signals to be transmitted and received, increasing the autonomy and concealment of the unmanned platform.

[0109] Those of ordinary skill in the art can understand that all or part of the steps in the above method can be completed by instructing relevant hardware through a program, and the program can be stored in a computer-readable storage medium, such as: read-only memory, magnetic disk or optical disc, etc. Optionally, all or part of the steps of the above embodiments can also be implemented using one or more integrated circuits. Correspondingly, each module / unit in the above embodiments can be implemented in the form of hardware or in the form of a software function module. The present invention is not limited to any specific form of combination of hardware and software.

[0110] Finally, it should be noted that the above embodiments are only specific embodiments of the present invention, used to illustrate the technical solutions of the present invention, rather than limiting it. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: any person skilled in the art within the technical scope disclosed by the present invention can still modify the technical solutions recorded in the foregoing embodiments, or can easily think of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A visual / inertial global positioning method in a GNSS-denied environment, characterized in that Including the following steps: Step 1: Calibrate the departure position of the unmanned platform to obtain the initial position and the initial heading angle Step 2: Establish a VIO coordinate system through visual-inertial joint initialization of the unmanned platform, and obtain the attitude and position of the unmanned platform in the VIO coordinate system and position Step 3: Align the Z-axis of the VIO coordinate system with the gravity direction using the gravity vector alignment method to obtain the rotation matrix Step 4. Rotate the gravity-aligned VIO coordinate system according to the initial heading angle to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system Convert the pose information in the VIO coordinate system to the local geodetic coordinate system to obtain and Step 5: Convert the pose information in the local geographic coordinate system into the global coordinates in the geodetic coordinate system 2. The visual / inertial global positioning method in a GNSS-denied environment according to claim 1, wherein Calibrating the departure position of the unmanned platform in step 1 includes: Obtain the geodetic coordinates of the departure position through RTK measurement as the initial position Obtain the initial azimuth angle through dual-antenna RTK, magnetometer or geodetic datum orientation method as the initial heading angle of the unmanned platform in the local geodetic coordinate system 3. The visual / inertial global positioning method in a GNSS-denied environment according to claim 1, wherein Step 2 specifically includes: Combining the internal and external parameters of the camera, triangulating the 2D feature points obtained by the binocular camera to solve the 3D coordinates of the feature points, and establishing an initial VIO coordinate system with the optical center of the left camera as the origin; Matching the 2D feature points observed by the current binocular camera with the 3D map points obtained by triangulation, and inversely calculating the initial pose estimation of the unmanned platform in the VIO coordinate system through the PnP algorithm, which is used as the initial value input for the backend optimization algorithm.

4. The visual / inertial global positioning method in a GNSS-denied environment according to claim 1, wherein Step 3 specifically includes: Collecting the output of the accelerometer of the IMU in the stationary state, and the gravity vector is the average value of the accelerometer output; Construct a rotation matrix using Rodrigues' formula Rotate the VIO coordinate system by an angle θ around the rotation axis k so that the negative Z-axis of the rotated VIO coordinate system, i.e., the v' system, coincides exactly with the direction of gravity.

5. The visual / inertial global positioning method in a GNSS-denied environment according to claim 4, wherein Rotation matrix The construction formula is as follows: where θ is the rotation angle, k is the rotation axis, and [k] × is a skew-symmetric matrix and I is the identity matrix.

6. The visual / inertial global positioning method in a GNSS-denied environment according to claim 4, wherein In step 4, according to the initial heading angle rotate the gravity-aligned VIO coordinate system to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system Specifically, it includes: Rotate the initial heading angle about the Z-axis of the v' system Align the X-axis of the v' system with the north direction of the local geodetic coordinate system, i.e., the Y-axis; Use the rotation matrix based on the initial heading angle and multiply it with the rotation matrix to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system ​ 7. The visual / inertial global positioning method in a GNSS-denied environment according to claim 6, wherein Rotation matrix The formula is as follows:

8. The visual / inertial global positioning method in a GNSS-denied environment according to claim 1, wherein In step 4, under the local geographic coordinate system, and are converted by the following formula: Among them, is the attitude of the unmanned platform in the local geographic coordinate system, is the position of the unmanned platform in the local geographic coordinate system, is the relative translation amount between the VIO coordinate system and the local geographic coordinate system.

9. The visual / inertial global positioning method in a GNSS-denied environment according to claim 1, wherein Step 5 specifically includes: Convert the initial position to the coordinates in the geocentric Earth coordinate system Convert the position in the local geographic coordinate system to the coordinates in the geocentric earth coordinate system Convert to global coordinates in the geodetic coordinate system through iterative solution ​ 10. A visual / inertial global positioning system in a GNSS-denied environment, characterized in that, For implementing the visual / inertial global positioning method in the GNSS-denied environment according to any one of claims 1-9, the system includes: Calibration module, used to calibrate the departure position of the unmanned platform and obtain the initial position and the initial heading angle Visual inertial combination module, which is used to establish a VIO coordinate system through the visual inertial joint initialization of an unmanned platform and obtain the attitude and position of the unmanned platform in the VIO coordinate system and position Alignment module, which is used to align the Z-axis of the VIO coordinate system with the gravity direction by using the gravity vector alignment method to obtain a rotation matrix A coordinate transformation module, which is used to rotate the VIO coordinate system aligned with gravity according to the initial heading angle to obtain the transformation matrix from the VIO coordinate system to the local geodetic coordinate system transform the pose information in the VIO coordinate system to the local geodetic coordinate system to obtain and Coordinate transformation module II, which is used to transform the pose information in the local geodetic coordinate system into the global coordinates in the geodetic coordinate system

Images