Unmanned aerial vehicle track correction method and device
Through the fusion of GNSS and SLAM data and the correction of global optimization algorithm, the deviation problem caused by the accumulation of errors in the stitching of drone aerial photography images is solved, and the stitching accuracy and image presentation effect are improved.
Patent Information
- Application Number
- CN202310543982.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-15
- Publication Date
- 2024-11-19
AI Technical Summary
In the stitching of drone aerial images, due to the accumulation of errors in visual SLAM, the results of puzzles in the later period of the voyage are largely deviated, which seriously affects the presentation effect of the panoramic image.
By obtaining the GNSS data of the drone and the deflection angle information of the carrier, the fusion of GNSS and SLAM data is initialized, and the pose data and map points are corrected using a global optimization improvement algorithm to reduce the impact of error accumulation.
It improves the accuracy of drone aerial photography images, provides more accurate rendering of the ground panoramic image, and reduces the deformation of the puzzle results.
Smart Images

Figure CN118981218A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to unmanned aerial vehicle navigation technology, and in particular to a method and device for correcting a trajectory of an unmanned aerial vehicle. Background Art
[0002] In recent years, with the rapid development of drones and related technologies, drones are becoming more and more professional, more efficient, more flexible, and less expensive. Due to their various excellent characteristics, drones have been widely used in various fields such as surveying and mapping, inspection, exploration, emergency response, and disaster relief.
[0003] An important application direction of drones is ground monitoring. Drones equipped with camera modules can be used to fly in the air to provide a wide aerial view for ground monitoring, allowing users to grasp the ground situation in real time. However, due to the limitations of the drone's flight altitude and the focal length of the digital camera, drone aerial images have many characteristics such as small image size, large number, and limited ground information displayed in a single image, which greatly limits the user's overall information grasp of the monitoring target area. Therefore, using small and large drone aerial images to obtain panoramic ground images with a large field of view through posture calculation, image transformation, image stitching, etc., so as to facilitate users to observe and analyze ground information in an all-round way has become one of the important needs of the drone industry.
[0004] At present, most drone aerial image stitching relies directly on the calculation results of visual SLAM, which calculates the pose of the key frames in the image sequence and the coordinates of the ground points corresponding to the key frames, further calculates the homography matrix of each key frame, and performs image stitching through homography transformation. Simultaneous Localization and Mapping (SLAM) refers to the process in which a subject equipped with a specific sensor builds an environmental model and estimates its own motion during movement without prior information about the environment; if the sensor here is a camera, it is called visual SLAM. Since SLAM simultaneously estimates pose and builds maps during the movement of the carrier, it has many advantages such as dynamic, real-time, and observable, which is very suitable for the needs of drone aerial photography and is currently a hot research direction in the field of drones.
[0005] During initialization, visual SLAM constructs a SLAM coordinate system based on the pose of the first initialized key frame. The direction and scale of this coordinate system are not fixed. When drone aerial photography puzzles are performed based on this system, the resulting panoramic image cannot describe the direction of the target area in the real world, and the pose needs to be converted into application-level coordinates later. Moreover, it directly relies on visual SLAM, and due to the problem of error accumulation, large deviations occur in the later stages of the flight, and the puzzle results are severely deformed, which greatly affects the presentation of the final panoramic image.
[0006] Therefore, how to provide a method that can effectively reduce the impact caused by error accumulation is a problem that needs to be solved urgently. Summary of the invention
[0007] The present invention provides a method and device for correcting the trajectory of a drone to solve the problem that due to the error accumulation of visual SLAM, large deviations occur in the later stages of the flight, the puzzle results are seriously deformed, and the presentation effect of the final panoramic image is greatly affected. The specific solution is as follows:
[0008] In a first aspect, the present application discloses a method for correcting a drone trajectory, comprising:
[0009] Obtaining GNSS data of the UAV and deflection angle information of the carrier and the pod, wherein the GNSS data includes geographic location information of the UAV at a time corresponding to a key frame;
[0010] Initialize the data fusion of GNSS and SLAM according to the GNSS data, where the SLAM data includes the pose data of the key frames of the drone aerial image sequence and the map points corresponding to the key frames;
[0011] In the data fusion initialization stage, the pose data and the map points are corrected using a global optimization improvement algorithm;
[0012] After the initialization is completed, during the operation of the SLAM system, when the SLAM system receives a data request initiated by the application layer, the global optimization improvement algorithm is used to correct the pose data and corresponding map points of all the current key frames in the SLAM system;
[0013] The corrected pose data and map points are transmitted to the application layer.
[0014] Optionally, the global optimization improvement algorithm specifically includes:
[0015] Rotate the pose data and map points in the GNSS coordinate system and SLAM coordinate system to the set correction coordinate system respectively;
[0016] Based on the scale between the coordinate systems, calculate the distance deviation between the position of the current key frame GNSS data in the corrected coordinate system and the exposure point of the current key frame in the corrected coordinate system;
[0017] If the distance deviation is greater than a preset error threshold, the global BA optimization data is nonlinearly optimized using the GNSS data.
[0018] Optionally, the GNSS coordinate system adopts the GPS north-east coordinate system, the SLAM coordinate system is a coordinate system constructed based on the posture of the initial reference key frame and used to describe the SLAM system, the revised coordinate system is a Cartesian three-dimensional coordinate system with the initial reference key frame as the origin, the south direction as the positive semi-axis of the Y axis, the east direction as the positive semi-axis of the X axis, and the direction toward the center of the earth as the positive semi-axis of the Z axis under the SLAM algorithm framework, and the steps of rotating the posture data and map points in the GNSS coordinate system and the SLAM coordinate system to the set revised coordinate system specifically include:
[0019] The pose data and map points of the GNSS coordinate system are converted to the GNSS corrected coordinate system, which is a Cartesian three-dimensional coordinate system with the position of the initial reference key frame as the origin, the south direction as the positive semi-axis of the Y axis, the east direction as the positive semi-axis of the X axis, and the direction toward the center of the earth as the positive semi-axis of the Z axis under the GPS geographic location framework;
[0020] Get the scale between the GNSS system scale and the SLAM system scale;
[0021] Convert the pose data and map points of the GNSS corrected coordinate system to the corrected coordinate system;
[0022] Convert the pose data and map points of the SLAM coordinate system to the corrected coordinate system.
[0023] Optionally, the step of converting the pose data and map points of the GNSS coordinate system to the GNSS corrected coordinate system specifically includes:
[0024] Obtaining the camera posture of the initial reference key frame in the GPS north-east coordinate system according to the geographic location information, the deflection angle information of the carrier aircraft and the deflection angle information of the pod;
[0025] Obtaining a rotation matrix of the initial reference key frame in the GNSS corrected coordinate system according to the camera posture of the initial reference key frame in the GPS north-east coordinate system and the rotation matrix from the GPS north-east coordinate system to the GNSS corrected coordinate system;
[0026] The geographic location information of the initial reference key frame is obtained, and according to the rotation matrix of the initial reference key frame in the GNSS corrected coordinate system, it is converted from the GPS north-east coordinate system to the GNSS corrected coordinate system, so as to calculate the location information of the geographic location information corresponding to the subsequent key frames in the GNSS corrected coordinate system.
[0027] Optionally, the step of obtaining a scale Scale between the GNSS system scale and the SLAM system scale specifically includes:
[0028] The position information of the UAV in the GNSS corrected coordinate system at the second key frame time [X w2 ,Y w2 ,Z w2 ], and the position information of the exposure point of the camera corresponding to the second key frame in the corrected coordinate system [X c2 ,Y c2 ,Z c2 ], calculate the scale between the GNSS system scale and the SLAM system scale, where
[0029]
[0030] Optionally, the step of converting the pose data and map points of the GNSS corrected coordinate system to the corrected coordinate system specifically includes:
[0031] Convert the position coordinate t of the current frame in the GNSS correction coordinate system to the GNSS correction translation coordinate t in the correction coordinate system slam_gnss ,in
[0032] t slam_gnss =Scale*t.
[0033] Optionally, the step of converting the pose data and map points of the SLAM coordinate system to the corrected coordinate system specifically includes:
[0034] The rotation matrix of the initial reference key frame in the GNSS correction coordinate system is used as the correction coefficient, and according to the rotation matrix from the current key frame to the initial reference key frame and the translation matrix from the current key frame to the initial reference key frame in the SLAM coordinate system, the pose data and map point coordinates converted from the SLAM coordinate system to the correction coordinate system are obtained.
[0035] Optionally, in the UAV trajectory correction method according to claim 7, the optimization objective function for performing the nonlinear optimization is:
[0036]
[0037] Among them, E(X KF ,X MP ) is the global BA optimization error term, F(X KF ) is the GNSS correction error term;
[0038] In the global BA optimization error term, N is the total number of key frames. is the key frame pose data corresponding to the kth error term, is the map point corresponding to the kth error term, z k is the map point in the current error term The coordinates of the key points matched in the current key frame; K is the camera internal parameter of the drone; information matrix Ω k =invSigma 2 *E, represents the inconsistency of each component; invSigma is the inverse of the scaling factor of the current frame in the corresponding level of the image pyramid in the SLAM system, and E is the unit matrix;
[0039] In the GNSS correction error term, M is the number of all key frames; the information matrix Indicates the inconsistency of each component; weight is the total number of map points included in the current key frame, scale K The transformation scale from the correction coordinate system to the image coordinate system is of the same magnitude as the camera intrinsic focal length f x Consistent.
[0040] Optionally, in the UAV trajectory correction method as described in claim 8, the global BA optimization error term in the optimization objective function is optimized by the least squares method.
[0041] In a second aspect, the present application discloses a drone trajectory correction device, comprising:
[0042] A data acquisition unit, used to acquire GNSS data of the UAV and deflection angle information of the carrier and the pod, wherein the GNSS data includes geographic location information of the UAV at a time corresponding to a key frame;
[0043] A data initialization unit, used for initializing the data fusion of GNSS and SLAM according to the GNSS data, wherein the SLAM data includes the pose data of the key frames of the drone aerial image sequence and the map points corresponding to the key frames;
[0044] A first data optimization unit, used to correct the position data and the map points using a global optimization improvement algorithm in a data fusion initialization stage;
[0045] The second data optimization unit is used to correct the pose data and corresponding map points of all current key frames in the SLAM system using a global optimization improvement algorithm when the SLAM system receives a data request initiated by the application layer;
[0046] The data transmission unit is used to transmit the corrected posture data and map points to the application layer.
[0047] According to a third aspect of an embodiment of the present invention, there is provided an electronic device, including:
[0048] One or more processors; a memory; one or more applications, wherein the one or more applications are stored in the memory and configured to be executed by the one or more processors, and the one or more programs are configured to execute the method as described in the first aspect.
[0049] According to a fourth aspect of an embodiment of the present invention, a computer-readable storage medium is provided, in which a program code is stored. The program code can be called by a processor to execute the method described in the first aspect.
[0050] In summary, the present invention provides a method and device for correcting the trajectory of a UAV, which constrains and optimizes the SLAM process based on the UAV GNSS data, optimizes and corrects the posture of the SLAM camera exposure point on the basis of ensuring that the SLAM coordinate direction scale corresponds to the actual geographical location, and obtains more accurate posture trajectory data for UAV puzzle application, which can effectively improve the accuracy of UAV aerial image stitching and provide users with more accurate ground panoramic image presentation. BRIEF DESCRIPTION OF THE DRAWINGS
[0051] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.
[0052] Figure 1 A method flow chart of a method for correcting a drone trajectory according to an embodiment of the present invention;
[0053] Figure 2 Schematic diagram of the execution flow of the UAV trajectory correction method according to an embodiment of the present invention;
[0054] Figure 3 A flowchart of a method for correcting a drone trajectory according to an embodiment of the present invention;
[0055] Figure 4 This is a functional module block diagram of a UAV trajectory correction device according to an embodiment of the present invention;
[0056] Figure 5 This is a structural block diagram of an electronic device for executing a method for correcting a drone trajectory according to an embodiment of the present application;
[0057] Figure 6 It is a structural block diagram of a computer-readable storage medium for storing or carrying program codes for implementing a method for correcting the trajectory of a drone according to an embodiment of the present application.
[0058] Reference numerals:
[0059] Data acquisition unit 110; data initialization unit 120; first data optimization unit 130; second data optimization unit 140; data transmission unit 150; electronic device 300; processor 310; memory 320; computer readable storage medium 400; program code 410. DETAILED DESCRIPTION
[0060] At present, most drone aerial image stitching relies directly on the calculation results of visual SLAM, which calculates the pose of the key frames in the image sequence and the coordinates of the ground points corresponding to the key frames, further calculates the homography matrix of each key frame, and performs image stitching through homography transformation. Simultaneous Localization and Mapping (SLAM) refers to the process in which a subject equipped with a specific sensor builds an environmental model and estimates its own motion during movement without prior information about the environment; if the sensor here is a camera, it is called visual SLAM. Since SLAM simultaneously estimates pose and builds maps during the movement of the carrier, it has many advantages such as dynamic, real-time, and observable, which is very suitable for the needs of drone aerial photography and is currently a hot research direction in the field of drones.
[0061] During initialization, visual SLAM constructs a SLAM coordinate system based on the pose of the first initialized key frame. The direction and scale of this coordinate system are not fixed. When drone aerial photography puzzles are performed based on this system, the resulting panoramic image cannot describe the direction of the target area in the real world, and the pose needs to be converted into application-level coordinates later. Moreover, it directly relies on visual SLAM, and due to the problem of error accumulation, large deviations occur in the later stages of the flight, and the puzzle results are severely deformed, which greatly affects the presentation of the final panoramic image.
[0062] Therefore, how to provide a method that can effectively reduce the impact caused by error accumulation is a problem that needs to be solved urgently.
[0063] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings 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. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.
[0064] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0065] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.
[0066] In the description of the present invention, it should be noted that the terms "top", "bottom", "inside", "outside", etc. indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, or the orientation or positional relationship in which the invented product is usually placed when in use, which is only for the convenience of describing the present invention and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", etc. are only used to distinguish the description, and cannot be understood as indicating or implying relative importance.
[0067] In the description of the present invention, it is also necessary to explain that, unless otherwise clearly specified and limited, the terms "set", "install", "connect", and "connect" 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 mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.
[0068] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.
[0069] like Figure 1 As shown, the present invention provides a method for correcting the trajectory of an unmanned aerial vehicle, comprising:
[0070] Step S101, obtaining GNSS data of the UAV and deflection angle information of the carrier aircraft and the deflection angle information of the pod, wherein the GNSS data includes geographic location information of the UAV at a time corresponding to a key frame.
[0071] Step S102, initializing the data fusion of GNSS and SLAM, where the SLAM data includes the pose data of the key frames of the drone aerial image sequence and the map points corresponding to the key frames.
[0072] Step S103, in the data fusion initialization stage, the pose data and the map points are corrected using a global optimization improvement algorithm.
[0073] Step S104, during the operation of the SLAM system after initialization, when the SLAM system receives a data request initiated by the application layer, the global optimization improvement algorithm is used to correct the pose data of all current key frames in the SLAM system and the corresponding map points.
[0074] Step S105, transmitting the corrected posture data and map points to the application layer.
[0075] In the existing SLAM system, the global BA (Bundle Adjustment) optimization of the SLAM system itself is to process all the key frames and map points in the global map after all the work in the LoopClosing thread is completed, and optimize the pose of the key frames and the 3D coordinates of the map points together. The overall idea of the present invention is to fuse GNSS data and SLAM data on the basis of the global BA optimization of SLAM, and correct the SLAM data through GNSS data.
[0076] In specific implementation, such as Figure 2 As shown, the correction of SLAM data is divided into two stages. The first stage is the stage of data initialization after the SLAM system is started. The object of execution is the data in the SLAM system, including the pose data of all key frames and the 3D coordinates of map points. After the execution of the complete global optimization improvement algorithm, the corrected data is transmitted back to the SLAM system. The main purpose of the correction in the first stage is to provide the SLAM system with more accurate initial pose and map point data. So far, before the SLAM system is running, the global optimization improvement algorithm is no longer called for correction until the second stage. The second stage of correcting the data is during the operation of the SLAM system. When the SLAM system receives the data request from the application layer, the corresponding global BA optimization data needs to be transmitted to the application layer. At this time, the global optimization improvement algorithm is called again for correction of the transmitted data to further improve the accuracy of the data and reduce error accumulation.
[0077] As a specific implementation of the embodiment of the present invention, the global optimization improved algorithm used in the above step S103 and step S104 has an algorithm flow as follows: Figure 3 Shown, including
[0078] Step S201, respectively rotating the pose data and map points in the GNSS coordinate system and the SLAM coordinate system to the set correction coordinate system.
[0079] For different coordinate systems, the representation of pose data and map points is not uniform, which makes it difficult to judge the size of the error and to correct or optimize the data. Therefore, it is necessary to rotate the pose data and map points in the GNSS coordinate system and the SLAM coordinate system to the same coordinate system.
[0080] As a preferred embodiment, a self-defined correction coordinate system is selected as the rotation target, and subsequent data correction is also performed in this coordinate system. The correction coordinate system is a Cartesian three-dimensional coordinate system with the initial reference key frame as the origin, the south direction as the positive semi-axis of the Y axis, the east direction as the positive semi-axis of the X axis, and the direction toward the center of the earth as the positive semi-axis of the Z axis under the SLAM algorithm framework.
[0081] The type of GNSS coordinate system corresponds to the type of GNSS data obtained. As a specific implementation of an embodiment of the present invention, the GNSS coordinate system can select any currently available coordinate system. In this embodiment, the GNSS coordinate system is the GPS North East Earth (NED) coordinate system, which is one of the GNSS coordinate systems. In other implementations, other types of GNSS coordinate systems can also be used. In order to facilitate calculation, the pose data and map points of the GNSS coordinate system are first converted to the GNSS corrected coordinate system, and after obtaining the scale Scale between the GNSS system scale and the SLAM system scale, the pose data and map points of the GNSS corrected coordinate system are converted to the corrected coordinate system. The GNSS corrected coordinate system is a Cartesian three-dimensional coordinate system under the GPS geographic location framework, with the location of the initial reference key frame as the origin, the south direction as the positive semi-axis of the Y axis, the east direction as the positive semi-axis of the X axis, and the direction toward the center of the earth as the positive semi-axis of the Z axis.
[0082] For the pose data and map points of the SLAM coordinate system, they can be directly converted to the corrected coordinate system. The specific conversion steps are as follows:
[0083] 1. Convert the pose data and map points of the GNSS coordinate system to the GNSS corrected coordinate system.
[0084] Specifically, the camera posture R of the initial reference key frame in the GPS north-east coordinate system is obtained according to the geographic location information [Lat, Lon, HMSL] (latitude, longitude, altitude), the deflection angle information [Heading, Pitch, Roll] (yaw angle, pitch angle, roll angle) of the carrier aircraft and the deflection angle information [Pan, Tilt, Roll] (yaw angle, pitch angle, roll angle) of the pod. w1 T ; Among them, R w1 =R2*R1, R w1 T For R wTransposed matrix.
[0085]
[0086]
[0087] According to the camera posture of the initial reference key frame in the GPS north-east coordinate system and the rotation matrix from the GPS north-east coordinate system to the GNSS corrected coordinate system, the rotation matrix R of the initial reference key frame in the GNSS corrected coordinate system is obtained. ini ;
[0088] R ini =R ref T *R w1 T *R ref ,in, is the rotation matrix from the GPS north-east coordinate system to the GNSS corrected coordinate system, R ref T For R ref Transposed matrix.
[0089] The geographic location information of the initial reference key frame is obtained, and according to the rotation matrix of the initial reference key frame in the GNSS corrected coordinate system, it is converted from the GPS north-east coordinate system to the GNSS corrected coordinate system, so as to calculate the location information of the geographic location information corresponding to the subsequent key frames in the GNSS corrected coordinate system.
[0090] The initial reference keyframe corresponds to the UAV geographic location [Lat1, Lon1, HMSL1]. Convert it from the GNSS north-east coordinate system to the GNSS corrected coordinate system, and then calculate the UAV geographic location [Lat i ,Lon i ,HMSL i ] The position information t in the GNSS corrected coordinate system is t=[X wi ,Y wi ,Z wi ]:
[0091] Among them, X wi =(Lon i *f-Lon1*f)*(R E +HMSL1)*cos(Lat1*f)
[0092] Y wi =-(Lat i *f-Lat1*f)*(R N +HMSL1)
[0093] Zwi =-(HMSL i -HMSL1)
[0094] Where f = π / 180, the meridian curvature radius Radius of curvature of the circle a is the major semi-axis of the ellipsoid, and e is the eccentricity of the ellipsoid.
[0095] 2. Obtain the scale between the GNSS system scale and the SLAM system scale. The position information of the UAV in the GNSS corrected coordinate system at the second key frame time [X w2 ,Y w2 ,Z w2 ], and the position information of the exposure point of the camera corresponding to the second key frame in the corrected coordinate system [X c2 ,Y c2 ,Z c2 ], calculate the scale between the GNSS system scale and the SLAM system scale, where
[0096] .
[0097] 3. Convert the pose data and map points of the GNSS corrected coordinate system to the corrected coordinate system. Convert the position coordinate t of the current frame in the GNSS corrected coordinate system to the GNSS corrected translation coordinate t in the corrected coordinate system. slam_gnss =Scale*t, the coordinate is the translation of the current key frame of the drone relative to the reference key frame GNSS in the corrected coordinate system.
[0098] 4. Convert the pose data and map points of the SLAM coordinate system to the corrected coordinate system. Use the rotation matrix of the initial reference key frame in the GNSS corrected coordinate system as the correction coefficient R ini According to the rotation matrix from the current key frame to the initial reference key frame and the translation matrix from the current key frame to the initial reference key frame in the SLAM coordinate system, the pose data and map point coordinates converted from the SLAM coordinate system to the corrected coordinate system are obtained.
[0099] Specifically, the pose T of the corrected coordinate system cw r =[R cw r t wc ]=[R cw *R ini t wc ], where T cw =[R cw t wc] is the transformation matrix of the current SLAM key frame equivalent to the initial reference key frame, R cw is the rotation matrix from the current SLAM key frame to the initial key reference frame, t wc It is the translation matrix from the current SLAM key frame to the initial reference key frame;
[0100] Corrected coordinate system diagram point coordinates p r =R ini T *p, p is the coordinate of the SLAM map point.
[0101] In particular, the transformation matrix of the SLAM initial reference keyframe is T cw =[E 0], E is the 3*3 unit matrix, 0 is the 3*1 zero matrix, and the modified SLAM initial reference key frame transformation matrix is T cw r =[R cw r t wc ]. All pose optimizations thereafter are performed based on the corrected coordinate system.
[0102] Through the above four steps, the coordinate system transformation is completed and data comparison can be performed.
[0103] Step S202 , based on the scale between the coordinate systems, calculate the distance deviation between the position of the current key frame GNSS data in the corrected coordinate system and the exposure point of the current key frame in the corrected coordinate system.
[0104] In this embodiment, the scale is a scale between the GNSS system scale and the SLAM system scale.
[0105] The method for calculating the distance deviation between the position of the current key frame GNSS data in the corrected coordinate system and the exposure point of the current key frame in the corrected coordinate system, in this embodiment, calculates the distance deviation by calculating the Euclidean distance. As other implementations of this embodiment, a suitable method can be selected from the existing algorithms for calculating the position deviation.
[0106] The error threshold for pose data optimization is set as:
[0107] threshold=Scale*ΔX
[0108] Wherein, ΔX is a preset acceptable distance deviation radius of the UAV, and in this embodiment, it is the Euclidean distance in the GNSS corrected coordinate system. This threshold will be used as a judgment condition for posture optimization to determine the optimization strategy of the key frame posture and map data of the SLAM system.
[0109] Step S303: If the distance deviation is greater than a preset error threshold, nonlinear optimization is performed on the global BA optimization data using the GNSS data.
[0110] The optimization strategy for key frame pose and map data is:
[0111] It is determined whether Δt is less than the set threshold value.
[0112] Among them, Δt is the difference between the position of the drone in the current key frame in the corrected coordinate system (converted from GNSS data) and the position of the exposure point of the current key frame in the corrected coordinate system (which is the unoptimized pose data of the SLAM coordinate system).
[0113] If Δt<the set threshold, it is considered that the calculation result of the SLAM system is close to the GNSS data, and there is no need to correct the corresponding posture. Otherwise, it is considered that the calculation result of the SLAM system has a large deviation. The GNSS data is used to correct the SLAM calculation result, and the key frame posture is nonlinearly optimized by integrating GNSS data to reduce the cumulative error of the key frame posture calculation, ensure the accuracy of the posture trajectory and mapping, and correct the UAV trajectory.
[0114] As a preferred implementation of an embodiment of the present invention, for the posture data that needs to be corrected, the optimization objective function of the nonlinear optimization is as shown in the following formula, and the optimization objective function is the minimum sum of the global BA optimization error term and the GNSS correction error term.
[0115]
[0116] Among them, E(X KF ,X MP ) is the global BA optimization error term, F(X KF ) is the GNSS correction error term;
[0117] In the global BA optimization error term, N is the total number of key frames, and the error is the key frame pose data corresponding to the kth error term, is the map point corresponding to the kth error term, z k is the map point in the current error term The coordinates of the key points matched in the current key frame (this value can be calculated by the SLAM system); K is the camera internal parameter of the drone; information matrix Ω k =invSigma 2*E represents the inconsistency of each component; invSigma is the reciprocal of the scaling factor of the current frame in the image pyramid of the SLAM system, and E is the identity matrix;
[0118] In the GNSS correction error term, M is the number of all key frames; the error where obs = (000) T is the error target vector; the information matrix represents the inconsistency of each component; weight is the total number of map points included in the current key frame, scale K is the transformation scale from the correction coordinate system to the image coordinate, and its order of magnitude is consistent with the focal length f of the camera internal parameters x consistent.
[0119] Furthermore, for the global BA optimization error term in the above global optimization improvement algorithm, its optimization method is the least squares method.
[0120] Specifically, its non-linear optimization strategy is:
[0121] Δt = ||t wc -t slam_gnss ||2, where ||x||2 is the 2-norm of x.
[0122] When Δt ≥ threshold, the Jacobian matrix J of the current error term is
[0123]
[0124] where P′^ represents P′ = T cw r *t slam_gnss +t wc the skew-symmetric matrix of the vector.
[0125] Correspondingly, when Δt < threshold, the Jacobian matrix J of the current error term is a 3×6 zero matrix, that is, no optimization correction is required.
[0126] In summary, the UAV trajectory correction method provided by the embodiments of the present invention optimizes and adjusts the visual SLAM initialization process using UAV GNSS data, and uses the Euclidean distance threshold to judge, constraints and adjusts the key frame poses and map points during the calculation process to obtain more accurate key frame poses. With a more accurate camera exposure point trajectory, applying its optimization results to, for example, UAV aerial photography image stitching applications can more accurately describe the target and improve the accuracy of the stitching result.
[0127] Such as Figure 4As shown, the UAV trajectory correction device provided by the present invention is implemented, and the system includes:
[0128] The data acquisition unit 110 is used to acquire the GNSS data of the UAV and the deflection angle information of the carrier and the deflection angle information of the pod, wherein the GNSS data includes the geographic location information of the UAV at the corresponding time of the key frame;
[0129] A data initialization unit 120 is used to initialize the data fusion of GNSS and SLAM according to the GNSS data, where the SLAM data includes the pose data of the key frames of the drone aerial image sequence and the map points corresponding to the key frames;
[0130] A first data optimization unit 130, configured to modify the position and posture data and the map points using a global optimization improvement algorithm in a data fusion initialization phase;
[0131] The second data optimization unit 140 is used to correct the pose data and corresponding map points of all current key frames in the SLAM system using a global optimization improvement algorithm when the SLAM system receives a data request initiated by the application layer;
[0132] The data transmission unit 150 is used to transmit the corrected posture data and map points to the application layer.
[0133] The drone trajectory correction device provided in the embodiment of the present invention is used to implement the above-mentioned drone trajectory correction method, so the specific implementation method is the same as the above-mentioned method and will not be repeated here.
[0134] like Figure 5 As shown, a structural block diagram of an electronic device 300 provided in an embodiment of the present invention. The electronic device 300 may be an electronic device 300 capable of running applications, such as a smart phone, a tablet computer, an e-book, etc. The electronic device 300 in the present application may include one or more of the following components: a processor 310, a memory 320, and one or more applications, wherein the one or more applications may be stored in the memory 320 and configured to be executed by one or more processors 310, and the one or more programs are configured to execute the method described in the aforementioned method embodiment.
[0135] The processor 310 may include one or more processing cores. The processor 310 uses various interfaces and lines to connect various parts of the entire electronic device 300, and executes various functions and processes data of the electronic device 300 by running or executing instructions, programs, code sets or instruction sets stored in the memory 320, and calling data stored in the memory 320. Optionally, the processor 310 can be implemented in at least one hardware form of digital signal processing (DSP), field-programmable gate array (FPGA), and programmable logic array (PLA). The processor 310 can integrate one or a combination of a central processing unit (CPU), a graphics processing unit (GPU), and a modem. Among them, the CPU mainly processes the operating system, user interface, and application programs; the GPU is responsible for rendering and drawing display content; and the modem is used to process wireless communications. It can be understood that the above-mentioned modem may not be integrated into the processor 310, but may be implemented separately through a communication chip.
[0136] The memory 320 may include a random access memory (RAM) or a read-only memory (ROM). The memory 320 may be used to store instructions, programs, codes, code sets or instruction sets. The memory 320 may include a program storage area and a data storage area, wherein the program storage area may store instructions for implementing an operating system, instructions for implementing at least one function (such as a touch function, a sound playback function, an image playback function, etc.), instructions for implementing the following various method embodiments, etc. The data storage area may also store data created by the terminal during use (such as a phone book, audio and video data, chat record data), etc.
[0137] like Figure 6 FIG. 4 is a block diagram of a computer-readable storage medium 400 provided in an embodiment of the present invention. The computer-readable storage medium stores program code 410, which can be called by a processor to execute the method described in the above method embodiment.
[0138] The computer readable storage medium 400 may be an electronic memory such as a flash memory, an EEPROM (electrically erasable programmable read-only memory), an EPROM, a hard disk, or a ROM. Optionally, the computer readable storage medium 400 includes a non-transitory computer-readable storage medium. The computer readable storage medium 400 has storage space for program codes 410 that perform any of the method steps in the above method. These program codes 410 may be read from or written to one or more computer program products. The program codes 410 may be compressed, for example, in an appropriate form.
[0139] In summary, the present invention provides a method and device for correcting the trajectory of a UAV, which constrains and optimizes the SLAM process based on the UAV GNSS data, optimizes and corrects the posture of the SLAM camera exposure point on the basis of ensuring that the SLAM coordinate direction scale corresponds to the actual geographical location, and obtains more accurate posture trajectory data for UAV puzzle application, which can effectively improve the accuracy of UAV aerial image stitching.
[0140] In several embodiments disclosed in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of the code, and a module, a program segment or a part of the code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.
[0141] In addition, the functional modules in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
[0142] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.
Claims
1. A method for correcting the trajectory of a drone. It is characterized in that include: Obtaining GNSS data of the UAV and deflection angle information of the carrier and the pod, wherein the GNSS data includes geographic location information of the UAV at a time corresponding to a key frame; Initialize the data fusion of GNSS and SLAM according to the GNSS data, where the SLAM data includes the pose data of the key frames of the drone aerial image sequence and the corresponding map points; In the data fusion initialization stage, the pose data and the map points are corrected using a global optimization improvement algorithm; During the operation of the SLAM system after initialization, when the SLAM system receives a data request initiated by the application layer, it uses the global optimization improvement algorithm to correct the pose data of all current key frames in the SLAM system and the corresponding map points; The corrected pose data and map points are transmitted to the application layer.
2. The method for correcting the trajectory of a drone as claimed in claim 1, It is characterized in that The global optimization improved algorithm specifically includes: Rotate the pose data and map points in the GNSS coordinate system and SLAM coordinate system to the set correction coordinate system respectively; Based on the scale between the coordinate systems, calculate the distance deviation between the position of the current key frame GNSS data in the corrected coordinate system and the exposure point of the current key frame in the corrected coordinate system; If the distance deviation is greater than a preset error threshold, the global BA optimization data is nonlinearly optimized using the GNSS data.
3. The method for correcting the trajectory of a drone as claimed in claim 2, It is characterized in that The GNSS coordinate system adopts the GPS north-east coordinate system, the SLAM coordinate system is a coordinate system for describing the SLAM system based on the position of the initial reference key frame, and the modified coordinate system is a Cartesian three-dimensional coordinate system with the initial reference key frame as the origin, the south direction as the positive semi-axis of the Y axis, the east direction as the positive semi-axis of the X axis, and the direction toward the center of the earth as the positive semi-axis of the Z axis under the SLAM algorithm framework. The steps of rotating the pose data and map points in the GNSS coordinate system and the SLAM coordinate system to the set correction coordinate system specifically include: The pose data and map points of the GNSS coordinate system are converted to the GNSS corrected coordinate system, which is a Cartesian three-dimensional coordinate system with the position of the initial reference key frame as the origin, the south direction as the positive semi-axis of the Y axis, the east direction as the positive semi-axis of the X axis, and the direction toward the center of the earth as the positive semi-axis of the Z axis under the GPS geographic location framework; Get the scale between the GNSS system scale and the SLAM system scale; Convert the pose data and map points of the GNSS corrected coordinate system to the corrected coordinate system; Convert the pose data and map points of the SLAM coordinate system to the corrected coordinate system.
4. The method for correcting the trajectory of a drone as claimed in claim 3, It is characterized in that The step of converting the pose data and map points of the GNSS coordinate system to the GNSS corrected coordinate system specifically includes: Obtaining the camera posture of the initial reference key frame in the GPS north-east coordinate system according to the geographic location information, the deflection angle information of the carrier aircraft and the deflection angle information of the pod; Obtaining a rotation matrix of the initial reference key frame in the GNSS corrected coordinate system according to the camera posture of the initial reference key frame in the GPS north-east coordinate system and the rotation matrix from the GPS north-east coordinate system to the GNSS corrected coordinate system; The geographic location information of the initial reference key frame is obtained, and according to the rotation matrix of the initial reference key frame in the GNSS corrected coordinate system, it is converted from the GPS north-east coordinate system to the GNSS corrected coordinate system, so as to calculate the location information of the geographic location information corresponding to the subsequent key frames in the GNSS corrected coordinate system.
5. The method for correcting the trajectory of a drone as claimed in claim 4, It is characterized in that The step of obtaining the scale Scale between the GNSS system scale and the SLAM system scale specifically includes: The position information of the UAV in the GNSS corrected coordinate system at the second key frame time [X w2 ,Y w2 ,Z w2 ], and the position information of the exposure point of the camera corresponding to the second key frame in the corrected coordinate system [X c2 ,Y c2 ,Z c2 ], calculate the scale between the GNSS system scale and the SLAM system scale, where 6. The method for correcting the trajectory of a drone as claimed in claim 5, It is characterized in that The step of converting the pose data and map points of the GNSS corrected coordinate system to the corrected coordinate system specifically includes: Convert the position coordinate t of the current frame in the GNSS correction coordinate system to the GNSS correction translation coordinate t in the correction coordinate system slam_gnss ,in t slam_gnss =Scale*t。 7. The method for correcting the trajectory of a drone as claimed in claim 4, It is characterized in that The step of converting the pose data and map points of the SLAM coordinate system to the corrected coordinate system specifically includes: The rotation matrix of the initial reference key frame in the GNSS correction coordinate system is used as the correction coefficient, and according to the rotation matrix from the current key frame to the initial reference key frame and the translation matrix from the current key frame to the initial reference key frame in the SLAM coordinate system, the pose data and map point coordinates converted from the SLAM coordinate system to the correction coordinate system are obtained.
8. The method for correcting the trajectory of a drone as claimed in claim 7, It is characterized in that The optimization objective function for the nonlinear optimization is: Among them, E(X KF ,X MP ) is the global BA optimization error term, F(X KF ) is the GNSS correction error term; In the global BA optimization error term, N is the total number of key frames. is the key frame pose data corresponding to the kth error term, is the map point corresponding to the kth error term, z k is the map point in the current error term The coordinates of the key points matched in the current key frame; K is the camera internal parameter of the drone; information matrix Ω k =invSigma 2 *E, represents the inconsistency of each component; invSigma is the inverse of the scaling factor of the current frame in the corresponding level of the image pyramid in the SLAM system, and E is the unit matrix; In the GNSS correction error term, M is the number of all key frames; the information matrix Indicates the inconsistency of each component; weight is the total number of map points included in the current key frame, scale K The transformation scale from the correction coordinate system to the image coordinate system is of the same magnitude as the camera intrinsic focal length f x Consistent.
9. The method for correcting the trajectory of a drone as claimed in claim 8, It is characterized in that The global BA optimization error term in the optimization objective function is optimized by the least squares method.
10. A drone trajectory correction device, It is characterized in that include: A data acquisition unit, used to acquire GNSS data of the UAV and deflection angle information of the carrier and the pod, wherein the GNSS data includes geographic location information of the UAV at a time corresponding to a key frame; A data initialization unit, used for initializing the data fusion of GNSS and SLAM according to the GNSS data, wherein the SLAM data includes the pose data of the key frames of the drone aerial image sequence and the map points corresponding to the key frames; A first data optimization unit, used to correct the position data and the map points using a global optimization improvement algorithm in a data fusion initialization stage; The second data optimization unit is used to correct the pose data and corresponding map points of all current key frames in the SLAM system using a global optimization improvement algorithm when the SLAM system receives a data request initiated by the application layer; The data transmission unit is used to transmit the corrected posture data and map points to the application layer.
Citation Information
Cited By
Laser radar SLAM and GNSS unmanned aerial vehicle global inspection system and method
CN120652994A
