Autonomous tracking flight control method of UAV driven by NDT task in GNSS-denied environment

Abstract

The present invention discloses a UAV autonomous tracking flight control method driven by a beam bottom NDT task in a GNSS-denied environment, which solves the automation problem of autonomous tracking operation of multi-rotor UAVs on the surface of large bridges and the problem of high-precision orthophoto shooting, and effectively improves the quality of UAV image acquisition, as well as the autonomy and intelligence when performing beam bottom defect detection tasks. The method is as follows: planning the flight track of the UAV at the beam bottom; guiding the UAV to autonomously track the beam bottom according to the planned track, and during the autonomous tracking flight, optimizing the UAV attitude measured by the IMU and the three-dimensional laser radar using a sliding window optimization unit combined with a priori constraints on the spatial position, so as to estimate the posture of the traversed node at the next moment; determining the orthophoto posture of the interest area at the beam bottom; estimating the posture and position of the UAV in the beam bottom coordinate system that needs to be adjusted according to the center point of the fitted beam bottom local plane, the orthophoto direction and the shooting distance, and adjusting the UAV posture and position in place.

Description

Technical Field

[0001] The present invention relates to the cross-fields of bridge defect detection and UAV autonomous navigation and artificial intelligence when GNSS at the bottom of a beam is unreliable, and in particular to a high-precision UAV autonomous tracking UAV posture estimation and control method for visual non-destructive detection of beam bottom defects. Background Art

[0002] At present, the health monitoring of large concrete (geotechnical) structures such as bridges still requires partial or complete manual operation, which is inefficient and dangerous. In recent years, drones have shown great practical potential and application value in the field of bridge health monitoring, but the degree of automation and work efficiency still need to be improved, and the error is also large. Using drones to autonomously track and fully cover the bottom of bridge beams to collect images can significantly reduce the manpower and material resources for bridge health monitoring and improve work efficiency and safety. The drones autonomously track and collect measurable image sequences of the bottom surface of bridges and piers to detect whether there are cracks and other diseases, and timely maintenance of bridges has important engineering and social benefits.

[0003] The environment under the bridge beam is complex, and autonomous tracking operation, high-precision orthophoto imaging, and disease location are still very challenging issues. Figure 3 ), the bottom of the beam usually has the characteristics of insufficient texture, weak lighting and unreliable GNSS signals. It is difficult for UAVs to perform the task of beam bottom defect detection and it is difficult to achieve autonomous tracking operation. At the same time, imaging posture is also an important factor limiting the imaging quality. Summary of the invention

[0004] In view of this, the present invention uses a radar-inertial odometer and a coordinated wide-field navigation optical camera to obtain the real-time, reliable and accurate posture of the UAV at the bottom of the beam. According to the path planning UAV posture and the orthophoto imaging posture Euler angle parameters estimated by the local surface normal vector of the autonomous tracking, the posture adjustment unit is used to achieve high-precision orthophoto imaging posture control and obtain high-precision NDT images.

[0005] The present invention solves the automation problem of autonomous tracking operation of multi-rotor UAVs on the surface of large bridges and the problem of high-precision orthophoto photography, effectively improving the quality of images collected by UAVs, as well as the autonomy and intelligence when performing beam bottom defect detection tasks.

[0006] On the first aspect, a UAV autonomous tracking flight control method driven by an NDT task at the bottom of a beam in a GNSS denied environment is provided, comprising: guiding the UAV to autonomously track and fly at the bottom of the beam according to a planned track, and during the autonomous tracking flight, optimizing the UAV attitude measured by an IMU and a three-dimensional lidar using a sliding window optimization unit in conjunction with a spatial position prior constraint, so as to estimate the attitude of the traversed node at the next moment; determining the front view imaging attitude of the area of ​​interest at the bottom of the beam; estimating the attitude and position of the UAV in the beam bottom coordinate system that needs to be adjusted based on the front view imaging attitude and the shooting distance, and adjusting the attitude and position of the UAV into place.

[0007] In some examples, the planned trajectory is controlled within a smooth surface that keeps a distance from the bridge structure. Based on the smooth surface, a reciprocating trajectory is generated that fully covers the bottom of the beam and in which adjacent image nodes / adjacent tracks meet the overlap rate requirements. The reciprocating trajectory serves as a local loop constraint for position-pose adjustment and optimization, and the distance between each image node is the moving step of the UAV along the flight direction.

[0008] In some examples, the flight direction is the X-axis of the bottom of the beam, the image node distance between two adjacent sub-tracks is the step length in the Y-axis direction of the bottom of the beam, the long side of the bottom of a span of the bridge beam is the Y-axis, and its vertical direction is the X-axis.

[0009] In some examples, the three-axis angular velocity and three-axis acceleration data of the UAV collected by the IMU are processed by the IMU pre-integration to obtain the IMU pre-integration factor, and the IMU pre-integration factor is sent to the sliding window optimization unit; the original data collected by the three-dimensional lidar is processed by the radar data preprocessing unit to obtain the radar relative posture factor, which is also sent to the sliding window optimization unit as input; the sliding window optimization unit estimates the posture of the UAV by combining the spatial position prior constraints.

[0010] In some examples, the spatial position prior constraints include: geometric spatial features of the bridge, UAV tracking step length, image overlap rate, and reciprocating track loop constraints.

[0011] In some examples, the graph optimization model and cost function of the radar inertial odometer are constructed by the IMU pre-integration factor and the radar relative pose factor. The graph optimization model determines the pose of the IMU at each moment and is collaboratively optimized with the three-dimensional lidar pose estimation. The LM algorithm is used to minimize the cost function to achieve joint optimization.

[0012] In some examples, the cost function is expressed as:

[0013] min = γ1R I +γ2β L +γ3R Img_n

[0014] R Iis the IMU pose residual, R L is the residual error of the 3D LiDAR pose term, R Img_n The spatial position constraints include the start and end positions, step position residual constraints, and the spatial position loop residual of adjacent trajectories, and γ is the weight factor.

[0015] The IMU is integrated to obtain a rough estimate of the relative position, velocity and attitude parameters of the beam bottom in the world coordinate system; the original beam bottom point cloud data and IMU real-time data are collected in real time using a three-dimensional laser radar, point cloud distortion correction is achieved based on the linear interpolation method, point cloud data filtering is completed using a statistical filter, and point cloud data is downsampled using a voxel filter; feature extraction and matching are performed based on the prior geometric structure of the beam bottom, and the edge points and plane points in the current field of view point cloud are extracted by calculating the curvature of the point cloud in the local area. The cost function is constructed from the distance between the edge point and the corresponding edge line, and from the distance between the plane point and the corresponding plane. The relative posture constraint factor R of the radar is constructed based on the residual between the observed value and the expected value of the carrier posture transformation between two adjacent frames. L , use the LM algorithm to complete the inter-frame motion parameter estimation and obtain the UAV beam bottom position estimation; construct the graph optimization model and cost function of the radar inertial odometer by the IMU pre-integration factor and the radar relative posture factor; under the prior constraints of the beam bottom tracking surface, bridge width and spatial geometry, the spacing between UAV traversal image nodes, and the local loop of adjacent trajectories, the local-global loop constraint of adjacent track imaging overlap is realized to estimate the UAV position-attitude under the beam bottom GNSS denial condition; take the beam bottom width as a parameter constraint, use the LM algorithm to iteratively optimize the local tracking sub-loop of the state variables in the sliding window, give the initial value, and then iteratively solve the increment ΔX, use the increment ΔX to update each state variable, and obtain the position of the laser inertial odometer in the world coordinate system through continuous iterative optimization. and posture

[0016] Combined with pose estimation, for the current frame point cloud data collected by the LiDAR, after point cloud distortion correction and filtering, PCA is used to calculate the normal direction of the surface formed by the current frame point cloud data. Then, the deviation between the planned path and the actual path is calculated while tracking to achieve dynamic adjustment.

[0017] The Lidar-IMU sliding window pose estimation at the bottom of the beam introduces position constraints such as bridge boundary parameters and neighborhood image overlap rate constraints to reduce the pose solution deviation and guide the drone to the predetermined position of the guidance tracking;

[0018] For a Lidar observation point on a certain tracking path, that is, the center point of the local imaging field of view, a set of n Lidar points in the neighborhood of the point is selected, and the set is denoted as {P1,P2,…,P n}, fit a local plane so that the vertical distance from all points to the local plane is minimized, and the objective function is constructed according to the constraint condition. Take the centroid m of all points in the set as the center point c of the fitted plane, and record the distance vector from the i-th point in the set to the center point of the plane as y i , the problem is transformed into solving the normal vector n so that the distance vector y i The sum of the squares of the projections on the normal vector n is minimized, and the direction of the local surface normal vector is determined, that is, the load gimbal is adjusted so that the direction of the imaging main optical axis is consistent with the direction and position of the normal line.

[0019] During the continuous tracking flight of the UAV beam bottom, the planned "lawn mower walking" 3D path is used as a reference to calculate the deviation between the planned path and the actual walking path, and the UAV posture parameters are dynamically adjusted through the posture deviation estimation module. This patent mainly includes angle, position and speed.

[0020] The full coverage imaging proposed in this invention increases the constraint of the overlap rate of adjacent images. This patent realizes dynamic position-pose adjustment and optimization of the traversal node at the next moment through the geometric spatial characteristics of the bridge, the UAV tracking traversal step length, and the image overlap rate. The adjusted position increment is recorded as Δp (x，y，z) Secondly, the normal vector n in the radar coordinate system is corrected by the coordinated optical image imaging external parameters, and the pitch angle adjustment increment of the traversal node load at the next moment is calculated. The above steps enable the UAV to fully cover the bridge image and perform orthophoto imaging, thus avoiding the poor imaging quality and NDT image detection errors caused by the load posture.

[0021] In a second aspect, a drone is provided, comprising: a processor; a memory, comprising one or more program modules; wherein the one or more program modules are stored in the memory and configured to be executed by the processor, and the one or more program modules include instructions for implementing the UAV autonomous tracking flight control method driven by the GNSS denied environment beam bottom NDT mission. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is the architecture diagram of the UAV pose estimation and tracking method for high-precision bridge image detection.

[0023] Figure 2 It is a block diagram of the UAV intelligent tracking system under the GNSS denial condition at the bottom of the beam.

[0024] Figure 3 This is a schematic diagram of the railway / highway beam bottom trajectory planning: Figure 3 (a) is the box girder model, pier model and UAV detection trajectory of the beam bottom of a typical railway bridge; Figure 3(b) Model of a typical highway bridge span, structural characteristics of the bottom of the cross-beam, bridge pier model and UAV detection trajectory of the bottom of the beam; Figure 3 (c) is a schematic diagram of the beam bottom coordinate system; Figure 3 (d) Schematic diagram of the UAV planning trajectory at the bottom of the beam.

[0025] Figure 4 This is a schematic diagram of the priori multi-constrained beam bottom lidar-inertial navigation-vision pose optimization model: Figure 4 (a) Autonomous tracking position-pose estimation and adjustment algorithm process of beam bottom multi-constraint Lidar-IMU coordinated with wide field of view navigation camera; Figure 4 (b) is the prior constrained Lidar-IMU optimization factor graph.

[0026] Figure 5 It is a schematic diagram of the local approximate plane fitting center point and normal vector at the bottom of the beam.

[0027] Figure 6 This is the measurable image result taken by the UAV: Figure 6 (a) is a panoramic view of the bottom of a highway bridge with dense ribs and a cross-beam. Figure 6 (b) NDT orthophoto image of the beam bottom with cracks. DETAILED DESCRIPTION

[0028] Aiming at the surface defects of the bottom structure of large bridges, especially box girders, including full coverage traversal of images of cracks, spalling, exposed reinforcement, rust, etc., and high-precision visual image NDT (Non-destructive testing) measurement, the present invention relates to a method for autonomous tracking, high-precision posture estimation and adjustment of unmanned aerial vehicles, which belongs to the intersection of unmanned aerial vehicle intelligent autonomous image navigation and visual non-destructive health monitoring (SHM: Structural Health Monitoring) of large concrete structures, and significantly improves the field autonomous operation of inspection UAS (Unmanned Aircraft System) and the accuracy, automation, intelligence level and work efficiency of visual NDT image measurement.

[0029] In view of the many problems such as weak surface texture features of bridge beam bottom, bad lighting environment, complex 3D spatial structure of beam bottom, low efficiency and danger of traditional manual detection of defects, this paper proposes a high-precision, full-coverage, image-nondestructive measurement method for autonomous tracking of beam bottom driven by GNSS (Global Navigation Satellite System) denial and UAV (Unmanned Aerial Vehicle) tracking flight. Based on multiple constraints such as full coverage of beam bottom image, beam bottom structural features, tracking traversal, laser radar Lidar and inertial navigation IMU (Inertial Measurement Unit) are tightly coupled, and the beam bottom positioning and local normal vector adjustment of the coordinated wide-field optical camera key frame are used to solve the problems of high-precision orthophoto imaging of drones, reduce occlusion, image distortion and low local image resolution.

[0030] The bottom of a large bridge is usually very hidden and its defects are easily overlooked, and the defects at the bottom of a large bridge are an important indicator of its health status. The present invention solves the problem of UAV beam bottom position estimation and high-precision orthophoto imaging in harsh imaging environments of the beam bottom and under GNSS denial conditions, thereby significantly improving the autonomous tracking automation and intelligence of the UAV for measuring surface defects of the beam bottom structure, and the visual NDT imaging accuracy. The method proposed in the present invention is a necessary prerequisite for target image measurement of beam bottom defects and defect positioning, indexing, panoramic stitching and 3D modeling of beam bottom defects, which effectively improves the automation, intelligence, image measurement accuracy and work efficiency of autonomous tracking operations of UAVs during beam bottom defect detection tasks.

[0031] Aiming at the weak surface texture features of the bridge beam bottom, harsh lighting environment, complex 3D structure of the beam bottom and problems existing in traditional manual detection of defects, the method of the present invention proposes a real-time accurate position and attitude estimation method of the UAV beam bottom driven by high-precision and full-coverage image measurement tasks under the conditions of GNSS denial and UAV attitude instability. Figure 1 The block diagram of the UAV autonomous tracking and control method driven by the NDT task in a GNSS-denied environment is shown.

[0032] Figure 2This is a block diagram of the UAV intelligent tracking system under the GNSS denial condition at the bottom of the beam. The system consists of two parts: hardware system structure and algorithm module. The hardware platform includes: inertial measurement unit IMU, three-dimensional laser radar Lidar, wide field of view navigation camera, narrow field of view measurement camera, embedded core processor, UAV and flight controller; the software platform includes: beam bottom path planning algorithm module; multi-constraint Lidar-IMU beam bottom tracking position-attitude algorithm module (pre-integration unit, radar data preprocessing unit, sliding window optimization unit); beam bottom UAV tracking surface normal vector calculation unit and position and attitude adjustment algorithm module. The following is a specific description of each part. The various parts of the software platform can also be divided as follows: beam bottom full coverage "lawn mower walking" 3D path planning module; beam bottom UAV position and attitude estimation algorithm module with full coverage, high precision and autonomous tracking at the bottom of the beam; UAV beam bottom autonomous tracking flight, imaging payload orthophoto imaging main optical axis attitude control algorithm module. The laser radar (Lidar) and IMU are tightly coupled for collaborative positioning and the local normal vector of the tracking surface is used to adjust the imaging posture of the UAV payload to solve the problems of high-precision orthophoto measurement of UAVs, reduce occlusion, image distortion and low local image resolution.

[0033] The following introduces the trajectory design and planning method of the UAV "lawnmower" image traversal mode for beam bottom NDT visual inspection.

[0034] Figure 3 This is a schematic diagram of the bottom trajectory planning of railway / highway beams. The bottom of bridge beams are mostly flat (box) beams, U-shaped (box) beams, cross beams, etc. Figure 3 (a) shows the box girder model, pier model and UAV detection trajectory of the beam bottom of a typical railway bridge. Figure 3 (b) Model of a typical highway bridge span, structural features of the bottom of the cross-beam, pier model and UAV detection trajectory of the bottom of the beam; Figure 3 (c) is a schematic diagram of the beam bottom coordinate system. For convenience, the present invention takes an arc beam as an example for explanation. Figure 3 (d) is a schematic diagram of the planned trajectory of the UAV at the bottom of the beam. The coordinate system of the bottom of the beam is shown in Figure 3 In (c), the long side of the bottom of a span of the bridge is the Y axis, and its vertical direction is the X axis. Taking into account the flight dynamics of the UAV, the structural characteristics of the box beam (rectangular box beam / circular box beam / etc.), the constraints of the UAV obstacle avoidance and frontal imaging, the overlap rate between the NDT image nodes, etc., the planned UAV trajectory is actually controlled within a smooth surface at a certain distance from the bridge structure (see Figure 3 In (c), the actual flight trajectory is projected on the horizontal plane. Figure 3(d) Based on the simplified model of the beam bottom, the smooth surface space of the track is constructed, and the full coverage of the beam bottom is generated. The reciprocating track with adjacent image nodes / adjacent tracks satisfying a certain overlap rate is planned as an important local loop constraint for tracking pose optimization estimation. Each NDT image node in the traversal mode is represented as x i The i-th step image node of the UAV imaging observation of the bridge axial normal direction beam bottom, y j The j-th step-length imaging observation sub-route of the UAV imaging of the bottom beam in the axial direction of the bridge. The distance between each NDT image node is the moving step length of the UAV along the flight direction (X-axis of the bottom beam), and the distance between the NDT image nodes of two adjacent sub-tracks is the step length in the Y-axis direction of the bottom beam. The step length in the beam bottom coordinate system is determined by the overlap rate between the NDT images Img_n and the overlap rate of the NDT images between adjacent tracks. The residual error of the node position in the planned trajectory and the actual trajectory of the planned trajectory of the bottom beam of the bridge span on the horizontal plane projection plane is as shown in Formula-1, which is an important spatial position constraint item for the posture optimization of the present invention.

[0035]

[0036]

[0037] It is the sum of the cumulative differences between the spatial position of the observation imaging node on the planned route and the estimated position information of the imaging observation node on the actual track. j is the axial direction of the bridge in the beam bottom coordinate system, the jth sub-line. i is the axial normal direction of the bridge in the beam bottom coordinate system, the node at the i-th step of the j-th sub-route; x i+k It is the i+kth observation node on the sub-route. is the residual between the spatial position of the planned track and the estimated spatial position during tracking flight of the i-th node of the j-th sub-track. It is the estimated position information of the i-th node of the j-th sub-track in tracking flight. It is the estimated position information of the i-1th node of the jth sub-track in tracking flight. is the estimated position information of the i-th node of the j-th sub-track of the planned track, It is the estimated position information of the i-1th node of the jth sub-track of the planned track.

[0038] The following introduces the multi-constrained UAV autonomous tracking, position-pose estimation and optimization methods such as full coverage traversal of the bottom surface of the beam.

[0039] The inertial measurement unit is processed by the IMU pre-integration unit to obtain the position, speed and attitude change information of the carrier, and the IMU pre-integration factor is constructed and sent to the sliding window optimization unit; at the same time, the raw data collected by the large-field-of-view three-dimensional laser radar is processed by the radar data pre-processing unit to obtain the radar relative posture factor, which is also sent to the sliding window optimization unit as input to achieve collaborative tight-coupled IMU-Lidar positioning and attitude determination; during the UAV's equal-step flight along the trajectory at the bottom of the beam, the UAV's own position and camera attitude angle in the beam bottom coordinate system are determined, and the UAV calculates the attitude offset angle that needs to be adjusted through the attitude adjustment unit. The core board controls the monocular camera to take orthophoto images. The core processor is the NVIDIA edge computing board, which is mainly used for the synchronous acquisition of sensor data and the real-time calculation of the carrier's posture. The IMU pre-integration factor and the radar relative posture factor are used to construct the graph optimization model of the laser inertial odometer, and the UAV's posture information is estimated based on the sliding window of tight coupling optimization. Figure 4 This is a schematic diagram of the priori multi-constrained beam bottom lidar-inertial navigation-vision pose optimization model: Figure 4 (a) shows the autonomous tracking position-pose estimation and adjustment algorithm process of the beam bottom multi-constraint Lidar-IMU coordinated with the wide field of view navigation camera; Figure 4 (b) is the prior constrained Lidar-IMU optimization factor graph. This part is introduced in detail below.

[0040] Step 0. Use a wide-field visible light camera and Lidar to detect the long side BL (Y-axis of the navigation coordinate system at the bottom of the beam) and the width BW (X-axis of the navigation coordinate system at the bottom of the beam) between one span and the bottom of the beam, as well as the position of the bridge pier at the bottom of the beam Pst. Take the bridge pier in the middle of the bridge span as the starting point of the UAV, and take the normal direction perpendicular to the long side of the bottom of the beam as the track direction to complete the initialization of the UAV position and attitude.

[0041] Step 1. Use the IMU sensor to collect the UAV's three-axis angular velocity and three-axis acceleration parameters, and integrate them to obtain the initial estimation of the IMU's position, velocity, and attitude in the world coordinate system.

[0042] Step 2. IMU pre-integration module

[0043] The state of the IMU system at time i includes attitude, position, velocity and IMU zero bias. The state quantity is located as The original measurement data of the IMU sensor are the three-axis angular velocity and three-axis acceleration. If the position of the IMU in the world coordinate system at time i is known, speed and posture By integrating the acceleration and angular velocity values ​​originally measured by the IMU, we can get the position of the IMU at time j speed and posture is the speed drift, It is the angular velocity drift. The present invention utilizes the bridge beam bottom position prior, overlap rate constraint, local loop constraint, and graph optimization model to determine the pose of the IMU at each moment, and performs collaborative optimization with the Lidar pose estimation. Combined with the bridge spatial structure prior, the graph optimization model is used to optimize the pose of the sequence frame IMU. The present invention uses IMU pre-integration to decouple the pose update of the IMU from the world coordinate system, reducing the amount of calculation generated by the frequent updates of the position, velocity, and attitude equations, and at the same time uses the long side of the beam bottom and its normal direction as spatial position constraints to improve the accuracy of pose estimation and suppress drift deviation.

[0044] The frame rate of the carrier IMU is higher than that of the Lidar. In order to save onboard computing resources, the IMU posture update is decoupled from the world coordinate system to obtain a pre-integrated discrete model, as shown in Equation 3. and It is the change in position, velocity and attitude of the IMU between time i and time j, which is only related to the measurement value and zero bias error of the IMU at time t. The zero bias error is obtained by pre-calibration to obtain the initial value, and then added to the graph optimization model for further optimization. Since the actual sampled data of the IMU is discrete, the median method is used to discretize the pre-integrated quantity, and the following formula is used to update the pre-integrated quantity:

[0045]

[0046] in is the Jacobian matrix of the IMU pre-integrated quantity with respect to the estimated value of the IMU random zero drift. A significant change has occurred, and the above formula is used to update the IMU pre-integration quantity. According to the residual between the pre-integration result and the carrier state change at different observation positions, the IMU pre-integration residual constraint factor is constructed, as shown in Formula 4. This is the IMU constraint item for posture optimization of the present invention.

[0047]

[0048] in represents the relative pose residual function between the kth keyframe and the k+1th keyframe of the IMU, is the observed value of relative pose, is the covariance matrix of the IMU residual function. The relative pose residual function of IMU The expression of is shown in formula-5:

[0049]

[0050] Step 3. Radar pose estimation submodule

[0051] The UAV beam bottom tracking process moves continuously along the trajectory and according to the set step size. Due to the complex spatial structure of the beam bottom, the radar point cloud has certain distortions in the non-stationary motion and turning points during the UAV tracking process. Since the IMU frame rate is much higher than the Lidar frame rate, the state vector change between the IMU at two moments i and j and the pre-calibrated external parameters between the IMU and the Lidar are used to calculate the value of the IMU state vector. The change in the motion state of the laser radar between two moments is calculated, and the points at all moments in a frame of data are converted to the radar coordinate system where the laser radar is located at the end of the frame through linear interpolation, thereby achieving distortion correction of the point cloud.

[0052]

[0053] Rotation matrix, Space translation matrix.

[0054] (1) Filtering and downsampling

[0055] The spatial structure of the beam bottom is complex and self-similar, and the point cloud data collected by Lidar is unevenly distributed in some areas. The denser the point cloud data in some areas, the more accurate the description of the surrounding environment, while sparse points are mostly outliers with large errors. First, the outliers with sparse distribution are filtered out; then the voxel grid divides the point cloud data into multiple small point cloud blocks, and the center point of the point cloud block is used as the description point of the point cloud block. The voxel filter is used to reduce the number of point clouds while retaining the original point cloud structure; secondly, the point cloud data is downsampled on the basis of maintaining the structural features.

[0056] (2) Beam bottom surface and line feature extraction

[0057] A frame of point cloud data usually contains edge points and plane points. The laser scan line is partitioned, the curvature of the point cloud in each area is calculated, and the curvatures of the points in multiple areas are sorted. Points with curvatures less than a threshold T are taken as plane points, and points with curvatures greater than the threshold T are taken as edge points. Edge points are usually located on sharp edges, while plane points are usually located on smooth planes.

[0058] (3) Feature matching and motion estimation residual term

[0059] Based on the feature extraction step, the bottom line and surface features of the beam are output. Through feature matching between the two frames, the LM method is used to realize motion estimation and calculate the relative pose transformation of the two-frame point cloud data. The LiDAR state vector is According to the residual between the observed value and the expected value of the carrier posture transformation between two adjacent frames, the relative posture constraint factor of the radar is constructed:

[0060]

[0061] Where N is the size of the sliding window, represents the relative pose residual function between the kth key frame and the k+1th key frame of the lidar, is the observed value of relative pose, is the covariance matrix of the radar residual function, where ρ is the Cauchy robust kernel function, which can reduce the weight of abnormal error values. The representation is as follows:

[0062]

[0063] in is the observed value of the carrier posture transformation between two adjacent frames, is the external parameter transformation matrix between the radar coordinate system and the IMU coordinate system, which can be obtained by pre-calibration. They are the pose transformation matrices of the feature points from the IMU coordinate system to the world coordinate system in the kth and k+1th frames, that is, the estimated poses that we need to optimize and solve.

[0064] Step 4. Construction of multi-constraint joint optimization equation for beam bottom characteristics and LM optimization

[0065] min = γ1R I +γ2β L +γ3R Img_n Formula-9R I is the IMU pose residual, R L is the residual error of Lidar pose term, R Img_n The spatial position constraints include the start and end positions, step position residual constraints and the spatial position loop residual of adjacent trajectories, and γ is the weight factor. The LM algorithm is used to achieve joint optimization by minimizing the target cost function formula-9.

[0066] The local normal vector estimation unit and the orthographic imaging pose are introduced below.

[0067] For the current frame point cloud data collected by the lidar, after point cloud distortion correction and filtering, the PCA method is used to estimate the local normal direction of the surface constituted by the current frame point cloud data, thereby realizing the front view imaging posture of the area of ​​interest at the bottom of the beam.

[0068] For a certain point, select n points in the neighborhood of the point, and the set of these points is denoted as {P1,P2,…,P n} Fit a local plane L so that the vertical distance from all points to the local plane is minimized, and then construct the objective function shown in formula-10:

[0069]

[0070] Where c is the center point of the fitting plane, n is the normal vector passing through the center point and perpendicular to the plane, P i is the three-dimensional coordinate of the i-th point in the set, and the centroid m is taken as the center point c of the fitting plane. Let the vector y be i is the distance vector from the i-th point in the set to the centroid:

[0071] y i =P i -m#-11

[0072] Since the sum of the squares of the distances from all points in the set to the centroid is the smallest, the centroid m is solved to be the center point c of the fitting plane, and we can get:

[0073]

[0074] Then, the normal vector n is solved by minimizing Equation 12, so that the cost function is minimized. Figure 5 . In fact, the normal imaging direction is obtained, thereby ensuring the imaging quality and overlap rate.

[0075] The beam bottom UAV posture adjustment unit is introduced below.

[0076] Take the curved continuous box girder bridge as an example, its model is shown in Figure 4 and Figure 2 According to the planned beam bottom track and beam bottom NDT image node R Img_n The parameters such as position, step length, speed, etc. are used to guide the UAV to fly autonomously at the bottom of the beam. On the other hand, under the GNNS rejection condition at the bottom of the beam, the real-time observation posture of the UAV is obtained based on the multi-constraint solution method of the bottom of the beam of the present invention.

[0077] The basic task of the present invention is to capture high-precision NDT images with no offset in the front view, and mainly realize the spatial position adjustment of the UAV in the navigation coordinate system at the bottom of the beam based on the residual optimization of formula-9.

[0078] In terms of attitude adjustment, the attitude and position of the UAV in the beam bottom coordinate system that needs to be adjusted are estimated based on the center point of the fitted local plane at the bottom of the beam, the direction of the local normal (i.e., the normal imaging direction), and the shooting distance. First, the local normal vector n in the radar coordinate system is converted to the attitude Euler angle in the world coordinate system, as shown in Equations 13 and 14:

[0079]

[0080] Calculate the ideal pitch angle in the UAV carrier coordinate system based on the normal vector n′ Yaw angle ψ′ and roll angle φ′:

[0081]

[0082] Secondly, according to the actual observation pitch angle of the bridge detection UAV The yaw angle ψ and the roll angle φ are used to obtain the angles that the carrier coordinate system needs to adjust on the X, Y, and Z axes, which are recorded as θ1, θ2, and θ3, respectively, as shown in Equation 15:

[0083]

[0084] The orthographic imaging angle that needs to be adjusted is the difference between the Euler angle calculated by the local normal solution and the Euler angle observed by the IMU-Lidar joint observation, thereby solving the problem of non-orthographic shooting of UAVs on bridges and improving the NDT image measurement accuracy. Figure 6 A measurement result is shown. Figure 6 (a) is a panoramic view of the bottom of a highway bridge with dense ribbed crossbeams. Figure 6 (b) NDT orthophoto of the beam bottom with cracks.

[0085] The present invention provides an embodiment of a drone. The drone includes a processor and a memory. The memory is used to store non-temporary instructions (e.g., one or more program modules). The processor is used to run the non-temporary instructions. When the non-temporary instructions are run by the processor, one or more steps in the method described above can be executed to guide the drone to autonomously track the bottom of the beam and obtain the real-time observation posture of the UAV. The memory and the processor can be interconnected through a bus system and / or other forms of connection mechanisms.

[0086] For example, the processor may be a central processing unit (CPU), a graphics processing unit (GPU), or other forms of processing units with data processing capabilities and / or program execution capabilities. For example, the central processing unit (CPU) may be an X86 or ARM architecture, etc. The processor may be a general-purpose processor or a dedicated processor, and may control other components in the electronic device to perform desired functions.

[0087] For example, the memory may be a volatile memory and / or a non-volatile memory. The volatile memory may include, for example, a random access memory (RAM) and / or a cache memory (cache), etc. The non-volatile memory may include, for example, a read-only memory (ROM), a hard disk, an erasable programmable read-only memory (EPROM), a compact disc read-only memory (CD-ROM), a USB memory, a flash memory, etc. One or more program modules may be stored in the memory, and the processor may run the one or more program modules to implement various functions of the electronic device.

Claims

1. A UAV autonomous tracking flight control method driven by NDT task in GNSS denied environment, characterized in that: include: The UAV is guided to fly autonomously according to the planned track at the bottom of the beam. During the autonomous tracking flight, the sliding window optimization unit is used to jointly optimize the UAV attitude measured by IMU and 3D lidar with the spatial position prior constraint to estimate the posture of the traversed node at the next moment. The objective cost function of the sliding window optimization unit that optimizes the UAV attitude is: min=γ1R I +γ2β L +γ3R Img_n R I is the residual error of IMU pose term, R L is the residual error of Lidar pose term, R Img_n is the spatial position constraint term, γ1, γ2, and γ3 are the weight factors of the corresponding terms; The LM algorithm is used to minimize the target cost function to achieve joint optimization; The method for determining the frontal imaging posture of the interest area at the bottom of the beam includes: for a 3D laser radar observation point on a tracking path, that is, the center point of the local imaging field of view, selecting a set of n 3D laser radar points in the neighborhood of the point, and recording the set as {P1, P2, ..., P n }, fit a local plane so that the vertical distance from all points to the local plane is minimized, construct the objective function according to the constraint condition, take the centroid m of all points in the set as the center point c of the fitting plane, and record the distance vector from the i-th point in the set to the center point of the plane as y i , solve the normal vector so that the distance vector y i The sum of the projection squares on the normal vector is minimized to determine the direction of the local surface normal vector, which is also the normal imaging direction; According to the frontal imaging posture and shooting distance, the UAV posture and position in the beam bottom coordinate system that need to be adjusted are estimated, and the UAV posture and position are adjusted in place so that the direction of the main optical axis of the camera imaging is consistent with the normal direction and position.

2. The UAV autonomous tracking flight control method driven by the GNSS-denied environment beam bottom NDT task according to claim 1 is characterized in that: The planned trajectory is controlled within a smooth surface that keeps a distance from the bridge structure. Based on this smooth surface, a reciprocating trajectory is generated that fully covers the bottom of the beam and satisfies the overlap rate requirements of adjacent image nodes / adjacent tracks. The distance between each image node is the moving step of the UAV along the flight direction.

3. The UAV autonomous tracking flight control method driven by the GNSS-denied environment beam bottom NDT task according to claim 2 is characterized in that: The flight direction is the X-axis of the beam bottom, the image node distance between two adjacent sub-tracks is the step length in the Y-axis direction of the beam bottom, the long side of the bottom of a span of the bridge beam is the Y-axis, and its vertical direction is the X-axis.

4. The UAV autonomous tracking flight control method driven by the GNSS-denied environment beam bottom NDT task according to claim 3 is characterized in that: The UAV three-axis angular velocity and three-axis acceleration data collected by the IMU are processed by the IMU pre-integration to obtain the IMU pose item residual, and the IMU pose item residual is sent to the sliding window optimization unit; the original data collected by the three-dimensional lidar is processed by the radar data preprocessing unit to obtain the lidar pose item residual, which is also sent to the sliding window optimization unit as input.

5. The UAV autonomous tracking flight control method driven by the GNSS-denied environment beam bottom NDT task according to claim 4 is characterized in that: The spatial position constraints include: geometric spatial characteristics of the bridge, UAV tracking step length, image overlap rate, and reciprocating track loop constraints.

6. A drone, characterized in that: include: processor; a memory, including one or more program modules; Wherein, the one or more program modules are stored in the memory and configured to be executed by the processor, and the one or more program modules include instructions for implementing the UAV autonomous tracking flight control method driven by the GNSS denied environment beam bottom NDT mission as described in any one of claims 1-5.

Images