Electric power inspection unmanned aerial vehicle navigation positioning method, medium and equipment

Abstract

The invention provides an electric power inspection unmanned aerial vehicle navigation positioning method, a medium and equipment. The method comprises the following steps: periodically collecting sensor data; judging whether the current visual frame is a key frame or not; detecting the power transmission line based on the visual key frame image information and calculating a relative pose between the visual sensor and the power transmission line; performing dynamic model-inertia augmentation pre-integration calculation between adjacent visual key frames by using the dynamic model data and the inertial sensor data; solving navigation information of the unmanned aerial vehicle relative to the local navigation coordinate system by combining vision, a kinetic model and inertial information; fusing relative navigation information of the unmanned aerial vehicle and satellite information based on a factor graph method, and resolving absolute navigation information of the unmanned aerial vehicle; and outputting absolute navigation information of the unmanned aerial vehicle. According to the method, more accurate attitude angle estimation of the unmanned aerial vehicle in a high-dynamic environment is realized by fusing kinetic model information, and short-time high-precision and long-time drifting-free navigation information estimation of the electric power inspection unmanned aerial vehicle is realized by utilizing a factor graph optimization technology.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle (UAV) power inspection, and specifically relates to a navigation and positioning method, medium and equipment for a power inspection UAV. Background Art

[0002] With the rapid construction of power transmission lines, the workload of power inspection has also increased significantly. Inspection by drones can not only improve work efficiency and reduce grid maintenance costs, but also ensure personnel safety, and has broad application prospects. Navigation and positioning is one of the indispensable core technologies in power drone inspection. When performing inspection tasks, power drones need to rely on accurate navigation and positioning information to ensure the safety and efficiency of the inspection process. However, existing navigation and positioning technologies face many challenges in power inspection scenarios.

[0003] Power inspections are usually carried out in complex terrain environments, which can easily block satellite signals. Long-term operation of a single inertial sensor can easily produce cumulative errors, leading to drift, so multi-source information fusion technology is needed to ensure the accuracy of the navigation system. Traditional power inspection drones use filtering methods to fuse inertial sensor information with satellite information. Affected by gyroscope bandwidth limitations and sampling delays, the attitude angle estimation accuracy in high-dynamic flight scenarios is insufficient; traditional methods do not consider the relative posture information between the drone and the transmission line, resulting in large positioning errors when the drone is close to the transmission line due to interference from electromagnetic signals, increasing the risk of collision; at the same time, traditional filtering-based methods only estimate the navigation information of the drone at the current moment, resulting in suboptimal state estimation results. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a navigation and positioning method, medium and device for a power inspection UAV. In order to improve the navigation reliability and stability of the power inspection UAV, the present invention combines the UAV dynamics model with the inertial sensor to improve the accuracy of attitude estimation in a high dynamic environment; based on the factor graph optimization technology, the visual information, inertial information, dynamics model information and satellite information are integrated to achieve high-precision estimation of the UAV navigation state in a short time, and drift-free estimation of the UAV navigation state under long flight time.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] A navigation and positioning method for a power inspection UAV, wherein the UAV is equipped with a visual sensor for sensing the surrounding environment of the UAV and a satellite receiver and an inertial sensor for measuring the UAV, and the UAV is characterized in that the method comprises the following steps:

[0007] Step 1: Periodically collect visual sensor data, satellite receiver data, inertial sensor data, and angular acceleration data obtained based on the UAV dynamics model at time k;

[0008] Step 2: For the visual sensor data, determine whether the current visual frame is a visual key frame based on the visual feature points. If it is a visual key frame, jump to step 3, otherwise return to step 1;

[0009] Step 3: Detect the transmission line based on the image information of the visual keyframe and calculate the relative pose between the visual sensor and the transmission line;

[0010] Step 4: Utilize the angular acceleration data and the inertial sensor data to perform dynamics model-inertial augmented pre-integration calculation between adjacent visual key frames, wherein the dynamics model-inertial augmented pre-integration is an inertial pre-integration that incorporates the dynamics model;

[0011] Step 5: Combine the relative position and posture between the visual sensor and the transmission line and the dynamic model-inertial augmented pre-integration to solve the relative navigation information of the UAV relative to the local navigation coordinate system;

[0012] Step 6: Based on the factor graph method, the relative navigation information of the UAV is integrated with the satellite receiver data to solve the absolute navigation information of the UAV relative to the global navigation coordinate system.

[0013] To optimize the above technical solutions, the specific measures taken also include:

[0014] Furthermore, in step 1, the visual sensor data Including visual image information at time k And the ORB feature points obtained from the previous visual keyframe tracking The angular acceleration data Includes angular acceleration data from time k-1 to time k i = 0, 1, 2, ..., (t(k) - t(k-1)) / Δt, t(k) is the sampling time corresponding to time k, Δt is the sampling period of the UAV dynamics model; the satellite receiver data is the absolute position information of the drone at time k The inertial sensor data Including accelerometer data and gyroscope data and Includes acceleration data from time k-1 to time k and angular velocity data

[0015] Further, in step 2, judging whether the current visual frame is a visual key frame based on the visual feature point information is specifically:

[0016] When the feature points tracked from the previous visual keyframe When the average disparity of is greater than a certain threshold, the frame is set as a new visual keyframe; when the feature points tracked from the previous visual keyframe When the number is less than a certain threshold, the frame is also set as a new key frame;

[0017] At the same time, the visual image information at time k Detect new ORB feature points to ensure the number of feature points in each frame image, and make the feature points evenly distributed by setting the minimum pixel interval between adjacent feature points.

[0018] Further, in step 3, the detection of the transmission line based on the image information of the visual key frame and the calculation of the relative position between the visual sensor and the transmission line are specifically as follows:

[0019] Based on the image information of the visual keyframe, the conductors in the transmission line are detected to obtain two conductors L1 and L2, where L1 is parallel to L2. The insulators in the transmission line are detected to obtain two insulators M1 and M2, which are connected to form a line segment L3, which is perpendicular to L1 and L2.

[0020] Based on the vanishing point, the relative position between the visual sensor and the transmission line is solved. The direction vector of L1 is l1 = (0, 1, 0) T , the direction vector of L3 is l3 = (1, 0, 0) T , the coordinates of the vanishing point in the image are expressed as follows:

[0021]

[0022] In the formula, x represents the coordinate of the vanishing point in the visual image coordinate system, K is the intrinsic parameter matrix of the visual sensor, represents the rotation matrix between the transmission line and the visual sensor, l represents the direction vector of the straight line corresponding to the vanishing point x; according to the properties of the vanishing point, the vanishing point x1 of the wires L1 and L2 and the vanishing point x2 of the line segment L3 are respectively expressed by the following formulas:

[0023]

[0024] In the formula, r1, r2, r3 are the rotation matrices The vanishing point x1 is the intersection of the wires L1 and L2. The coordinates of x1 are directly obtained according to the straight line characteristics, and r2 = K is obtained. -1x1; The column vectors of the rotation matrix are mutually orthogonal, and the vanishing point x2 is on the line L3. Solve x2 by the following set of equations:

[0025]

[0026] In the formula, a, b, and c are the coefficients of the equation of the straight line L3. According to the coordinate value of x2, we can solve r1=K -1 x2, according to the characteristics of the rotation matrix, we can get r3 = r1 × r2; so far, the rotation matrix between the transmission line and the visual sensor is solved.

[0027] Then the translation vector between the transmission line and the visual sensor is solved by the following set of equations:

[0028]

[0029] Wherein, m1 and m2 are the coordinates of the two insulators M1 and M2 in the image coordinate system, M1 and M2 are the coordinates of the two insulators M1 and M2 in the transmission line coordinate system, and Z1 and Z2 are the Z-axis components of the coordinate values ​​of the two insulators M1 and M2 after the coordinates in the transmission line coordinate system are transformed to the visual sensor coordinate system.

[0030] Furthermore, the detection of the conductor adopts a line segment detection algorithm, and the detection of the insulator uses YOLO V10.

[0031] Furthermore, in step 4, the dynamic model-inertia augmentation pre-integration calculation between adjacent visual key frames is specifically:

[0032] The information of angular velocity and acceleration is modeled as:

[0033]

[0034] In the formula, and Represent the true values ​​of acceleration and angular velocity, respectively. and They represent the deviation of acceleration and angular velocity, n a With n w They represent the noise of the accelerometer output and the noise of the estimated angular velocity respectively; represents the attitude transfer matrix from the local navigation coordinate system to the body coordinate system at time i, g n represents the gravity vector in the local navigation coordinate system, represents the output of the accelerometer at time i, represents the angular velocity estimated by fusing the angular acceleration of the dynamic model and the angular velocity of the gyroscope through the Kalman filter at time i;

[0035] The angular velocity prediction model based on angular acceleration is:

[0036]

[0037] In the formula, is the predicted value of angular velocity at time i, ΔT is the discrete sampling period;

[0038] The one-step prediction covariance matrix of the state is:

[0039] P i|i-1 =A i P i-1|i-1 A i T +G i W i G i T ;

[0040] Where P i-1|i-1 is the state covariance matrix estimated at time i-1, P i|i-1 is the one-step forecast covariance matrix estimated at time i, A i is the state transfer matrix, G i is the noise coefficient matrix, W i is the noise matrix, expressed as follows:

[0041]

[0042] In the formula, ε rx , ε ry , ε rz Represent the noise in the x-direction, y-direction, and z-direction respectively;

[0043] The gain of the Kalman filter at time i is expressed as follows:

[0044] K i =P i | i-1 H i T [H i P i|i-1 H i T +R i ] -1 ;

[0045] In the formula, H i represents the measurement matrix, R i represents the measurement noise matrix, whose main diagonal elements are composed of the gyroscope x-axis noise ε ωx , y-axis noise ε ωy and z-axis noise ε ωz constitute;

[0046] The estimated state of the Kalman filter at time i is:

[0047]

[0048] The covariance matrix P of the state at time i i|i for:

[0049] P i|i =[IK i H i ]P i|i-1 ;

[0050] Where I is the unit matrix;

[0051] Finally, the dynamic model-inertial augmented pre-integral between adjacent visual key frames is calculated by the following formula:

[0052]

[0053] In the formula, They are the estimated values ​​of position pre-integration, velocity pre-integration, and attitude pre-integration between time k-1 and time k, They represent the attitude rotation matrix and attitude quaternion between the body coordinate system at time i and the body coordinate system at time k-1 respectively. The Ω(w) operation is expressed as follows:

[0054]

[0055] Where the [·]× operator represents mapping a three-dimensional vector to a 3×3 antisymmetric matrix.

[0056] Furthermore, in step 5, the relative navigation information of the drone relative to the local navigation coordinate system is obtained by:

[0057] Create optimization variable S:

[0058]

[0059] In the formula, Represents the position and posture of the UAV relative to the local navigation coordinate system, Represents the speed of the drone relative to the local navigation coordinate system, represents the accelerometer bias, represents the estimated angular velocity deviation, n s is the number of key frames in the sliding window;

[0060] Create an optimization function:

[0061]

[0062] In the formula, ||r p -Hp S|| 2 is the marginalization constraint, {r p , H p} is the marginalized prior information, H p The Hessian matrix representing marginalized information, r p The residual vector representing the marginalized information; is the dynamic model-inertia augmented pre-integration factor, B is the set of all dynamic model-inertia measurements in the sliding window, is the covariance matrix of the dynamic model-inertial augmented pre-integration factor, is the visual transmission line factor, C is the set of images of the transmission line observed in any two frames in the sliding window, is the covariance matrix of the visual transmission line factors;

[0063] in,

[0064]

[0065] In the formula, represents the dynamic model between time k-1 and time k - the inertia augmented pre-integration factor, They represent the positions of the drone at time k-1 and time k respectively, They represent the speed of the drone at time k-1 and time k respectively, Respectively represent the UAV attitude quaternion at time k-1 and time k, They represent the acceleration deviations at time k-1 and time k respectively, They represent the angular velocity deviations at time k-1 and time k respectively, represents the rotation matrix from the navigation coordinate system to the body coordinate system at time k-1, Δt k represents the time difference between time k-1 and time k, [γ] xyz It means taking the x, y, and z components of the quaternion γ. Represents quaternion multiplication;

[0066]

[0067] In the formula, The visual transmission line residual term is the visual transmission line residual between key frame o and key frame j, R is the rotation matrix representation of the posture, q is the quaternion representation of the posture, are the transmission line position measurement information in the visual sensor coordinate system of the oth frame and the jth frame respectively. is the external parameter between the visual sensor and the inertial sensor, are the pose state variables of the oth frame and the jth frame respectively;

[0068] The optimization function is solved by the nonlinear optimization solver ceres-solver, and the position and velocity of the UAV relative to the local navigation coordinate system are estimated.

[0069] Furthermore, in step 6, the absolute navigation information of the drone relative to the global navigation coordinate system is calculated as follows:

[0070] Create optimization variables:

[0071]

[0072] In the formula, represents the position of the UAV in the global navigation coordinate system, m is the number of UAV positions in the sliding window, m>n s ;

[0073] Create an optimization function:

[0074]

[0075] In the formula, is the relative pose factor, rel is the set of all relative pose measurements in the sliding window, P rel is the covariance matrix of the relative pose factor, is the satellite factor, abs is the satellite measurement set in the sliding window, P abs is the covariance matrix of satellite absolute measurement factors;

[0076] The relative pose residual between two adjacent moments k-1 and k is defined as:

[0077]

[0078] In the formula, Indicates that the relative pose residual term is the relative pose residual between time k-1 and time k, Represents the rotation matrix from the body coordinate system to the local navigation coordinate system at time k-1, Represents the rotation matrix from the body coordinate system to the global navigation coordinate system at time k-1, are the positions of the UAV relative to the local navigation coordinate system at time k-1 and time k, respectively. are the UAV posture state variables in the global navigation coordinate system at time k-1 and time k respectively;

[0079] The absolute position residual between the UAV position and the satellite position at time k is:

[0080]

[0081] In the formula, The absolute position residual term between the UAV position and the satellite position is the absolute position residual between the UAV position and the satellite position at time k;

[0082] The optimization function is solved by the nonlinear optimization solver ceres-solver, and the drone posture is estimated without drift. After each optimization is completed, the posture transformation matrix between the local navigation coordinate system and the global navigation coordinate system is calculated by the following formula:

[0083]

[0084] The relative pose estimation result is transformed into the global navigation coordinate system through the pose transformation matrix, and the absolute navigation information of the UAV is obtained by combining the dynamic model-inertial augmented pre-integration.

[0085] Accordingly, the present invention proposes a computer-readable storage medium storing a computer program, characterized in that the computer program enables a computer to execute the power inspection drone navigation and positioning method as described above.

[0086] Correspondingly, the present invention proposes an electronic device, characterized in that it includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, the navigation and positioning method for the power inspection UAV as described above is implemented.

[0087] The beneficial effects of the present invention are as follows: the present invention can more accurately capture the motion state of the carrier in a high dynamic environment, improve the accuracy of attitude angle estimation, combine visual information, dynamic model information, and inertial information in a local navigation coordinate system, improve the estimation accuracy of navigation information in a short period of time, and fuse the relative navigation information of the UAV with the satellite information in an absolute navigation coordinate system for global joint optimization, thereby realizing drift-free estimation of the UAV navigation information. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 This is a basic flow chart of a navigation and positioning method for power inspection UAVs.

[0089] Figure 2 Schematic diagram of drone power inspection.

[0090] Figure 3 Simulate the flight trajectory of the drone.

[0091] Figure 4 This is a comparison chart of positioning errors between the traditional method and the method of the present invention. DETAILED DESCRIPTION

[0092] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application.

[0093] In one embodiment, the present invention proposes a navigation and positioning method for a power inspection drone, such as Figure 1 As shown, the specific steps include the following steps.

[0094] Step 1: Periodically collect visual sensor data at time k Satellite receiver data Inertial sensor data And the angular acceleration data obtained based on the constructed UAV dynamics model

[0095] The visual sensor, satellite receiver and inertial sensor are all mounted on the UAV. The inertial sensor, dynamic model and satellite receiver measure the UAV, while the visual sensor measures the surrounding environment.

[0096] Visual sensors are used to perceive the environment around the drone. Including visual image information at time k ORB feature point information obtained from the previous visual keyframe tracking Kinetic model data The angular acceleration information of the drone itself, including the angular acceleration data from time k-1 to time k i=0,1,2,…,(t(k)-t(k-1)) / Δt,t(k) is the sampling time corresponding to time k,t(k-1) is the sampling time corresponding to time k-1,Δt is the sampling period of the UAV dynamics model data, which is consistent with the sampling period of the inertial sensor; satellite data is the absolute position information of the drone at time k Inertial sensors are used to sense the acceleration and angular velocity of the drone itself. Contains accelerometer data and gyroscope data and Includes acceleration data from time k-1 to time k and angular velocity data

[0097] Step 2: Determine whether the current visual frame is a key frame based on the visual feature point information. If it is a key frame, jump to step 3, otherwise jump to step 1.

[0098] When the feature points tracked from the previous visual keyframe When the average disparity of is greater than a certain threshold, the frame is set as a new visual keyframe; when the feature points tracked from the previous visual keyframe When the number is less than a certain threshold, the frame is set as a new key frame; at the same time, the visual image information at time k Detect new ORB feature point information to ensure the number of feature points in each frame image, and make the feature points evenly distributed by setting the minimum pixel interval between adjacent features.

[0099] Step 3: Detect the transmission line based on the visual keyframe image information and calculate the relative pose between the visual sensor and the transmission line.

[0100] like Figure 2 As shown, the line segment detector (LSD) algorithm is used to detect the wires, and two wires L1 and L2 are obtained, where L1 is parallel to L2. YOLO V10 is used to detect the insulators in the transmission line, and two insulators M1 and M2 are obtained. The two insulators are connected to form a line segment L3, which is perpendicular to L1 and L2.

[0101] Based on the vanishing point, the relative position between the visual sensor and the transmission line is solved, and the direction vector of the wire L1 is set to l1 = (0, 1, 0) T , the direction vector of L3 is l3 = (1, 0, 0) T , the coordinates of the vanishing point in the image are related to the camera posture and can be expressed by the following formula:

[0102]

[0103] In the formula, x represents the coordinate of the vanishing point in the visual image coordinate system, K is the intrinsic parameter matrix of the visual sensor, represents the rotation matrix between the transmission line and the visual sensor, and l represents the direction vector of the straight line corresponding to the vanishing point x. According to the properties of the vanishing point, the vanishing point x1 of the wires L1 and L2 and the vanishing point x2 of the line segment L3 can be expressed by the following formulas:

[0104]

[0105] In the formula, r1, r2, r3 are the rotation matrices The vanishing point x1 is the intersection of the wires L1 and L2. The coordinates of x1 can be directly obtained based on the straight line characteristics. Therefore, substituting the coordinates of x1 into the above formula, we can obtain r2 = K -1 x1. The column vectors of the rotation matrix are mutually orthogonal, and the vanishing point x2 is on the line L3. Solve x2 by the following set of equations:

[0106]

[0107] In the formula, a, b, and c are the coefficients of the equation of the straight line L3. According to the coordinate value of x2, r1 = K -1x2, according to the characteristics of the rotation matrix, we can get r3 = r1 × r2. So far, the rotation matrix between the transmission line and the visual sensor has been solved.

[0108] The translation vector between the transmission line and the visual sensor is solved by the following set of equations

[0109]

[0110] In the formula, m1 and m2 are the coordinates of the two insulators M1 and M2 in the image coordinate system, M1 and M2 are the coordinates of the two insulators M1 and M2 in the transmission line coordinate system, and Z1 and Z2 are the Z-axis components of the coordinate values ​​of the two insulators M1 and M2 after the coordinates in the transmission line coordinate system are transformed to the visual sensor coordinate system. At this point, the translation vector between the transmission line and the visual sensor is solved.

[0111] Step 4: Use the dynamic model data and inertial sensor data to perform dynamic model-inertial augmented pre-integration calculations between adjacent visual key frames.

[0112] The dynamic model-inertial augmented pre-integration integrates the UAV dynamic model information on the basis of traditional inertial pre-integration. It can track the UAV motion state more accurately in a high dynamic environment and improve the accuracy and reliability of navigation. At the same time, it avoids the repeated integration of the dynamic model-inertial information in the factor graph optimization process and improves the real-time performance of the navigation solution.

[0113] The angular acceleration output by the combined dynamics model, the angular velocity output by the gyroscope, and the acceleration information provided by the accelerometer are used to construct the dynamics model-inertial augmented pre-integration model. The angular velocity and acceleration information can be modeled as follows:

[0114]

[0115] In the formula, and Represent the true values ​​of acceleration and angular velocity, respectively. and Indicates the deviation between acceleration and angular velocity, n a With n w represents the noise of the accelerometer output and the noise of the estimated angular velocity. represents the attitude transfer matrix from the local navigation coordinate system to the body coordinate system at time i, g n represents the gravity vector in the local navigation coordinate system, represents the output of the accelerometer at time i, It represents the angular velocity estimated by fusing the angular acceleration of the dynamic model and the angular velocity of the gyroscope through the Kalman filter at time i.

[0116] The angular velocity prediction model based on angular acceleration can be expressed as follows:

[0117]

[0118] In the formula, is the angular velocity estimated by fusing the angular acceleration of the dynamics model and the angular velocity of the gyroscope at time i-1, ΔT is the discrete sampling period, is the predicted value of angular velocity at time i. The current angular velocity is predicted by the angular velocity at the previous moment and the angular acceleration information of the dynamic model, and The Kalman filter is updated using the angular velocity output by the gyroscope. The one-step prediction covariance matrix of the state can be expressed as follows:

[0119] P i|i-1 =A i P i-1|i-1 A i T +G i W i G i T ;

[0120] Where P i-1|i-1 is the state covariance matrix estimated at time i-1, P i|i-1 is the one-step forecast covariance matrix estimated at time i, A i is the state transfer matrix, G i is the noise coefficient matrix, W i is the noise matrix, which can be expressed as follows:

[0121]

[0122] In the formula, ε rx , ε ry , ε rz They represent the noise in the x-direction, y-direction, and z-direction of the system respectively. The gain of the Kalman filter at time i can be expressed as follows:

[0123] K i =P i|i-1 H i T [H i P i|i-1 H i T +R i ] -1 ;

[0124] In the formula, H i Represents the measurement matrix, which is the unit matrix, R i represents the measurement noise matrix, whose main diagonal elements are composed of the gyroscope x-axis noise ε ωx , y-axis noise ε ωy and z-axis noise ε ωz The Kalman filter state estimate at time i can be expressed as:

[0125]

[0126] Finally, we need to calculate the covariance matrix P of the state at time i i|i , which can be expressed as follows:

[0127] P i|i =[IK i H i ]P i|i-1 ;

[0128] Where I is the unit matrix.

[0129] The dynamic model-inertia augmented pre-integral value between adjacent visual key frames is calculated by the following formula:

[0130]

[0131] In the formula, They are the estimated values ​​of position pre-integration, velocity pre-integration, and attitude pre-integration between time k-1 and time k, They represent the attitude rotation matrix and attitude quaternion between the body coordinate system at time i and the body coordinate system at time k-1 respectively. The Ω(w) operation is specifically expressed as follows:

[0132]

[0133] In the formula, [·] × The operator represents mapping a three-dimensional vector to a 3×3 antisymmetric matrix, as follows:

[0134]

[0135] In the formula, w x 、w y 、w z Represents the components of the three-dimensional vector w.

[0136] Step 5: Combine vision, dynamics model, and inertial information to solve the navigation information of the UAV relative to the local navigation coordinate system (the UAV body coordinate system at the initial moment).

[0137] Create optimization variable S:

[0138]

[0139] In the formula, Represents the position and posture of the UAV relative to the local navigation coordinate system, Represents the speed of the drone relative to the local navigation coordinate system, represents the accelerometer bias, represents the estimated angular velocity deviation, n s is the number of key frames in the sliding window.

[0140] Create an optimization function:

[0141]

[0142] In the formula, ||r p -H p S|| 2 is the marginalization constraint, {r p , H p} is the marginalized prior information, H p The Hessian matrix representing marginalized information, r p The residual vector representing the marginalized information; is the dynamic model-inertia augmented pre-integration factor, B is the set of all dynamic model-inertia measurements in the sliding window, is the covariance matrix of the dynamic model-inertial augmented pre-integration factor, is the visual transmission line factor, C is the set of images of the transmission line observed in any two frames in the sliding window, is the covariance matrix of the visual transmission line factors.

[0143] The dynamic model-inertia augmented pre-integration factor between two adjacent visual key frames is the difference between the pre-integration prediction value and the pre-integration calculation value:

[0144]

[0145] In the formula, Indicates that the dynamic model-inertia augmented pre-integration factor term is the dynamic model-inertia augmented pre-integration factor between time k-1 and time k, They represent the positions of the drone at time k-1 and time k respectively, They represent the speed of the drone at time k-1 and time k respectively, Respectively represent the UAV attitude quaternion at time k-1 and time k, They represent the acceleration deviations at time k-1 and time k respectively, They represent the angular velocity deviations at time k-1 and time k respectively, represents the rotation matrix from the navigation coordinate system to the body coordinate system at time k-1, Δtk represents the time difference between time k-1 and time k, [γ] xyz It means taking the x, y, and z components of the quaternion γ.

[0146] The visual transmission line residual between two key frames o and j is the difference between the predicted value and the measured value of the relative pose:

[0147]

[0148] In the formula, It indicates that the visual transmission line residual term is the visual transmission line residual between key frame o and key frame j, R is the rotation matrix representation of the posture, q is the quaternion representation of the posture, are the transmission line position measurement information in the visual sensor coordinate system of the oth frame and the jth frame respectively. is the external parameter between the visual sensor and the inertial sensor, obtained by prior calibration. ) are the pose state variables of the oth frame and the jth frame respectively, Represents a quaternion multiplication operation.

[0149] The optimization function is solved by the nonlinear optimization solver ceres-solver to achieve accurate estimation of the position and velocity of the UAV relative to the local coordinate system in a short time.

[0150] Step 6: Based on the factor graph method, the relative navigation information of the UAV is integrated with the satellite information to solve the absolute navigation information of the UAV.

[0151] Create optimization variables:

[0152]

[0153] In the formula, represents the position of the UAV in the global navigation coordinate system, m is the number of UAV positions in the sliding window, and m is much larger than the number of key frames n in the sliding window in step 5 s .

[0154] Create an optimization function:

[0155]

[0156] In the formula, is the relative pose factor, rel is the set of all relative pose measurements in the sliding window, P rel is the covariance matrix of the relative pose factor, is the satellite factor, abs is the satellite measurement set in the sliding window, P abs is the covariance matrix of the satellite absolute measurement factors.

[0157] The relative pose residual between two adjacent moments k-1 and k is defined as:

[0158]

[0159] In the formula, Indicates that the relative pose residual term is the relative pose residual between time k-1 and time k, Represents the rotation matrix from the body coordinate system to the local navigation coordinate system at time k-1, Represents the rotation matrix from the body coordinate system to the global navigation coordinate system at time k-1, are the positions of the UAV at time k-1 and time k relative to the local navigation coordinate system obtained by integrating the dynamic model, inertia and vision information in step 5, They are the UAV posture state variables in the global navigation coordinate system (northeast sky coordinate system) at time k-1 and time k respectively.

[0160] The absolute position residual between the UAV position and the satellite position at time k is defined as:

[0161]

[0162] In the formula, The absolute position residual term between the UAV position and the satellite position is the absolute position residual between the UAV position and the satellite position at time k.

[0163] The optimization function is solved by the nonlinear optimization solver ceres-solver to achieve drift-free estimation of the UAV posture under long flight time. After each optimization is completed, the posture transformation matrix between the local navigation coordinate system and the global navigation coordinate system is calculated by the following formula

[0164]

[0165] The relative pose estimation results are transformed into the global navigation coordinate system through this matrix, and combined with the dynamic model-inertial augmented pre-integration information to obtain high-speed, high-precision, and drift-free UAV absolute navigation information.

[0166] Step 7: Output the absolute navigation information of the drone.

[0167] Next, the positioning accuracy of the method of the present invention is verified by using simulation data testing. Figure 3 In the simulation, the white noise standard deviation of the accelerometer and gyroscope is set to 0.1m / s 2 , 0.01rad / s, and the standard deviation of random walk white noise is set to 0.001m / s 2, 0.0001rad / s, and the update frequency is 100Hz; the standard deviation of the angular acceleration white noise in the dynamic model is set to 0.0001rad / s / s, and the update frequency is 100Hz; the standard deviation of the satellite position measurement white noise is set to 2m, and the update frequency is 5Hz; the standard deviation of the white noise of the relative position and relative attitude based on vision is set to 0.5m and 0.1deg respectively, and the update frequency is 10Hz.

[0168] Figure 4 The positioning error comparison between the traditional method and the method of the present invention is shown in Figure 4 It can be seen that the positioning error of the method of the present invention is smaller than that of the traditional method, and it can be seen that the positioning accuracy of the method of the present invention is better than that of the traditional method.

[0169] In another embodiment, the present invention proposes a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the navigation and positioning method for the electric power inspection drone as described in the first embodiment.

[0170] In another embodiment, the present invention proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the navigation and positioning method for the power inspection UAV as described in Example 1 is implemented.

[0171] In the embodiments disclosed in the present application, the computer storage medium can be a tangible medium that can contain or store a program for use by or in conjunction with an instruction execution system, device or equipment. The computer storage medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or equipment, or any suitable combination of the above. More specific examples of computer storage media can include electrical connections based on one or more lines, portable computer disks, hard disks, random access memories (RAM), read-only memories (ROM), erasable programmable read-only memories (EPROM or flash memory), optical fibers, portable compact disk read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above.

[0172] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0173] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A navigation and positioning method for a power inspection drone, wherein the drone is equipped with a visual sensor for sensing the drone's surrounding environment and a satellite receiver and an inertial sensor for measuring the drone, wherein the drone is characterized in that: The steps include: Step 1: Periodically collect visual sensor data, satellite receiver data, inertial sensor data, and angular acceleration data obtained based on the UAV dynamics model at time k; Step 2: For the visual sensor data, determine whether the current visual frame is a visual key frame based on the visual feature points. If it is a visual key frame, jump to step 3, otherwise return to step 1; Step 3: Detect the transmission line based on the image information of the visual keyframe and calculate the relative pose between the visual sensor and the transmission line; Step 4: Utilize the angular acceleration data and the inertial sensor data to perform dynamics model-inertial augmented pre-integration calculation between adjacent visual key frames, wherein the dynamics model-inertial augmented pre-integration is an inertial pre-integration that incorporates the dynamics model; Step 5: Combine the relative position and posture between the visual sensor and the transmission line and the dynamic model-inertial augmented pre-integration to solve the relative navigation information of the UAV relative to the local navigation coordinate system; Step 6: Based on the factor graph method, the relative navigation information of the UAV is integrated with the satellite receiver data to solve the absolute navigation information of the UAV relative to the global navigation coordinate system.

2. A navigation and positioning method for electric power inspection drone according to claim 1, characterized in that: In step 1, the visual sensor data Including visual image information at time k And the ORB feature points obtained from the previous visual keyframe tracking The angular acceleration data Includes angular acceleration data from time k-1 to time k i = 0, 1, 2, ..., (t(k) - t(k-1)) / Δt, t(k) is the sampling time corresponding to time k, Δt is the sampling period of the UAV dynamics model; the satellite receiver data is the absolute position information of the drone at time k The inertial sensor data Including accelerometer data and gyroscope data and Includes acceleration data from time k-1 to time k and angular velocity data 3. A navigation and positioning method for electric power inspection drone according to claim 2, characterized in that: In step 2, judging whether the current visual frame is a visual key frame based on the visual feature point information is specifically as follows: When the feature points tracked from the previous visual keyframe When the average disparity of is greater than a certain threshold, the frame is set as a new visual keyframe; when the feature points tracked from the previous visual keyframe When the number is less than a certain threshold, the frame is also set as a new key frame; At the same time, the visual image information at time k Detect new ORB feature points to ensure the number of feature points in each frame image, and make the feature points evenly distributed by setting the minimum pixel interval between adjacent feature points.

4. A navigation and positioning method for electric power inspection drone according to claim 2, characterized in that: In step 3, the power transmission line is detected based on the image information of the visual key frame and the relative position between the visual sensor and the power transmission line is calculated, specifically: Based on the image information of the visual keyframe, the conductors in the transmission line are detected to obtain two conductors L1 and L2, where L1 is parallel to L2. The insulators in the transmission line are detected to obtain two insulators M1 and M2, which are connected to form a line segment L3, which is perpendicular to L1 and L2. Based on the vanishing point, the relative position between the visual sensor and the transmission line is solved. The direction vector of L1 is l1 = (0, 1, 0) T , the direction vector of L3 is l3 = (1, 0, 0) T , the coordinates of the vanishing point in the image are expressed as follows: In the formula, x represents the coordinates of the vanishing point in the visual image coordinate system, K is the intrinsic parameter matrix of the visual sensor, represents the rotation matrix between the transmission line and the visual sensor, l represents the direction vector of the straight line corresponding to the vanishing point x; according to the properties of the vanishing point, the vanishing point x1 of the wires L1 and L2 and the vanishing point x2 of the line segment L3 are respectively expressed by the following formulas: In the formula, r1, r2, r3 are the rotation matrices The vanishing point x1 is the intersection of the wires L1 and L2. The coordinates of x1 are directly obtained according to the straight line characteristics, and r2 = K is obtained. -1 x1; The column vectors of the rotation matrix are mutually orthogonal, and the vanishing point x2 is on the line L3. Solve x2 by the following set of equations: In the formula, a, b, and c are the coefficients of the equation of the straight line L3. According to the coordinate value of x2, we can solve r1=K -1 x2, according to the characteristics of the rotation matrix, we can get r3 = r1 × r2; so far, the rotation matrix between the transmission line and the visual sensor is solved. Then the translation vector between the transmission line and the visual sensor is solved by the following set of equations: Wherein, m1 and m2 are the coordinates of the two insulators M1 and M2 in the image coordinate system, M1 and M2 are the coordinates of the two insulators M1 and M2 in the transmission line coordinate system, and Z1 and Z2 are the Z-axis components of the coordinate values ​​of the two insulators M1 and M2 after the coordinates in the transmission line coordinate system are transformed to the visual sensor coordinate system.

5. A navigation and positioning method for electric power inspection drone according to claim 4, characterized in that: The detection of the conductor adopts a line segment detection algorithm, and the detection of the insulator uses YOLO V10.

6. A navigation and positioning method for electric power inspection drone according to claim 4, characterized in that: In step 4, the dynamic model-inertia augmentation pre-integration calculation between adjacent visual key frames is specifically as follows: The information of angular velocity and acceleration is modeled as: In the formula, and Represent the true values ​​of acceleration and angular velocity, respectively. and They represent the deviation of acceleration and angular velocity, n a With n w They represent the noise of the accelerometer output and the noise of the estimated angular velocity respectively; represents the attitude transfer matrix from the local navigation coordinate system to the body coordinate system at time i, g n represents the gravity vector in the local navigation coordinate system, represents the output of the accelerometer at time i, represents the angular velocity estimated by fusing the angular acceleration of the dynamic model and the angular velocity of the gyroscope through the Kalman filter at time i; The angular velocity prediction model based on angular acceleration is: In the formula, is the predicted value of angular velocity at time i, ΔT is the discrete sampling period; The one-step prediction covariance matrix of the state is: P i|i-1 =A i P i-1|i-1 A i T +G i W i G i T ; Where P i-1|i-1 is the state covariance matrix estimated at time i-1, P i|i-1 is the one-step forecast covariance matrix estimated at time i, A i is the state transfer matrix, G i is the noise coefficient matrix, W i is the noise matrix, expressed as follows: In the formula, ε rx , ε ry , ε rz Represent the noise in the x-direction, y-direction, and z-direction respectively; The gain of the Kalman filter at time i is expressed as follows: K i =P i|i-1 H i T [H i P i|i-1 H i T +R i ] -1 ; In the formula, H i represents the measurement matrix, R i represents the measurement noise matrix, whose main diagonal elements are composed of the gyroscope x-axis noise ε ωx , y-axis noise ε ωy and z-axis noise ε ωz constitute; The estimated state of the Kalman filter at time i is: The covariance matrix P of the state at time i i|i for: P i|i =[I-K i H i ]P i|i-1 ; Where I is the unit matrix; Finally, the dynamic model-inertial augmented pre-integral between adjacent visual key frames is calculated by the following formula: In the formula, They are the estimated values ​​of position pre-integration, velocity pre-integration, and attitude pre-integration between time k-1 and time k, They represent the attitude rotation matrix and attitude quaternion between the body coordinate system at time i and the body coordinate system at time k-1 respectively. The Ω(w) operation is expressed as follows: Where the [·]× operator represents mapping a three-dimensional vector to a 3×3 antisymmetric matrix.

7. A navigation and positioning method for electric power inspection drone according to claim 6, characterized in that: In step 5, the relative navigation information of the drone relative to the local navigation coordinate system is obtained by: Create optimization variable S: In the formula, Represents the position and posture of the UAV relative to the local navigation coordinate system, Represents the speed of the drone relative to the local navigation coordinate system, represents the accelerometer bias, represents the estimated angular velocity deviation, n s is the number of key frames in the sliding window; Create an optimization function: In the formula, ||r p -H p S|| 2 is the marginalization constraint, {r p , H p } is the marginalized prior information, H p The Hessian matrix representing marginal information, r p The residual vector representing the marginalized information; is the dynamic model-inertia augmented pre-integration factor, B is the set of all dynamic model-inertia measurements in the sliding window, is the covariance matrix of the dynamic model-inertial augmented pre-integration factor, is the visual transmission line factor, C is the set of images of the transmission line observed in any two frames in the sliding window, is the covariance matrix of the visual transmission line factors; in, In the formula, represents the dynamic model between time k-1 and time k - the inertia augmented pre-integration factor, They represent the positions of the drone at time k-1 and time k respectively, They represent the speed of the drone at time k-1 and time k respectively, Respectively represent the UAV attitude quaternion at time k-1 and time k, They represent the acceleration deviations at time k-1 and time k respectively, They represent the angular velocity deviations at time k-1 and time k respectively, represents the rotation matrix from the navigation coordinate system to the body coordinate system at time k-1, Δt k represents the time difference between time k-1 and time k, [γ] xyz It means taking the x, y, and z components of the quaternion γ. Represents quaternion multiplication; In the formula, The visual transmission line residual term is the visual transmission line residual between key frame o and key frame, R is the rotation matrix representation of the posture, q is the quaternion representation of the posture, are the transmission line position measurement information in the visual sensor coordinate system of the oth frame and the jth frame respectively. is the external parameter between the visual sensor and the inertial sensor, are the pose state variables of the oth frame and the jth frame respectively; The optimization function is solved by the nonlinear optimization solver ceres-solver, and the position and velocity of the UAV relative to the local navigation coordinate system are estimated.

8. A navigation and positioning method for electric power inspection drone according to claim 7, characterized in that: In step 6, the absolute navigation information of the drone relative to the global navigation coordinate system is calculated as follows: Create optimization variables: In the formula, represents the position of the UAV in the global navigation coordinate system, m is the number of UAV positions in the sliding window, m>n s ; Create an optimization function: In the formula, is the relative pose factor, rel is the set of all relative pose measurements in the sliding window, P rel is the covariance matrix of the relative pose factor, is the satellite factor, abs is the satellite measurement set in the sliding window, P abs is the covariance matrix of satellite absolute measurement factors; The relative pose residual between two adjacent moments k-1 and k is defined as: In the formula, Indicates that the relative pose residual term is the relative pose residual between time k-1 and time k, Represents the rotation matrix from the body coordinate system to the local navigation coordinate system at time k-1, Represents the rotation matrix from the body coordinate system to the global navigation coordinate system at time k-1, are the positions of the UAV relative to the local navigation coordinate system at time k-1 and time k, respectively. are the UAV posture state variables in the global navigation coordinate system at time k-1 and time k respectively; The absolute position residual between the UAV position and the satellite position at time k is: In the formula, The absolute position residual term between the UAV position and the satellite position is the absolute position residual between the UAV position and the satellite position at time k; The optimization function is solved by the nonlinear optimization solver ceres-solver, and the drone posture is estimated without drift. After each optimization is completed, the posture transformation matrix between the local navigation coordinate system and the global navigation coordinate system is calculated by the following formula: The relative pose estimation result is transformed into the global navigation coordinate system through the pose transformation matrix, and the absolute navigation information of the UAV is obtained by combining the dynamic model-inertial augmented pre-integration.

9. A computer-readable storage medium storing a computer program, characterized in that: The computer program enables the computer to execute the power inspection UAV navigation and positioning method as described in any one of claims 1-8.

10. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the navigation and positioning method for the electric power inspection UAV as described in any one of claims 1 to 8 is implemented.

Images