A method for calculating the posture of an unmanned aerial vehicle, an electronic device and a storage medium

Abstract

The present invention provides a method for calculating the posture of a drone, comprising the following steps: step 1, extracting feature points in a satellite map that match the drone aerial image; step 2, establishing a solution equation for the posture of the drone and the height of the feature points according to the matching feature points and the coordinate system transformation relationship, so as to obtain the posture of the drone and the height of the ground feature points. The present invention can calculate the posture of an aircraft according to a single aerial image, and at the same time solve the height of the feature points on the satellite map, thereby eliminating the influence of different heights of the feature points on the satellite map on the accuracy of the drone posture estimation.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicles, and in particular to a method for calculating the position and posture of an unmanned aerial vehicle, an electronic device and a storage medium. Background Art

[0002] Drone visual positioning is a technology that uses image data from an onboard camera to locate the drone. This technology is extremely important for the safe operation of drones when GPS fails. The drone's visual positioning system continuously recursively calculates the current position of the aircraft during flight, but the position obtained by this recursion will produce cumulative errors over time. In order to eliminate the cumulative error, calibration is required at regular intervals, that is, calculating the absolute position of the aircraft in the world coordinate system.

[0003] At present, one method to calculate the absolute position of an aircraft is to match the feature points between the aircraft observation image and the satellite map, and then solve the aircraft position based on the matching relationship. Satellite maps can only obtain the latitude and longitude coordinates of the feature points, but not the height of the feature points. When solving the aircraft position, the traditional method assumes that all feature points are at the same height. When the aircraft is flying at a high altitude, the height difference of points on the ground can be ignored, but when the aircraft is flying at a low altitude, the height difference of feature points on the ground is more obvious (protruding objects such as buildings and mountains or undulating terrain block the picture of the aircraft observation image). Highly consistent assumptions will cause large errors in the position solution of the drone. The highly consistent assumption is: all objects on the ground are assumed to be on the same horizontal plane and are two-dimensional. Summary of the invention

[0004] In view of the problems existing in the prior art, a method for calculating the posture of a UAV, an electronic device and a storage medium are provided. When solving the position of the aircraft based on the matching of the satellite map and the feature points of the aerial image, the height of the feature points on the satellite map can be solved at the same time, thereby eliminating the influence of the different heights of the feature points on the satellite map on the accuracy of the UAV posture estimation. This solution is mainly aimed at the situation where the UAV is flying at a low altitude, at which time the different heights of the feature points have a greater impact on the estimation of the UAV posture.

[0005] The technical solution adopted by the present invention is as follows: A method for calculating the posture of an unmanned aerial vehicle comprises the following steps:

[0006] Step 1: Extract feature points in the satellite map that match the drone aerial image;

[0007] Step 2: According to the matching feature points and the coordinate system transformation relationship, the equation for solving the drone's posture and feature point height is established, and the drone's posture and feature point height can be solved;

[0008] Among them, the process of establishing the solution equation in step 2 is: transforming the coordinates of the feature points in the satellite map to the geocentric rectangular coordinate system and the northeast celestial coordinate system in turn, and then transforming the coordinates to the camera coordinate system through the relative transformation relationship from the northeast celestial coordinate system to the camera coordinate system; projecting the feature points in the drone aerial image onto the depth normalized plane of the camera coordinate system, and the line connecting the projection point and the camera optical center forms a three-dimensional direction vector, and two direction vectors r and s orthogonal to the three-dimensional direction vector are obtained in the null space; the direction vector formed by the line connecting the feature point in the satellite map under the camera coordinate system and the camera optical center satisfies that the inner product with the two orthogonal direction vectors r and s is zero, thereby establishing the solution equation.

[0009] Furthermore, in step 2, the northeast celestial coordinate system is established with the aircraft take-off point as the station.

[0010] Furthermore, in step 2, the coordinates of the feature points in the satellite map in the northeast sky coordinate system are:

[0011]

[0012] in, is the coordinate of the feature point in the satellite map in the geocentric rectangular coordinate system, is the feature point p i The latitude and longitude coordinates obtained from the satellite map, the latitude and longitude coordinates Calculated from the pixel coordinates of the satellite map, is the height of the feature point on the ground, which is the variable to be solved; N is The height of the Earth's reference ellipsoid at , a and b are the major and minor axes of the Earth's longitude tangent plane ellipse respectively; R enu and t enu It represents the relative transformation relationship from the geocentric rectangular coordinate system to the northeast celestial coordinate system, which is calculated from the longitude and latitude coordinates of the take-off point and expressed as:

[0013]

[0014] Furthermore, in step 2, the relative transformation relationship from the northeast sky coordinate system to the camera coordinate system is:

[0015]

[0016] Among them, θ yam is the heading angle of the camera, obtained by the magnetometer of the drone.

[0017] Furthermore, in step 2, the coordinates of the feature points in the satellite map in the northeast sky coordinate system are Transformed to the camera coordinate system:

[0018]

[0019] in, is the position of the camera.

[0020] Furthermore, in step 2, the feature point p in the aerial image and the feature point p in the satellite map i Matching feature points q i , the three-dimensional vector is obtained by transforming the camera's intrinsic parameter matrix to the depth normalized image plane in the camera coordinate system:

[0021]

[0022] Among them, K -1 is the inverse matrix of the internal parameter matrix; the direction vectors r and s are obtained in the null space so that satisfy:

[0023] Furthermore, in step 2, the equation to be solved is:

[0024]

[0025] Furthermore, in the solution process, there are n pairs of matching feature points. One of the points is randomly selected, and its height is set to 0 as the benchmark for the aircraft's flight altitude and the heights of other feature points. The heights of the remaining n-1 feature points are calculated. In solving the system of equations, the unknown quantities include the three-degree-of-freedom relative translation of the camera, the heights of the n-1 feature points relative to the benchmark point, and a total of n+2 unknown quantities. Each pair of matching feature points provides two equality constraints, and the equations satisfy the constraints: 2*(n-1)≥n+2, that is, n≥4, which means that the solution can be performed if the number of matching feature points is greater than or equal to 4. When it is greater than 4, the least squares method is used to solve the linear equation system.

[0026] The present invention also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program corresponding to the above-mentioned drone posture calculation method that can be loaded and executed by the processor.

[0027] The present invention also provides a computer-readable storage medium on which computer program instructions are stored, wherein the program instructions are used to implement the process corresponding to the above-mentioned drone posture calculation method when executed by a processor.

[0028] Compared with the prior art, the beneficial effect of adopting the above technical solution is that the present invention can calculate the aircraft posture according to a single aerial image, and at the same time solve the height of the feature points on the satellite map, thereby eliminating the influence of the different heights of the feature points on the satellite map on the accuracy of the drone posture estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1This is a flow chart of the UAV posture calculation method proposed in the present invention. DETAILED DESCRIPTION

[0030] Embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar modules or modules with the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be construed as limitations on the present application. On the contrary, the embodiments of the present application include all changes, modifications and equivalents that fall within the spirit and connotation of the appended claims.

[0031] Example 1

[0032] like Figure 1 As shown, this embodiment proposes a method for calculating the posture of a UAV. Based on the matching feature points of satellite maps and aerial images, when establishing the solution equation, the height of the feature points on the map is also used as a variable and solved together with the aircraft position, thereby eliminating the influence of different feature point heights on the accuracy of UAV posture estimation. The specific scheme is as follows:

[0033] A method for calculating the position and posture of a drone comprises the following steps:

[0034] Step 1: Extract feature points in the satellite map that match the drone aerial image;

[0035] Step 2: According to the matching feature points and the coordinate system transformation relationship, the equation for solving the drone's posture and feature point height is established, and the drone's posture and feature point height can be solved;

[0036] Among them, the process of establishing the solution equation in step 2 is: transforming the coordinates of the feature points in the satellite map to the geocentric rectangular coordinate system and the northeast celestial coordinate system in turn, and then transforming the coordinates to the camera coordinate system through the relative transformation relationship from the northeast celestial coordinate system to the camera coordinate system; projecting the feature points in the drone aerial image onto the depth normalized plane of the camera coordinate system, and the line connecting the projection point and the camera optical center forms a three-dimensional direction vector, and two direction vectors r and s orthogonal to the three-dimensional direction vector are obtained in the null space; the direction vector formed by the line connecting the feature point in the satellite map under the camera coordinate system and the camera optical center satisfies that the inner product with the two orthogonal direction vectors r and s is zero, thereby establishing the solution equation.

[0037] Specifically, for each feature point p on the satellite map i , its latitude and longitude coordinates can be calculated based on its pixel coordinates on the satellite map p i The height of is unknown, so we assume that The coordinates at this time become longitude and latitude high coordinates Transform it to the geocentric rectangular coordinate system and get p i The geocentric coordinates of:

[0038]

[0039] Among them, N is The height of the Earth's reference ellipsoid at , a and b are the major and minor axes of the Earth's longitude tangent plane ellipse respectively.

[0040] At the same time, the northeast sky coordinate system is established with the take-off point of the drone as the station. i Transform from the geocentric rectangular coordinate system to the northeast celestial coordinate system:

[0041]

[0042] Among them, R enu and t enu The relative transformation relationship between the geocentric rectangular coordinate system and the northeast celestial coordinate system is calculated by the longitude and latitude coordinates of the take-off point and is used as a known quantity in this embodiment.

[0043] In this embodiment, let R enu and t enu They are:

[0044]

[0045] but It can be expressed as:

[0046]

[0047] Assuming that the drone camera is facing the center of the earth, the camera's heading angle θ yam It can be read by the drone's gyroscope to obtain the relative transformation relationship from the northeast sky coordinate system to the camera coordinate system:

[0048]

[0049] So that we can Transform to the camera coordinate system and obtain the direction vector formed by the line connecting the feature point in the satellite map and the camera optical center in the camera coordinate system:

[0050]

[0051] The position of the camera is the unknown quantity to be solved.

[0052] In the aerial image taken by the drone camera, there is a feature point p i Matching feature points q i , where p iis a feature point on the satellite map, q i It is a feature point on the aerial image. The two feature points are a pair of matching feature points. The matching feature points are calculated by the feature point matching algorithm based on the feature point descriptors of the two images.

[0053] The feature points in the drone aerial image are projected onto the depth normalized plane of the camera coordinate system. The line connecting the projection point and the camera optical center forms a three-dimensional direction vector, and the result is:

[0054]

[0055] Among them, K -1 is the inverse matrix of the internal parameter matrix, which is a known quantity in this embodiment.

[0056] is a three-dimensional vector. In its null space, there are two orthogonal direction vectors (r and s), which are recorded as: Among them, r and s can be calculated by singular value decomposition.

[0057] When the drone camera posture is correct, it needs to meet The inner product with r and s is zero, resulting in two linear equations:

[0058]

[0059] There are only two unknown quantities in the expression and i∈[1,n-1], substitute the known parameters to complete the solution.

[0060] 1. In this embodiment, during the solution process, there are n pairs of matching feature points. One of the points is randomly selected, and its height is assumed to be 0 as a reference for the aircraft flight altitude and the heights of other feature points. Therefore, only the heights of n-1 feature points need to be estimated. In solving the system of equations, the unknown quantities include the three-degree-of-freedom relative translation of the camera, the heights of the n-1 feature points relative to the reference point, and a total of n+2 unknown quantities. Each pair of matching feature points provides two equality constraints, and the system of equations satisfies the constraints: 2*(n-1)≥n+2, that is, n≥4, which means that the solution can be performed when the number of matching feature points is greater than or equal to 4. When it is greater than 4, the least squares method is used to solve the linear system of equations.

[0061] The method proposed in the present invention can relatively accurately calculate the position of the aircraft and the relative height of the feature points on the ground based on the matching results of the feature points of a single aerial image and a satellite map.

[0062] Example 2

[0063] This embodiment also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program corresponding to the drone posture calculation method described in Embodiment 1 that can be loaded by the processor and executed.

[0064] Example 3

[0065] This embodiment also provides a computer-readable storage medium on which computer program instructions are stored, wherein the program instructions, when executed by a processor, are used to implement the process corresponding to the drone posture calculation method described in Example 1.

[0066] It should be noted that in the description of the embodiments of the present invention, it should also be noted that, unless otherwise clearly specified and limited, the terms "setting" and "connection" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a direct connection, or an indirect connection through an intermediate medium. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances; the drawings in the embodiments are used to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings herein can be arranged and designed in various different configurations.

[0067] Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limitations on the present application. Ordinary technicians in this field can change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A method for calculating the posture of a drone. It is characterized in that The following steps are involved: Step 1: Extract feature points in the satellite map that match the drone aerial image; Step 2: According to the matching feature points and the coordinate system transformation relationship, the equation for solving the drone's posture and feature point height is established, and the drone's posture and ground feature point height can be solved; Among them, the process of establishing the solution equation in step 2 is: transforming the coordinates of the feature points in the satellite map to the geocentric rectangular coordinate system and the northeast celestial coordinate system in turn, and then transforming the coordinates to the camera coordinate system through the relative transformation relationship from the northeast celestial coordinate system to the camera coordinate system; projecting the feature points in the drone aerial image onto the depth normalized plane of the camera coordinate system, and the line connecting the projection point and the camera optical center forms a three-dimensional direction vector, and two direction vectors r and s orthogonal to the three-dimensional direction vector are obtained in the null space; the direction vector formed by the line connecting the feature point in the satellite map under the camera coordinate system and the camera optical center satisfies that the inner product with the two orthogonal direction vectors r and s is zero, thereby establishing the solution equation; In step 2, the coordinates of the feature points in the satellite map under the northeast sky coordinate system are: in, is the coordinate of the feature point in the satellite map in the geocentric rectangular coordinate system, is the feature point p i The latitude and longitude coordinates obtained from the satellite map, the latitude and longitude coordinates Calculated from the pixel coordinates of the satellite map, is the height of the feature point on the ground, which is the variable to be solved; N is The height of the Earth's reference ellipsoid at , a and b are the major and minor axes of the Earth's longitude tangent plane ellipse respectively; R enu and t enu The relative transformation relationship between the geocentric rectangular coordinate system and the northeast celestial coordinate system is calculated from the longitude and latitude coordinates of the take-off point and is expressed as: 。 2. The method for calculating the position and posture of a drone according to claim 1, It is characterized in that In step 2, the northeast celestial coordinate system is established with the aircraft take-off point as the station.

3. The method for calculating the position and posture of a drone according to claim 1, It is characterized in that In step 2, the relative transformation relationship from the northeast sky coordinate system to the camera coordinate system is: Among them, θ yam is the heading angle of the camera, obtained by the magnetometer of the drone.

4. The method for calculating the position and posture of a drone according to claim 3, It is characterized in that In step 2, the coordinates of the feature points in the satellite map under the northeast sky coordinate system Transform to the camera coordinate system and get: in, is the position of the camera, which is the variable to be solved.

5. The method for calculating the position and posture of a drone according to claim 4, It is characterized in that In step 2, the feature point p in the aerial image and the feature point p in the satellite map i Matching feature points q i , transformed to the depth normalized image plane in the camera coordinate system through the camera's intrinsic parameter matrix, we get: Among them, K -1 is the inverse matrix of the internal parameter matrix; In the null space of , we get two direction vectors r and s that are orthogonal to it:

6. The method for calculating the position and posture of a drone according to claim 5, It is characterized in that In step 2, the equation to be solved is: 。 7. The method for calculating the position and posture of a drone according to claim 1, It is characterized in that In the solution process, there are n pairs of matching feature points. One of the points is randomly selected and its height is set to 0 as the benchmark for the aircraft flight altitude and the height of other feature points. The height of the remaining n-1 feature points is calculated. In solving the system of equations, the unknown quantities include the three-degree-of-freedom relative translation of the camera, the height of the n-1 feature points relative to the benchmark point, and a total of n+2 unknown quantities. Each pair of matching feature points provides two equality constraints, and the system of equations satisfies the constraints: 2*(n-1)≥n+2, that is, n≥4, which means that the solution can be performed when the number of matching feature points is greater than or equal to 4. When it is greater than 4, the least squares method is used to solve the linear system of equations.

8. An electronic device, It is characterized in that It includes a memory and a processor, wherein the memory stores a computer program that can be loaded by the processor and executes the method for calculating the posture of a drone as described in any one of claims 1 to 7.

9. A computer-readable storage medium having computer program instructions stored thereon, in, When the program instructions are executed by the processor, they are used to implement the process corresponding to the drone posture calculation method described in any one of claims 1-7.

Images