Unmanned aerial vehicle navigation positioning method based on bridge detection

Abstract

The invention belongs to the technical field of bridge detection, and particularly relates to an unmanned aerial vehicle navigation positioning method based on bridge detection, which comprises the following steps of: receiving signals of a plurality of satellites through a satellite navigation system receiver, and calculating a pseudo-range measurement value of each satellite according to a satellite navigation observation equation; the method comprises the following steps: acquiring three-dimensional point cloud data of a bridge by adopting a laser radar sensor, and carrying out data preprocessing on the three-dimensional point cloud data; performing fusion processing on the pseudo-range measurement value and the three-dimensional point cloud data to generate fused positioning information and an environment sensing result; processing three-dimensional point cloud data measured by the laser radar sensor by adopting a laser SLAM algorithm to generate relative pose change of the laser radar sensor and bridge map information; and inputting the fused positioning information and the environment sensing result into an unmanned aerial vehicle flight control algorithm, generating a control instruction, and controlling the flight of the unmanned aerial vehicle, so that accurate positioning of the unmanned aerial vehicle can be realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of bridge detection, and in particular relates to a navigation and positioning method for an unmanned aerial vehicle based on bridge detection. Background Art

[0002] At present, operating and maintenance units and professional inspection agencies mostly use traditional inspection methods, relying on the naked eye or auxiliary tools (such as bridge inspection vehicles, telescopes, etc.) to detect whether the main components and parts of bridges have cracks, cracks and damage, exposed steel bars and rust, and support hollowing. Bridges have the special characteristics of high piers, crossing rivers, crossing valleys, and crossing the sea. Traditional bridge inspections require the use of bridge inspection vehicles as inspection auxiliary tools to send personnel and equipment to the defective parts for inspection. This leads to the occurrence of inspection vehicles occupying the road and blocking the road, which seriously affects the orderly operation of traffic organization. At the same time, the occurrence of safety accidents caused by personnel working at high altitudes and road closures not only causes loss of life and property, but also causes great adverse social impacts.

[0003] Due to the obstruction of the bridge, there is no satellite signal at the bottom of the bridge, and ordinary drones cannot fly normally. At the same time, the positioning deviation of satellite signals is usually very large and the accuracy is low. Summary of the invention

[0004] In order to solve the above technical problems, the present invention provides a UAV navigation and positioning method based on bridge detection to solve the problem that ordinary UAVs cannot fly normally due to the lack of satellite signals at the bottom of the bridge due to bridge obstruction, and the satellite signal usually has a very large positioning deviation and low accuracy.

[0005] The technical solution adopted by the present invention is as follows: A UAV navigation and positioning method based on bridge detection comprises the following steps:

[0006] S100, receiving signals from a plurality of satellites through a satellite navigation system receiver, and calculating a pseudo-range measurement value of each satellite according to a satellite navigation observation equation;

[0007] S200, using a laser radar sensor to obtain three-dimensional point cloud data of the bridge, and performing data preprocessing on the three-dimensional point cloud data;

[0008] S300, fusing the pseudo-range measurement value with the three-dimensional point cloud data to generate fused positioning information and environmental perception results;

[0009] S400 uses the laser SLAM algorithm to process the 3D point cloud data measured by the laser radar sensor to generate the relative position change of the laser radar sensor and the bridge map information;

[0010] S500 inputs the fused positioning information and environmental perception results into the UAV flight control algorithm, generates control instructions, and controls the flight of the UAV.

[0011] Further, in S100, the satellite navigation observation equation calculation formula is as follows:

[0012] ρ (n) =r (n) +c·δt r +δe (n) -c·δt (n) +I (n) +T (n) +R (n) +d (n) +ε ρ (n)

[0013] In the formula, ρ (n) is the pseudorange measurement from the receiver to the nth satellite, r (n) is the actual geometric distance from the receiver to the satellite, c is the speed of light, δt r is the receiver clock error, δe (n) is the satellite clock error, δt (n) is the satellite clock offset, I (n) is the ionospheric delay, T (n) is the tropospheric delay, R (n) is the error caused by the relativistic effect, d (n) is the multipath error, ε ρ (n) is the unmodeled error.

[0014] Furthermore, S200 includes:

[0015] S201, obtaining three-dimensional point cloud data of the bridge;

[0016] S202, removing abnormal points and noise in the point cloud data;

[0017] S203, downsampling the three-dimensional point cloud data;

[0018] S204, converting the three-dimensional point cloud data from the laser radar sensor coordinate system to the global coordinate system.

[0019] Furthermore, S300 includes:

[0020] S301, associating feature points in the three-dimensional point cloud data with pseudo-range measurement values;

[0021] S302, performing weight allocation according to the reliability of feature points in the three-dimensional point cloud data and the accuracy of pseudo-range measurement values;

[0022] S303, using an extended Kalman filter algorithm to fuse the feature points and pseudo-range measurement values ​​in the three-dimensional point cloud data to generate fused positioning information and environmental perception results.

[0023] Furthermore, S400 includes:

[0024] S401, using ICP algorithm to perform point cloud registration, aligning point cloud data at different times;

[0025] S402, calculating the relative position change of the laser radar sensor at different times through the transformation matrix obtained by point cloud registration;

[0026] S403, accumulating the relative pose change calculated each time to generate a global pose of the laser radar sensor, and constructing a motion trajectory of the laser radar sensor;

[0027] S404, accumulating the aligned three-dimensional point cloud data into the global map, constructing a three-dimensional model of the bridge, and generating a bridge map.

[0028] Furthermore, S500 includes:

[0029] S501, inputting the fused positioning information and environmental perception results into the UAV flight control algorithm to perform state estimation fusion;

[0030] S502, generating a flight path according to the three-dimensional model of the bridge and the current state of the UAV;

[0031] S503, calculating a control instruction of the UAV according to the flight path, wherein the control instruction includes a steering angle and an acceleration of the UAV;

[0032] S504, design a PID controller to convert the path tracking target into a control command to control the flight of the UAV.

[0033] Beneficial effects of the present invention:

[0034] 1. Fusion processing of coupled satellite navigation position information and lidar point cloud data

[0035] Accurate position output: By fusing satellite navigation position information with the global point cloud formed by the lidar, accurate position output for photo taking points or inspection points can be provided.

[0036] Seamless switching: Realize seamless switching between good satellite navigation signals and fully covered enclosed environments, ensuring the accuracy of fusion positioning in ultra-high altitude and sparse point cloud environments.

[0037] 2. Laser SLAM algorithm processes point cloud data

[0038] Acquisition of relative position and posture changes of sensors: By comparing two sets of point cloud data at different times, the relative position and posture changes of sensors can be obtained using the laser SLAM algorithm. This helps to understand the movement trajectory and posture changes of the sensor in space, providing an important reference for subsequent positioning and navigation.

[0039] Map information generation: In addition to the relative position change of the sensor, the laser SLAM algorithm can also generate map information. This map information can be used to guide the flight path planning of the drone, helping the drone avoid obstacles and choose the optimal path.

[0040] 3. Import UAV flight control algorithm for state estimation fusion and control

[0041] State estimation fusion: The fused positioning information and environmental perception results are input into the UAV’s flight control algorithm for state estimation fusion. This helps to comprehensively consider the positioning information and environmental perception results to make a more accurate estimate of the UAV’s current state.

[0042] Control command generation: Based on the results of state estimation fusion, the UAV flight control algorithm can generate specific control commands. These commands include parameters such as the UAV’s steering angle and acceleration, which are used to guide the UAV to fly along a predetermined path.

[0043] 4. Laser SLAM algorithm and satellite navigation fusion application

[0044] Improved positioning accuracy: The satellite navigation system provides absolute position information, which is combined with the relative positioning results of the laser SLAM algorithm to effectively reduce the cumulative error and improve the overall positioning accuracy.

[0045] Enhanced environmental perception: The lidar SLAM algorithm is responsible for high-precision local environment modeling, while the satellite navigation system provides large-scale positioning information. The two complement each other to enhance environmental perception capabilities.

[0046] Improved robustness: In an environment where satellite signals are lost or of poor quality, the laser SLAM algorithm can work independently to ensure the system's continuous positioning and navigation capabilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is a flow chart of a UAV navigation and positioning method based on bridge detection according to the present invention; DETAILED DESCRIPTION

[0048] The following embodiments of the present invention are described in further detail in conjunction with the accompanying drawings and examples. The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0049] In the description of the present invention, unless otherwise specified, "plurality" means two or more than two; the orientations or positional relationships indicated by the terms "upper", "lower", "left", "right", "inner", "outer", "front end", "rear end", "head", "tail", etc. are based on the orientations or positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", "third", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance.

[0050] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "connected" and "connection" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0051] like Figure 1 As shown, a UAV navigation and positioning method based on bridge detection includes the following steps:

[0052] S100, receiving signals from a plurality of satellites through a satellite navigation system receiver, and calculating a pseudo-range measurement value of each satellite according to a satellite navigation observation equation; acquiring satellite signals from a GNSS (satellite navigation and positioning system) receiver, and obtaining pseudo-range measurement values ​​and satellite position information after decoding;

[0053] As a preferred solution, in S100, the satellite navigation observation equation calculation formula is as follows:

[0054] ρ (n) =r (n) +c·δt r +δe (n) -c·δt (n) +I (n) +T (n) +R (n) +d (n) +ε ρ (n)

[0055] In the formula, ρ (n) is the pseudorange measurement from the receiver to the nth satellite, r (n) is the actual geometric distance from the receiver to the satellite, c is the speed of light, δt r is the receiver clock error, δe (n) is the satellite clock error, δt (n) is the satellite clock offset, I(n) is the ionospheric delay, T (n) is the tropospheric delay, R (n) is the error caused by the relativistic effect, d (n) is the multipath error, ε ρ (n) is the unmodeled error.

[0056] Through the satellite navigation observation equation, various errors can be corrected, including ionospheric delay, δt r is the receiver clock error, δe (n) is the satellite clock error, I (n) is the ionospheric delay, T (n) These corrections can significantly improve positioning accuracy, such as tropospheric delay, multipath error d (n) It can also be estimated and corrected through appropriate algorithms to further improve positioning accuracy; by modeling and estimating the errors in the equation, other unmodeled errors ε can be reduced ρ (n) The influence of the satellite navigation system can be reduced, and the stability and reliability of the system can be improved; the satellite navigation system receiver receives signals from several satellites, and calculates the pseudo-range measurement value of each satellite according to the satellite navigation observation equation, which can significantly improve the accuracy, robustness and real-time performance of positioning.

[0057] By coupling satellite navigation position information, the global point cloud formed by the lidar can be integrated with the precise position output of satellite navigation, and the precise position output of the photo-taking point or inspection point can be provided in the process. The seamless switching between a good satellite navigation signal and a fully covered enclosed environment can be achieved, and fusion positioning can be realized in ultra-high altitude and sparse point cloud environments.

[0058] S200 uses a laser radar sensor to obtain the three-dimensional point cloud data of the bridge and performs data preprocessing on the three-dimensional point cloud data. The laser radar sensor scans the bridge to obtain the three-dimensional coordinate information of its surface points. The laser radar can quickly and accurately capture the geometric shape and detailed features of the bridge surface. At the same time, the laser radar obtains obstacle information within its measurement range by emitting laser beams within a certain angle range and receiving return signals.

[0059] S200 includes:

[0060] S201, obtaining three-dimensional point cloud data of the bridge;

[0061] S202, remove abnormal points and noise in the point cloud data; use a filtering algorithm (such as Gaussian filtering, mean filtering, etc.) to smooth the point cloud data to reduce the impact of noise on data quality. The filtering process can remove high-frequency noise and retain the low-frequency characteristics of the data.

[0062] S203, down-sampling the three-dimensional point cloud data; down-sampling is performed using voxel grid filtering and random sampling, thereby reducing the amount of data, improving subsequent processing efficiency, saving storage space and accelerating processing speed.

[0063] S204, converting the three-dimensional point cloud data from the laser radar sensor coordinate system to the global coordinate system. Converting the point cloud data from the laser radar sensor coordinate system to the global coordinate system can achieve fusion and comparison between different sensor data. This helps to build a more complete scene model and improve the accuracy and robustness of environmental perception.

[0064] The pre-processed point cloud data can be applied to many fields such as bridge inspection, maintenance planning, structural health monitoring, etc. Through accurate 3D modeling and data analysis, it can provide strong support for the design, construction and maintenance of bridges.

[0065] S300, fuses pseudo-range measurements with three-dimensional point cloud data to generate fused positioning information and environmental perception results; Fusing pseudo-range measurements with three-dimensional point cloud data to generate fused positioning information and environmental perception results is a key technology that helps improve positioning accuracy, enhance environmental perception capabilities, optimize sensor fusion algorithms and expand application areas.

[0066] As a preferred solution, S300 includes:

[0067] S301, associate the feature points in the 3D point cloud data with the pseudo-range measurement values; identify the feature points in the 3D point cloud data, including corner points and edge points, and associate these feature points with the corresponding pseudo-range measurement values. This step is the basis for data fusion and ensures the correspondence between different data sources;

[0068] S302, weight allocation is performed according to the reliability of feature points in the three-dimensional point cloud data and the accuracy of pseudo-range measurement values; the reliability of feature points in the three-dimensional point cloud data includes stability and repeatability, and the accuracy of pseudo-range measurement values ​​includes error size and confidence. Appropriate weights are allocated to each feature point and pseudo-range measurement value. Weight allocation helps to balance the contribution of different data sources during the fusion process and improve the accuracy of the fusion result.

[0069] S303, using the extended Kalman filter algorithm to fuse the feature points and pseudo-range measurements in the three-dimensional point cloud data to generate fused positioning information and environmental perception results. Using the extended Kalman filter algorithm for data fusion can effectively handle nonlinear and non-Gaussian noise problems and improve the performance and robustness of the fusion algorithm. This helps to cope with complex and changing environmental conditions and improve the stability and adaptability of the system.

[0070] S400 uses the laser SLAM algorithm (laser simultaneous localization and mapping algorithm) to process the 3D point cloud data measured by the laser radar sensor to generate the relative position change of the laser radar sensor and the bridge map information;

[0071] S400 includes:

[0072] S401, using ICP algorithm (iterative closest point algorithm) to perform point cloud registration, aligning point cloud data at different times;

[0073] Specifically, the optimal rigid body transformation matrix between two sets of point clouds is calculated as follows:

[0074] T=ICP(source,target)

[0075] Where T is the transformation matrix, source and target are the source point cloud and target point cloud.

[0076] S402, calculating the relative position change of the laser radar sensor at different times through the transformation matrix obtained by point cloud registration;

[0077] According to the transformation matrix T calculated by the ICP algorithm, the relative position change of the sensor at different time points is determined, which is expressed as follows:

[0078] T k|k-1 =T

[0079] Where, T k|k-1 Represents the transformation matrix from the 1k-1th moment to the kth moment.

[0080] S403, accumulating the relative pose change calculated each time to generate a global pose of the laser radar sensor, and constructing a motion trajectory of the laser radar sensor;

[0081] The global pose is expressed as follows:

[0082] T k =T k-1 ·T k|k-1

[0083] Where, T k represents the global pose at the kth moment.

[0084] S404, accumulating the aligned three-dimensional point cloud data into the global map, constructing a three-dimensional model of the bridge, and generating a bridge map.

[0085] In this embodiment, the global map is further optimized through rasterization and filtering processing methods to remove duplicate points and noise, thereby improving the accuracy and sparsity of the map. It also includes closed-loop detection, which detects whether the lidar sensor has returned to a previously visited position through a bag-of-words model and loop detection methods to achieve closed-loop detection. Based on the closed-loop detection results, the accumulated position errors are corrected to improve the global consistency of the map.

[0086] S500 inputs the fused positioning information and environmental perception results into the UAV flight control algorithm, generates control instructions, and controls the flight of the UAV.

[0087] As a preferred solution, S500 includes:

[0088] S501, input the fused positioning information and environmental perception results into the UAV flight control algorithm for state estimation fusion; the positioning information includes position and speed, and the environmental perception results include obstacle positions and three-dimensional models of bridges; the current state of the UAV is accurately estimated to provide a basis for subsequent path planning and control.

[0089] S502, generating a flight path based on the three-dimensional model of the bridge and the current state of the UAV; the generated flight path is an optimal path that can avoid obstacles, taking into account the structure of the bridge, the terrain, and the flight performance of the UAV.

[0090] S503, calculating the control instructions of the UAV according to the flight path, wherein the control instructions include the steering angle and acceleration of the UAV; these control instructions include the steering angle and acceleration of the UAV and are used to guide the UAV to fly along a predetermined path.

[0091] S504, design a PID controller (proportional-integral-differential controller) to convert the path tracking target into specific control instructions to control the flight of the UAV.

[0092] The control instructions are as follows:

[0093]

[0094]

[0095] In the formula, roll command Indicates roll command, pitch command Indicates pitch command, K P Represents proportional gain, K i Indicates the integral gain, K d represents the differential gain, error x and error y Indicates position error.

[0096] Through the above steps, the laser SLAM algorithm can be used to process the three-dimensional point cloud data measured by the laser radar sensor, generate the relative posture change of the laser radar sensor and the bridge map information, and fuse it with the GNSS pseudo-range measurement value to generate the fused positioning information and environmental perception results. These results are finally input into the UAV flight control algorithm to generate control instructions to control the flight of the UAV. This method provides high-precision positioning and environmental perception in complex environments.

[0097] The present invention has been described in detail above. The description of the specific embodiments is only used to help understand the method of the present invention and its core concept. It should be pointed out that for ordinary technicians in this technical field, without departing from the principle of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A UAV navigation and positioning method based on bridge detection, characterized in that: The following steps are involved: S100, receiving signals from a plurality of satellites through a satellite navigation system receiver, and calculating a pseudo-range measurement value of each satellite according to a satellite navigation observation equation; S200, using a laser radar sensor to obtain three-dimensional point cloud data of the bridge, and performing data preprocessing on the three-dimensional point cloud data; S300, fusing the pseudo-range measurement value with the three-dimensional point cloud data to generate fused positioning information and environmental perception results; S400 uses the laser SLAM algorithm to process the 3D point cloud data measured by the LiDAR sensor to generate the relative position change of the LiDAR sensor and the bridge map information; S500 inputs the fused positioning information and environmental perception results into the UAV flight control algorithm, generates control instructions, and controls the flight of the UAV.

2. The method for navigation and positioning of an unmanned aerial vehicle based on bridge detection according to claim 1, characterized in that: In S100, the satellite navigation observation equation calculation formula is as follows: r (n) =r (n) +c·δt r +the (n) -c·δt (n) +I (n) +T (n) +R (n) +d (n) +e ρ (n) In the formula, ρ (n) is the pseudorange measurement from the receiver to the nth satellite, r (n) is the actual geometric distance from the receiver to the satellite, c is the speed of light, δt r is the receiver clock error, δe (n) is the satellite clock error, δt (n) is the satellite clock offset, I (n) is the ionospheric delay, T (n) is the tropospheric delay, R (n) is the error caused by the relativistic effect, d (n) is the multipath error, ε ρ (n) is the unmodeled error.

3. The method for navigation and positioning of an unmanned aerial vehicle based on bridge detection according to claim 1, characterized in that: S200 includes: S201, obtaining three-dimensional point cloud data of the bridge; S202, removing abnormal points and noise in the point cloud data; S203, downsampling the three-dimensional point cloud data; S204, converting the three-dimensional point cloud data from the laser radar sensor coordinate system to the global coordinate system.

4. The method for navigation and positioning of an unmanned aerial vehicle based on bridge detection according to claim 1, characterized in that: S300 includes: S301, associating feature points in the three-dimensional point cloud data with pseudo-range measurement values; S302, performing weight allocation according to the reliability of feature points in the three-dimensional point cloud data and the accuracy of pseudo-range measurement values; S303, using an extended Kalman filter algorithm to fuse the feature points and pseudo-range measurement values ​​in the three-dimensional point cloud data to generate fused positioning information and environmental perception results.

5. The method for navigation and positioning of an unmanned aerial vehicle based on bridge detection according to claim 1, characterized in that: S400 includes: S401, using ICP algorithm to perform point cloud registration, aligning point cloud data at different times; S402, calculating the relative position change of the laser radar sensor at different times through the transformation matrix obtained by point cloud registration; S403, accumulating the relative pose change calculated each time to generate a global pose of the laser radar sensor, and constructing a motion trajectory of the laser radar sensor; S404, accumulating the aligned three-dimensional point cloud data into the global map, constructing a three-dimensional model of the bridge, and generating a bridge map.

6. The method for navigation and positioning of an unmanned aerial vehicle based on bridge detection according to claim 1, characterized in that: S500 includes: S501, inputting the fused positioning information and environmental perception results into the UAV flight control algorithm to perform state estimation fusion; S502, generating a flight path according to the three-dimensional model of the bridge and the current state of the UAV; S503, calculating a control instruction of the UAV according to the flight path, wherein the control instruction includes a steering angle and an acceleration of the UAV; S504, design a PID controller to convert the path tracking target into a control command to control the flight of the UAV.

Images