Cooperative target wide-range adaptive relative positioning method based on multi-focal-length compound eye

Abstract

The invention belongs to the technical field of visual relative positioning, and discloses a multi-focal-length compound eye-based cooperative target wide-range adaptive relative positioning method, which comprises the following steps of: establishing a coordinate system; establishing a single-camera model; establishing a multi-focus compound eye camera imaging model; establishing a relative positioning motion model; establishing a self-adaptive observation model based on a multi-focus compound eye camera; and establishing a self-adaptive relative pose estimation method based on the multi-focus compound eye camera. According to the cooperative target wide-range adaptive relative positioning method based on the multi-focal-length compound eye, a multi-focal-length compound eye camera is formed by combining a plurality of single cameras with different focal lengths, and the established adaptive relative pose estimation method can perform pose estimation under the condition that a target point is limited; and meanwhile, the robustness of the algorithm is enhanced through a Kalman filter, the limitation of traditional optical zooming and digital zooming in the relative positioning application of the vision of the unmanned aerial vehicle is solved, the wide-range and long-distance sensing capability is achieved, and the practical engineering value is achieved.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual relative positioning, and particularly relates to a wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye. Background Art

[0002] An insect compound eye consists of many small and simple units called ommatidia. Each ommatidium has its own optical system and can independently sense the light in the surrounding environment. Each ommatidium has an independent crystalline cone and photoreceptor cells. When light passes through the crystalline cone, refraction and focusing occur, enabling the photoreceptor cells to collect the corresponding light information. Visual nerve cells aggregate all the light information and perform corresponding perception and planning, thereby obtaining the ability to widely and rapidly perceive the surrounding environment.

[0003] Currently, the structures of artificial compound eyes are mostly in the laboratory research stage, and there are no examples of using artificial compound eyes for robot perception and control. Currently, a wide-range adaptive perception can be achieved by means of a structure similar to a compound eye, which can simultaneously meet the two indicators of long distance and wide range.

[0004] When the size of a camera is fixed, lenses with different focal lengths determine the size of the field of view. Different focal lengths bring different scales of visual perception. When the focal length is small, the field of view is large, and when the focal length is large, the field of view is small. The combination of multiple cameras can achieve wide-range adaptive perception. However, there are certain limitations in the existing two zoom methods for changing the perception perspective, namely optical zoom and digital zoom. Optical zoom brings a complex mechanical zoom structure, increasing the cost and weight of the optical system, making it inapplicable to the application scenarios of micro and small unmanned aerial vehicles. Digital zoom only magnifies the image equivalently through image interpolation to achieve zooming. This operation will amplify the noise of the sensor, making the image blurrier. At the same time, large-size image processing will also increase the load on the processor. Both optical zoom and digital zoom have their limitations and cannot solve the contradiction between the perception range and the perception distance under limited weight, power supply, and heat dissipation conditions.

[0005] Currently, there is an urgent need to develop a wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye, so as to overcome the limitations of traditional optical zoom and digital zoom in the field of visual relative positioning.

[0007] When humans search for targets in a large area, they first look for relatively macroscopic features such as color or shape. After screening out possible targets, they then conduct more detailed feature screening. The wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye of the present invention utilizes compound eye vision and adopts a target discrimination process similar to that of humans, that is, obtaining perception in a large range through a short-focus lens, screening the existence area of possible targets through an algorithm, and then using a long-focus lens to further perceive the details of the possible area.

[0008] The wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye of the present invention includes the following steps: S10. Establish a coordinate system; The coordinate system includes an inertial coordinate system, a body coordinate system, a camera coordinate system, and a pixel coordinate system, and each coordinate system conforms to the right-hand rule; S20. Establish a single-camera model; Establish a single-camera model and determine the single-camera imaging equation; S30. Establish a multi-focal compound eye camera imaging model; A multi-focal compound eye camera is composed of several single cameras with different focal lengths. Establish a multi-focal compound eye camera imaging model; obtain the coordinate transformation relationship of the pixel points of different single cameras corresponding to the same target point in the pixel coordinate systems of the corresponding single cameras; S40. Establish a motion model for relative positioning; Establish a motion model for relative positioning, which is used to describe the relative positioning motion of the target UAV from the perspective of the observation UAV; S50. Establish an adaptive observation model based on a multi-focal compound eye camera; Set a target on the target UAV, and the target consists of target points; set several single cameras on the observation UAV to form a multi-focal compound eye camera; establish an adaptive observation model based on the multi-focal compound eye camera to find the pairing relationship between the target points and the single cameras; S60. Establish an adaptive relative pose estimation method based on a multi-focal compound eye camera; According to the observed target image on the target UAV, calculate the relative position and attitude of the target UAV and the observation UAV, referred to as the relative pose, and improve the robustness of the adaptive relative pose estimation method through Kalman filtering.

[0009] Furthermore, the coordinate system in S10 is defined as follows: Inertial coordinate system , set on the horizontal ground, used to characterize the motion state of the UAV relative to the ground and determine the three-dimensional space coordinates of the UAV; Body coordinate system , fixedly connected to the UAV body, and the coordinate origin Located at the center of gravity of the UAV, Pointing in the nose direction within the symmetry plane of the UAV, Within the symmetry plane of the UAV and perpendicular to Upward, The direction is determined by the right - hand rule; Camera coordinate system , Fixed to the camera, and the coordinate origin Is located at the optical center of the camera, Is the optical axis direction of the camera, And Are parallel to the Axis and Axis of the pixel coordinate system; A multi - focal compound - eye camera is composed of several cameras with different focal lengths. Each camera coordinate system in the multi - focal compound - eye camera is denoted as , Is the total number of cameras; Pixel coordinate system , With pixels as the unit, and the coordinate origin Is located at the upper - left corner of the image. Pixel coordinates are dimensionless, and pixel coordinates Are the number of columns and rows of the pixel in the matrix respectively.

[0010] Furthermore, the establishment of a single - camera model in S20 includes the following: A point In the inertial coordinate system, whose coordinates in the single - camera coordinate system are , And projected onto the corresponding pixel coordinate system of the single - camera as a point , The point Has coordinates In the pixel coordinate system, then the single - camera imaging equation is: ; Among them, Is the camera internal parameter matrix, which is composed of the camera focal length , The scaling factor Corresponding to the pixel coordinates And As well as the pixel coordinates And Of the image center, and satisfies And , Is the focal - length equivalent factor in the x - direction, Is the focal - length equivalent factor in the y - direction; The camera internal parameter matrix Is fixed after the camera leaves the factory and will not change during use. The camera internal parameter matrix Is determined through camera calibration.

[0011] Further, the establishment of the multi-focal compound eye camera imaging model in S30 includes the following: Install two single cameras with parallel optical axes on the observation UAV. The two single cameras with parallel optical axes form a multi-focal compound eye camera. The multi-focal compound eye camera provides image data from different perspectives as a complete sensor, processes the image data from different perspectives, and obtains the relative pose between the target UAV and the observation UAV. Let the internal parameter matrices corresponding to the two single cameras be and , the point on the target UAV has coordinates in the camera coordinate system , and the pixel coordinates in the corresponding pixel coordinate system are ; the coordinates in the camera coordinate system are , and the pixel coordinates in the corresponding pixel coordinate system are ; is an n-dimensional vector, is an n×n matrix; the offset between the two single cameras is , then there is: ; Considering that the distance between the observation UAV and the target UAV is between 5m and 50m or even farther, compared with the distance of mm level between the two single cameras, the offset can be ignored, then there is: ; The pixel coordinates of the two single cameras are converted by the following formula to obtain the coordinate conversion relationship between the two single cameras under the condition of parallel optical axes: .

[0012] Further, the establishment of the kinematic model for relative positioning in S40 includes the following: Let represent the three-dimensional position vector of the UAV in the inertial coordinate system, is the position component of the axis, is the position component of the axis, is the position component of the axis, represents the three-dimensional velocity of the UAV in the inertial coordinate system, is the velocity component of the axis, is the velocity component on the axis, is the velocity component of the axis; use the quaternion Indicates the attitude of the UAV , is the scalar part of the rotation, are the projections of the rotation axis on the body coordinate system respectively; The three-dimensional angular velocity of the UAV in the body coordinate system is expressed as , is the angular velocity about the axis, is the angular velocity about the axis, is the angular velocity about the axis; Then the motion model of the UAV is as follows: ; where, is the UAV velocity vector, is the acceleration vector; is the derivative of the UAV position vector, is the derivative of the UAV velocity vector; Using quaternions to describe rotational motion, the rotational kinematic equation of the UAV is as follows: ; where, is the derivative of the quaternion; is the auxiliary matrix for quaternion differentiation, , in the form of: ; where, the symbol is the operation symbol for converting a three-dimensional vector into a cross-product skew-symmetric matrix; Denote the variables of the target UAV with the subscript tgt, and the body coordinate system of the target UAV is ; Denote the variables of the observing UAV with the subscript ace, and the body coordinate system of the observing UAV is ; Then the motion model of the target UAV relative to the observing UAV is: ; where, is the relative position, is the relative acceleration, is the relative velocity; is the relative attitude represented by quaternions; is the velocity of the target UAV in the inertial frame; is the velocity of the observing UAV in the inertial frame; is the derivative of the relative attitude represented by quaternions; is the acceleration of the target UAV in the inertial frame; is the acceleration of the observing UAV in the inertial frame; Obtain the state variables of the relative motion of the target UAV with respect to the observation UAV which is the relative position in the inertial coordinate system , relative velocity and the quaternion representing the relative attitude , that is: , namely: ; After discretization, the state variables of the target UAV at time are as follows , as follows ; ; Then the discrete motion model of the target UAV at time is: ; where the subscript represents time , and the subscript represents the previous moment of time ; is the velocity measurement noise ; is the attitude measurement noise ; and are both zero-mean Gaussian white noises is the sampling time is the acceleration of the observation UAV at time Let the state noise be: ; Let the control input be as follows ; Rewrite the discrete motion model of the target UAV at time into matrix form , then ; where is the state matrix is the identity matrix of the corresponding dimension is the state variable of the target UAV at time is the angular velocity at time

[0013] Furthermore, the adaptive observation model for the multi-focal compound eye camera in S50 includes the following Let the observation vector be the relative pose calculated by the multi-focal compound eye camera, that is ; is the observation value at time k; establish the observation equation , where is the observation equation under ideal conditions; is the observation noise, assuming is white noise with zero mean, the covariance matrix of is ; Set target marking points on the target UAV; set single cameras on the observation UAV to form a multi-focal compound eye camera; through target marking points assist the multi-focal compound eye camera to perform relative positioning of the target UAV and the observation UAV; find the pairing relationship between the target marking points and the single cameras through the brute-force matching algorithm. The brute-force matching algorithm includes the following steps: S51. Select a single camera; Select the th single camera from the multi-focal compound eye camera; if the number of target marking points observed by the th single camera is greater than or equal to the target marking points on the target UAV, then continue to the next step, otherwise jump to S54; S52. Calculate the pose; Select target marking points from the target marking points of the th single camera, and use the Perspective-n-Point (PnP) algorithm to calculate the pose; S53. Perform validity judgment; Judge whether the current pose is valid according to the preset validity conditions. If it is valid, record the current pose and the reprojection error of the current pose; S54. Perform termination judgment; Let , and then judge whether it is less than the number of single cameras . If it is less, jump to S51 to continue the calculation, otherwise end; select the target marking point with the smallest reprojection error and the corresponding single camera as the required 3D-2D matching point pair.

[0014] Furthermore, the reprojection error calculation method is as follows: The reprojection error of the target marking points on the single cameras is: ; Among them, is the transformation matrix from the body coordinate system of the target UAV to the camera coordinate system; is the weight coefficient of the th single camera, is the re-projection error of the th target feature point in the th single camera; Define as the augmented internal parameter matrix of the th single camera, is the internal parameter matrix of the th single camera; define the transformation matrix from the target UAV to the th single camera as , is the rotation matrix, is the translation matrix; define and as the homogeneous coordinates of the th target feature point in the body coordinate system of the target UAV and the camera coordinate system of the th single camera respectively; define the re-projection error of the th target feature point in the camera coordinate system of the th single camera as: ; Among them, is the depth scale factor of the th single camera; Solving the relative pose of the target UAV is equivalent to finding the that minimizes the re-projection error , which is expressed as follows: ; Single cameras with different focal lengths have the same re-projection error, but the corresponding real distance errors are different. Therefore, when optimizing the re-projection errors of all single cameras, set the corresponding weight coefficients for each single camera; at the same time, due to influencing factors including installation error and orientation error, the noises of each single camera are also different; Assume that the weight coefficient of the th single camera in the multi-focal compound eye camera consists of two parts, namely ; among them, is used to correct the error caused by the camera focal length of the th single camera, and take ; is used to correct the error caused by influencing factors including installation error and orientation error, Determined by statistics; For the th camera, the focal length equivalent factor in the x - direction, For the th camera, the focal length equivalent factor in the y - direction; Conditionally optimize according to the number of target points of the target UAV observed by the observation UAV to achieve relative pose estimation when some target points cannot be observed; when 3 or more target points are observed, the process of solving Equation (18) remains unchanged, and S50 is directly used for solving; when 2 target points are observed, perform position optimization, let , where, is the change in camera pose, is the change in the translation part, retain the first three dimensions to represent the optimization of the translation part, ignore the part representing rotation in the last three dimensions, and solve for the that minimizes the reprojection error ; when 1 target point is observed, then take , where, and are the translation changes in the x and y directions respectively, optimize the translation position of the camera to solve for the that minimizes the reprojection error , that is, both the depth and the pose remain unchanged, and the relative position is optimized in the plane.

[0015] Furthermore, the establishment of S60 based on the adaptive relative pose estimation method of the multi - focal compound - eye camera includes the following: S61. Obtain the images of each single camera of the multi - focal compound - eye camera; S62. Initialize; Obtain 3D - 2D matching point pairs through the brute - force matching algorithm; S63. Adaptive observation; Perform adaptive observation according to the relative position and pose of the target UAV based on the 3D - 2D matching point pairs; S64. Update the filter through Kalman filtering to improve the robustness of the adaptive relative pose estimation method, and then perform filter prediction; ; Where, is the state at time k, which is predicted based on the information at time k - 1; is the optimal estimate at time k - 1; is the optimal estimate at time k; is the state transition matrix, is the state transition function; is the covariance matrix obtained at time k based on the information at time k - 1; is the covariance matrix of the optimal state estimate at time k-1; is the covariance matrix of the optimal state estimate at time k; is the covariance matrix of the process noise; is the observation function; is the Kalman gain matrix; is the observation value at time k; is the observation matrix; is the covariance matrix of the process noise; S65. Complete the relative position and attitude observation of the target UAV; Judge whether the maximum value of is less than a pre-set threshold. If it exceeds the threshold, it means that the Kalman filter result diverges. At this time, stop the filter update, return to S62, and re-initialize until the maximum value of is less than the pre-set threshold, and complete the relative position and attitude observation of the target UAV.

[0016] The cooperative target wide-range adaptive relative positioning method based on a multi-focal compound eye of the present invention forms a multi-focal compound eye camera by combining multiple single cameras with different focal lengths. The established adaptive relative pose estimation method can perform pose estimation under the condition of limited target points. At the same time, the robustness of the algorithm is enhanced through a Kalman filter, solving the limitations of traditional optical zoom and digital zoom in the application of UAV vision relative positioning, having the ability of large-range and long-distance perception, and having engineering practical value. Brief Description of the Drawings

[0017] Figure 1 is the flow chart of the cooperative target wide-range adaptive relative positioning method based on a multi-focal compound eye of the present invention; Figure 2 is the schematic diagram of the imaging principle of the compound eye camera under the condition of parallel optical axes; Figure 3 is the schematic diagram of the observation range of the multi-focal compound eye camera; Figure 4 is the flow chart of the adaptive relative pose estimation method based on the multi-focal compound eye camera. Detailed Embodiment

[0018] The present invention will be described in detail below with reference to the drawings and embodiments.

[0019] As Figure 1 shown, the cooperative target wide-range adaptive relative positioning method based on a multi-focal compound eye of the present invention includes the following steps: S10. Establish a coordinate system; The coordinate system includes an inertial coordinate system, a body coordinate system, a camera coordinate system, and a pixel coordinate system, and each coordinate system conforms to the right-hand rule; S20. Establish a single-camera model; Establish a single-camera model and determine the single-camera imaging equation; S30. Establish a multi-focal compound-eye camera imaging model; A multi-focal compound-eye camera is composed of several single cameras with different focal lengths. Establish a multi-focal compound-eye camera imaging model; obtain the coordinate transformation relationship of the pixel points of different single cameras corresponding to the same target point in the pixel coordinate system of the corresponding single camera; S40. Establish a motion model for relative positioning; Establish a motion model for relative positioning to describe the relative positioning motion of the target UAV in the view of the observing UAV; S50. Establish an adaptive observation model based on the multi-focal compound-eye camera; Set a target on the target UAV, and the target consists of target points; set several single cameras on the observing UAV to form a multi-focal compound-eye camera; establish an adaptive observation model based on the multi-focal compound-eye camera to find the pairing relationship between the target points and the single cameras; S60. Establish an adaptive relative pose estimation method based on the multi-focal compound-eye camera; According to the observed target image on the target UAV, calculate the relative position and attitude of the target UAV and the observing UAV, referred to as the relative pose, and improve the robustness of the adaptive relative pose estimation method through Kalman filtering.

[0020] Furthermore, the coordinate system in S10 is defined as follows: Inertial coordinate system , set on the horizontal ground, used to characterize the motion state of the UAV relative to the ground and determine the three-dimensional space coordinates of the UAV; Body coordinate system , fixedly connected to the UAV body, and the coordinate origin is located at the center of gravity of the UAV, points in the nose direction within the symmetry plane of the UAV, is perpendicular to within the symmetry plane of the UAV and points upward, and the direction is determined by the right-hand rule; Camera coordinate system , fixedly connected to the camera, and the coordinate origin is located at the optical center of the camera, is the optical axis direction of the camera, and are parallel to the axis and axis of the pixel coordinate system; several single cameras with different focal lengths form a multi-focal compound-eye camera, and each camera coordinate system in the multi-focal compound-eye camera is denoted as , is the total number of cameras; Pixel coordinate system , in pixels, with the coordinate origin located at the upper left corner of the image, pixel coordinates are dimensionless, and pixel coordinates are respectively the column number and row number of the pixel in the matrix.

[0021] Furthermore, the establishment of the single-camera model in S20 includes the following: A point in the inertial coordinate system , whose coordinates in the single-camera coordinate system are , and when projected onto the pixel coordinate system of the corresponding single camera, it is the point , and the point has pixel coordinates in the pixel coordinate system of , then the single-camera imaging equation is: ; where, is the camera internal parameter matrix, which consists of the camera focal length , the scaling factor corresponding to the pixel coordinates and and as well as the pixel coordinates of the image center and , and satisfies and , is the focal length equivalent factor in the x direction, is the focal length equivalent factor in the y direction; the camera internal parameter matrix is fixed after the camera leaves the factory and will not change during use. The camera internal parameter matrix is determined through camera calibration.

[0022] Furthermore, the establishment of the multi-focus compound eye camera imaging model in S30 includes the following: Install two single cameras with parallel optical axes on the observation UAV. The two single cameras with parallel optical axes form a multi-focus compound eye camera. The multi-focus compound eye camera provides image data from different perspectives as a complete sensor, processes the image data from different perspectives, and obtains the relative pose between the target UAV and the observation UAV; As Figure 2 shown, let the internal parameter matrices corresponding to the two single cameras be and , and the point on the target UAV has coordinates in the camera coordinate system of , and the pixel coordinates in the corresponding pixel coordinate system are ; in the camera coordinate system has coordinates of , the pixel coordinates in the corresponding pixel coordinate system are ; is an n-dimensional vector, is an n×n matrix; the offset between two single cameras is , then there is: ; Considering that the distance between the observation UAV and the target UAV is between 5m and 50m or even farther, compared with the mm-level distance between two single cameras, the offset can be ignored, then there is: ; The pixel coordinates of two single cameras are converted by the following formula to obtain the coordinate transformation relationship between two single cameras under the condition that the optical axes are parallel: .

[0023] Further, the establishment of the kinematic model for relative positioning in S40 includes the following content: Let represent the three-dimensional position vector of the UAV in the inertial coordinate system, is the position component of the axis, is the position component of the axis, is the position component of the axis, represent the three-dimensional velocity of the UAV in the inertial coordinate system, is the velocity component of the axis, is at axis velocity component, is the axis velocity component; using the quaternion represents the attitude of the UAV , is the scalar part of the rotation, are the projections of the rotation axis on the body coordinate system respectively; the three-dimensional angular velocity of the UAV in the body coordinate system is expressed as , is the angular velocity around the axis, is the angular velocity around the axis, is the angular velocity around the axis; then the motion model of the UAV is as follows: ; Among them, is the UAV velocity vector, is the acceleration vector; is the derivative of the UAV position vector, is the derivative of the UAV velocity vector; using quaternions to describe rotational motion, the UAV rotational kinematic equation is as follows: ; where, is the derivative of the quaternion; is the auxiliary matrix for quaternion differentiation, , in the form of: ; where the symbol is the operation symbol for converting a three-dimensional vector to a cross-product skew-symmetric matrix; The variables of the target UAV are denoted by the subscript tgt, and the body coordinate system of the target UAV is ; the variables of the observing UAV are denoted by the subscript ace, and the body coordinate system of the observing UAV is ; then the motion model of the target UAV relative to the observing UAV is: ; where, is the relative position, is the relative acceleration, is the relative velocity; is the derivative of the relative attitude represented by quaternions; is the relative attitude represented by quaternions; is the velocity of the target UAV in the inertial frame; is the velocity of the observing UAV in the inertial frame; is the acceleration of the target UAV in the inertial frame; is the acceleration of the observing UAV in the inertial frame; Taking the state variables of the relative motion of the target UAV relative to the observing UAV as the relative position in the inertial coordinate system, relative velocity and the quaternion representing the relative attitude , , that is: ; After discretization, the state variables of the target UAV at time are as follows, ; ; Then the discrete motion model of the target UAV at time is: ; Among them, the subscript represents the moment , and the subscript represents the moment before that moment; is the velocity measurement noise, ; is the attitude measurement noise, ; and are both zero-mean Gaussian white noises; is the sampling time; is the acceleration of the observed UAV at moment; Let the state noise be: ; Let the control input be as follows: ; Transcribe the discrete motion model of the target UAV at moment into matrix form , then: ; Among them, is the state matrix, is the identity matrix of the corresponding dimension, is the state variable of the target UAV at moment, is the angular velocity at moment.

[0024] Furthermore, the establishment of S50 is based on the adaptive observation model of the multi-focal compound eye camera, including the following: Let the observation vector be the relative pose calculated by the multi-focal compound eye camera, that is ; is the observation value at time k; establish the observation equation , where is the observation equation under ideal conditions; is the observation noise, assuming is white noise with zero mean, the covariance matrix of ; As Figure 3 shown, set target points on the target UAV; set single cameras on the observed UAV to form a multi-focal compound eye camera; through A target point assists a multi-focal compound eye camera to perform relative positioning of the target UAV and the observation UAV; the brute-force matching algorithm is used to find the pairing relationship between the target point and the single camera. The brute-force matching algorithm includes the following steps: S51. Select a single camera; Select the th single camera from the multi-focal compound eye camera; if the number of target points observed by the th single camera is greater than or equal to the number of target points on the target UAV , then continue to the next step, otherwise jump to S54; S52. Solve the pose; Select target points from the target points of the th single camera, and use the Perspective-n-Point algorithm (abbreviated as PnP) to solve the pose; S53. Perform validity judgment; Judge whether the current pose is valid according to the pre-set validity conditions. If it is valid, record the current pose and the reprojection error of the current pose; S54. Perform termination judgment; Let , and then judge whether is less than the number of single cameras . If it is less, jump to S51 to continue the calculation, otherwise end; select the target point with the smallest reprojection error and the corresponding single camera as the required 3D-2D matching point pair.

[0025] Furthermore, the calculation method of the reprojection error is as follows: The reprojection error of the target points on the th single camera is: Among them, is the transformation matrix from the body coordinate system of the target UAV to the camera coordinate system; is the weight coefficient of the th single camera, is the reprojection error of the th target point in the th single camera; Define as the augmented internal parameter matrix of the th single camera, is the internal parameter matrix of the th single camera; define the transformation matrix from the target UAV to the th single camera as , is a rotation matrix, and is a translation matrix; define and to be the homogeneous coordinates of the -th target point in the body coordinate system of the target UAV and in the camera coordinate system of the -th single camera; define the reprojection error of the -th target point in the camera coordinate system of the -th single camera as: wherein, is the depth scale factor of the -th single camera; Solving for the relative pose of the target UAV is equivalent to finding the that minimizes the reprojection error , expressed as follows: ; Single cameras with different focal lengths have the same reprojection error, but the corresponding real distance errors are different. Therefore, when optimizing the reprojection errors of all single cameras, set the corresponding weight coefficients for each single camera; at the same time, due to influencing factors including installation error and orientation error, the noises of each single camera are also different; Assume that the weight coefficient of the -th single camera in the multi-focal compound eye camera consists of two parts, namely ; wherein, is used to correct the error caused by the camera focal length of the -th single camera, taking ; is used to correct the error caused by influencing factors including installation error and orientation error, determined by statistics; is the focal length equivalent factor of the -th camera in the x direction, is the focal length equivalent factor of the -th camera in the y direction; Perform conditional optimization according to the number of target points of the target UAV observed by the observation UAV to achieve relative pose estimation when some target points cannot be observed; when 3 or more target points are observed, the process of solving Equation (18) remains unchanged, and directly solve using S50; when 2 target points are observed, perform position optimization, let , wherein, is the change in the camera pose, For the change of the translation part, the optimization of the translation part is retained in the first three dimensions, and the part representing rotation in the last three dimensions is ignored. Solve to make the reprojection error minimum ; When 1 target marking point is observed, then take , where and are the translation changes in the x and y directions respectively. Optimize the translation position of the camera to solve for the minimum , that is, both the depth and the pose remain unchanged, and the relative position is optimized within the plane.

[0026] Furthermore, as Figure 4 shown, the establishment of S60's adaptive relative pose estimation method based on a multi-focal compound eye camera includes the following: S61. Obtain the images of each single camera of the multi-focal compound eye camera; S62. Initialize; Obtain 3D-2D matching point pairs through a brute-force matching algorithm; S63. Adaptive observation; Perform adaptive observation on the relative position and pose of the target UAV according to the 3D-2D matching point pairs; S64. Update the filter through Kalman filtering to improve the robustness of the adaptive relative pose estimation method, and then perform filter prediction; ; where is the state at time k, which is predicted based on the information at time k-1; is the optimal estimate at time k-1; is the optimal estimate at time k; is the state transition matrix, is the state transition function; is the covariance matrix obtained at time k based on the information at time k-1; is the covariance matrix of the optimal state estimate at time k-1; is the covariance matrix of the optimal state estimate at time k; is the covariance matrix of the process noise; is the observation function; is the Kalman gain matrix; is the observation value at time k; is the observation matrix; is the covariance matrix of the process noise; S65. Complete the observation of the relative position and pose of the target UAV; Judge whether the maximum value of is less than a pre-set threshold. If it exceeds the threshold, it indicates that the Kalman filter result diverges. At this time, stop the filter update, return to S62, and re-initialize until

[0027] the maximum value of is less than the pre-set threshold, and the relative position and attitude of the target UAV are observed. Each single camera of the multi-focal compound eye camera on the observation UAV obtains the infrared image of the circular target on the target UAV. Step 1: Obtain the images of each single camera of the multi-focal compound eye camera; Process the infrared images of each single camera, extract the position coordinates of the infrared images, and solve the 3D-2D correspondence between the infrared images of each single camera and the target points of the target UAV through the brute-force matching algorithm; Step 3: Adaptive observation; Solve to obtain the relative position and attitude between the target UAV and the observation UAV; Step 4: Update the filter through Kalman filtering to improve the robustness of the adaptive relative pose estimation method, and then perform filter prediction; Step 5: Judge whether the maximum value of is less than the pre-set threshold. If it exceeds the threshold, it indicates that the Kalman filter result diverges. At this time, stop the filter update, return to Step 2, and re-initialize until

[0028] Although the embodiments of the present invention have been disclosed as above, they are not limited to the applications listed in the specification and embodiments. For those skilled in the art, without departing from the principle of the present invention, all the features disclosed in the present invention, or all the steps in the disclosed methods or processes, except for mutually exclusive features and / or steps, can be combined in any way. The present invention is not limited to the specific details and the examples shown and described here.

Claims

1. A wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye, characterized in that, It includes the following steps: S10. Establish a coordinate system; The coordinate system includes an inertial coordinate system, a body coordinate system, a camera coordinate system, and a pixel coordinate system, and each coordinate system conforms to the right-hand rule; S20. Establish a single-camera model; Establish a single-camera model and determine the single-camera imaging equation; S30. Establish a multi-focus compound eye camera imaging model; A multi-focus compound eye camera is composed of several single cameras with different focal lengths. Establish a multi-focus compound eye camera imaging model; obtain the coordinate conversion relationship of the pixel points of different single cameras corresponding to the same target point in the pixel coordinate systems of the corresponding single cameras; S40. Establish a kinematic model for relative positioning; Establish a kinematic model for relative positioning, which is used to describe the relative positioning movement of the target UAV from the perspective of the observing UAV; S50. Establish an adaptive observation model based on the multi-focus compound eye camera; Set a target on the target UAV, and the target consists of target points; set a number of single cameras on the observation UAV to form a multi-focal compound eye camera; Establish an adaptive observation model based on the multi-focus compound eye camera and find the pairing relationship between the target point and the single camera; S60. Establish an adaptive relative pose estimation method based on the multi-focus compound eye camera; According to the target image observed on the target UAV, calculate the relative position and attitude of the target UAV and the observing UAV, referred to as the relative pose, and improve the robustness of the adaptive relative pose estimation method through Kalman filtering.

2. The wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye according to claim 1, characterized in that, The coordinate system in S10 is defined as follows: Inertial coordinate system , which is set on the horizontal ground, used to characterize the motion state of the drone relative to the ground and determine the three-dimensional spatial coordinates of the drone; Body coordinate system , fixedly connected to the UAV body, with the coordinate origin located at the center of gravity of the UAV, pointing in the nose direction within the symmetry plane of the UAV, perpendicular to upward within the symmetry plane of the UAV, and the direction is determined by the right-hand rule; Camera coordinate system , fixedly attached to the camera, with the coordinate origin located at the optical center of the camera, being the optical axis direction of the camera, and being parallel to the axis and the axis of the pixel coordinate system; A multi-focal compound eye camera is composed of several cameras with different focal lengths. Each camera coordinate system in the multi-focal compound eye camera is denoted as , being the total number of cameras; Pixel coordinate system , in pixels, with the coordinate origin located at the upper left corner of the image. Pixel coordinates are dimensionless, and the pixel coordinates are respectively the column number and row number of the pixel in the matrix.

3. The wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye according to claim 2, characterized in that, The establishment of the single-camera model in S20 includes the following contents: Point in the inertial coordinate system , the coordinates in the single-camera coordinate system are , and when projected onto the pixel coordinate system of the corresponding single camera, it is the point , the point has coordinates in the pixel coordinate system of , then the single-camera imaging equation is: ； Among them, is the camera intrinsic matrix, which consists of the camera focal length , the scaling factor corresponding to the pixel coordinates , and and the pixel coordinates of the image center and , and satisfies and . is the focal length equivalent factor in the x direction, is the focal length equivalent factor in the y direction; the camera intrinsic matrix is fixed after the camera leaves the factory and will not change during use. The camera intrinsic matrix is determined by camera calibration.

4. The wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye according to claim 3, characterized in that, The establishment of the multi-focus compound eye camera imaging model in S30 includes the following contents: Install two single cameras with parallel optical axes on the observing UAV. The two single cameras with parallel optical axes form a multi-focus compound eye camera. The multi-focus compound eye camera provides image data from different perspectives as a complete sensor, processes the image data from different perspectives, and obtains the relative pose between the target UAV and the observing UAV; Let the internal parameter matrices corresponding to the two single cameras be and , the coordinates of the point on the target UAV in the camera coordinate system are , and the pixel coordinates in the corresponding pixel coordinate system are ; the coordinates in the camera coordinate system are , and the pixel coordinates in the corresponding pixel coordinate system are ; is an n-dimensional vector, is an n×n matrix; the offset between the two single cameras is , then there is: ； Considering that the distance between the observation UAV and the target UAV is between 5m and 50m or even farther, compared with the distance of mm level between two single cameras, the offset can be ignored, so there is: ； The pixel coordinates of the two single cameras are converted by the following formula to obtain the coordinate conversion relationship of the two single cameras under the condition of parallel optical axes: 。 5. The wide-range adaptive relative positioning method for cooperative targets based on a multi-focal compound eye according to claim 4, characterized in that,The establishment of the kinematic model for relative positioning in S40 includes the following contents: Let represent the three - dimensional position vector of the UAV in the inertial coordinate system, be the position component along the axis, be the position component along the axis, be the position component along the axis; Let represent the three - dimensional velocity of the UAV in the inertial coordinate system, be the velocity component along the axis, represent the attitude of the UAV using the quaternion , be the scalar part of the rotation, be the projections of the rotation axis on the body coordinate system respectively; The three - dimensional angular velocity of the UAV in the body coordinate system is represented as , be the angular velocity about the axis, be the angular velocity about the axis, be the angular velocity about the axis; Then the motion model of the UAV is as follows: ； Among them, is the UAV speed vector, is the acceleration vector; is the derivative of the UAV position vector, is the derivative of the UAV speed vector; Using quaternions to describe rotational motion, the rotational kinematic equation of the UAV is as follows: ； wherein, is the derivative of the quaternion; is the auxiliary matrix for quaternion derivative, in the form of: ； Among them, the symbol is the operation symbol for converting a three-dimensional vector into a cross-product skew-symmetric matrix; The variables of the target UAV are denoted by the subscript tgt, and the body coordinate system of the target UAV is ; The variables of the observing UAV are denoted by the subscript ace, and the body coordinate system of the observing UAV is ; Then the motion model of the target UAV relative to the observing UAV is: ； Among them, is the relative position, is the relative acceleration, is the relative velocity; is the derivative of the relative attitude represented by quaternion; is the relative attitude represented by quaternion; is the velocity of the target UAV in the inertial frame; is the velocity of the observing UAV in the inertial frame; is the acceleration of the target UAV in the inertial frame; is the acceleration of the observing UAV in the inertial frame; Obtain the state variables of the relative motion of the target UAV with respect to the observation UAV which are the relative positions in the inertial coordinate system , the relative velocities and the quaternions representing the relative attitudes , namely: That is: ； After discretization, the state variables of the target UAV at time are as follows: ; ; ； Then the discrete motion model of the target UAV at time is as follows: ； Among them, the subscript represents the moment , and the subscript represents the moment preceding that moment; is the velocity measurement noise, ; is the attitude measurement noise, ; and are both zero-mean Gaussian white noises; is the sampling time; is the acceleration of the observed UAV at the moment ; Let the state noise be ； Let the control input be as follows: ； Transcribe the discrete motion model of the target UAV at time into matrix form , then: ； Among them, is the state matrix, is the identity matrix of the corresponding dimension, is the state variable of the target UAV at moment, is the angular velocity at moment.

6. The cooperative target wide-range adaptive relative positioning method based on a multi-focal length compound eye according to claim 5, wherein, The establishment of the adaptive observation model based on the multi-focus compound eye camera in S50 includes the following contents: Let the observation vector be the relative pose solved by the multi-focus compound eye camera, that is ； is the observed value at time k; establish the observation equation , where is the observation equation under ideal conditions; is the observation noise, assuming is white noise with zero mean, the covariance matrix of is ; Set on the target UAV target points; Set on the observation UAV single cameras to form a multi-focal compound eye camera; Through target points to assist the multi-focal compound eye camera in relative positioning of the target UAV and the observation UAV; Find the pairing relationship between the target points and the single cameras through a brute-force matching algorithm, and the brute-force matching algorithm includes the following steps: S51. Select a single camera; Select the th single camera from the multi-focal compound eye camera; if the number of target points observed by the th single camera is greater than or equal to the number of target points on the target UAV, then proceed to the next step; otherwise, jump to S54; S52. Solve the pose; Select from the target points of a single camera to select target points, and use the Perspective-n-Point algorithm (abbreviated as PnP) to solve the pose; S53. Perform validity judgment; Judge whether the current pose is valid according to the preset validity condition. If it is valid, record the current pose and the reprojection error of the current pose; S54. Perform termination judgment; Let , and then judge is less than the number of single cameras . If it is less, jump to S51 to continue the calculation; otherwise, end. Select the target point with the smallest reprojection error and the corresponding single camera as the required 3D-2D matching point pair.

7. The cooperative target wide-range adaptive relative positioning method based on a multi-focal compound eye according to claim 6, characterized in that The calculation method of the reprojection error is as follows: The reprojection error of a target point on a single camera is: ； Among them, is the transformation matrix from the body coordinate system of the target UAV to the camera coordinate system; is the weight coefficient of the th single camera, and is the reprojection error of the th target point in the th single camera; Definition is the th intrinsic parameter matrix after augmentation of the single camera, is the th intrinsic parameter matrix of the single camera; define the transformation matrix from the target UAV to the th single camera as , is the rotation matrix, is the translation matrix; define and as the homogeneous coordinates of the th target point in the body coordinate system of the target UAV and the camera coordinate system of the th single camera respectively; define the reprojection error of the th target point in the camera coordinate system of the th single camera as: ； Among them, is the depth scale factor of the th single camera; Solving for the relative pose of the target UAV is equivalent to finding the one that minimizes the , which is expressed as follows: ； Single cameras with different focal lengths have the same reprojection error, but the corresponding real distance errors are different. Therefore, when optimizing the reprojection errors of all single cameras, set corresponding weight coefficients for each single camera; at the same time, due to influencing factors including installation error and orientation error, the noises of each single camera are also different; Assume that the weight coefficient of the th single camera in the multi-focal compound eye camera consists of two parts, namely ; among them, is used to correct the error caused by the camera focal length of the th single camera, and take ; is used to correct the error caused by influencing factors including installation error and orientation error, which is determined by statistics; is the focal length equivalent factor in the x direction of the th camera, is the focal length equivalent factor in the y direction of the th camera; Conditionally optimize according to the number of target points of the target UAV observed by the observation UAV to achieve relative pose estimation when some target points cannot be observed; when 3 or more target points are observed, the process of solving Equation (18) remains unchanged, and S50 is directly used for solving; when 2 target points are observed, position optimization is performed, and let , where is the change in camera pose, is the change in the translational part. Keep the first three dimensions to represent the optimization of the translational part and ignore the last three dimensions representing the rotational part. Solve for the that minimizes the reprojection error ; when 1 target point is observed, then take , where and are the translational changes in the x and y directions respectively. Optimize the translational position of the camera to solve for the that minimizes the reprojection error , that is, both the depth and the pose remain unchanged, and the relative position is optimized in the plane.

8. The cooperative target wide-range adaptive relative positioning method based on a multi-focal compound eye according to claim 7, characterized in that The establishment of the adaptive relative pose estimation method based on the multi-focus compound eye camera in S60 includes the following contents: S61. Obtain the images of each single camera of the multi-focus compound eye camera; S62. Initialize; Obtain 3D-2D matching point pairs through the brute-force matching algorithm; S63. Adaptive observation; Adaptively observe the relative position and attitude of the target UAV based on 3D-2D matching point pairs; S64. Update the filter through Kalman filtering to improve the robustness of the adaptive relative pose estimation method, and then perform filter prediction; ； Among them, is the state at time k, which is predicted based on the information at time k - 1; is the optimal estimate at time k - 1; is the optimal estimate at time k; is the state transition matrix, is the state transition function; is the covariance matrix obtained based on the information at time k - 1 at time k; is the covariance matrix of the optimal state estimate at time k - 1; is the covariance matrix of the optimal state estimate at time k; is the covariance matrix of the process noise; is the observation function; is the Kalman gain matrix; is the observation value at time k; is the observation matrix; is the covariance matrix of the process noise; S65. Complete the observation of the relative position and attitude of the target UAV; Determine whether the maximum value is less than a pre-set threshold. If it exceeds the threshold, it indicates that the Kalman filter result diverges. At this time, stop the filter update, return to S62, and re-initialize until the maximum value is less than the pre-set threshold, and the relative position and attitude of the target UAV are observed.

Images