CN112184812B - Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system - Google Patents
Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system Download PDFInfo
- Publication number
- CN112184812B CN112184812B CN202011008679.0A CN202011008679A CN112184812B CN 112184812 B CN112184812 B CN 112184812B CN 202011008679 A CN202011008679 A CN 202011008679A CN 112184812 B CN112184812 B CN 112184812B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- camera
- positioning
- april
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Processing (AREA)
Abstract
The invention provides a method for improving the identification and positioning precision of an unmanned aerial vehicle camera to an april tag, which comprises the steps of calibrating the camera; fusing the multiple april tag space point information; adding a Gaussian Newton iteration to solve the camera gesture after using PnP; and optimizing a transformation matrix between april tags by using a pose diagram optimization theory. The invention also provides an unmanned aerial vehicle positioning system and method, the positioning process is simple and easy to operate, and the precision improving method is reasonably applied. The invention can realize high-precision positioning of the unmanned aerial vehicle under the condition of weaker indoor gps signals, and positioning is only needed to be performed through the two-dimensional code array, so that the positioning is accurate, the cost is low, the performance is reliable, and the environmental interference is small.
Description
Technical Field
The invention relates to the field of unmanned aerial vehicle positioning, in particular to a method and a system for improving the accuracy of the unmanned aerial vehicle camera to the april tag identification positioning.
Background
Indoor positioning is an important problem in the fields of AR, VR, robot and the like, and is a precondition for indoor autonomous navigation. Although the GPS can solve the positioning problem, in indoor positioning, the signal is weak, so that the accuracy cannot meet the requirement, and even positioning cannot be performed, so that other positioning technologies are required to be adopted for indoor positioning. In order to solve the problem of indoor positioning, schemes such as UWB positioning, WIFI positioning, bluetooth positioning, SLAM positioning and the like have been presented.
However, the above solutions have large technical limitations or lack of economical practicability:
1. the positioning based on Bluetooth is greatly disturbed by the environment and cannot shade the equipment;
2. the positioning scheme based on UWB is applied to unmanned aerial vehicle, but at least 3 base stations are needed, signal propagation is blocked by barriers such as walls, ceilings, doors, people and the like, signals are reflected, refracted and diffracted, transmitted signals reach a UWB receiving end through different time and different paths, and finally UWB precision is reduced, and more complex algorithms are needed for filtering and optimizing;
3. the positioning scheme based on the visual SLAM occupies a large amount of resources of a CPU in the running process, needs a stronger CPU to run in real time, has high equipment cost and high technical difficulty, needs abundant environmental texture features, and does not have particularly abundant texture features of the overlooking view angle of the indoor unmanned aerial vehicle, so that the general SLAM algorithm is not completely suitable for the overlooking view angle of the unmanned aerial vehicle, and the positioning track is easy to drift;
4. the three-dimensional laser radar is adopted for drawing and positioning, so that the cost is high, the weight is large, and the requirement on the unmanned aerial vehicle is extremely high.
Disclosure of Invention
The invention aims to overcome at least one defect of the prior art, and provides a method and a system for improving the identification and positioning accuracy of an unmanned aerial vehicle camera to april tag, which are used for solving the problems that the indoor unmanned aerial vehicle positioning is greatly interfered by the environment, the positioning accuracy is low, the CPU occupies large resources and the cost is high.
The technical scheme adopted by the invention is that the method for improving the identification and positioning precision of the unmanned aerial vehicle camera to the april tag comprises the following steps:
s1, calibrating a camera on an unmanned aerial vehicle, and calculating to obtain internal parameters and distortion coefficients of the camera;
s2, acquiring an april tag image by the camera on the basis of acquiring internal parameters and distortion coefficients of the camera, identifying space point information from the acquired april tag image, and fusing a plurality of april tag space point information;
s3, after the information of a plurality of space points is obtained, solving the pose of the camera under the world coordinate system by adopting PnP in OpenCV, and after solving the pose by adopting PnP, adding a Gaussian Newton method to iterate and optimize the pose;
and S4, optimizing a transformation matrix between the april Tags by using a pose diagram optimization theory.
In the step S1, calibration is performed on the unmanned aerial vehicle camera, in the field of machine vision, calibration of the camera is a key link, accuracy of a calibration result and stability of an algorithm directly affect accuracy of a result generated by working of the camera, whether the machine vision system can effectively position and calculate a target object is determined, and therefore, calibration of the camera is performed on the premise that follow-up work is performed. In the step S2, since a picture may identify a plurality of april tag information, only one april tag information is adopted with low reliability, so that the precision can be improved by fusing a plurality of april tag space point information. In the step S3, after solving the gesture by using PnP, a further iteration of the gaussian newton method is added, so that the gesture calculation error can be further reduced. In the step S4, the transformation matrix between the april tags after the pose chart optimization can be basically used for indoor positioning of the unmanned aerial vehicle, and the positioning accuracy is high.
Further, the process of solving the camera internal parameters in the step S1 is as follows: according to the imaging principle of a pinhole camera model, a 3D point P (X, Y, Z) under a camera coordinate system, a projection point P1 of the point P1 under a pixel coordinate system has coordinates of (u, v), and the corresponding relation between the two points is that
Wherein K is a camera reference matrix,for x-axis focal length, +.>For y-axis focal length, +.>For x-axis translation, +.>Is the y-axis translation;
the distortion coefficient solution can be divided into radial distortion and tangential distortion, and the solving formula mathematical model is as follows:
wherein (x, y) is the coordinates of the P point in the normalized plane,and->For post-distortion coordinates, r is the (x, y) to origin distance, +.>、/>、/>Is a radial distortion coefficient>、/>Is the tangential distortion coefficient.
Further, in the step S2, a plurality of april tag space point information is fused, and the specific process is to use the euclidean distance between the center of april tag and the center of the image as the credibility weight, fuse two april tag space point information, and use the formula:
wherein the method comprises the steps ofFor the fused spatial point +.>、/>Two AprilTag corresponding spatial points, respectively->、/>Is the euclidean distance of the center pixel of two different april tags from the center of the image.
Further, the specific process of the step S3 is to solve the BA (Bundle Adjustment) problem by using a Levenberg-Marquardt algorithm in a nonlinear optimization algorithm, and the formula is as follows:
in the formulaFor 2*6 Jacobian matrix, +.>As an error function, I is an identity matrix,>is->Transpose of matrix, form as follows:
wherein the method comprises the steps ofIs the spatial point coordinate under the camera coordinate system, < >>For transforming matrix->In special European style groupDisturbance of the lower part->Is the pixel coordinates. />、/>For the distortion coefficient +>For x-axis focal length, +.>For y-axis focal length, +.>For x-axis translation, +.>And r is the distance between the pixel coordinate and the origin in the normalized coordinate system.
The Levenberg-Marquardt algorithm is as follows:
s31, giving an initial transformation matrixDamping factor->；
S32, for the nth iteration, solving an error functionAt->The Jacobian matrix is used for obtaining the increment according to the formula>Order-making
；
S33, calculating a damping factor updating scale factor；
S34、，/>Accept this update->；
S35、，/>Refusing the update->；
S36, judging whether the algorithm converges, if so, ending iteration, otherwise, returning to S32;
wherein the method comprises the steps ofThe scale factor is updated for the damping factor, expressed as
Further, in the step 4, the specific process of optimizing the transformation matrix between april tags by the pose graph optimization theory is as follows:
the pose of each april tag was，…，/>，/>And->The transformation matrix is +.>The following equation can be obtained
If there is an error in the transformation between the poses of each april tag, the equation is not fully established, the error is set asThen there is
All sides are gathered intoThe overall objective function is as follows:
the invention also provides a technical scheme, and an unmanned aerial vehicle positioning method comprises the following steps:
b1, placing an april tag array in a scene, calculating internal parameters and distortion coefficients of a camera in the step S1, and carrying out configuration correction on the unmanned aerial vehicle;
b2, the unmanned aerial vehicle receives a remote controller take-off instruction and automatically flies to a specified height to hover;
b3, the unmanned aerial vehicle camera recognizes the april tag information, optimizes the april tag positioning information by utilizing the steps S2 and S3, and sends the optimized april tag positioning information to the upper computer;
b4, the upper computer receives the april tag positioning information transmitted by the unmanned aerial vehicle camera, displays the information in a designated message box, names an ID number for each april tag positioning information area, and draws a two-dimensional track on a visual interface;
and B5, the upper computer designates one ID number to be sent to the unmanned aerial vehicle, the unmanned aerial vehicle flies to the april tag with the designated ID after receiving the designated ID number, and the transformation matrix between the april tags is optimized by continuously using the step S4 in the process.
Further, in step B5, the upper computer designates the ID number to be sent to the unmanned aerial vehicle, and further includes designating the flight trajectory to be sent to the unmanned aerial vehicle. The upper computer can only send a certain appointed ID number to the unmanned aerial vehicle, so that the unmanned aerial vehicle flies on the april tag with the appointed ID number, and can also appoint the flight track, so that the unmanned aerial vehicle flies according to the appointed flight track.
Further, in the step B5, the unmanned aerial vehicle receives the specified ID number and flies to the april tag with the specified ID, which specifically includes:
b51, the upper computer designates one ID number to be sent to the unmanned aerial vehicle, and the unmanned aerial vehicle converts the designated ID number into the pixel coordinate of the target AprilTag;
b52, decoupling the obtained pixel coordinates from the roll and the pitch by the unmanned aerial vehicle;
b53, the unmanned plane fuses the height information h;
and B54, controlling the unmanned aerial vehicle to fly by using a PID controller.
The PID controller is used for controlling the unmanned aerial vehicle to fly, so that the hysteresis of the unmanned aerial vehicle flying can be effectively reduced, and the flying efficiency is improved.
The technical scheme also provides an unmanned aerial vehicle positioning system, which comprises an indoor space, a two-dimensional code, an unmanned aerial vehicle and an upper computer, wherein the two-dimensional code is placed on the ground of the indoor space, the unmanned aerial vehicle is connected with Bluetooth or WIFI of the upper computer, and the upper computer is used for controlling the unmanned aerial vehicle.
Further, the unmanned aerial vehicle comprises an unmanned aerial vehicle main body, a drive-free camera, an optical flow sensor and an OpenMV, wherein the drive-free camera, the optical flow sensor and the OpenMV are all connected with the unmanned aerial vehicle main body; the upper computer is a PC terminal. The drive-free camera is used for performing offline positioning or online aerial photographing on the PC end after image acquisition; the optical flow sensor is used for detecting the state of the ground by utilizing the change processing of the image, so as to monitor the movement of the aircraft, and is mainly used for keeping the horizontal position of the aircraft; the OpenMV is used for carrying out april tag identification, and is transmitted to the unmanned aerial vehicle through a serial port and is visualized by transmitting positioning information to an upper computer through Bluetooth.
Compared with the prior art, the invention has the beneficial effects that: the method for improving the identification and positioning precision of the unmanned aerial vehicle camera to the april tag provided by the invention has the advantage that the identification and positioning precision of the unmanned aerial vehicle camera to the april tag is effectively improved by using a simple and innovative method. Meanwhile, the unmanned aerial vehicle positioning method can realize accurate positioning of the unmanned aerial vehicle when the indoor GPS signal is weak, and the unmanned aerial vehicle indoor positioning is carried out only by using the AprilTag two-dimensional code, so that the unmanned aerial vehicle positioning method is little in environmental interference, occupies little CPU resource and is low in cost.
Drawings
Fig. 1 is an overall schematic of the present invention.
Fig. 2 is a block diagram of hardware connection in embodiment 1 of the present invention.
Fig. 3 is a schematic diagram of unmanned aerial vehicle flight control in embodiment 1 of the present invention.
Fig. 4 is a schematic diagram of a gyroscope and an accelerometer of the unmanned aerial vehicle in embodiment 1 of the present invention.
Fig. 5 is a schematic diagram of an electronic compass for an unmanned aerial vehicle in embodiment 1 of the present invention.
Fig. 6 is a pinhole camera pinhole imaging model in example 2 of the present invention.
Fig. 7 is a schematic view showing the x-axis projection of the pinhole imaging model in embodiment 2 of the present invention.
Fig. 8 is a schematic view of image distortion in embodiment 2 of the present invention.
Fig. 9 is a schematic view of tangential distortion in example 2 of the present invention.
FIG. 10 is a schematic diagram of a checkerboard calibration plate according to example 2 of the present invention.
Fig. 11 is a schematic diagram of Matlab identifying chessboard angular points in embodiment 2 of the present invention.
Fig. 12 is a diagram showing the pose of the camera in the checkerboard coordinate system in embodiment 2 of the present invention.
Fig. 13 is a diagram showing pixel errors at each camera pose in embodiment 2 of the present invention.
Fig. 14 is a correspondence relationship between feature points and space points in embodiment 2 of the present invention.
FIG. 15 is a reprojection error of the prior art april tag pose solution in example 2 of the present invention.
FIG. 16 is a reprojection error of PnP gesture resolution in embodiment 2 of the present invention.
Fig. 17 is a structure of the diagram in embodiment 2 of the present invention.
FIG. 18 is a graph showing the comparison of the g2o optimizing effect in example 2 of the present invention.
Fig. 19 is a pose chart optimization result of the general camera acquired image optimization in embodiment 2 of the present invention.
Fig. 20 is a result of optimizing the OpenMV acquired image by pose map optimization in embodiment 2 of the present invention.
Fig. 21 is a top view trace of the prior art method ORBSLAM2 positioning in embodiment 2 of the present invention.
Fig. 22 is a top view trace of the pose resolving API positioning in the prior art method university of dense root open source code in embodiment 2 of the present invention.
FIG. 23 is a top view trace of a single april tag information localization in example 2 of the present invention.
FIG. 24 is a top view trace of a plurality of april tag information fusion locations in example 2 of the present invention.
Fig. 25 is a flowchart of a procedure at the unmanned aerial vehicle end in embodiment 3 of the present invention.
Fig. 26 is a flowchart of the steps performed by the unmanned aerial vehicle end after the upper computer in embodiment 3 of the present invention gives the april tag target ID.
FIG. 27 is a flowchart of a procedure at the upper computer end in embodiment 3 of the present invention.
Fig. 28 is a display interface of a host computer in embodiment 3 of the present invention.
Fig. 29 is a diagram of the flight result of the unmanned aerial vehicle in embodiment 3 of the present invention.
FIG. 30 is a diagram showing and visualizing the positioning information of the upper computer in embodiment 3 of the present invention.
Fig. 31 is a flowchart of a method for improving the positioning accuracy of april tag recognition by an unmanned aerial vehicle camera in embodiment 2 of the present invention.
Detailed Description
The drawings are for illustrative purposes only and are not to be construed as limiting the invention. For better illustration of the following embodiments, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the actual product dimensions; it will be appreciated by those skilled in the art that certain well-known structures in the drawings and descriptions thereof may be omitted.
Example 1
As shown in fig. 1, the present embodiment provides an unmanned aerial vehicle positioning system, which includes an indoor space, a two-dimensional code 200, an unmanned aerial vehicle 100 and an upper computer, where the unmanned aerial vehicle 100 includes an unmanned aerial vehicle main body, a drive-free camera, an optical flow sensor and an OpenMV, and the drive-free camera, the optical flow sensor and the OpenMV are all connected with the unmanned aerial vehicle main body; the upper computer is a PC terminal.
Specifically, as shown in fig. 2, the two-dimensional code 200 adopted in the embodiment is an april tag array of 3*3, the execution environment of the PC end of the upper computer is Windows 10, the drive-free camera can be connected with a raspberry group 3B, the raspberry group can shoot a front image and transmit the front image to a remote interface through WiFi, the remote interface can be realized through a lightvnc, and the image on the raspberry group can be stored locally and then can be used for offline SLAM positioning or online aerial photography; openMV may perform april tag recognition for a resolution of 160 x 120. The unmanned aerial vehicle 100 flight control can adopt anonymous flight control, the CPU is STM32F407, the system requirement is met, the open source is convenient for operating the bottom layer program, the specific flight control schematic diagram is shown in figure 3, the flight control can lead out 5 serial communication interfaces, and the communication with the OpenMV and optical flow sensors is convenient to realize. In addition, in this embodiment, the flight control of the unmanned aerial vehicle 100 may also be connected to a remote controller through a wireless receiver, and the remote controller may control the unmanned aerial vehicle 100 to take off by one key and rescue through the remote controller when the PC control fails. The unmanned aerial vehicle 100 may further include a gyroscope, an accelerometer, and an electronic compass, the principles of the gyroscope and the accelerometer may be shown in fig. 4, and the schematic diagram of the electronic compass may be shown in fig. 5.
Example 2
The embodiment provides a method for improving the identification and positioning precision of an unmanned aerial vehicle camera to april tag, as shown in fig. 31, comprising the following steps:
s1, calibrating a camera on an unmanned aerial vehicle, and calculating to obtain internal parameters and distortion coefficients of the camera;
s2, acquiring an april tag image by the camera on the basis of acquiring internal parameters and distortion coefficients of the camera, identifying space point information from the acquired april tag image, and fusing a plurality of april tag space point information;
s3, after the information of a plurality of space points is obtained, solving the pose of the camera under the world coordinate system by adopting PnP in OpenCV, and after solving the pose by adopting PnP, adding a Gaussian Newton method to iterate and optimize the pose;
and S4, optimizing a transformation matrix between the april Tags by using a pose diagram optimization theory.
In the step S1, the pinhole camera model imaging principle can be explained by using a pinhole imaging model, as shown in fig. 6, which is a two-dimensional plane coordinate system for converting an object in a three-dimensional space into a two-dimensional plane coordinate system, where the two-dimensional plane coordinate system is a normalized plane coordinate system in the camera.
The process of solving the camera internal parameters can set the length of the object AP in the x-axis direction asAs shown in FIG. 7, BP' with length +.>The length of AP on the y-axis is +.>Length after administration is +.>The focal length is known as +.>The length in the z-axis direction is +.>Is obtainable from similar triangles
The use of the above formula in a camera model requires the use of,/>Converting to a pixel coordinate system, wherein the origin of the coordinate system is the upper left corner of the image, the image is subjected to certain scaling and translation in the conversion process, and the x-axis magnification is assumed to be +.>The translation amount is->The magnification of the y-axis is +.>The translation amount is->Can be obtained
Wherein the method comprises the steps of，/>。
Converted into matrix form
Wherein the method comprises the steps ofIs a camera internal reference matrix.
Since the camera has a lens, it is easy to deform a picture, for example, a straight line is changed into a curve, and distortion caused by the lens is called radial distortion, and can be classified into pincushion type distortion and barrel type distortion, which are normal images, pincushion type distortion, and barrel type distortion in this order as shown in fig. 8. Opposite to the radial distortion is tangential distortion, as in fig. 9, which is the result of the lens being non-parallel to the imaging plane.
The mathematical model for radial and tangential distortion is as follows:
wherein the method comprises the steps ofFor normalizing the coordinates under the plane, +.>Is->Distance to origin, +.>、/>、/>Is a radial distortion coefficient>、/>Is the tangential distortion coefficient.
Specifically, as shown in fig. 10 and 11, in this embodiment, the Matlab checkerboard calibration method is used to solve the internal parameters and distortion coefficients of the camera, and besides the internal parameters and distortion coefficients, the Matlab also solves a transformation matrix T, where the transformation matrix T is linked with the outside world and is formed by a rotation matrix R and a translation vector T. The parameters of Matlab solution are defaulted to have no radial distortion, so the radial distortion is obtained by OpenCV solution, and finally the internal parameters and distortion coefficients are as follows:
K=[509.0703, 0 ,349.8988; 0, 507.1801, 178.7456;0, 0, 1]
k1=0.095175, k1=0.040872, k3= -0.275938
p1=-0.010368, p2=-0.001062
matlab can detect angular points of a chessboard calibration board to obtain angular point pixel coordinates, a world coordinate system is built on the chessboard, the characteristic of the angular points on the chessboard in the world coordinate system is that a z-axis is 0, and finally, the angular point coordinates under the pixel coordinate system are connected with the angular point coordinates under the world coordinate system, and internal parameters and distortion coefficients are solved through PnP and other methods. In the embodiment, 16 chessboard pictures are adopted, the side length in the chessboard grids is 24mm, the pose of each camera relative to a chessboard coordinate system can be seen after Matlab solution, the result is shown in fig. 12, and finally, the pixel error is less than 0.35, so that the use requirement is basically met, and the result is shown in fig. 13.
In the step S2, since a picture may identify a plurality of april tag information, only one april tag information is adopted with low reliability, and a transformation matrix is required to be obtained by combining the april tag information. In this embodiment, under the condition that only barrel distortion of the camera is considered, the farther the pixel point is from the center point, the noise introduced by the pixel point is increased, so that the euclidean distance between the center of april tag and the center of the image is adopted as the credibility weight, and a plurality of space points are fused together. Considering that the accuracy can be effectively improved without fusing more information, the embodiment only fuses two april tag information, and the application formula is as follows:
wherein the method comprises the steps ofFor the fused spatial point +.>、/>Two AprilTag corresponding spatial points, respectively->、/>For two different AThe euclidean distance of the center pixel of the prilTag from the center of the image.
In the step S3, after the april tag is successfully identified, a corresponding relationship is required to be established between four corner points of the rectangle and the three-dimensional space point, and gesture calculation is performed. In the embodiment, the pose of the camera under the world coordinate system is solved by combining PnP in OpenCV with the initial value of the homography matrix, wherein the world coordinate system is an april tag coordinate system. After solving the gesture through PnP, this embodiment further adds one iteration of gaussian newton method to optimize the gesture.
The homography matrix is used for describing the mapping relation between two planes, and when the spatial points are coplanar, the homography matrix can be used for solving the camera gesture.
Let homography matrix beThe spatial point coordinates in the april tag coordinate system are +.>Since each april tag is coplanar and overlaps with the x-y plane, all april tags are +.>The corner coordinates of the april tag matrix are +.>，/>For camera internal reference, R is a rotation matrix, t is a translation matrix, if there is
、/>For rotating matrix->Is obtained by sorting the first and second columns of (a)
Order the，/>Finally can get
The matrix can be obtained by DLT algorithm solution, +.>The matrix has 9 variables, but because of scale uncertainty, the degree of freedom is 8, so at least 4 pairs of 2D-3D points are needed to solve the matrix.
PnP is a common method for solving the world coordinate system to the camera coordinate system, when the corresponding relation between a certain feature point in the known image and a space point under the world coordinate system is as shown in FIG. 14, and the logarithm of the feature point and the space point is not lower than three pairs, the PnP can be used for solving the transformation matrixThe PnP solution method generally includes DLT, P3P, EPnP, and iterative methods.
In this embodiment, the BA problem is solved by using the Levenberg-Marquardt algorithm in the nonlinear optimization algorithm, and a good transformation matrix needs to be provided as an initial value before iteration, otherwise, the error function converges to a local extremum, and the initial value can be selected to be the camera pose obtained by the homography matrix。
The Levenberg-Marquardt algorithm is an improved version of Gauss Newton method, and damping factors are addedThe error can be effectively controlled to go on in the descending direction, and the positive quality of the matrix is ensured, and the formula is as follows:
in the formulaFor 2*6 Jacobian matrix, +.>As an error function, I is an identity matrix,>is->Transpose of matrix, form as follows:
wherein the method comprises the steps ofIs the spatial point coordinate under the camera coordinate system, < >>For transforming matrix->In special European style groupDisturbance of the lower part->Is the pixel coordinates. />、/>For the distortion coefficient +>For x-axis focal length, +.>For y-axis focal length, +.>For x-axis translation, +.>And r is the distance between the pixel coordinate and the origin in the normalized coordinate system.
The Levenberg-Marquardt algorithm is as follows:
s31, giving an initial transformation matrixDamping factor->；
S32, for the nth iteration, solving an error functionAt->The Jacobian matrix is used for obtaining the increment according to the formula>Order-making
；
S33, calculating a damping factor updating scale factor；
S34、，/>Accept this update->；
S35、，/>Refusing the update->；
S36, judging whether the algorithm converges, if so, ending iteration, otherwise, returning to S32;
wherein the method comprises the steps ofThe scale factor is updated for the damping factor, expressed as
In step S3 in this embodiment, the pose of the camera under the world coordinate system is solved by adopting PnP in OpenCV, and after the pose is solved by PnP, the iteration optimization pose of the gaussian newton method is added once, so that compared with the existing april tag pose solution, the pose settlement error is lower, and in this embodiment, the two methods are compared by the reprojection error.
Re-castingShadow error is a method for comparing the precision of a transformation matrix solved by 2D-3D point pairs, when the coordinates of space points under a known world coordinate system areThe space point is a homogeneous coordinate point under a camera coordinate systemRotation matrix of world coordinate system to camera coordinate system +.>The translation vector is +.>Internal parameters->The reprojection error can be obtained>Is that
Performing error analysis on 50 pictures according to the existing april tag gesture resolving algorithm and the PnP gesture resolving algorithm in OpenCV according to the reprojection error definition, wherein after a PnP result is obtained, the camera gesture is performed againIterative optimization is performed. The final two re-projection error results are shown in fig. 15 and 16, from which it can be seen that the april tag pose solution error stabilizes around 0.5 pixels, while the OpenCV pose solution error is lower than the april tag pose solution error.
In the invention, the transformation matrix between the april tags is required to be acquired, but the acquired matrix has larger error and the error is required to be reduced, so that the step S4 optimizes the transformation matrix between the april tags by adopting a pose chart optimization theory.
The pose diagram optimization is an application of SLAM back end optimization, the feature points in the visual SLAM basically converge after being optimized for a plurality of times, the feature points do not need to be optimized continuously in the subsequent optimization process, only the track is optimized, and the pose diagram optimization provides a theoretical basis for track optimization only.
The pose of each april tag was，…，/>，/>And->The transformation matrix is +.>The following equation can be obtained
However, if there is an error in the transformation between the poses of each april tag, the equation is not fully established, and the error is set asThen there is
Let the set of all sides beThe overall objective function is as follows:
wherein the method comprises the steps ofAnd (3) performing nonlinear optimization on the target function obtained by representing uncertainty of corresponding variables for a diagonal information matrix of 6*6, wherein a Levenberg-Marquardt algorithm iterative solution result can still be adopted in pose diagram optimization.
In this embodiment, the optimization error can be performed by using a g2o nonlinear optimization library, and g2o is commonly used for optimizing an open source c++ frame of a graph structure, is commonly used for optimizing and solving Bundle Adjustment problems at the back end of SLAM, encapsulates many iterative algorithms including gaussian newton method, dog-Leg algorithm, LM algorithm, and the like, and encapsulates many solving APIs of linear equations.
Both robotics and computer vision problems related to minimizing errors can represent graph-based nonlinear error functions. In SLAM or BA, the overall objective of these problems is to solve for the global minimum of the error function. The structure of the graph is shown in fig. 17, where the motion model and the observation model form the graph optimization edges. The edges of the pose map optimization in this embodiment are the transformation errors between the poses of each april tag. The trace effect by g2o plot optimization is shown in fig. 18.
In the embodiment, pose map optimization is adopted to optimize the pose of the two-dimensional code array, and a common camera and an OpenMV camera are used for comparison respectively.
The april tag pose acquired using a conventional camera was optimized and the results are shown in fig. 19.
The april tag pose acquired using OpenMV was optimized and the results are shown in fig. 20.
From the result, the pose graph optimization theory is completely effective for april tag optimization, so that errors can be reduced as much as possible, for example, the april tag pose obtained by OpenMV is optimized, and the optimization effect is obviously better, but because the image obtained by OpenMV is not subjected to distortion removal, the optimization result is not optimal, and the influence of distortion coefficients can be reduced as much as possible. For a common camera, the image distortion removal and other treatments are performed in advance, so that the april tag attitude error obtained before the optimization is smaller, and the optimized effect is ideal.
In this embodiment, the method for improving positioning accuracy in this embodiment is also compared with the prior art method ORBSLAM2 and the attitude calculation API positioning effect in the open source code of the university of dense root. In this embodiment, the comparison data is compact irregular elliptical motion performed above a 3×3april tag two-dimensional code array, the comparison platform is ubuntu 16.04, and the output data is a spatial coordinate pointThe visual data platform is Matlab2019a.
The comparison results are shown in fig. 21-24, from the top view of the trace, the following conclusions can be drawn:
(1) The positioning result of ORBSLAM2 has significant drift and the occurrence of jumping line segments as in FIG. 21, while the positioning track based on april Tag is significantly compact and substantially jumping-free line segments as in FIGS. 23 and 24.
(2) The track located by AptilTag is true scale information, in this embodiment the AptilTag location unit is meters, whereas the orblam 2 cannot learn about a specific scale due to uncertainty of the monocular scale.
(3) After the API is calculated by adopting the posture in the open source code of the university of mi-li, and the result after the API is converted into the world coordinate system is shown in fig. 22, it can be known from the graph that the partial posture obtained by the algorithm has obvious errors, and the effect is inferior to the posture of april tag calculated by the posture in the embodiment. The positioning effect of fusion of a plurality of april tag information is shown in fig. 24, and is obviously superior to that of a position chart 23 without fusion of a plurality of april tag information at local positions.
In summary, the positioning track of the present embodiment is slightly better than the ORBSLAM2 positioning track with a definite marker and only in a top view. The positioning track of the embodiment fused with a plurality of april tag information is slightly better than the positioning track using only a single april tag information.
Example 3
The embodiment provides an unmanned aerial vehicle positioning method, which comprises the following steps:
b1, placing an april tag array in a scene, calculating internal parameters and distortion coefficients of a camera by using the camera calibration method in the embodiment 2, and carrying out configuration correction on the unmanned aerial vehicle;
b2, the unmanned aerial vehicle receives a remote controller take-off instruction and automatically flies to a specified height to hover;
b3, the unmanned aerial vehicle camera recognizes the april tag information, optimizes the april tag positioning information by using the method in the embodiment 2, and sends the optimized april tag positioning information to the upper computer;
b4, the upper computer receives the april tag positioning information transmitted by the unmanned aerial vehicle camera, displays the information in a designated message box, names an ID number for each april tag positioning information area, and draws a two-dimensional track on a visual interface;
and B5, the upper computer designates one ID number to be sent to the unmanned aerial vehicle, and the unmanned aerial vehicle flies to the AprilTag with the designated ID after receiving the designated ID number, and in the process, the transformation matrix between the AprilTag is optimized by continuously using the bitmap optimization in the embodiment 2.
Specifically, in step B5, the upper computer designates the ID number to be sent to the unmanned aerial vehicle, and further includes designating the flight trajectory to be sent to the unmanned aerial vehicle.
In this embodiment, the unmanned aerial vehicle camera is OpenMV.
Before the unmanned aerial vehicle takes off, need dispose unmanned aerial vehicle through ground station, such as gyroscope, accelerometer and optical flow module etc. correct, still need set up unmanned aerial vehicle low voltage and drop etc. safe flight measure, after all configurations are accomplished and confirm and then carry out unmanned aerial vehicle test, unmanned aerial vehicle waits for the remote controller instruction of taking off, this instruction gives through remote controller auxiliary channel, after obtaining the instruction of taking off, unmanned aerial vehicle opportunity flies to appointed height automatically and hovers, openMV begins to discern april tag around this moment, if discerned the ID that the host computer gave, unmanned aerial vehicle then hovers at appointed ID's april tag, otherwise hover according to optical flow data. A specific unmanned aerial vehicle end flow chart is shown in fig. 25.
After the upper computer gives the april tag target ID, the unmanned aerial vehicle performs the following specific steps:
b51, the upper computer designates one ID number to be sent to the unmanned aerial vehicle, and the unmanned aerial vehicle converts the designated ID number into the pixel coordinate of the target AprilTag;
b52, decoupling the obtained pixel coordinates from the roll and the pitch by the unmanned aerial vehicle;
b53, the unmanned plane fuses the height information h;
b54, controlling unmanned aerial vehicle to fly by using PID controller
The specific flow is shown in fig. 26, and in order to reduce the hysteresis of the unmanned aerial vehicle, the controller adopts PID control.
In this embodiment, the upper computer design platform is Windows, the GUI framework is Qt 5.9, and the flowchart is shown in fig. 27.
When the upper computer is started, the program automatically searches the serial port, then manually selects to start the serial port, if the serial port is not started, the upper computer does not operate until the serial port is started, after the serial port is started, the AprilTag information transmitted by OpenMV is received, the message is displayed in a designated message box, and the two-dimensional track drawing is performed on a visual interface, when the spatial information of the AprilTag is received, the coordinate system of the spatial information is established on the identified ID, so that the world coordinate system is required to be converted to be defined at the central position of the AprilTag array, namely the central position of the central AprilTag matrix. And then any two-dimensional code of the upper computer can be clicked to control the unmanned aerial vehicle to fly, and an operation interface of the upper computer is shown in fig. 28.
In this embodiment, in the 3×3april tag array, IDs are 0, 1, 2, 6, 7, 8, 3, 4, 5 from left to right and from top to bottom in sequence, the upper computer designates that the unmanned aerial vehicle flies onto the april tag with ID 7, the flying result of the unmanned aerial vehicle is shown in fig. 29, the display and visualization of the positioning information of the upper computer are shown in fig. 30, the shadow is the overlooking track of the unmanned aerial vehicle, it can be seen that the unmanned aerial vehicle is basically gathered near the april tag with ID 7, and the running result basically accords with the actual situation.
It should be understood that the foregoing examples of the present invention are merely illustrative of the present invention and are not intended to limit the present invention to the specific embodiments thereof. Any modification, equivalent replacement, improvement, etc. that comes within the spirit and principle of the claims of the present invention should be included in the protection scope of the claims of the present invention.
Claims (8)
1. A method for improving the identification and positioning precision of an unmanned aerial vehicle camera to april tag is characterized by comprising the following steps:
s1, calibrating a camera on an unmanned aerial vehicle, and calculating to obtain internal parameters and distortion coefficients of the camera;
s2, acquiring an april tag image by the camera on the basis of acquiring internal parameters and distortion coefficients of the camera, identifying space point information from the acquired april tag image, and fusing a plurality of april tag space point information;
s3, after the information of a plurality of space points is obtained, solving the pose of the camera under the world coordinate system by adopting PnP in OpenCV, and after solving the pose by adopting PnP, adding a Gaussian Newton method to iterate and optimize the pose;
s4, optimizing a transformation matrix between april Tags by using a pose diagram optimization theory;
in the step S2, a plurality of april tag space point information are fused, and the specific process is that the Euclidean distance between the center of april tag and the center of an image is adopted as a credibility weight, two april tag space point information are fused, and a formula is used:
wherein Q is the spatial point after fusion,、/>two AprilTag corresponding spatial points, respectively->、/>Center pixel and image for two different april tagsEuclidean distance of the center;
in the step S4, the specific process of optimizing the transformation matrix between april tags by using the pose graph optimization theory is as follows:
the pose of each april tag was，…，/>，/>And->The transformation matrix is +.>The following equation->
If there is an error in the transformation between the poses of each april tag, the equation is not fully established, the error is set asThen there is
All sides are gathered intoThe overall objective function is as follows:
wherein the method comprises the steps ofThe diagonal information matrix of 6*6 represents uncertainty of the corresponding variable.
2. The method for improving the identification and positioning accuracy of the unmanned aerial vehicle camera to april tag according to claim 1, wherein the process of solving the camera internal parameters in the step S1 is as follows:
according to the imaging principle of a pinhole camera model, a 3D point P (X, Y, Z) under a camera coordinate system, a projection point P1 of the point P1 under a pixel coordinate system has coordinates of (u, v), and the corresponding relation between the two points is that
Wherein K is a camera reference matrix,for x-axis focal length, +.>For y-axis focal length, +.>For x-axis translation, +.>Is the y-axis translation;
the distortion coefficient solution can be divided into radial distortion and tangential distortion, and the solving formula mathematical model is as follows:
wherein (x, y) is the coordinates of the P point in the normalized plane,and->For post-distortion coordinates, r is the (x, y) to origin distance, +.>、/>、/>Is a radial distortion coefficient>、/>Is the tangential distortion coefficient.
3. The method for improving the identification and positioning accuracy of the unmanned aerial vehicle camera to the april tag according to claim 1, wherein the specific process in the step S3 is to solve the BA (Bundle Adjustment) problem by adopting a Levenberg-Marquardt algorithm in a nonlinear optimization algorithm, and the formula is as follows:
，
in the formula2*6 Jacobian matrix, μ damping factor,>as an error function, I is an identity matrix,>is->Transposition of matrixΔx is the delta of the jacobian matrix at x for the error function e, in the form:
wherein the method comprises the steps ofIs the spatial point coordinate under the camera coordinate system, < >>For transforming matrix->In special European style group->Disturbance of the lower part->Is pixel coordinates; />、/>For the distortion coefficient +>For x-axis focal length, +.>For y-axis focal length, +.>For x-axis translation, +.>And r is the distance between the pixel coordinate and the origin in the normalized coordinate system.
4. The unmanned aerial vehicle positioning method is characterized by comprising the following steps of:
b1, placing an april tag array in a scene, calculating camera internal parameters and distortion coefficients by using the step S1 of any one of claims 1 to 3, and carrying out configuration correction on the unmanned aerial vehicle;
b2, the unmanned aerial vehicle receives a remote controller take-off instruction and automatically flies to a specified height to hover;
b3, the unmanned aerial vehicle camera recognizes the april tag information, optimizes the april tag positioning information by adopting the steps S2 and S3 in any one of claims 1-3, and sends the optimized april tag positioning information to the upper computer;
b4, the upper computer receives the april tag positioning information transmitted by the unmanned aerial vehicle camera, displays the information in a designated message box, names an ID number for each april tag positioning information area, and draws a two-dimensional track on a visual interface;
and B5, the upper computer designates one ID number to be sent to the unmanned aerial vehicle, and the unmanned aerial vehicle flies to the april tag with the designated ID after receiving the designated ID number, and in the process, the transformation matrix between the april tags is optimized by continuously using the step S4 in the claim 1.
5. The method according to claim 4, wherein in the step B5, the upper computer designates the ID number to be sent to the unmanned aerial vehicle, and further includes designating the flight trajectory to be sent to the unmanned aerial vehicle.
6. The method for positioning an unmanned aerial vehicle according to claim 4, wherein in the step B5, the unmanned aerial vehicle flies on the april tag with the specified ID after receiving the specified ID number, specifically comprising:
b51, the upper computer designates one ID number to be sent to the unmanned aerial vehicle, and the unmanned aerial vehicle converts the designated ID number into the pixel coordinate of the target AprilTag;
b52, decoupling the obtained pixel coordinates from the roll and the pitch by the unmanned aerial vehicle;
b53, the unmanned plane fuses the height information h;
and B54, controlling the unmanned aerial vehicle to fly by using a PID controller.
7. An unmanned aerial vehicle positioning system applying the positioning method according to any one of claims 4-6, comprising an indoor space, a two-dimensional code, an unmanned aerial vehicle and an upper computer, wherein the two-dimensional code is placed on the ground of the indoor space, the unmanned aerial vehicle is connected with Bluetooth or WIFI of the upper computer, and the upper computer controls the unmanned aerial vehicle.
8. The unmanned aerial vehicle positioning system of claim 7, wherein the unmanned aerial vehicle comprises an unmanned aerial vehicle body, a drive-free camera, an optical flow sensor, and an OpenMV, all connected to the unmanned aerial vehicle body; the upper computer is a PC terminal.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011008679.0A CN112184812B (en) | 2020-09-23 | 2020-09-23 | Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011008679.0A CN112184812B (en) | 2020-09-23 | 2020-09-23 | Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112184812A CN112184812A (en) | 2021-01-05 |
| CN112184812B true CN112184812B (en) | 2023-09-22 |
Family
ID=73955357
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011008679.0A Active CN112184812B (en) | 2020-09-23 | 2020-09-23 | Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112184812B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113674340A (en) * | 2021-07-05 | 2021-11-19 | 北京物资学院 | Binocular vision navigation method and device based on landmark points |
| CN113790668B (en) * | 2021-07-26 | 2023-06-06 | 广东工业大学 | Intelligent cargo measurement system based on multi-rotor unmanned aerial vehicle |
| CN113485449A (en) * | 2021-08-16 | 2021-10-08 | 普宙科技(深圳)有限公司 | Unmanned aerial vehicle autonomous landing method and system based on nested two-dimensional code |
| CN114007047B (en) * | 2021-11-02 | 2024-03-15 | 广西电网有限责任公司贺州供电局 | Machine vision-based real-time monitoring and alarming system for operation of power transformation site |
| CN114200948B (en) * | 2021-12-09 | 2023-12-29 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle autonomous landing method based on visual assistance |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109658461A (en) * | 2018-12-24 | 2019-04-19 | 中国电子科技集团公司第二十研究所 | A kind of unmanned plane localization method of the cooperation two dimensional code based on virtual simulation environment |
| CN109697753A (en) * | 2018-12-10 | 2019-04-30 | 智灵飞（北京）科技有限公司 | A kind of no-manned plane three-dimensional method for reconstructing, unmanned plane based on RGB-D SLAM |
| WO2019136613A1 (en) * | 2018-01-09 | 2019-07-18 | 深圳市沃特沃德股份有限公司 | Indoor locating method and device for robot |
| CN110929642A (en) * | 2019-11-21 | 2020-03-27 | 扬州市职业大学(扬州市广播电视大学) | Real-time estimation method for human face posture based on two-dimensional feature points |
| CN111197984A (en) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | Vision-inertial motion estimation method based on environmental constraint |
-
2020
- 2020-09-23 CN CN202011008679.0A patent/CN112184812B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019136613A1 (en) * | 2018-01-09 | 2019-07-18 | 深圳市沃特沃德股份有限公司 | Indoor locating method and device for robot |
| CN109697753A (en) * | 2018-12-10 | 2019-04-30 | 智灵飞（北京）科技有限公司 | A kind of no-manned plane three-dimensional method for reconstructing, unmanned plane based on RGB-D SLAM |
| CN109658461A (en) * | 2018-12-24 | 2019-04-19 | 中国电子科技集团公司第二十研究所 | A kind of unmanned plane localization method of the cooperation two dimensional code based on virtual simulation environment |
| CN110929642A (en) * | 2019-11-21 | 2020-03-27 | 扬州市职业大学(扬州市广播电视大学) | Real-time estimation method for human face posture based on two-dimensional feature points |
| CN111197984A (en) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | Vision-inertial motion estimation method based on environmental constraint |
Non-Patent Citations (2)
| Title |
|---|
| Moving target tracking method for unmanned aerial vehicle/unmanned ground vehicle heterogeneous system based on AprilTags;Xiao Liang et al;《Measurement and Control》;第53卷(第3-4期);第427-440页 * |
| 基于AprilTag二维码的无人机着陆引导方法;高嘉瑜 等;《现代导航》(第01期);第20-25页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112184812A (en) | 2021-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112184812B (en) | Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system | |
| CN110825101B (en) | Unmanned aerial vehicle autonomous landing method based on deep convolutional neural network | |
| US9355453B2 (en) | Three-dimensional measurement apparatus, model generation apparatus, processing method thereof, and non-transitory computer-readable storage medium | |
| CN110058602A (en) | Multi-rotor unmanned aerial vehicle autonomic positioning method based on deep vision | |
| CN111338383B (en) | GAAS-based autonomous flight method and system, and storage medium | |
| Sanfourche et al. | Perception for UAV: Vision-Based Navigation and Environment Modeling. | |
| CN112396656B (en) | Outdoor mobile robot pose estimation method based on fusion of vision and laser radar | |
| CN112734765A (en) | Mobile robot positioning method, system and medium based on example segmentation and multi-sensor fusion | |
| CN116222543B (en) | Multi-sensor fusion map construction method and system for robot environment perception | |
| CN114004977A (en) | Aerial photography data target positioning method and system based on deep learning | |
| Natraj et al. | Omnidirectional vision for UAV: Applications to attitude, motion and altitude estimation for day and night conditions | |
| CN111968228A (en) | Augmented reality self-positioning method based on aviation assembly | |
| CN111812978B (en) | Cooperative SLAM method and system for multiple unmanned aerial vehicles | |
| Magree et al. | Monocular visual mapping for obstacle avoidance on UAVs | |
| Kim et al. | As-is geometric data collection and 3D visualization through the collaboration between UAV and UGV | |
| CN116619358A (en) | Self-adaptive positioning optimization and mapping method for autonomous mining robot | |
| Luo et al. | Docking navigation method for UAV autonomous aerial refueling | |
| Moore et al. | A stereo vision system for uav guidance | |
| US20210156710A1 (en) | Map processing method, device, and computer-readable storage medium | |
| Mansur et al. | Real time monocular visual odometry using optical flow: study on navigation of quadrotors UAV | |
| CN112945233A (en) | Global drift-free autonomous robot simultaneous positioning and map building method | |
| CN117115271A (en) | Binocular camera external parameter self-calibration method and system in unmanned aerial vehicle flight process | |
| Klavins et al. | Unmanned aerial vehicle movement trajectory detection in open environment | |
| Aminzadeh et al. | Implementation and performance evaluation of optical flow navigation system under specific conditions for a flying robot | |
| CN112489118B (en) | Method for quickly calibrating external parameters of airborne sensor group of unmanned aerial vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |