A High-Precision Positioning Method, System and Electronic Device in a Satellite Denial Environment

Abstract

The present invention discloses a high-precision positioning method, system and electronic device in a satellite-denied environment. The high-precision positioning method provided by the present invention includes obtaining the pose of a lidar in the geodetic coordinate system; fusing the pose of the lidar in the geodetic coordinate system and the information collected by an IMU; and performing information fusion and global correction on the fused pose of the lidar in the geodetic coordinate system through factor graph optimization with the lidar and the IMU. The present invention can globally correct the lidar in a denied environment by arranging a small number of aruco markers, thereby greatly improving the navigation accuracy and the robustness of the system.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular, to a high-precision positioning method, system, and electronic device in a satellite-denied environment. Background Art

[0002] Currently, most positioning algorithms rely on satellite navigation systems, and the number of positioning algorithms applicable to denied environments is relatively limited. Existing positioning algorithms in denied environments all operate in a local coordinate system or an odometer coordinate system, such as LIOM, VINS, ORB_SLAM, mapping and localization from planar marker, UWB positioning system, and so on.

[0003] Among them, maps drawn by odometry algorithms such as LIOM and VINS are in the odometer coordinate system or the local coordinate system, and cannot establish a conversion relationship with the global coordinate system in a denied environment. Although the above algorithms have correction methods such as loop detection, these are all corrections based on the local coordinate system and do not really establish a connection with the global coordinate system. This leads to the situation that when the algorithm runs in a large-scale denied environment for a long time, due to error accumulation, the odometer cannot effectively and truly correct the result according to the actual position.

[0004] The navigation algorithm in mapping and localization from planar marker can only be applied to small-scale environments, requiring the sensor to see multiple marker points at a single time and using the markers as a kind of easily recognizable markers. However, arranging the marker density extremely high not only affects the operation of other tasks but is also unrealistic; moreover, this algorithm also cannot reflect the position of the odometer in the geodetic coordinate system through the markers.

[0005] The UWB positioning system is similar to the satellite navigation system in a denied environment, providing position information for the odometer through pre-deployed electromagnetic waves in the environment. However, it is costly, has extremely strict requirements for the electromagnetic environment, has a short operating range, is vulnerable to interference, and cannot achieve low-cost positioning through a small number of base stations in a large-scale denied environment.

[0006] Therefore, it is necessary to study a high-precision positioning method in a denied environment that can solve the above technical problems. Summary of the Invention

[0007] In view of the problems existing in the above-mentioned prior art, the present invention provides a high-precision positioning method, system, and electronic device in a satellite-denied environment.

[0008] To achieve the above object, in a first aspect, the present invention provides a high-precision positioning method in a satellite-denied environment, which includes:

[0009] Obtain the pose of the lidar in the geodetic coordinate system;

[0010] Fuse the pose of the lidar in the geodetic coordinate system and the information collected by the IMU; and

[0011] Use the fused pose of the lidar in the geodetic coordinate system to optimize through a factor graph for information fusion and global correction with the lidar and the IMU.

[0012] Preferably, the process of obtaining the pose of the lidar in the geodetic coordinate system includes:

[0013] Obtain the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system;

[0014] According to the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system and the side length of the corresponding aruco marker, obtain the rotation matrix and translation matrix of at least one aruco marker relative to the camera coordinate system;

[0015] According to the rotation matrix and translation matrix of at least one aruco marker relative to the camera coordinate system, the attitude angle of the pan-tilt, the relative position between the lidar and the pan-tilt, and the pose of at least one aruco marker in the geodetic coordinate system, calculate the pose of the lidar in the geodetic coordinate system.

[0016] In a second aspect, the present invention provides a high-precision positioning system for a satellite-denied environment, which includes:

[0017] A module for obtaining the pose of the lidar in the geodetic coordinate system;

[0018] A module for fusing the pose of the lidar in the geodetic coordinate system and the information collected by the IMU; and

[0019] A module for using the fused pose of the lidar in the geodetic coordinate system to optimize through a factor graph for information fusion and global correction with the lidar and the IMU.

[0020] In a third aspect, the present invention provides an electronic device, which includes: a memory, a processor;

[0021] The memory is used to store executable instructions for the processor;

[0022] The processor is used to implement the high-precision positioning method for a satellite-denied environment in the first aspect according to the executable instructions stored in the memory.

[0023] The beneficial effects of the high-precision positioning method, system and electronic device for a satellite-denied environment of the present invention include:

[0024] (1) Compared with the prior art which is applicable to satellite navigation environment or two-dimensional navigation, the present invention is more applicable to three-dimensional navigation in a satellite-denied environment;

[0025] (2) Based on the fact that the aruco marker can obtain the pose of the lidar relative to the real environment in a denied environment, the present invention can achieve high-precision positioning. However, the prior art can only obtain the pose of the lidar in the lidar odometry coordinate system and cannot obtain the conversion relationship between this coordinate system and the real geodetic coordinate system;

[0026] (3) In a denied environment, the present invention can correct the lidar error according to the real environment, solving the problems of large cumulative error and inability to correct in the prior art;

[0027] (4) The present invention is easy to operate and can be achieved only by arranging a small number of aruco markers in the environment without the need for a large number of other sensors and equipment, saving costs. Description of the Drawings

[0028] Figure 1 is a schematic flow chart of a high-precision positioning method for a satellite-denied environment according to a preferred embodiment of the present invention;

[0029] Figure 2 is a schematic structural diagram of a high-precision positioning system for a satellite-denied environment according to a preferred embodiment of the present invention;

[0030] Figure 3 is a schematic diagram of the factor graph structure of the present invention;

[0031] Figure 4 is a simulation result graph of the trajectory results of the examples and Comparative Examples 1-2 of the present invention. Detailed Embodiments

[0032] The following elaborates on the preferred embodiments of the present invention in conjunction with the drawings, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby making the protection scope of the present invention more clearly defined.

[0033] It should be noted that in this article, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. Without further limitation, the elements defined by the statement "including..." do not exclude the existence of additional identical elements in the process, method, article or device including the elements.

[0034] Currently, most positioning algorithms rely on satellite navigation systems, and the number of positioning algorithms applicable to denied environments is relatively limited. Existing positioning algorithms in denied environments, such as LIOM, VINS, ORB_SLAM, etc., all operate in a local coordinate system or an odometer coordinate system, and the conversion relationship between the generated global map and the true geodetic coordinate system is unknown; or the mapping and positioning of planar markers, and the UWB positioning system require a large number of marker points or sensors to be arranged in advance, and have relatively strict requirements for the sensor environment.

[0035] Through research, it is found that obtaining the pose of the lidar in the real environment and correcting its error using the true position can greatly improve the navigation accuracy in a large-scale denied environment.

[0036] To achieve the above process, the present invention adds an aruco factor to the factor graph, avoiding the uncorrectable error generated by the satellite navigation system using only IMU and lidar data for UAV pose calculation over a long time. The aruco factor can fix and align the global coordinate system used in the IMU and lidar solution with the geodetic coordinate system, and correct the error of the UAV pose generated by the IMU and lidar solution through the aruco position information. At the same time, it serves as an information source to maintain the normal operation of the satellite navigation system when the lidar matching degrades.

[0037] Exemplarily, the application environment of the high-precision positioning method for satellite-denied environments of the present invention is described. A UAV system including various sensors is built. The specific process is to select a UAV (a conventional model can be used), and various sensors are respectively installed on the UAV, including: visual inertial odometer (VIO) and lidar, etc.; among them, the visual inertial odometer includes a camera (which may include a gimbal) and an inertial measurement unit (IMU), and the camera lens direction is consistent with the UAV forward direction. Both the visual inertial odometer and the lidar can be integrated on the flight control board of the UAV. Both the visual inertial odometer and the lidar can use conventional model components, and the total weight of the visual inertial odometer and the lidar does not exceed the rated load of the UAV. When the UAV system works, the lidar collects point cloud information, the IMU collects acceleration, attitude angles (angles), and angular rates, the camera collects image information, the gimbal collects the attitude angle of the camera (or gimbal), and at the same time, each sensor transmits the collected information to a terminal or server capable of data processing.

[0038] In the first aspect, the present invention provides a high-precision positioning method for satellite-denied environments, as Figure 1 shown. The method mainly includes the following steps:

[0039] Step S101, obtain the pose of the lidar in the geodetic coordinate system.

[0040] Among them, the denial environment refers to an environment that shields satellite signals. In this environment, navigation systems that use satellites for positioning cannot normally receive satellite signals and cannot work properly.

[0041] Preferably, before step S101, each sensor can be calibrated to obtain the parameters corresponding to each sensor respectively. For example, it may include: the internal and external parameter matrices of the camera, the internal parameters and measurement noise of the IMU, the lidar installation parameters, the relative pose relationship between the camera coordinate system and the IMU coordinate system, the pose of the IMU and lidar odometer, the pose of the camera and lidar, etc. Among them, there are already mature calibration methods for cameras, IMUs, lidars, etc., such as Zhang Zhengyou calibration method for cameras, error analysis calibration method for IMUs, and external automatic calibration method with maximized mutual information for lidars, etc.

[0042] Preferably, before step S101, aruco markers need to be arranged in the selected area (real environment), and the pose of any point in each aruco marker in the geodetic coordinate system is recorded. For the convenience of calculation, it is preferably any point or the center point on the periphery, and more preferably the pose of the center point of the aruco marker in the geodetic coordinate system The Aruco marker is placed as a reference marker on the object or scene to be imaged. It is a square with a black background, and the white pattern inside the square is used to represent the uniqueness of the marker and store some information. The purpose of the black boundary is to improve the accuracy and performance of aruco marker detection.

[0043] The present invention does not make specific restrictions on the arrangement and size of the aruco marker, which are determined by those skilled in the art according to the actual situation. Among them, the size of the aruco marker can be changed arbitrarily. In order to successfully detect, an appropriate size can be selected according to the size of the selected area and the scene. In actual use, if the size of the marker is too small, it may not be detected. At this time, a larger-sized marker can be selected, or the camera can be placed closer to the marker.

[0044] Exemplarily, when the selected area is a factory workshop, aruco markers can be arranged at the four corners of the factory workshop floor. In order to increase the detection accuracy, aruco markers can also be set at certain intervals on the four sides of the floor, for example, at intervals of 5 meters.

[0045] In a preferred embodiment of the present invention, the process of obtaining the pose of the lidar in the geodetic coordinate system may include the following steps:

[0046] S101-1. Obtain the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system.

[0047] In the present invention, the pixel coordinate system, the camera coordinate system, the earth coordinate system, the pan-tilt coordinate system, the airframe coordinate system, and the world coordinate system are all coordinate systems known in the art, and the various coordinate systems can be converted into each other; among them, the pixel coordinate system is a coordinate system representing the camera imaging plane, the camera coordinate system is a coordinate system fixedly connected to the camera, the pan-tilt coordinate system is a coordinate system fixedly connected to the pan-tilt base (the part connected to the airframe), the earth coordinate system refers to a coordinate system fixedly connected to the earth, and in the present invention, it refers to a preset coordinate system in a known scene. The airframe coordinate system is a three-dimensional orthogonal right-handed coordinate system fixed on the UAV, and its origin is located at the centroid of the UAV.

[0048] Specifically, when the UAV system performs a task, the camera collects image information in the selected area, and then sequentially performs gray-scale processing, binarization, bit extraction on the image information, identifies at least one aruco marker, and finally obtains the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system.

[0049] Among them, gray-scale processing is to process the three primary color signals of an RGB-format image into a single color signal of a gray-scale image.

[0050] Binarization is to process the gray-scale signal into 0 or 255 according to the processed gray-scale image and a specified threshold.

[0051] Bit extraction is to identify the average value of the gray-scale signals of each aruco pixel within the range of the two-dimensional code, binarize the average value according to a preset threshold, and use it as the signal value of the aruco pixel, and then identify the aruco marker number according to the arrangement of the respective pixel signal values.

[0052] Through the above steps, the accurate coordinate values of the four vertices of the aruco marker in the pixel coordinate system can be obtained, avoiding false detection of the aruco marker.

[0053] S101-2. Obtain the rotation matrix and translation matrix of at least one aruco marker relative to the camera coordinate system according to the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system and the side length of the corresponding aruco marker.

[0054] Since the coordinates of each aruco marker in the pixel coordinate system are different, the positions of the corresponding aruco markers in the camera coordinate system are also different. Therefore, it is necessary to calculate the rotation matrix and translation matrix of the center point of each aruco marker relative to the camera coordinate system.

[0055] Specifically, according to the camera parameters, the coordinate values of the four vertices of the aruco marker, and the side length of the aruco marker, the PNP algorithm is used to calculate the rotation matrix and translation matrix of the center point of the corresponding aruco marker relative to the camera coordinate system, which are respectively denoted as r i and ti , where i is the number of the aruco marker.

[0056] Specifically, the PNP algorithm is an existing algorithm, and its calculation process can be referred to at https: / / blog.csdn.net / u014709760 / article / details / 88029841.

[0057] S101-3. Solve the pose of the lidar in the earth coordinate system according to the rotation matrix and translation matrix of at least one aruco marker relative to the camera coordinate system, the attitude angle of the pan-tilt, the relative position between the lidar and the pan-tilt, and the pose of at least one aruco marker in the earth coordinate system.

[0058] Existing positioning algorithms in denied environments all run in the local coordinate system or the odometry coordinate system. The generated global map (the entire map) is not fixedly connected to the real earth coordinate system, which will cause the lidar to be unable to effectively and truly correct the cumulative error and deviation according to the real position when the algorithm runs in a large-scale denied environment for a long time.

[0059] Therefore, in order to obtain the pose of the lidar in the earth coordinate system and ensure the accuracy of the pose of the lidar in the earth coordinate system, it is necessary to fuse the results of each sensor.

[0060] Specifically, the pose of the lidar in the earth coordinate system is represented by Equation 1:

[0061]

[0062] Among them, represents the pose of the lidar in the earth coordinate system when the i-th aruco marker is detected; represents the transformation matrix of the relative position relationship between the lidar and the pan-tilt; represents the inverse matrix of the pose transformation matrix of the camera in the pan-tilt coordinate system when the i-th aruco marker is detected; represents the transformation matrix of the pose of the i-th aruco marker relative to the camera coordinate system; represents the transformation matrix of the pose of the i-th aruco marker in the earth coordinate system.

[0063] In the present invention, through a series of coordinate transformations in Equation 1, the pose of the lidar in the earth coordinate system when the i-th aruco marker is detected can be obtained.

[0064] It can be seen from Equation 1 that is related to the lidar and the pan-tilt and can be obtained by calibrating the relative position between the lidar and the pan-tilt.

[0065] In a preferred embodiment of the present invention, It is represented by Equation (2):

[0066]

[0067] where x g , y g , z g respectively represent the relative positions between the lidar and the pan-tilt head.

[0068] It can be seen from Equation (1) that is related to the camera and the pan-tilt head and can be obtained from the attitude angles of the pan-tilt head.

[0069] In a preferred embodiment of the present invention, It is represented by Equation (3):

[0070]

[0071] where

[0072]

[0073] where φ gi , θ gi , respectively represent the roll angle, pitch angle and yaw angle of the pan-tilt head when the i-th aruco marker is detected

[0074] It can be seen from Equation (1) that is related to the aruco marker and can be obtained from the rotation matrix and translation matrix of the aruco marker relative to the camera coordinate system.

[0075] In a preferred embodiment of the present invention, It is represented by Equation (4):

[0076]

[0077] where

[0078]

[0079] r i =(r ix r iy r iz ) T

[0080] where i is the number of the aruco marker; r i represents the rotation vector of the i-th aruco marker with respect to the camera coordinate system; t i represents the translation vector of the i-th aruco marker with respect to the camera coordinate system; rix , r iy , r iz respectively represent the components in the rotation vector r i ; I represents the identity matrix; α i represents the rotation angle of the i-th aruco marker with respect to the camera coordinate system, which is also the modulus of the rotation vector r i .

[0081] It can be seen from Equation (1) that is related to the aruco marker and can be obtained from the pose of the center point of the aruco marker in the geodetic coordinate system.

[0082] In a preferred embodiment of the present invention, it is expressed by Equation (5):

[0083]

[0084] where x i , y i , z i respectively represent the coordinate values of the center point of the i-th aruco marker on the X-axis, Y-axis, and Z-axis of the geodetic coordinate system; φ i , θ i , respectively represent the roll angle, pitch angle, and yaw angle of the center point of the i-th aruco marker in the geodetic coordinate system.

[0085] In the present invention, by setting a small number of aruco markers, the pose of the lidar relative to the real environment in the denied environment can be obtained, so as to be applicable to three-dimensional navigation in the satellite denied environment.

[0086] Step S102: Fuse the pose of the lidar in the geodetic coordinate system and the information collected by the IMU.

[0087] Preferably, perform Kalman filtering on the pose of the lidar in the geodetic coordinate system and the information collected by the IMU to obtain the pose and covariance matrix of the lidar in the geodetic coordinate system.

[0088] According to the present invention, since the lidar, IMU, and UAV system are fixedly connected, the pose of the lidar is the same as the attitude of the UAV system, that is, they rotate and translate together, but the obtained attitude angles, etc. will be different according to the different initial positions. Therefore, the attitude covariance matrix of the UAV can be obtained through the pose state equation of the lidar (or the attitude state equation of the UAV system).

[0089] In the present invention, according to the pose of the i-th aruco marker in the camera coordinate system and the position adjustment in the camera coordinate system when detecting the i-th aruco marker the corresponding measurement noise covariance matrix is denoted as Ri (C i1 (ξ i (r i ,t i ))),C i2 (d i ). Among them, two influencing factors include: ξ i (r i ,t i ), which is calculated by the rotation vector r i , the translation vector t i and the side length l of the corresponding aruco marker. It is caused by the different measurement perspectives due to the pose; and d i , which is the distance from the center of gravity of the aruco marker to the optical axis and is caused by the different positions of the aruco marker in the camera coordinate system. Before the operation of the UAV system, after calibrating the camera, according to ξ i (r i ,t i ) and d i , calibrate the measurement noise covariance matrix and fit the C i1 , C i2 functions.

[0090] Adjust the measurement noise covariance matrix dynamically according to the above, which can ensure the accuracy of the Kalman filter.

[0091] Specifically, the state equation can be expressed as:

[0092]

[0093] The observation equation can be expressed as:

[0094]

[0095] Among them, k represents the current moment; k - 1 represents the previous moment; x represents the state vector; x k represents the state vector at the kth moment; x k-1 represents according to the state vector at the (k - 1)th moment; Z represents the observation variable; Z k represents the observation variable at the kth moment; A represents the state transition matrix, H is the conversion matrix from the state vector to the observation variable; V k is the noise in the observation equation, and W k-1 is the prior noise.

[0096] During the Kalman filter, for all the aruco markers detected at the kth moment, the R i (C i1 (ξ i (r i ,t i ))),Ci2 (d i ) is sorted, select and record R i (C i1 (ξ i (r i , t i ))), C i2 (d i )) the number i of the aruco marker corresponding to the minimum value and Among them, represents the position of the lidar in the earth coordinate system, which is the translation vector part in represents the attitude of the lidar in the earth coordinate system, which is solved from the rotation vector part in. a imu , q imu , ω imu are the acceleration, attitude, and angular velocity observed by the IMU respectively.

[0097] In the present invention where the superscript b represents the body coordinate system and w represents the world coordinate system. Among them

[0098] are the position vectors of the lidar at times k - 1 and k is the position vector of the lidar at the initial time (0, 0, 0) T , C is the conversion matrix of the lidar from the body coordinate system to the earth coordinate system, and C is represented by the following formula:

[0099]

[0100] are the velocity vectors of the lidar at times k - 1 and k is the velocity vector of the lidar at the initial time (0, 0, 0) T ;

[0101] are the acceleration vectors of the lidar at times k - 1 and k is the acceleration vector of the lidar at the initial time is obtained by collecting through the IMU;

[0102] are the attitude angle vectors of the lidar at times k - 1 and k The attitude angle vector at the initial moment of the lidar is (0, 0, 0) T ;

[0103] are the angular velocity vectors of the lidar at the (k - 1)th and kth moments represents the attitude angular velocity vector of the lidar at the initial moment (0, 0, 0) T .

[0104] Kalman filtering is a commonly used filtering method, which can be expressed as:

[0105]

[0106]

[0107]

[0108]

[0109]

[0110] Among them, represents the correction value of the lidar state vector at the kth moment, and P k represents x k corresponding covariance matrix, represents the estimated value of P k , and P k-1 represents x k-1 corresponding covariance matrix, K represents the Kalman filtering gain coefficient, ∑ k represents the measurement noise covariance, I represents the identity matrix, and Q represents the covariance matrix of the system process.

[0111] ∑ k is converted from the dynamic covariance R i (C i1 (ξ i (r i , t i ))), C i2 (d i ) to the covariance of the Euler angles and adding the covariance R corresponding to the IMU imu obtained, where

[0112] Taking (p zk q zak a imu q imu ω imu ) T as the observation, and taking As a state vector, the covariance matrix P of the attitude of the lidar (UAV system) can thus be obtained k . At time k, the state x k in the position and attitude as well as the corresponding components of the above two components in P k are combined to form an aruco factor, which is used as the initial value of X k for factor graph optimization.

[0113] In the present invention, when no aruco marker is detected within 3 seconds, the EKF process is stopped and initialized, and the calculation of EKF is continued when the aruco marker is detected again.

[0114] It has been found through research that according to the above process, the covariance matrix P of the attitude of the lidar (UAV system) can be accurately obtained k .

[0115] Step S103: Optimize the pose of the fused lidar in the geodetic coordinate system through a factor graph and perform information fusion and global correction with the lidar and IMU.

[0116] In the UAV system, there are also sensors such as IMU. To ensure the accuracy and robustness of UAV system navigation, the information of multiple sensors can be fused.

[0117] In the present invention, the factor graph also includes predicted sub-factors, lidar factors, loop factors, etc., as shown in Figure 3 . Among them, the acquisition processes of the predicted sub-factors, lidar factors, and loop factors can be existing methods. At time k, the aruco factor is used as the optimization initial value, and the predicted sub-factor calculated by IMU prediction decomposition between time k-1 and time k, the lidar factor calculated by lidar motion estimation between time k-1 and time k are added, and the loop is determined through the aruco marker. If a loop occurs at time j, the loop factor between time j and time k is continuously added to the factor graph.

[0118] Among them, the state of the UAV at time k in the factor graph is:

[0119] X k =(p k v k R k b ak b gk ) T

[0120] Among them, p k is the translation vector of the UAV in the geodetic coordinate system, v k is the velocity vector of the UAV in the geodetic coordinate system, R kis the rotation matrix of the drone in the geodetic coordinate system, b ak and b gk are the accelerometer bias and gyroscope bias in the IMU respectively.

[0121] Preferably, the process of obtaining the prediction factor is as follows:

[0122] Perform prediction and division on the acceleration, attitude angle, and angular velocity collected by the IMU;

[0123] Take the pose T IMU between two frames in the prediction and division result and the corresponding covariance C IMU as the IMU factor.

[0124] Preferably, the process of obtaining the lidar factor is as follows:

[0125] (1) Obtain multiple frames of images: When the lidar point cloud is collected, first perform motion compensation and timestamp alignment on each point, and project the point cloud in one period onto a frame of point cloud image, denoted as the nth frame;

[0126] (2) Feature extraction: Perform feature extraction on this frame of point cloud image, and calculate the average distance k k from the five points before and after (surrounding points) of a certain point b k on each line to this point b k , as shown in the following formula:

[0127]

[0128] When the difference between k k and the surrounding points is less than the preset distance threshold, the curvature near this point is small and relatively smooth, generally on a plane, and it is used as a surface feature point, denoted as F mn ; when the difference between k k and the surrounding points is greater than the preset distance threshold, the curvature near this point is large and the mutation is severe, generally a corner point, and it is used as an edge feature point, denoted as F bn .

[0129] (3) Determine the key frame: When selecting the key frame, the first frame is used as the key frame; for the rest, when determining whether the nth frame is a key frame, compare the co-visibility relationship between the set of feature points {F mn , F bn} in the nth frame and the feature points in the previous key frame k m . When the co-visibility relationship changes by more than the set threshold, set the nth frame as the key frame, denoted as k m+1 .

[0130] (4) Feature matching: For two key frames, respectively use the surface feature points F m+1 in the latest key frame k mn and the previous 5 key frames km Calculate the distance from a point to the plane formed by three adjacent face feature points corresponding thereto Use the latest key frame k m+1 Edge feature point F in bn And the previous 5 key frames k m To k m Calculate the distance from a point to the straight line formed by the adjacent edge feature points corresponding thereto Take As the cost function for motion estimation, optimize the rotation and translation changes between two key frames. If no degeneracy occurs during the optimization solution process, add the state X to the factor graph m+1 , project k m+1 Onto the map, and take the optimization result as the measurement between the state X m And X m+1 If the optimization solution process degenerates and the aruco marker is continuously detected, project the key frame k m+1 Onto the map according to the pose of the lidar solved from the aruco marker, but do not add the optimization result as the measurement between the state X m And X m+1 Between measurements

[0131] In addition, if the i-th aruco marker is detected while reading the n-th frame of the lidar, even if the co-visibility relationship is not lower than the preset threshold, add this frame as the key frame k ai ; During the subsequent continuous detection of the i-th aruco marker, if the covariance of the aruco factor is less than k ai The corresponding covariance at the moment, then update k ai ; When the continuous detection of the i-th aruco marker is completed, match k ai With the key frame k generated by the co-visible feature closest to its time m , and k m The adjacent k before and after m-2 To k m+2 And add k ai To the map. The discontinuous threshold for detecting the i-th aruco marker is preferably set to 3s

[0132] It is found through research that by introducing the aruco factor, the lidar factor becomes more accurate and further improves the positioning accuracy

[0133] Preferably, the process of obtaining the loop closure factor is as follows

[0134] When the navigation system collects the continuous detection of the i-th aruco marker for the l-th time, it is determined as a loop closure

[0135] The key frame k corresponding to the loop detected by the i-th aruco marker for the l-1th time ai,1 , k ai,2 to k ai,l-1 , and the key frames {k q-2 , k q-1 , k q , k q+1 , k q+2}, {k w-2 , k w-1 , k w , k w+1 , k w+2} to {k e-2 , k e-1 , k e , k e+1 , k e+2} obtained by feature co-visibility in its vicinity, are feature-matched with the key frame k ai,l nearest to the l-th loop key frame k r , and motion estimation is performed respectively. If the optimization has a solution and is not degenerate, it is added to the factor graph as a measurement between state X r and state X m .

[0136] In the present invention, by fusing the information of multiple sensors, the advantages of each sensor can be exerted, thereby improving the robustness of the system.

[0137] In a second aspect, the present invention provides a high-precision positioning system in a satellite-denied environment, as Figure 2 shown. The system includes:

[0138] A module 201 for obtaining the pose of the lidar in the geodetic coordinate system;

[0139] A module 202 for fusing the pose of the lidar in the geodetic coordinate system and the information collected by the IMU; and

[0140] A module 203 for performing information fusion and global correction with the lidar and the IMU through factor graph optimization using the fused pose of the lidar in the geodetic coordinate system.

[0141] The high-precision positioning system in a satellite-denied environment provided by the present invention can be used to execute a high-precision positioning method in a satellite-denied environment described in any of the above embodiments, and its implementation principle and technical effects are similar and will not be elaborated here.

[0142] Preferably, each module in the high-precision positioning system in a satellite-denied environment of the present invention can be directly in hardware, in a software module executed by a processor, or in a combination of both.

[0143] The software module may reside in a RAM memory, flash memory, ROM memory, EPROM memory, EEPROM memory, register, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art. The exemplary storage medium is coupled to the processor such that the processor can read information from and write information to the storage medium.

[0144] The processor may be a central processing unit (CPU), or may also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic, discrete hardware components, or any combination thereof. The general-purpose processor may be a microprocessor, but in an alternative, the processor may be any conventional processor, controller, microcontroller, or state machine. The processor may also be implemented as a combination of computing devices, such as a combination of a DSP and a microprocessor, multiple microprocessors, one or more microprocessors in conjunction with a DSP core, or any other such configuration. In an alternative, the storage medium may be integral with the processor. The processor and the storage medium may reside in an ASIC. The ASIC may reside in a user terminal. In an alternative, the processor and the storage medium may reside in the user terminal as discrete components.

[0145] In a third aspect, the present invention provides an electronic device, which includes: a memory, a processor;

[0146] The memory is used to store executable instructions that can be executed by the processor;

[0147] The processor is used to implement the high-precision positioning method in the satellite denial environment of the first aspect according to the executable instructions stored in the memory.

[0148] In a fourth aspect, the present invention provides a computer-readable storage medium, characterized in that the computer-readable storage medium stores computer-executable instructions, and when the computer-executable instructions are executed by the processor, they are used to implement the high-precision positioning method in the satellite denial environment as described in the first aspect.

[0149] In a fifth aspect, a program product includes a computer program. The computer program is stored in a readable storage medium, and at least one processor can read the computer program from the readable storage medium. When at least one processor executes the computer program, it implements the high-precision positioning method in the satellite denial environment described in Embodiment 1.

[0150] In several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of the devices or units can be in electrical, mechanical or other forms.

[0151] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0152] Example

[0153] Inside the first-phase dry coal shed of Fengcheng Thermal Power Plant in Fengcheng City, Jiangxi Province, which is 40m * 110m, except for the take-off point and two aruco markers nearby on the ground, the rest are all arranged on the pedestrian walkway 1.5m above the take-off point. The y-axis coordinate is -14.8m, with a spacing of 10m. There are no markers placed at 70m due to fixed equipment. There are a total of 13 aruco markers. For the specific layout, see Figure 4 , where range represents the foundation range of the dry coal shed in the real scene. The unmanned aerial vehicle is equipped with a camera (model Zenmuse X5s), an IMU (model MTI-300), and a lidar (model Velodyne-16).

[0154] When the unmanned aerial vehicle executes the task, the camera collects the image information inside the shed, and then performs gray processing, binarization, and bit extraction on the image information in sequence to identify each aruco marker.

[0155] According to the camera parameters and PNP, the rotation matrix and translation matrix of the centers of the 13 aruco markers relative to the camera coordinate system are calculated, and are respectively denoted as r i and t i , where i is the number of the aruco marker.

[0156] According to the rotation matrix and translation matrix of the centers of the 13 aruco markers relative to the camera coordinate system, the attitude angle of the pan-tilt, the relative position between the lidar and the pan-tilt, and the poses of the centers of the 13 aruco markers in the geodetic coordinate system, the pose of the lidar in the geodetic coordinate system is obtained.

[0157] The pose of the lidar in the geodetic coordinate system is expressed by Equation (1):

[0158]

[0159] Wherein, represents the pose of the lidar in the geodetic coordinate system when the i-th aruco marker is detected; represents the transformation matrix of the relative position relationship between the lidar and the pan-tilt; represents the inverse matrix of the pose transformation matrix of the camera in the pan-tilt coordinate system when the i-th aruco marker is detected; represents the pose transformation matrix of the i-th aruco marker in the camera coordinate system; represents the pose transformation matrix of the i-th aruco marker in the geodetic coordinate system.

[0160] It is expressed by Equation (2):

[0161]

[0162] Wherein, x g , y g , z g respectively represent the relative positions between the lidar and the pan-tilt.

[0163] It is expressed by Equation (3):

[0164]

[0165] Wherein,

[0166]

[0167] φ gi , θ gi , respectively represent the roll angle, pitch angle and yaw angle of the pan-tilt when the i-th aruco marker is detected.

[0168] It is expressed by Equation (4):

[0169]

[0170] Wherein,

[0171]

[0172] r i =(r ix r iy r iz )T

[0173] Among them, i is the number of the aruco marker; r i represents the rotation matrix of the i-th aruco marker; t i represents the translation matrix of the i-th aruco marker; r ix 、r iy 、r iz respectively represent the components in the rotation matrix r i ; I represents the identity matrix; α i represents the rotation angle of the i-th aruco marker.

[0174] It is represented by Equation Five:

[0175]

[0176] Among them, x i 、y i 、z i respectively represent the coordinate values of the center point of the i-th aruco marker on the X-axis, Y-axis, and Z-axis of the geodetic coordinate system; φ i 、θ i 、 respectively represent the roll angle, pitch angle, and yaw angle of the center point of the i-th aruco marker in the geodetic coordinate system.

[0177] Perform Kalman filtering on the pose of the lidar in the geodetic coordinate system and the information collected by the IMU to obtain the error covariance matrix and covariance matrix of the lidar.

[0178] Specifically, the state equation can be expressed as:

[0179]

[0180] The observation equation can be expressed as:

[0181]

[0182] Among them, k represents the current moment; k - 1 represents the previous moment; x represents the state vector; x k represents the state vector at the k-th moment; x k-1 represents the state vector according to the k - 1-th moment; Z represents the observation variable; Z k represents the observation variable at the k-th moment; A represents the state transition matrix, H is the conversion matrix from the state vector to the observation variable; V k is the noise in the observation equation, W k-1 is the prior noise.

[0183] When performing Kalman filtering, sort all the aruco markers detected at time k to calculate R i (C i1 (ξ i (r i ,t i ))), C i2 (d i )) and select and record the R i (C i1 (ξ i (r i ,t i ))), C i2 (d i )) corresponding to the minimum value, as well as the number i of the aruco marker and Among them, represents the position of the lidar in the earth coordinate system, which is the translation vector part in represents the attitude of the lidar in the earth coordinate system, which is solved from the rotation vector part in. a imu 、q imu 、ω imu are the acceleration, attitude, and angular velocity observed by the IMU respectively.

[0184] Among them, the superscript b represents the body coordinate system, and w represents the world coordinate system. Among them

[0185] are the position vectors of the lidar at times k-1 and k is the position vector of the lidar at the initial time (0, 0, 0) T , C is the transformation matrix of the lidar from the body coordinate system to the earth coordinate system, and C is expressed by the following formula:

[0186]

[0187] are the velocity vectors of the lidar at times k-1 and k is the velocity vector of the lidar at the initial time (0, 0, 0) T ;

[0188] are the acceleration vectors of the lidar at times k-1 and k is the acceleration vector of the lidar at the initial time Obtained by collecting through the IMU;

[0189] are the attitude angle vectors of the lidar at times k-1 and k is the attitude angle vector (0, 0, 0) of the lidar at the initial time T ;

[0190] are the angular velocity vectors of the lidar at times k-1 and k represents the attitude angular velocity vector (0, 0, 0) of the lidar at the initial time T .

[0191] Take (p zk q zak a imu q imu ω imu ) T as the observation, and take as the state vector, so that the covariance matrix P of the attitude of the lidar (UAV system) can be obtained k . Take the position p k in the state x k at time k and the attitude q k and the corresponding components of the above two components in P k to form the aruco factor, which is used as the initial value of X k for factor graph optimization. Then add the predicted sub-factors calculated by the IMU prediction decomposition between times k-1 and k, and the lidar factors calculated by the lidar motion estimation between times k-1 and k, and judge the loop through the aruco marker. If a loop occurs at time j, then continue to add the loop factor between times j and k in the factor graph to achieve information fusion and global correction. See the specific simulation results in Figure 4 .

[0192] Comparative Example 1

[0193] Use the existing loam algorithm to locate the UAV, and the loam algorithm can be referred to at https: / / blog.csdn.net / shoufei403 / article / details / 103664877. See the specific simulation results in Figure 4 .

[0194] Comparative Example 2

[0195] The existing lio_sam algorithm is used to locate the UAV. The lio_sam algorithm can be referred to at https: / / blog.csdn.net / tiancailx / article / details / 109483450. See the specific simulation results in Figure 4 . rotate represents the result of manually rotating the result of the embodiment to a state approximate to the two algorithms of loam and lio_sam.

[0196] From Figure 4 It can be seen that the trajectories calculated by the loam and lio_sam algorithms are affected by the pose deviation of the UAV during operation, the installation angle deviation of the lidar, etc., and there is no function to correct the rotation deviation of the trajectory. The calculated trajectories are far from the actual flight trajectory, greatly increasing the risk of UAV collision in applications, and unable to meet subsequent tasks such as visual lateral measurement, not meeting the working conditions requirements. However, through the aruco marker, the embodiment completes the global optimization of the navigation system. The optimization not only greatly corrects the direction of the flight path, but also corrects the cumulative error.

[0197] The present invention has been described in detail above in combination with specific embodiments and exemplary examples, but these descriptions should not be construed as limiting the present invention. Those skilled in the art understand that without departing from the spirit and scope of the present invention, various equivalent replacements, modifications or improvements can be made to the technical solutions and implementation manners of the present invention, and these all fall within the scope of the present invention.

Claims

1. A high-precision positioning method in a satellite denial environment, characterized in that Including: Obtain the pose of the lidar in the earth coordinate system; Fuse the pose of the lidar in the earth coordinate system and the information collected by the IMU; And Use the fused pose of the lidar in the earth coordinate system to perform information fusion and global correction with the lidar and IMU through factor graph optimization; The process of obtaining the pose of the lidar in the earth coordinate system includes: Obtain the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system; According to the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system and the side length of the corresponding aruco marker, obtain the rotation matrix and translation matrix of the at least one aruco marker relative to the camera coordinate system; According to the rotation matrix and translation matrix of at least one aruco marker relative to the camera coordinate system, the attitude angle of the pan-tilt, the relative position between the lidar and the pan-tilt, and the pose of the at least one aruco marker in the earth coordinate system, calculate the pose of the lidar in the earth coordinate system; The pose of the lidar in the earth coordinate system is represented by Equation 1: Among them, represents the pose of the lidar in the earth coordinate system when the i-th aruco marker is detected; represents the transformation matrix of the relative position between the lidar and the pan-tilt; represents the inverse matrix of the pose transformation matrix of the camera in the pan-tilt coordinate system when the i-th aruco marker is detected; represents the pose transformation matrix of the i-th aruco marker in the camera coordinate system; represents the pose transformation matrix of the i-th aruco marker in the earth coordinate system; The aruco factor in the factor graph is used as the initial value of the factor graph optimization. The factor graph also includes a predicted factor, a lidar factor, and a loop closure factor. At time k, the aruco factor is used as the optimization initial value, and the predicted factor calculated by IMU predicted decomposition between time k-1 and k, the lidar factor calculated by lidar motion estimation between time k-1 and k are added. And loop closure is determined by the aruco marker. If loop closure occurs at time j, then the loop closure factor between time j and k is continuously added to the factor graph.

2. The high-precision positioning method in a satellite denial environment according to claim 1, characterized in that It is expressed by formula (2) as follows: Among them, x g , y g , z g respectively represent the relative positions between the lidar and the pan-tilt head.

3. The high-precision positioning method in a satellite denial environment according to claim 1, wherein, The through-type three indicates: Wherein, Among them, φ gi , θ gi , φ gi respectively represent the roll angle, pitch angle, and yaw angle of the pan-tilt when the i-th aruco marker is detected.

4. The high-precision positioning method in a satellite denial environment according to claim 1, wherein, The through-type four indicates: Among them, r i ＝(r ix r iy r iz ) T Among them, i is the number of the aruco marker; r i represents the rotation vector of the i-th aruco marker with respect to the camera coordinate system; t i represents the translation vector of the i-th aruco marker with respect to the camera coordinate system; r ix 、r iy 、r iz respectively represent the components in the rotation vector r i ; I represents the identity matrix; α i represents the rotation angle of the i-th aruco marker with respect to the camera coordinate system.

5. The high-precision positioning method in a satellite denial environment according to claim 1, wherein The through-type five indicates: where x i , y i , z i respectively represent the coordinate values of the center point of the i-th aruco marker on the X-axis, Y-axis, and Z-axis of the geodetic coordinate system; φ i , θ i , respectively represent the roll angle, pitch angle, and yaw angle of the center point of the i-th aruco marker in the geodetic coordinate system.

6. The high-precision positioning method in a satellite denial environment according to claim 1, characterized in that The process of fusing the pose of the lidar in the earth coordinate system and the information collected by the IMU is: Perform Kalman filtering on the pose of the lidar in the earth coordinate system and the information collected by the IMU.

7. A high-precision positioning system for a satellite-denied environment, characterized in that, Including: A module for obtaining the pose of the lidar in the earth coordinate system; A module for fusing the pose of the lidar in the earth coordinate system and the information collected by the IMU; And A module for performing information fusion and global correction with the lidar and IMU through factor graph optimization using the fused pose of the lidar in the earth coordinate system; The process of obtaining the pose of the lidar in the earth coordinate system includes: Obtain the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system; According to the coordinate values of the four vertices of at least one aruco marker in the pixel coordinate system and the side length of the corresponding aruco marker, obtain the rotation matrix and translation matrix of the at least one aruco marker relative to the camera coordinate system; According to the rotation matrix and translation matrix of at least one aruco marker relative to the camera coordinate system, the attitude angle of the pan-tilt, the relative position between the lidar and the pan-tilt, and the pose of the at least one aruco marker in the earth coordinate system, calculate the pose of the lidar in the earth coordinate system The pose of the lidar in the earth coordinate system is represented by Equation 1: Among them, represents the pose of the lidar in the earth coordinate system when the i-th aruco marker is detected; represents the transformation matrix of the relative position between the lidar and the pan-tilt; represents the inverse matrix of the pose transformation matrix of the camera in the pan-tilt coordinate system when the i-th aruco marker is detected; represents the pose transformation matrix of the i-th aruco marker in the camera coordinate system; represents the pose transformation matrix of the i-th aruco marker in the earth coordinate system; In the factor graph, the aruco factor serves as the initial value for factor graph optimization. The factor graph also includes predicted sub-factors, lidar factors, and loop closure factors. At time k, the aruco factor is used as the initial value for optimization, and the predicted sub-factors calculated by IMU prediction decomposition between times k-1 and k, and the lidar factors calculated by lidar motion estimation between times k-1 and k are added. Loop closure is determined by aruco markers. If loop closure occurs at time j, the loop closure factor between times j and k is further added to the factor graph.

8. An electronic device, characterized in that, Comprising: a memory, a processor; the memory is used to store executable instructions executable by the processor; the processor is used to implement the high-precision positioning method in a satellite denial environment according to any one of claims 1 to 6 based on the executable instructions stored in the memory.

Images