Stereoscopic vision unmanned aerial vehicle high-precision positioning method based on quaternion coordinate transformation

Abstract

The invention discloses a stereoscopic vision unmanned aerial vehicle high-precision positioning method based on quaternion coordinate transformation, and relates to the technical field of unmanned aerial vehicles, and the method comprises the following steps: 1, obtaining at least one pair of stereoscopic images collected by a stereoscopic camera system carried by an unmanned aerial vehicle; 2, determining a three-dimensional coordinate Pc = (xc, yc, zc) T of the target object in a camera coordinate system of the stereo camera system through a stereo vision processing algorithm based on the at least one pair of stereo images; and 3, acquiring an attitude quaternion q = (qw, qx, qy, qz) representing the attitude of the camera coordinate system relative to the body coordinate system of the unmanned aerial vehicle. According to the method, the coordinate transformation precision and stability are improved, so that the quaternion can represent any rotation in a three-dimensional space without singular points, the problem of universal locking of an Euler angle under a specific attitude (for example, the pitch angle is close to + / -90 degrees) is avoided, and thus the continuity, stability and calculation precision of the attitude description and coordinate transformation process are ensured; and the rotation matrix generated by the quaternion is more accurate.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles (UAVs), and in particular to a high-precision positioning method for UAVs using stereo vision based on quaternion coordinate transformation. Background Art

[0002] Unmanned aerial vehicles (UAVs), or drones, have rapidly developed and been applied in fields such as aerial photography, surveying and mapping, inspection, logistics, and search and rescue due to their high maneuverability, low cost, and wide range of applications. Accurate environmental perception and target positioning are crucial for autonomous navigation and intelligent operations. Stereo vision systems, which mimic human binocular vision and can acquire three-dimensional depth information from one or more pairs of images, have become a crucial means for drones to acquire environmental information, perform obstacle avoidance, and locate targets.

[0003] In stereo vision-based drone positioning applications, it's often necessary to convert target position information sensed in the camera coordinate system to the drone's body coordinate system, or even the global world coordinate system, to facilitate subsequent drone decision-making and control. The core of this conversion process lies in the precise calibration and real-time transformation of attitude and position relationships between coordinate systems.

[0004] Currently, many UAV systems use Euler angles (such as roll, pitch, and yaw) to describe the attitude of the drone or its onboard sensors. However, this Euler-based attitude representation suffers from an inherent singularity, known as the "gimbal lock" phenomenon. When the pitch angle approaches ±90°, a degree of freedom is lost, causing discontinuities or instability in the orientation solution, severely impacting the accuracy of attitude estimation and coordinate transformations. Furthermore, rotation operations based on Euler angles often involve complex trigonometric calculations, which can impose a significant computational burden in real-time UAV applications. Furthermore, Euler angles are less convenient and stable than other representation methods for interpolation and combined rotations.

[0005] Therefore, it is necessary to propose a high-precision positioning method for UAVs using stereo vision based on quaternion coordinate transformation to solve the above problems. Summary of the Invention

[0006] The purpose of the present invention is to provide a high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation to solve the problems raised in the above background technology.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation, comprising the following steps:

[0008] Step 1: Obtain at least one pair of stereo images captured by a stereo camera system carried by the UAV;

[0009] Step 2: Based on at least one pair of stereo images, determine the three-dimensional coordinates P of the target object in the camera coordinate system of the stereo camera system through the stereo vision processing algorithm c =(x c ,y c , z c ) T ;

[0010] Step 3: Get the attitude quaternion q = (q w ,q x ,q y ,q z );

[0011] Step 4: Construct the rotation matrix R(q) based on the attitude quaternion q. The rotation matrix is ​​used to rotate the vector from the camera coordinate system to the body coordinate system.

[0012] Step 5: Use the rotation matrix R(q) and the preset translation vector T between the camera and the body ext , the camera coordinate system three-dimensional coordinate P of the target object c Transform to the body coordinate system to obtain the three-dimensional coordinates P of the target object in the body coordinate system b =(x b ,y b , z b ) T , the transformation relationship is

[0013] P b =R(q)P c +T ext

[0014] Among them, T ext Represents the position vector of the camera coordinate system origin in the body coordinate system.

[0015] Preferably, the target object determined in step 2 is at the three-dimensional coordinate P c The method includes: extracting and matching feature points of at least one pair of stereo images, calculating the disparity between the matched feature points, and reconstructing the three-dimensional coordinates of the target object by triangulation principle.

[0016] Preferably, the attitude quaternion q obtained in step 3 is obtained by real-time measurement of an inertial measurement unit (IMU) carried by the UAV, and is optionally fused with data from other navigation sensors for correction.

[0017] Preferably, the posture quaternion q in step 4 is q=(qw ,q x ,q y ,q z ) The rotation matrix R(q) constructed is specifically expressed as:

[0018]

[0019] It is assumed that the attitude quaternion q is a unit quaternion, satisfying

[0020] Preferably, the translation vector T ext It is a fixed parameter obtained by pre-calibrating the relative position relationship between the stereo camera system carried by the drone and the drone's body coordinate system.

[0021] Preferably, after step 5, the method further includes:

[0022] Step 6: Obtain the global pose information of the drone in the world coordinate system;

[0023] Step 7: Based on the global pose information of the UAV, the three-dimensional coordinates P of the target object in the body coordinate system are b Transform to world coordinate system.

[0024] Preferably, the method is applied to autonomous navigation or target tracking tasks of a UAV in a complex environment with changing light or dense obstacles.

[0025] Technical effects and advantages of the present invention:

[0026] 1. In the present invention, by improving the accuracy and stability of coordinate transformation, quaternions can represent any rotation in three-dimensional space without singularities, avoiding the universal lock problem that occurs when Euler angles are in specific postures (such as when the pitch angle is close to ±90°), thereby ensuring the continuity, stability and calculation accuracy of the posture description and coordinate transformation process, and making the rotation matrix generated by the quaternion more accurate;

[0027] 2. This invention improves positioning robustness in complex environments, enabling the stereo vision system to directly acquire scene depth information, making it less dependent on environmental texture and motion than monocular vision. Combined with precise quaternion coordinate transformation, this allows the drone to maintain high target positioning robustness in complex environments with drastic lighting changes, sparse textures, or dense obstacles.

[0028] 3. The present invention applies quaternion multiplication (for combined rotation), which is more efficient than multiple matrix multiplications or complex trigonometric functions, and is conducive to achieving real-time high-precision positioning on resource-constrained UAV platforms, greatly improving computational efficiency.

[0029] 4. In the present invention, quaternions support spherical linear interpolation (Slerp), which can achieve smooth attitude transition, which is beneficial for UAV attitude control and trajectory planning, and is easy to interpolate and smooth attitude;

[0030] 5. The present invention provides more accurate and robust target positioning information, which helps improve the performance and reliability of drones in advanced applications such as autonomous navigation, precision landing, target identification and tracking, and intelligent obstacle avoidance, and also helps enhance the autonomous navigation and operational capabilities of drones. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 The figure is a flow chart of the high-precision positioning method of a stereoscopic UAV based on quaternion coordinate transformation according to the present invention.

[0032] Figure 2 Schematic diagram of the relationship between the camera coordinate system, the drone body coordinate system and the world coordinate system of the present invention.

[0033] Figure 3 Schematic diagram of the left viewpoint stereo image collected in the present invention.

[0034] Figure 4 Schematic diagram of a right-viewpoint stereo image collected in the present invention.

[0035] Figure 5 For the present invention Figure 3 and Figure 4 Schematic diagram of stereo correction results.

[0036] Figure 6 This is an example of a disparity map calculated from the corrected stereo image pair in the present invention. DETAILED DESCRIPTION

[0037] In order to make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the drawings in the present invention. Obviously, the embodiments described are part of the embodiments of the present invention, not all of the embodiments, and they should not be understood as limitations on the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. In the description of the present invention, it should be understood that the terms used are only for descriptive purposes and cannot be understood as indicating or implying relative importance.

[0038] The present invention provides Figures 1-6 The method for high-precision positioning of a stereoscopic UAV based on quaternion coordinate transformation includes the following steps:

[0039] Step 1: Obtain at least one pair of stereo images captured by the drone's onboard stereo camera system. When the drone is performing a mission, its onboard stereo camera system (usually composed of two cameras with known relative positions) synchronously captures at least one pair of (left and right) stereo images of the environment. These images are the basis for subsequent 3D reconstruction and target positioning ( Figure 3 and Figure 4 Examples of the captured left-viewpoint infrared image and right-viewpoint infrared image are shown respectively). The internal and external parameters of the stereo camera (such as focal length, principal point, distortion coefficient, baseline length, relative rotation and translation between the two cameras) need to be obtained in advance through camera calibration.

[0040] Step 2: Based on at least one pair of stereo images, determine the three-dimensional coordinates P of the target object in the camera coordinate system of the stereo camera system through the stereo vision processing algorithm c =(x c ,y c , z c ) T ;

[0041] It should be noted that the method for determining the three-dimensional coordinates of the target object in step 2 includes: extracting and matching feature points of at least one pair of stereo images, calculating the disparity between the matched feature points, and reconstructing the three-dimensional coordinates of the target object through the triangulation principle.

[0042] It should also be noted that the three-dimensional coordinates of the target in the camera coordinate system are determined and obtained (such as Figure 3 and Figure 4 ), a series of stereo vision processing algorithms are used to determine the three-dimensional coordinates P of the target object of interest in the camera coordinate system (for example, the coordinate system with the optical center of the left camera as the origin and the optical axis as the Z axis). c =(x c ,y c , z c ) T The process typically involves:

[0043] 1. Image Rectification: The original image is dedistorted using camera intrinsic parameters and distortion coefficients, and stereo rectification (EpipolarRectification) is performed using the relative pose between the two cameras (extrinsic parameters) to make the epipolar lines of corresponding points horizontal, simplifying subsequent matching searches. Figure 5 Shows the Figure 3 and Figure 4 The result of stereo rectification of the image pair. The superimposed green horizontal line in the figure verifies that the epipolar lines of the rectified images are aligned.

[0044] 2. Feature point extraction and matching: Extract stable feature points from the rectified left and right images (e.g., using SIFT, SURF, ORB, etc.) and match them using feature descriptors to find pairs of points with the same name in the left and right images. Alternatively, for dense matching, this can be performed directly on the rectified images.

[0045] 3. Disparity calculation: For each pair of matching points with the same name (u L , v L ) and (u R , v R ), calculate the difference in the horizontal direction, that is, disparity d = u L -u R For dense matching, the disparity of each pixel (or most pixels) in the image is calculated to form a disparity map. Figure 5 This example shows a disparity map calculated from a rectified image. Different grayscale values ​​in the map represent different disparity values, reflecting the depth information of the scene. In this embodiment, the disparity map can be calculated using an algorithm such as SGBM (Semi-Global Block Matching) (e.g., the 'stereoMatchSGBM' function in the Python code).

[0046] 4. 3D coordinate reconstruction (triangulation): Based on the camera focal length f (or more precisely the focal length component f x , f y ), main point (c x , c y ), baseline length B and the calculated pixel coordinates of the specific target point on the image (u L , v L ) and its parallax d, the three-dimensional coordinates of the target point in the camera coordinate system can be reconstructed using the triangulation principle. For example, the depth value z c Can be achieved through (Assuming f x is the horizontal focal length), then |x c | and y c :

[0047]

[0048] Alternatively, the reprojection matrix Q and disparity map obtained in the stereo correction step can be used to directly calculate the 3D coordinates of each pixel. If the specific pixel position of the target in the image is known (for example, the target center point (bbx, bby) obtained by the target detection algorithm), the camera coordinate system 3D coordinate P of the point can be directly extracted from the reprojection result. c .

[0049] Direct 3D reconstruction through stereo vision provides more direct and generally more reliable depth information than monocular vision combined with structure from motion (SfM) or deep learning depth estimation, especially in scenes with rich textures or obvious structures. This binocular redundancy also helps improve localization robustness in the face of partial occlusion or poor single-view feature extraction.

[0050] This invention improves positioning robustness in complex environments, enabling the stereo vision system to directly acquire scene depth information, making it less dependent on environmental texture and motion than monocular vision. Combined with precise quaternion coordinate transformation, this allows drones to maintain high target positioning robustness in complex environments with drastic lighting changes, sparse textures, or dense obstacles.

[0051] Step 3: Get the attitude quaternion q = (q w ,q x ,q y ,q s ); The attitude quaternion obtained in step 3 is measured in real time by the inertial measurement unit (IMU) carried by the UAV, and is optionally fused with data from other navigation sensors for correction.

[0052] It should be noted that the attitude quaternion q is obtained at the three-dimensional coordinate P of the target object. c While being determined in the camera coordinate system, it is necessary to obtain the current attitude of the camera coordinate system relative to the drone body coordinate system. The present invention uses quaternions to represent this rotation attitude. The attitude quaternion q is usually defined as a four-dimensional vector:

[0053] q=(q w ,q x ,q y ,q z )=q w +q x i+q y j+q z k

[0054] where q w is the scalar part, (q x ,q y ,q z ) is the vector part, and i 2 =j 2 =k 2 =ijk=-1. For the unit quaternion representing rotation, its norm (module) is 1, that is, The attitude quaternion q is usually output in real time by the inertial measurement unit (IMU) carried by the drone. The IMU measures angular velocity through a gyroscope and linear acceleration (including gravity acceleration) through an accelerometer. After integration and attitude solution algorithms (such as complementary filtering, Kalman filtering, gradient descent method, etc.), the attitude quaternion of the drone (or IMU's own coordinate system) can be obtained. If there is a fixed relative pose (calibrated) between the camera and the IMU, the attitude of the IMU can be converted to the attitude of the camera relative to the body. In more advanced systems, the IMU data will also be fused with the output of GPS, magnetometer, visual odometry (VO) or SLAM system to obtain a more accurate and robust attitude estimate. In the example code of this embodiment, a specific quaternion value q = (0.983648, 0, 0, -0.180098) is used as an example.

[0055] Step 4: Construct a rotation matrix R(q) based on the attitude quaternion q. The rotation matrix is ​​used to rotate the vector from the camera coordinate system to the body coordinate system. In step 4, the attitude quaternion q = (q w ,q x ,q y ,q z ) The rotation matrix R(q) constructed is specifically expressed as:

[0056]

[0057] It is assumed that the attitude quaternion q is a unit quaternion, satisfying

[0058] It should be noted that the rotation matrix R(q) is constructed according to the attitude quaternion to obtain the attitude quaternion q=(q w ,q x ,q y ,q z ), it needs to be converted into a 3×3 rotation matrix R(q), which describes the rotation transformation from the camera coordinate system to the body coordinate system. Comparison with traditional Euler angle representation and advantages of quaternions: Traditional Euler angles (for example, yaw angle ψ around the Z axis, pitch angle θ around the Y axis, roll angle φ around the X axis) are intuitive when representing posture, but there are problems such as universal lock. Quaternions can overcome these shortcomings well, and provide a posture representation without singularity, efficient and stable calculation, and easy interpolation. Therefore, the present invention uses quaternions for posture representation and rotation calculation. From the unit quaternion q=(q w ,q x ,q y ,q z ) to the rotation matrix R(q) is:

[0059]

[0060] Or equivalently written as:

[0061]

[0062] Although the Python function "cal rotation" in this invention first converts the quaternion to Euler angles and then constructs the rotation matrix, conceptually, its purpose is to achieve the conversion from quaternion to rotation matrix and can be replaced by the above direct formula to obtain better numerical stability and avoid potential problems of intermediate conversion.

[0063] The present invention applies quaternion multiplication (for combined rotation), which is more efficient than multiple matrix multiplications or complex trigonometric function operations, is conducive to achieving real-time high-precision positioning on resource-constrained UAV platforms, and greatly improves computing efficiency.

[0064] In the present invention, quaternions support spherical linear interpolation (Slerp), which can achieve smooth attitude transition, which is beneficial for UAV attitude control and trajectory planning, and is easy to interpolate and smooth attitude.

[0065] Step 5: Use the rotation matrix R(q) and the preset translation vector T between the camera and the body ext , the camera coordinate system three-dimensional coordinate P of the target object c Transform to the body coordinate system to obtain the three-dimensional coordinates P of the target object in the body coordinate system b =(x b ,y b , z b ) T , the transformation relationship is

[0066] P b =R(q)P c +T ext

[0067] Among them, T ext Represents the position vector of the camera coordinate system origin in the body coordinate system.

[0068] It should be noted that after the coordinate transformation to the body coordinate system is obtained, the rotation matrix R(q) is combined with the translation vector of the origin of the camera coordinate system in the body coordinate system The three-dimensional coordinates P of the target in the camera coordinate system can be c Transform to the three-dimensional coordinate P in the drone body coordinate system b =(x b ,y b , z b ) T The transformation formula is:

[0069] P b=R(q)P c +T ext

[0070] Among them, the translation vector T ext Represents the relative position of the camera mounted on the drone body. This is a fixed geometric relationship, usually obtained through pre-calibration of external parameters (Hand-eye calibration or direct measurement). In the sample code, td corresponds to T ext , which is calculated by combining the external measurement value (such as (-3.4036, -6.1023, 0) meters and the Z distance from the camera to the nose of the aircraft -707 mm) and the aircraft azimuth angle through the enu to xyz function. ext It is the key to ensure the final positioning accuracy.

[0071] Through the above steps, the present invention leverages the advantages of quaternions to achieve a high-precision and highly stable coordinate transformation from the camera coordinate system to the aircraft coordinate system, providing reliable position information for the drone's subsequent navigation decisions and target manipulation. For example, in the "get3dPoint" function of the sample code, the final calculated value "N" is the target's three-dimensional coordinate (unit: meters) in the aircraft coordinate system.

[0072] In the present invention, by improving the accuracy and stability of coordinate transformation, quaternions can represent arbitrary rotations in three-dimensional space without singularities, avoiding the universal lock problem that occurs in Euler angles under specific postures (such as pitch angles close to ±90°), thereby ensuring the continuity, stability and calculation accuracy of the posture description and coordinate transformation process, and making the rotation matrix generated by the quaternion more accurate.

[0073] In the present invention, the translation vector T ext It is a fixed parameter obtained by pre-calibrating the relative position relationship between the stereo camera system carried by the drone and the drone's body coordinate system.

[0074] The present invention further includes after step 5:

[0075] Step 6: Obtain the global pose information of the drone in the world coordinate system;

[0076] Step 7: Based on the global pose information of the UAV, the three-dimensional coordinates P of the target object in the body coordinate system are b Transform to the world coordinate system to achieve global positioning of the target object;

[0077] It should be noted that in some applications, the target position needs to be expressed in a globally unified world coordinate system (e.g., WGS-84 or local plane coordinate system). This requires obtaining the global pose of the drone in the world coordinate system, that is, the position vector T of the origin of its body coordinate system in the world coordinate system. WB and the posture of the body coordinate system relative to the world coordinate system (which can be expressed using another quaternion q WB Or the rotation matrix R WB These global pose information are usually provided by GPS, RTK-GPS, SLAM (Simultaneous Localization and Mapping) system or ground station. After obtaining the global pose of the UAV, the coordinates P of the target in the body coordinate system are b Can be further transformed to the coordinate P in the world coordinate system w =(x w ,y w , z w ) T :

[0078] P w =R WB P b +T WB

[0079] where R WB The attitude quaternion q WB Construct the rotation matrix from the body coordinate system to the world coordinate system.

[0080] The present invention emphasizes combining quaternion coordinate transformation with stereo vision to improve positioning robustness. Stereo vision systems enhance positioning robustness in the following aspects compared to monocular vision by providing direct depth perception.

[0081] In the present invention, the method is applied to autonomous navigation or target tracking tasks of UAVs in complex environments with changing light or dense obstacles;

[0082] It should be noted that the scale is clear: stereo vision directly recovers the absolute scale of the 3D scene (when the baseline length is known), avoiding the scale uncertainty problem common in monocular vision;

[0083] Adaptability to environmental changes: Binocular information provides richer scene features and geometric constraints. In complex environments with sudden changes in lighting conditions, partial texture loss, or the presence of dynamic obstacles, stereo matching and 3D reconstruction are often more robust than monocular methods that rely on continuous frame tracking (such as the pure vision part of monocular SLAM). For example, even if one camera's view is disturbed by strong light or temporarily blocked, the other camera may still provide valid information;

[0084] Obstacle-dense scenes: In scenes with dense obstacles, accurate depth perception is crucial for obstacle avoidance. Stereo vision can provide dense or semi-dense depth maps of nearby obstacles. Combined with the high-precision coordinate transformation proposed in this paper, this enables drones to more reliably perceive and locate these obstacles.

[0085] In the present invention, by providing more accurate and robust target positioning information, it helps to improve the performance and reliability of drones in advanced applications such as autonomous navigation, precise landing, target identification and tracking, and intelligent obstacle avoidance, and is also conducive to enhancing the autonomous navigation and operation capabilities of drones.

Claims

1. A high-precision positioning method for UAVs using stereo vision based on quaternion coordinate transformation, characterized in that: The following steps are involved: Step 1: Obtain at least one pair of stereo images captured by a stereo camera system carried by the UAV; Step 2: Based on at least one pair of stereo images, determine the three-dimensional coordinates P of the target object in the camera coordinate system of the stereo camera system through the stereo vision processing algorithm c =(x c ,y c , z c ) T ; Step 3: Get the attitude quaternion q = (q w ,q x ,q y ,q z ); Step 4: Construct the rotation matrix R(q) based on the attitude quaternion q. The rotation matrix is ​​used to rotate the vector from the camera coordinate system to the body coordinate system. Step 5: Use the rotation matrix R(q) and the preset translation vector T between the camera and the body ext , the camera coordinate system three-dimensional coordinate P of the target object c Transform to the body coordinate system to obtain the three-dimensional coordinates P of the target object in the body coordinate system b =(x b ,y b , z b ) T , the transformation relationship is P b =R(q)P c +T ext Among them, T ext Represents the position vector of the camera coordinate system origin in the body coordinate system.

2. The high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation according to claim 1, characterized in that: The target object determined in step 2 is at the three-dimensional coordinate P c The method includes: extracting and matching feature points of at least one pair of stereo images, calculating the disparity between the matched feature points, and reconstructing the three-dimensional coordinates of the target object by triangulation principle.

3. The high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation according to claim 1, characterized in that: The attitude quaternion q obtained in step 3 is measured in real time by an inertial measurement unit (IMU) carried by the UAV, and is optionally fused with data from other navigation sensors for correction.

4. The high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation according to claim 1, characterized in that: In step 4, the attitude quaternion q=(q w ,q x ,q y ,q z ) The rotation matrix R(q) constructed is specifically expressed as: It is assumed that the attitude quaternion q is a unit quaternion, satisfying 5. The high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation according to claim 1, characterized in that: The translation vector T ext It is a fixed parameter obtained by pre-calibrating the relative position relationship between the stereo camera system carried by the drone and the drone's body coordinate system.

6. The high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation according to claim 1, characterized in that: After step five, it also includes: Step 6: Obtain the global pose information of the drone in the world coordinate system; Step 7: Based on the global pose information of the UAV, the three-dimensional coordinates P of the target object in the body coordinate system are b Transform to world coordinate system.

7. The high-precision positioning method for a stereoscopic drone based on quaternion coordinate transformation according to claim 1, characterized in that: The method is applied to autonomous navigation or target tracking tasks of UAVs in complex environments with changing light or dense obstacles.

Images