A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles

Abstract

The application provides a kind of unmanned aerial vehicle-oriented visual / laser ranging high-altitude navigation method, comprising: visual triangulation solving three-dimensional map point depth, scale initial value is solved using timestamp synchronized laser ranging data;Construct visual re-projection error and laser ranging error joint optimization function;Establish a new graph optimization model, define node, edge and error Jacobian matrix, and obtain optimization parameters by iteration;Finally, according to the optimization parameter update whole visual tracking thread, complete scale update and pose update, obtain accurate positioning of unmanned aerial vehicle.The technical problem of the application scheme is to solve the technical problem of high-altitude unmanned aerial vehicle scene, monocular camera exists scale deviation, and it is difficult to realize high-precision navigation positioning and depth estimation.

Description

Technical Field

[0001] This invention belongs to the field of computer vision technology, specifically relating to a visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs). Background Technology

[0002] Modern military warfare is gradually shifting towards intelligent warfare, with unmanned aerial vehicles (UAVs) playing an increasingly important role on the battlefield as an intelligent combat carrier. UAV navigation methods are the technologies and methods used to guide aircraft along a predetermined speed and direction to complete a prescribed flight process. One of the most critical technologies is real-time navigation and positioning of the UAV. Specifically, during the UAV's flight, highly accurate six-degree-of-freedom position and attitude information is acquired in real time, converted into the UAV's spatial latitude and longitude coordinates, and fed back to the aircraft control system, thereby helping the UAV achieve autonomous navigation and landing capabilities.

[0003] Traditional UAV navigation methods largely rely on satellite information for navigation and positioning, with differential GPS-based methods typically providing relatively accurate spatial positions. However, battlefield situations are unpredictable, and satellite denial is frequent, making autonomous navigation solutions independent of satellites urgently needed. Inertial measurement unit (IMU)-based methods are significantly affected by noise, resulting in large accumulated errors and a tendency for navigation deviations. Visual sensors, with their advantages of concealment, portability, low power consumption, low cost, and high positioning accuracy, have become a research hotspot in the field of UAV autonomous navigation in recent years.

[0004] High-altitude applications present certain challenges for visual sensors, with the key difficulty lying in depth estimation in high-altitude scenes. For binocular cameras, the method of determining depth based on the parallax of the left and right cameras fails in high-altitude scenarios; for depth cameras, the depth acquisition range is mostly a few meters or tens of meters; for monocular cameras, vision can calculate the scene depth, but performing visual initialization directly during high-altitude flight makes it impossible to establish evaluation standards for monocular cameras, resulting in scale bias. Summary of the Invention

[0005] The present invention aims to solve at least one of the technical problems existing in the prior art or related art.

[0006] Therefore, this invention provides a visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs).

[0007] The technical solution of this invention is as follows: A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) is provided, the method comprising:

[0008] Initial scale estimates are obtained using laser ranging and visual triangulation.

[0009] Based on the visual feature matching results, a visual reprojection error equation is constructed. Based on the correlation between the laser rangefinder and the visual observation, a laser ranging error equation is constructed. The two equations are combined to obtain a joint optimization function for visual reprojection error and laser ranging error. The joint optimization function includes the parameters to be optimized: scale factor, pose transformation matrix, and 3D map points in the world coordinate system.

[0010] Based on the initial scale estimate, the joint optimization function is solved using a graph optimization method to obtain the optimization parameters;

[0011] The entire visual tracking thread is updated based on optimized parameters to complete scale and pose updates and obtain the precise positioning of the UAV.

[0012] Furthermore, the step of obtaining the initial scale estimate using laser ranging and visual triangulation includes:

[0013] Initial tracking during the high-altitude level flight phase is performed using a visual sensor to create keyframes;

[0014] The depth value d of the 3D map point corresponding to the j′ feature point in the i-th keyframe is obtained using triangulation techniques. j′ , 0≤j′≤n;

[0015] The average depth of the 3D map point corresponding to the i-th keyframe is obtained based on the depth value of the 3D map point. i :

[0016]

[0017] Accept the laser ranging input value, align its timestamp with the keyframe image timestamp, and obtain the laser ranging height corresponding to the i-th keyframe after filtering by timestamp. i ;

[0018] According to the laser ranging height i and average depth i Obtain the initial scale estimate s corresponding to the i-th keyframe. begin :

[0019]

[0020] Furthermore, the visual reprojection error equation is constructed based on the visual feature matching results in the following manner:

[0021] For the first n keyframes, the reprojection error It can be expressed as the following formula:

[0022]

[0023]

[0024] Where j represents the map point ID, p represents the three-dimensional coordinates of the j-th map point in the world coordinate system. ij Indicates the i-th keyframe The corresponding pixel observation point, where K represents the camera intrinsic parameter matrix. Let be the pose matrix, representing the transformation from the world coordinate system corresponding to the i-th keyframe to the camera coordinate system, where π is the depth.

[0025] Furthermore, the laser ranging error equation is constructed based on the correlation between the laser rangefinder and visual observations in the following manner:

[0026]

[0027] in, This represents the laser ranging error, where 's' refers to the scale factor. This represents the three-dimensional coordinates of the j-th map point in the world coordinate system within the laser direction region. This represents the transformation from the camera coordinate system to the world coordinate system corresponding to the i-th keyframe. Simplified representation as The rotation matrix is ​​R. wc The translation vector is t wc , L represents the vector representing the laser ranging direction in the camera coordinate system. i This indicates the length of the laser output.

[0028] Furthermore, the joint optimization function for visual reprojection error and laser ranging error is obtained through the following formula:

[0029]

[0030]

[0031] in, This is due to visual reprojection error. The distance is the laser ranging error, where m represents the maximum number of map points used in the visual reprojection, and m' represents the maximum number of map points used in the visual reprojection. Since lasers are directional, m' ≤ m. k The parameters to be optimized include the scale factor s and the pose transformation matrix T. cw , can be represented as T cw =[R cw |t cw ], and 3D map points in the world coordinate system

[0032] Furthermore, based on the initial scale estimate, the joint optimization function is solved using a graph optimization approach to obtain the optimization parameters, including:

[0033] 1) Establish a new graph optimization model based on the joint optimization function, define the model nodes, edges and node update methods, and calculate the Jacobian matrix of the optimization parameters;

[0034] 2) Based on the new graph optimization model and the Jacobian matrix of the optimization parameters, the optimized scale factor, 3D map point coordinates and visual keyframe pose are obtained by iterative solution, wherein the initial value of the scale of the parameter to be optimized is set to the initial scale estimate.

[0035] Furthermore, model nodes and edges are defined as follows:

[0036] In the graph optimization model, nodes are defined as parameters to be optimized, including: scale factor, pose transformation matrix, and 3D map points;

[0037] In the graph optimization model, the type of edge is defined as a hyperedge, which connects three nodes.

[0038] Furthermore, in defining the node update method, the scale factor update method is set as shown in the following formula:

[0039] s′=sexp(δs).

[0040] Furthermore, the Jacobian matrix of the optimization parameters is calculated as follows:

[0041] Define the model error function according to the joint optimization function;

[0042] Calculate the Jacobian matrix of the model error function for the three nodes, including:

[0043] Obtain the Jacobian matrix of the three nodes corresponding to the visual reprojection error and the Jacobian matrix of the three nodes corresponding to the laser ranging error respectively;

[0044] The Jacobian matrix of each node is obtained by adding the Jacobian matrices of the three nodes corresponding to the visual reprojection error and the three nodes corresponding to the laser ranging error.

[0045] Furthermore, the Jacobian matrix of the three nodes corresponding to the laser error component is obtained in the following manner:

[0046] For a node on the 3D map, the Jacobian matrix of that node is obtained by taking its partial derivative with respect to the laser ranging error equation. As shown in the following formula:

[0047]

[0048] For the node scale factor, taking the partial derivative of the laser ranging error equation yields the Jacobian matrix r of that node. d (s) is shown in the following formula:

[0049]

[0050] For the node pose transformation matrix, based on the laser ranging error equation, the rotation matrix R in the pose transformation matrix under small perturbations is obtained respectively. cw Translational displacement t cw The corresponding Jacobian matrix.

[0051] Furthermore, the translation t under small perturbations is obtained by the following formula. cw Corresponding Jacobian matrix r d (t cw ):

[0052]

[0053] Furthermore, the translation amount R under small perturbations is obtained by the following formula. cw Corresponding Jacobian matrix r d (R cw ):

[0054]

[0055] Furthermore, updating the entire visual tracking thread based on optimized parameters to complete scale and pose updates includes:

[0056] The depth of the current 3D map point is updated based on the obtained optimized parameter scale factor to obtain the updated 3D map point depth, wherein the 3D map point is the 3D map point with the obtained optimized parameter.

[0057] The rotation matrix and translation vector for each frame are updated based on the obtained optimized pose transformation matrix.

[0058] The aforementioned technical solution addresses two main issues. First, it tackles the difficulty of scale estimation in real-world high-altitude initialization scenarios for UAVs. It utilizes both visual sensors and laser rangefinders, combining observed laser ranging height with estimated 3D map point depths obtained through visual calculations to acquire initial scale values. An error equation is established to effectively solve for the scene's scale factor, enabling scale updates and improving the overall accuracy of depth estimation in high-altitude tracking and positioning. Second, it addresses the low accuracy of visual pose estimation in real-world high-altitude applications by adding new constraints. It uses visual measurements to obtain 3D map point coordinates and laser ranging observations to add error constraints. By establishing a joint error optimization equation with these effective constraints, a more accurate camera pose transformation matrix is ​​obtained, significantly improving the accuracy and robustness of six-degree-of-freedom pose estimation in high-altitude tracking and positioning. In summary, this invention solves the technical problem of scale bias in monocular cameras during high-altitude initialization scenarios, hindering high-precision navigation, positioning, and depth estimation. It offers high accuracy, real-time performance, robustness, and practicality for pose and depth estimation of high-altitude UAVs. Attached Figure Description

[0059] The accompanying drawings, which form part of this specification, are provided to further illustrate embodiments of the invention and, together with the textual description, explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any creative effort.

[0060] Figure 1 A flowchart illustrating the method of an embodiment of the present invention is shown. Detailed Implementation

[0061] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the present invention or its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0062] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0063] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0064] like Figure 1 As shown, in one embodiment of the present invention, a visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) is provided, the method comprising:

[0065] Step 1: Obtain initial scale estimates using laser ranging and visual triangulation;

[0066] Step 2: Construct a visual reprojection error equation based on the visual feature matching results, construct a laser ranging error equation based on the correlation between the laser rangefinder and the visual observations, and combine the two equations to obtain a joint optimization function for visual reprojection error and laser ranging error. The joint optimization function includes the parameters to be optimized: scale factor, pose transformation matrix, and 3D map points in the world coordinate system.

[0067] Step 3: Based on the initial scale estimate, solve the joint optimization function using a graph optimization method to obtain the optimization parameters;

[0068] Step 4: Update the entire visual tracking thread based on the optimized parameters to complete the scale update and pose update, and obtain the precise positioning of the UAV.

[0069] In other words, in the face of the problem that monocular cameras are difficult to achieve high-precision navigation, positioning and depth estimation in high-altitude UAV application scenarios, this invention introduces a laser rangefinder to establish a depth evaluation standard, proposes a visual laser ranging high-altitude initialization method for UAVs, establishes a sensor depth coupling model, solves the scale information of monocular vision, and achieves high-precision aerial autonomous navigation and positioning.

[0070] This invention addresses the challenge of scale estimation in real-world high-altitude initialization scenarios for UAVs by utilizing both visual sensors and laser rangefinders. It combines observed laser-ranged altitude data with estimated 3D map point depths obtained through visual calculations to acquire initial scale values. An error equation is established to effectively solve for the scene's scale factor, enabling scale updates and improving the overall accuracy of depth estimation in high-altitude tracking and positioning. Furthermore, it addresses the low accuracy of visual pose estimation in real-world high-altitude applications by adding new constraints. 3D map point coordinates are obtained through visual measurements, and laser-ranged observations are used to add error constraints. By establishing a joint error optimization equation with these effective constraints, a more accurate camera pose transformation matrix is ​​obtained, significantly improving the accuracy and robustness of six-DOF pose estimation in high-altitude tracking and positioning. In summary, this invention solves the technical problem of scale bias in monocular cameras during high-altitude initialization scenarios, hindering high-precision navigation, positioning, and depth estimation. It offers high accuracy, real-time performance, robustness, and practicality for pose and depth estimation of high-altitude UAVs.

[0071] In the above embodiments, in order to accurately obtain the initial scale estimate, the step of obtaining the initial scale estimate using laser ranging and visual triangulation includes:

[0072] Initial tracking during the high-altitude level flight phase is performed using a visual sensor to create keyframes;

[0073] The depth value d of the 3D map point corresponding to the j′ feature point in the i-th keyframe is obtained using triangulation techniques. j′ , 0≤j′≤n;

[0074] The average depth of the 3D map point corresponding to the i-th keyframe is obtained based on the depth value of the 3D map point. i :

[0075]

[0076] Accept the laser ranging input value, align its timestamp with the keyframe image timestamp, and obtain the laser ranging height corresponding to the i-th keyframe after filtering by timestamp. i ;

[0077] According to the laser ranging height iand average depth i Obtain the initial scale estimate s corresponding to the i-th keyframe. begin :

[0078]

[0079] The triangulation technique used in this invention is well-known in the field and will not be described in detail here.

[0080] Specifically, during the high-altitude level flight phase, the visual sensor performs initial tracking and creates keyframes; ORB features are extracted from the images, and initial pose calculation is performed based on feature matching between adjacent frames; triangulation techniques are used to solve the matching between two image frames, obtaining the depth value d of the 3D map point corresponding to the j′ feature point in the i-th keyframe. j′ For example, the depth of the 3D map points corresponding to the first 10 keyframes, and the average depth of the 3D map points corresponding to the i-th keyframe is calculated based on this depth value. i Simultaneously, the laser ranging height corresponding to the i-th keyframe is selected using timestamp alignment. i The initial scale estimate is then obtained from the average depth and the laser ranging height.

[0081] In the above embodiments, in order to accurately establish the visual reprojection error equation and the laser ranging error equation, the visual reprojection error equation is constructed based on the visual feature matching results in the following manner:

[0082] For the first n keyframes, the reprojection error It can be expressed as the following formula:

[0083]

[0084]

[0085] Where j represents the map point ID, p represents the three-dimensional coordinates of the j-th map point in the world coordinate system. ij Indicates the i-th keyframe The corresponding pixel observation point, where K represents the camera intrinsic parameter matrix. Let be the pose matrix, representing the transformation from the world coordinate system corresponding to the i-th keyframe to the camera coordinate system, where π is the depth.

[0086] And the laser ranging error equation is constructed based on the correlation between the laser rangefinder and visual observations in the following manner:

[0087]

[0088] in, This represents the laser ranging error, where 's' refers to the scale factor. This represents the three-dimensional coordinates of the j-th map point in the world coordinate system within the laser direction region. This represents the transformation from the camera coordinate system to the world coordinate system corresponding to the i-th keyframe. Simplified representation as The rotation matrix is ​​R. wc The translation vector is t wc , L represents the vector representing the laser ranging direction in the camera coordinate system. i This indicates the length of the laser output.

[0089] Specifically, for visual navigation, three coordinate systems are mainly involved: image coordinate system, camera coordinate system, and world coordinate system. This paper defines the camera coordinate system as the c-frame and the world coordinate system as the w-frame. By projecting 3D map points on the world coordinate system onto the image coordinate system, an error model between the observed pixels and the estimated projected pixels can be established.

[0090] Using camera intrinsics, two pixels can be transformed from the image coordinate system to the camera coordinate system. In the image coordinate system, a pixel is defined as p, which can be represented as p = (u, v); in the camera coordinate system, a feature point is defined as P, which can be represented as P = (x, y, z). The coordinate transformation formula is as follows:

[0091]

[0092] Where K represents the camera intrinsic parameter matrix, and f is the camera focal length, including f x and f y c represents the offset of the camera plane center point, including c x and c y .

[0093] The transformation between the camera coordinate system and the world coordinate system involves the camera's navigation pose matrix T, which includes two parts: rotation matrix R and translation t.

[0094] T = [R|t]

[0095] If the coordinates in the world coordinate system are represented as X, then the coordinate transformation can be expressed as:

[0096] P = TX

[0097] Visual sensors typically use Bundle Adjustment (BA) optimization, which essentially minimizes the reprojection error. The visual reprojection error is calculated as the difference between the observed and estimated values ​​in the image coordinate system. For the first i keyframes, minimizing the reprojection error... It is represented as shown above.

[0098] Furthermore, the principle behind the laser ranging error equation is that the depth observation value in the laser ranging direction should differ from the depth value calculated by the camera in that direction by a scale factor. It is particularly important to note that since laser ranging has a pointing direction, the 3D map points need to be selected from the region corresponding to that pointing direction. In this embodiment of the invention, a 15*15 pixel matrix region is defined in the center of the camera area, and only the 3D map points corresponding to these pixels are selected for calculation.

[0099] The specific laser ranging error equation is expressed as follows:

[0100]

[0101] in This represents the laser ranging error, where 's' refers to the scale factor. P represents the three-dimensional coordinates of the j-th map point in the world coordinate system of the laser direction region. o w This represents the coordinates of the camera's center point in the world coordinate system. This is the unit vector of the laser ranging direction in the world coordinate system.

[0102] Assume the transformation from camera coordinates to world coordinates for the i-th keyframe is... Simplified representation as The rotation matrix is ​​R. wc The translation vector is t wc .

[0103] Since the origin of the camera coordinate system is the camera center point, then P o w It can be converted to represent as t wc Therefore, the expression can be transformed into:

[0104]

[0105] It can be further decomposed and transformed into the camera coordinate system, as shown in the following equation:

[0106]

[0107] in This represents the vector indicating the direction of the laser ranging in the camera coordinate system. Since the camera and the laser ranging are mounted on the same plane, the direction of this vector is (0, 0, 1).

[0108] Therefore, the expression can be further transformed into:

[0109]

[0110] That is, by combining the two error equations mentioned above, the joint optimization function for visual reprojection error and laser ranging error can be obtained through the following formula:

[0111]

[0112]

[0113] in, This is due to visual reprojection error. The distance is the laser ranging error, where m represents the maximum number of map points used in the visual reprojection, and m' represents the maximum number of map points used in the visual reprojection. Since lasers are directional, m' ≤ m. k The parameters to be optimized include the scale factor s and the pose transformation matrix T. cw , can be represented as T cw =[R cw |t cw ], and 3D map points in the world coordinate system

[0114] In the above embodiments, in order to accurately solve the optimization parameters in the joint optimization function, based on the initial scale estimate, a graph optimization method is used to solve the joint optimization function to obtain the optimization parameters, including:

[0115] 1) Establish a new graph optimization model based on the joint optimization function, define the model nodes, edges and node update methods, and calculate the Jacobian matrix of the optimization parameters;

[0116] 2) Based on the new graph optimization model and the Jacobian matrix of the optimization parameters, the optimized scale factor, 3D map point coordinates and visual keyframe pose are obtained by iterative solution, wherein the initial value of the scale of the parameter to be optimized is set to the initial scale estimate.

[0117] That is, the embodiments of the present invention use graph optimization to solve the error model. Graph optimization requires the establishment of a new graph optimization model, confirmation of the model's edges, nodes and node update methods, and calculation of the Jacobian matrix of the three optimization parameters. Iterative optimization is then used to continuously optimize the Jacobian matrix until its value is reduced to the minimum. At this point, the corresponding optimization parameters can be obtained.

[0118] Preferably, in this embodiment of the invention, model nodes and edges are defined in the following manner:

[0119] In the graph optimization model, nodes are defined as parameters to be optimized, including: scale factor, pose transformation matrix, and 3D map points;

[0120] In the graph optimization model, the type of edge is defined as a hyperedge, which connects three nodes.

[0121] In this embodiment of the invention, the Jacobian matrix of the optimization parameters is calculated in the following manner:

[0122] Define the model error function according to the joint optimization function;

[0123] Calculate the Jacobian matrix of the model error function for the three nodes, including:

[0124] Obtain the Jacobian matrix of the three nodes corresponding to the visual reprojection error and the Jacobian matrix of the three nodes corresponding to the laser ranging error respectively;

[0125] The Jacobian matrix of each node is obtained by adding the Jacobian matrices of the three nodes corresponding to the visual reprojection error and the three nodes corresponding to the laser ranging error.

[0126] That is, to solve the error function, we need to solve the Jacobian matrix of the function with respect to the three nodes. The error function is expressed in the form of a summation. Therefore, the visual redefinition error part and the laser error part can be solved separately, and finally combined again in the same summation mode.

[0127] The Jacobian matrix corresponding to the visual reprojection error can be constructed with reference to the derivation in the ORB-SLAM3 algorithm, which will not be detailed here. The key to this embodiment lies in obtaining the Jacobian matrix of the laser error component.

[0128] In this embodiment of the invention, the Jacobian matrix of the three nodes corresponding to the laser error portion is obtained in the following manner:

[0129] For a node on the 3D map, the Jacobian matrix of that node is obtained by taking its partial derivative with respect to the laser ranging error equation. As shown in the following formula:

[0130]

[0131] For the node scale factor, taking the partial derivative of the laser ranging error equation yields the Jacobian matrix r of that node. d (s) is shown in the following formula:

[0132]

[0133] For the node pose transformation matrix, based on the laser ranging error equation, the rotation matrix R in the pose transformation matrix under small perturbations is obtained respectively. cw Translational displacement t cw The corresponding Jacobian matrix.

[0134] In this embodiment of the invention, the translation amount t under a small perturbation is obtained by the following formula. cwCorresponding Jacobian matrix r d (t cw ):

[0135]

[0136] And the translation R under small perturbations can be obtained by the following formula. cw Corresponding Jacobian matrix r d (R cw ):

[0137]

[0138] That is, solving for pose nodes is relatively complex, involving the rotation matrix R. cw Translational displacement t cw Solve them separately. By adding a small perturbation, its Jacobian can be calculated.

[0139] For the translation amount t cw Its tiny perturbation can be expressed as δt cw The following are ways to establish update variables that include perturbations:

[0140] t wc '=t wc +R wc δt cw

[0141] Therefore, after a small perturbation, the laser error equation should change to:

[0142]

[0143] The translation t can be obtained from the above formula. cw Jacobi is:

[0144]

[0145] For the rotation matrix R cw Its small perturbation is a small rotation angle, denoted as δφ. The method for establishing the update variable that includes the perturbation is as follows:

[0146] R cw '=R cw Exp(δφ)

[0147] Therefore, after a small perturbation, the laser error equation should change to:

[0148]

[0149] Based on the principle of small-angle approximation, the formula can be obtained:

[0150] Exp(δφ)=I+δφ^

[0151] Where δφ^ represents the antisymmetric matrix of δφ. Therefore, the above equation can be further expressed as:

[0152]

[0153] From the properties of antisymmetric matrices, we can obtain:

[0154]

[0155] Based on the above formula, the rotation matrix R can be obtained. cw Jacobi is:

[0156]

[0157] Furthermore, during iterative computation, a nonlinear optimization framework (g2o) can be used to construct a linear solver. The maximum number of iterations is set to 30, and the initial scale value is set to the initial scale value obtained in step one. The Levenberg-Marquardt method is then used to iteratively converge to the minimum Jacobian. This allows for the acquisition of optimized camera pose, scale factor, and 3D map points of the scene.

[0158] Preferably, in defining the node update method, to ensure the non-negativity of the scale factor update, the scale factor update method is set as shown in the following equation:

[0159] s′=sexp(δs).

[0160] Where δs represents the increase, and s′, s represent the scale factors before and after the update, respectively.

[0161] According to one embodiment of the present invention, updating the entire visual tracking thread according to optimized parameters to complete scale update and pose update includes:

[0162] The depth of the current 3D map point is updated based on the obtained optimized parameter scale factor to obtain the updated 3D map point depth, wherein the 3D map point is the 3D map point with the obtained optimized parameter.

[0163] The rotation matrix and translation vector for each frame are updated based on the obtained optimized pose transformation matrix.

[0164] Specifically:

[0165] After optimizing the joint error of visual / laser ranging, a precise scale factor 's' is obtained. The depth of the current 3D map points is then updated according to this scale, specifically including:

[0166] Call the current local map set to obtain the IDs of all map points; for each map point ID, obtain the corresponding 3D map point coordinate information (the 3D map point coordinate information optimized in step three); then multiply the new scale factor with the original depth information to obtain the new map point depth; finally, use the new map point depth to solve for the normal vector of the local map set and the median depth of the 3D map points.

[0167] Pose update mainly involves updating the rotation matrix and translation vector for each frame, specifically including:

[0168] After obtaining the new pose matrix, first set the optimization flag to True and the tracking thread flag to TrackOK; then save the pose transformation matrix of each keyframe and call the adjacent two keyframes; finally, use the new pose matrix to reproject the vision and obtain the optimized pose matrix between non-keyframes.

[0169] In summary, this invention solves for the depth of 3D map points through visual triangulation and calculates the initial scale value using timestamp-synchronized laser ranging data. It constructs a joint optimization function for visual reprojection error and laser ranging error; establishes a new graph optimization model, defining nodes, edges, and error Jacobian matrices, and obtains optimization parameters through iterative solutions; finally, it updates the entire visual tracking thread based on the optimization parameters, completing scale and pose updates to achieve accurate UAV positioning. This invention is designed for UAVs, accurately recovering scale information that is difficult to obtain visually during the initial stage of high-altitude flight, and optimizing the six-degree-of-freedom pose of the camera through multi-source constraints. The method of this invention exhibits high accuracy, real-time performance, robustness, and practicality for pose and depth estimation of high-altitude UAVs. This invention solves the technical problem of high-precision navigation, positioning, and depth estimation in high-altitude UAV scenarios due to scale bias in monocular cameras.

[0170] The features described and / or illustrated above with respect to one embodiment may be used in the same or similar manner in one or more other embodiments, and / or in combination with or in lieu of features in other embodiments.

[0171] It should be emphasized that the term "including / comprises" as used herein refers to the presence of a feature, whole, step, or component, but does not exclude the presence or addition of one or more other features, wholes, steps, components, or combinations thereof.

[0172] The methods described above in this invention can be implemented in hardware or in combination with software. This invention relates to computer-readable programs that, when executed by a logic component, enable the logic component to implement the aforementioned apparatus or constituent parts, or to implement the various methods or steps described above. This invention also relates to storage media for storing the above programs, such as hard disks, magnetic disks, optical disks, DVDs, flash memory, etc.

[0173] Many features and advantages of these embodiments are apparent from this detailed description, and therefore the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since many modifications and alterations will readily occur to those skilled in the art, the embodiments of the invention are not intended to be limited to the precise structures and operations illustrated and described, but rather to encompass all suitable modifications and equivalents falling within their scope.

[0174] The parts of this invention not described in detail are techniques known to those skilled in the art.

Claims

1. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs), characterized in that, The method includes: Initial scale estimates are obtained using laser ranging and visual triangulation. Based on the visual feature matching results, a visual reprojection error equation is constructed. Based on the correlation between the laser rangefinder and the visual observation, a laser ranging error equation is constructed. The two equations are combined to obtain a joint optimization function for visual reprojection error and laser ranging error. The joint optimization function includes the parameters to be optimized: scale factor, pose transformation matrix, and 3D map points in the world coordinate system. Based on the initial scale estimate, the joint optimization function is solved using a graph optimization method to obtain the optimization parameters; The entire visual tracking thread is updated based on the optimized parameters to complete scale and pose updates and obtain the precise positioning of the UAV. The method of obtaining the initial scale estimate using laser ranging and visual triangulation includes: Initial tracking during the high-altitude level flight phase is performed using a visual sensor to create keyframes; Using triangulation techniques to obtain the first The first keyframe The depth value of the 3D map point corresponding to each feature point , ; The depth value of the three-dimensional map point is used to obtain the first... The average depth of the 3D map points corresponding to each keyframe : ； Accept the input value from laser ranging, align its timestamp with the timestamp of the keyframe image, and obtain the [value] after timestamp filtering. Laser ranging height corresponding to each keyframe ; According to the laser ranging height and average depth Get the Initial scale estimate corresponding to each keyframe : 。 2. The visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 1, characterized in that, The visual reprojection error equation is constructed based on the visual feature matching results in the following manner: For the first n keyframes, the reprojection error It can be expressed as the following formula: ， ,in, Indicates the map point ID. Represents the world coordinate system. The three-dimensional coordinates of a map point Indicates the first On each key image frame The corresponding pixel observation point, where K represents the camera intrinsic parameter matrix. Let be the pose matrix, representing the position from the first position... The transformation from world coordinates to camera coordinates corresponding to each keyframe. For depth.

3. The visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 2, characterized in that, The laser ranging error equation is constructed based on the correlation between the laser rangefinder and visual observations in the following manner: ,in, This represents the laser ranging error, where 's' stands for scale factor. The world coordinate system representing the laser direction region. The three-dimensional coordinates of a map point This represents the transformation from the camera coordinate system to the world coordinate system corresponding to the i-th keyframe. Simplified representation as The rotation matrix is The translation vector is , The vector representing the laser ranging direction in the camera coordinate system. This indicates the length of the laser output.

4. The visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 3, characterized in that, The joint optimization function for visual reprojection error and laser ranging error is obtained by the following formula: ,in, This is due to visual reprojection error. For laser ranging error, This refers to the maximum number of map points used in visual reprojection. This refers to the maximum number of map points used in visual reprojection. Because laser light has directionality, therefore... , The parameters to be optimized include the scale factor s and the pose transformation matrix. , can be represented as and 3D map points in the world coordinate system .

5. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 4, characterized in that, Based on the initial scale estimate, the joint optimization function is solved using a graph optimization approach to obtain the optimization parameters, including: 1) Establish a new graph optimization model based on the joint optimization function, define the model nodes, edges and node update methods, and calculate the Jacobian matrix of the optimization parameters; 2) Based on the new graph optimization model and the Jacobian matrix of the optimization parameters, the optimized scale factor, 3D map point coordinates and visual keyframe pose are obtained by iterative solution, wherein the initial value of the scale of the parameter to be optimized is set to the initial scale estimate.

6. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 5, characterized in that, Define model nodes and edges in the following manner: In the graph optimization model, nodes are defined as parameters to be optimized, including: scale factor, pose transformation matrix, and 3D map points; In the graph optimization model, the type of edge is defined as a hyperedge, which connects three nodes.

7. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 6, characterized in that, In defining the node update method, the scale factor update method is set as shown in the following formula: 。 8. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 6, characterized in that, The Jacobian matrix of the optimization parameters is calculated as follows: Define the model error function according to the joint optimization function; Calculate the Jacobian matrix of the model error function for the three nodes, including: Obtain the Jacobian matrix of the three nodes corresponding to the visual reprojection error and the Jacobian matrix of the three nodes corresponding to the laser ranging error respectively; The Jacobian matrix of each node is obtained by adding the Jacobian matrices of the three nodes corresponding to the visual reprojection error and the three nodes corresponding to the laser ranging error.

9. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 8, characterized in that, The Jacobian matrix of the three nodes corresponding to the laser error component is obtained in the following manner: For a node on the 3D map, the Jacobian matrix of that node is obtained by taking its partial derivative with respect to the laser ranging error equation. As shown in the following formula: ； For the node scale factor, taking the partial derivative of the laser ranging error equation yields the Jacobian matrix of that node. As shown in the following formula: ； For the node pose transformation matrix, based on the laser ranging error equation, the rotation matrix in the pose transformation matrix under small perturbations is obtained respectively. Translational displacement The corresponding Jacobian matrix.

10. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 9, characterized in that, The translation under a small perturbation is obtained by the following formula. Corresponding Jacobian matrix : 。 11. A visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 9 or 10, characterized in that, The translation under a small perturbation is obtained by the following formula. Corresponding Jacobian matrix : 。 12. The visual / laser ranging high-altitude navigation method for unmanned aerial vehicles (UAVs) according to claim 1, characterized in that, The step of updating the entire visual tracking thread according to optimized parameters to complete scale and pose updates includes: The depth of the current 3D map point is updated based on the obtained optimized parameter scale factor to obtain the updated 3D map point depth, wherein the 3D map point is the 3D map point with the obtained optimized parameter. The rotation matrix and translation vector for each frame are updated based on the obtained optimized pose transformation matrix.

Images