Individual relative pose measurement method in UAV formation based on monocular vision

Abstract

The present application relates to a method for measuring the relative posture of individuals in a UAV formation based on monocular vision. The method includes: obtaining a target image from image data taken by a camera, determining the position of the feature points of the target, constructing a coordinate correspondence between the three-dimensional coordinates of the feature points in the coordinate system of the UAV body and the two-dimensional coordinates in the target image, and establishing a projection equation, combining the projection equations corresponding to multiple feature points, and obtaining the initial value of the relative posture of the target relative to the camera of the camera through the EPnP algorithm; calculating the reprojection residual value of the feature point, obtaining the weight according to the absolute value of the reprojection residual value of each point, constructing a weight matrix, constructing an optimization objective function, taking the initial value of the relative posture as the iteration initial value, iteratively solving the optimization objective function, outputting the precise value of the relative posture of the target relative to the camera of the camera, and further obtaining the relative posture information of the target relative to the camera.

Description

Technical Field

[0001] The present application relates to the technical field of unmanned cluster formation organization and maintenance, and in particular to a method for measuring the relative posture of individuals in a UAV formation based on monocular vision. Background Art

[0002] UAV formations play an increasingly important role in flight performances, package delivery, complex mission collaboration, etc. Accurate estimation of the relative position and attitude of each UAV in the formation is the basis for safe and stable flight of the formation and successful completion of the mission.

[0003] At present, the main forms of unmanned formation organization include ground station controlling all individuals separately, ground station controlling some individuals in the formation, and autonomous control of the formation. The relative position determination between individuals mainly relies on the mutual communication of positioning devices and inertial navigation data between individuals in the formation or communication with the ground. However, in an environment with complex electromagnetic environment, this communication-based formation method has the risk of failure.

[0004] Therefore, for UAV formation flying, it is necessary to study a relative posture measurement method that can operate stably in a communication denial environment. Summary of the invention

[0005] Based on this, it is necessary to provide a method for measuring the relative pose of individuals in a UAV formation based on monocular vision that can operate stably in a communication denial environment to address the above technical problems.

[0006] A method for measuring relative posture of individuals in a UAV formation based on monocular vision, the method comprising:

[0007] Acquire image data taken by a camera in the UAV formation, obtain a target aircraft image from the image data, determine the position of feature points of the target aircraft through a pre-trained feature recognition network model based on the target aircraft image, and further construct a coordinate correspondence between the three-dimensional coordinates of the feature points in the UAV body coordinate system and the two-dimensional coordinates in the target aircraft image;

[0008] According to the coordinate correspondence, a projection equation is established, the projection equations corresponding to the multiple feature points are combined, and the initial value of the relative posture of the target machine relative to the camera of the camera is obtained through the EPnP algorithm;

[0009] According to the initial relative pose value, the reprojection residual value of the feature point is obtained, and the weight is further obtained according to the absolute value of the reprojection residual value of each point to construct a weight matrix; wherein the larger the absolute value of the reprojection residual value, the smaller the corresponding weight;

[0010] According to the weight matrix, an optimization objective function is constructed for multiple feature points, and the optimization objective function is iteratively solved with the initial value of the relative posture as the iteration initial value until the iteration convergence condition is met, and the relative posture accurate value of the target machine relative to the camera of the camera is output;

[0011] The relative position information of the target machine relative to the camera is obtained according to the precise value of the relative position of the target machine relative to the camera of the camera and the known installation parameter information of the camera on the drone.

[0012] In one of the embodiments, the method further includes: acquiring image data captured by a camera in the drone formation;

[0013] The target drone is detected from the image data through a pre-trained drone recognition network model, and the detected target drone area is cropped to obtain a target drone image; the drone network model is a YOLOv4 network model.

[0014] In one of the embodiments, the method further includes: obtaining a training image set, and marking the positions of feature points of the UAV in the training image set;

[0015] Input the labeled training image set into the pre-designed feature recognition network model;

[0016] The feature recognition network model is trained to obtain a trained feature recognition network model.

[0017] In one embodiment, it also includes:

[0018]

[0019]

[0020] Among them, res xi is the reprojection error of the i-th feature point in the x direction, res yi is the reprojection error of the i-th feature point in the y direction, res i is the reprojection residual value of the i-th feature point, [x, y] T with [X,Y,Z] T are the two-dimensional coordinates of the feature point in the target image and the three-dimensional coordinates of the feature point in the coordinate system of the drone body, respectively. x 、f y , C x , C y are the equivalent focal length and principal point coordinates of the camera, both of which are known quantities, Z c is the third component of the feature’s three-dimensional coordinates in the camera coordinate system, R c0is the initial value of the attitude matrix in the relative posture initial value, T c0 is the initial value of the position vector in the relative posture initial value.

[0021] In one embodiment, the method further includes: further obtaining weights according to the absolute values ​​of the reprojection residual values ​​of each point, and constructing a weight matrix as follows:

[0022]

[0023] Among them, W is the weight matrix and n is the number of feature points.

[0024] In one embodiment, the method further includes: constructing an optimization objective function for a plurality of feature points according to the weight matrix:

[0025] F=f T Wf.

[0026] in, is the system of equations obtained by combining the n characteristic point equations, is the first independent equation obtained from the projection equation, is the second independent equation obtained from the projection equation.

[0027] In one embodiment, the method further includes: obtaining a relative position and posture accurate value R of the target machine relative to the camera of the camera. c ,T c , and the predicted installation parameter information R of the camera on the camera U ,T U ; Among them, R U is the attitude matrix of the camera relative to the camera, T U is the position vector of the camera relative to the camera;

[0028] The relative position information of the target machine relative to the camera is obtained as follows:

[0029] R=R U R c

[0030] T=R U T c +T U

[0031] Wherein, R is the attitude matrix of the target machine relative to the camera, and T is the position vector of the target machine relative to the camera.

[0032] The above-mentioned method for measuring the relative posture of individuals in a UAV formation based on monocular vision obtains the target machine image from the image data taken by the camera, determines the position of the feature points of the target machine, and further constructs the coordinate correspondence between the three-dimensional coordinates of the feature points in the coordinate system of the UAV body and the two-dimensional coordinates in the target machine image; according to the coordinate correspondence, a projection equation is established, and the projection equations corresponding to multiple feature points are combined, and the initial value of the relative posture of the target machine relative to the camera of the camera is obtained by the EPnP algorithm; the reprojection residual value of the feature point is calculated, and the weight is obtained according to the absolute value of the reprojection residual value of each point, and a weight matrix is ​​constructed; according to the weight matrix, an optimization objective function is constructed for multiple feature points, and the initial value of the relative posture is used as the initial value of the iteration, and the optimization objective function is iteratively solved, and the accurate value of the relative posture of the target machine relative to the camera of the camera is output, and the relative posture information of the target machine relative to the camera is further obtained. The present invention constructs an optimization objective function through the reprojection residual value for iterative solution, which improves the accuracy of the individual relative posture measurement results in the UAV formation based on monocular vision, and the method has strong applicability and can operate stably in a communication denial environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 This is a diagram of an application scenario of a method for measuring relative pose of individuals in a UAV formation based on monocular vision in one embodiment;

[0034] Figure 2 Schematic diagram of a process flow of a method for measuring relative posture of individuals in a UAV formation based on monocular vision in one embodiment;

[0035] Figure 3 A schematic diagram of image cutting in one embodiment;

[0036] Figure 4 Schematic diagram of on-board feature point detection in one embodiment. DETAILED DESCRIPTION

[0037] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0038] The method for measuring the relative position of individuals in a UAV formation based on monocular vision provided in this application can be applied to Figure 1 In the application environment shown, a camera is mounted on the shooting machine, and the shooting machine shoots the target machine, and the relative position information of the target machine relative to the shooting machine is obtained through the method of the present application.

[0039] In one embodiment, Figure 2As shown, a method for measuring the relative posture of individuals in a UAV formation based on monocular vision is provided, comprising the following steps:

[0040] Step 202, obtain the image data taken by the camera in the drone formation, obtain the target aircraft image from the image data, determine the location of the feature points of the target aircraft based on the target aircraft image through a pre-trained feature recognition network model, and further construct a coordinate correspondence between the three-dimensional coordinates of the feature points in the drone body coordinate system and the two-dimensional coordinates in the target aircraft image.

[0041] When drones fly in formation, there is a risk of collision if the relative distance is too close. Therefore, the size of other individuals in the formation is small in the image. In addition, the distribution of drones in the formation is uneven in the image, so it is difficult to directly solve the pose. In addition, due to the characteristics of drone movement, the relative movement between each drone is in a dynamic stable state. The position of the target drone in the camera image changes greatly between adjacent frames. Therefore, it is necessary to first perform drone target detection on the entire image to facilitate subsequent processing. Specifically, the target drone is detected from the image data through the pre-trained YOLOv4-based drone recognition network model, and the detected target drone area is cropped to obtain the target drone image, such as Figure 3 shown.

[0042] After cutting, mark the wings, engines and other characteristic parts, such as Figure 4 As shown, the labeled training image set is input into a pre-designed feature recognition network model; the feature recognition network model is trained to obtain a trained feature recognition network model, and the feature point positions of the target machine are determined through the trained feature recognition network model.

[0043] According to the position of the feature point of the target machine, the three-dimensional coordinates of the feature point in the UAV body coordinate system and the two-dimensional coordinates in the target machine image are determined, and the coordinate correspondence is established.

[0044] Step 204, based on the coordinate correspondence, a projection equation is established, the projection equations corresponding to the multiple feature points are combined, and the initial value of the relative posture of the target machine relative to the camera of the camera is obtained through the EPnP algorithm.

[0045] The projection equation is:

[0046]

[0047] Where R c ,T c To obtain the attitude matrix and position vector of the drone relative to the camera, [x, y] T with [X,Y,Z] T are the two-dimensional coordinates of the image coordinate system and the three-dimensional coordinates of the body coordinate system, respectively, x 、fy , C x , C y are the equivalent focal length and principal point coordinates of the camera, both of which are known quantities, Z c The third component of the feature's 3D coordinates in the camera coordinate system.

[0048] The EPnP algorithm is a method for solving 3D to 2D point pair motion. It describes how to estimate the position of the camera when we know n 3D space points and their projection positions. This application uses the EPnP algorithm to obtain the initial value of the relative position of the target machine relative to the camera, denoted as R c0 ,T c0 .

[0049] Step 206, according to the initial value of the relative posture, the reprojection residual value of the feature point is obtained, and the weight is further obtained according to the absolute value of the reprojection residual value of each point to construct a weight matrix.

[0050] Since the center of the feature component is not in the same position under different viewing angles, and the accuracy of the target detection result cannot meet the requirements of precise positioning of the two-dimensional coordinates of the feature points, after solving the EPnP problem, its result is used as the initial value, and the inverse of the absolute value of the reprojection residual of each point is used as the weight for weighted iterative solution to further improve the accuracy of the relative pose.

[0051] Step 208, construct an optimization objective function for multiple feature points based on the weight matrix, use the relative posture initial value as the iteration initial value, iteratively solve the optimization objective function until the iterative convergence condition is met, and output the precise value of the relative posture of the target machine relative to the camera.

[0052] Due to the limitations of the YOLOv4 algorithm itself, the feature point recognition results may be inaccurate. The larger the reprojection residual value, the worse the quality of feature point recognition. This application proposes to use the reciprocal of the absolute value of the reprojection residual as a weight for weighted iterative solution, which can weaken the impact of poor quality feature points on pose estimation.

[0053] R c0 ,T c0 The Levenberg–Marquardt (LM) method is used to iteratively solve the above six unknowns, and then the pose matrix is ​​calculated using the Euler angle calculation results to obtain the pose R of the target machine relative to the camera. c ,T c .

[0054] Step 210, according to the precise value of the relative posture of the target machine relative to the camera of the camera, and the predicted installation parameter information of the camera on the drone, the relative posture information of the target machine relative to the camera is obtained.

[0055] For a feature point P on a target aircraft, its coordinates in the aircraft coordinate system are [X U ,Y U ,Z U ] T , the coordinates in the camera coordinate system are [X c ,Y c ,Z c ] T , there is a relation

[0056]

[0057]

[0058] Right now

[0059]

[0060] The installation parameters of the camera on the drone are usually calibrated when the drone leaves the factory or before flight, that is, R U ,T U Known. The target drone’s attitude matrix relative to the shooting drone is measured

[0061] R=R U R c

[0062] Position vector

[0063] T=R U T c +T U

[0064] In the above-mentioned method for measuring the relative posture of individuals in a UAV formation based on monocular vision, the target machine image is obtained from the image data taken by the camera, the position of the feature point of the target machine is determined, and the coordinate correspondence between the three-dimensional coordinates of the feature point in the coordinate system of the UAV body and the two-dimensional coordinates in the target machine image is further constructed; according to the coordinate correspondence, a projection equation is established, and the projection equations corresponding to multiple feature points are combined, and the initial value of the relative posture of the target machine relative to the camera of the camera is obtained by the EPnP algorithm; the reprojection residual value of the feature point is calculated, and the weight is obtained according to the absolute value of the reprojection residual value of each point, and a weight matrix is ​​constructed; according to the weight matrix, an optimization objective function is constructed for multiple feature points, and the initial value of the relative posture is used as the initial value of the iteration, and the optimization objective function is iteratively solved, and the accurate value of the relative posture of the target machine relative to the camera of the camera is output, and the relative posture information of the target machine relative to the camera is further obtained. The present invention constructs an optimization objective function through the reprojection residual value for iterative solution, which improves the accuracy of the individual relative posture measurement results in the UAV formation based on monocular vision, and the method has strong applicability and can operate stably in a communication denial environment.

[0065] In one embodiment, it also includes:

[0066]

[0067]

[0068] Among them, res xi is the reprojection error of the i-th feature point in the x direction, res yi is the reprojection error of the i-th feature point in the y direction, res i is the reprojection residual value of the i-th feature point, [x, y] T with [X,Y,Z] T are the two-dimensional coordinates of the feature point in the target image and the three-dimensional coordinates of the feature point in the coordinate system of the drone body, respectively. x 、f y , C x , C y are the equivalent focal length and principal point coordinates of the camera, both of which are known quantities, Z c is the third component of the feature’s three-dimensional coordinates in the camera coordinate system, R c0 is the initial value of the attitude matrix in the relative pose initial value, T c0 is the initial value of the position vector in the relative pose initial value.

[0069] In one embodiment, the method further includes: further obtaining weights according to the absolute values ​​of the reprojection residual values ​​of each point, and constructing a weight matrix as follows:

[0070]

[0071] Among them, W is the weight matrix and n is the number of feature points.

[0072] In one embodiment, the method further includes: constructing an optimization objective function for multiple feature points according to the weight matrix:

[0073] F=f T Wf.

[0074] in, is the system of equations obtained by combining the n characteristic point equations, is the first independent equation obtained from the projection equation, is the second independent equation obtained from the projection equation.

[0075] In one embodiment, the method further includes: obtaining an accurate relative position value R of the target machine relative to the camera of the camera. c ,T c , and the predicted installation parameter information R of the camera on the camera U ,T U; Among them, R U is the attitude matrix of the camera relative to the camera, T U is the position vector of the camera relative to the camera;

[0076] The relative position information of the target machine relative to the camera is obtained as follows:

[0077] R=R U R c

[0078] T=R U T c +T U

[0079] Among them, R is the attitude matrix of the target machine relative to the camera, and T is the position vector of the target machine relative to the camera.

[0080] It should be understood that although Figure 2 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 2 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0081] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0082] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A method for measuring the relative posture of individuals in a UAV formation based on monocular vision, characterized in that: The method comprises: Acquire image data taken by a camera in the UAV formation, obtain a target aircraft image from the image data, determine the position of feature points of the target aircraft through a pre-trained feature recognition network model based on the target aircraft image, and further construct a coordinate correspondence between the three-dimensional coordinates of the feature points in the UAV body coordinate system and the two-dimensional coordinates in the target aircraft image; According to the coordinate correspondence, a projection equation is established, the projection equations corresponding to the multiple feature points are combined, and the initial value of the relative posture of the target machine relative to the camera of the camera is obtained through the EPnP algorithm; According to the initial relative pose value, the reprojection residual value of the feature point is obtained, and the weight is further obtained according to the absolute value of the reprojection residual value of each point to construct a weight matrix; wherein the larger the absolute value of the reprojection residual value, the smaller the corresponding weight; According to the weight matrix, an optimization objective function is constructed for multiple feature points, and the optimization objective function is iteratively solved with the initial value of the relative posture as the iteration initial value until the iteration convergence condition is met, and the relative posture accurate value of the target machine relative to the camera of the camera is output; According to the precise value of the relative position and posture of the target machine relative to the camera of the camera, and the known installation parameter information of the camera on the drone, the relative position and posture information of the target machine relative to the camera is obtained; According to the initial relative pose value, the reprojection residual value of the feature point is obtained, including: According to the initial relative pose value, the reprojection residual value of the feature point is obtained as: Among them, res xi is the reprojection error of the i-th feature point in the x direction, res yi is the reprojection error of the i-th feature point in the y direction, res i is the reprojection residual value of the i-th feature point, [x, y] T with [X,Y,Z] T are the two-dimensional coordinates of the feature point in the target image and the three-dimensional coordinates of the feature point in the coordinate system of the drone body, respectively. x 、f y , C x , C y are the equivalent focal length and principal point coordinates of the camera, both of which are known quantities, Z c is the third component of the feature’s three-dimensional coordinates in the camera coordinate system, R c0 is the initial value of the attitude matrix in the relative posture initial value, T c0 is the initial value of the position vector in the relative posture initial value; The weights are obtained according to the absolute values ​​of the reprojection residual values ​​of each point, and a weight matrix is ​​constructed, including: The reciprocal of the absolute value of the reprojection residual value of each point is used as the weight, and the weight matrix is ​​constructed as follows: Among them, W is the weight matrix, n is the number of feature points; According to the weight matrix, an optimization objective function is constructed for multiple feature points, including: According to the weight matrix, the optimization objective function is constructed for multiple feature points as follows: F=f T Wf in, is the system of equations obtained by combining the equations of n characteristic points, f i 1 , i=1,2…n is the first independent equation obtained from the projection equation, f i 2 is the second independent equation obtained from the projection equation.

2. The method according to claim 1, characterized in that: Acquiring image data taken by a camera in the UAV formation, and obtaining a target aircraft image from the image data, including: Obtain image data taken by cameras in the UAV formation; The target drone is detected from the image data through a pre-trained drone recognition network model, and the detected target drone area is cropped to obtain a target drone image; the drone network model is a YOLOv4 network model.

3. The method according to claim 2, characterized in that Before determining the position of the feature points of the target machine by using a pre-trained feature recognition network model according to the target machine image, the method further includes: Obtain a training image set, and mark the positions of feature points of the UAV in the training image set; Input the labeled training image set into the pre-designed feature recognition network model; The feature recognition network model is trained to obtain a trained feature recognition network model.

4. The method according to claim 1, characterized in that According to the precise value of the relative position and posture of the target machine relative to the camera of the camera, and the known installation parameter information of the camera on the drone, the relative position and posture information of the target machine relative to the camera is obtained, including: Get the relative position and position accuracy R of the target machine relative to the camera. c ,T c , and the predicted installation parameter information R of the camera on the camera U ,T U ; Among them, R U is the attitude matrix of the camera relative to the camera, T U is the position vector of the camera relative to the camera; The relative position information of the target machine relative to the camera is obtained as follows: R=R U R c T=R U T c +T U Wherein, R is the attitude matrix of the target machine relative to the camera, and T is the position vector of the target machine relative to the camera.

Images