A navigation and positioning method, medium and equipment for electric power inspection drones

Abstract

This invention proposes a navigation and positioning method, medium, and equipment for a power inspection drone, including: periodically collecting sensor data; determining whether the current visual frame is a keyframe; detecting power transmission lines and calculating the relative pose between the visual sensor and the transmission line based on the visual keyframe image information; using dynamic model data and inertial sensor data to perform dynamic model-inertial augmented pre-integration calculations between adjacent visual keyframes; combining visual, dynamic model, and inertial information to determine the drone's navigation information relative to a local navigation coordinate system; fusing the drone's relative navigation information with satellite information based on a factor graph method to calculate the drone's absolute navigation information; and outputting the drone's absolute navigation information. By fusing dynamic model information, this invention achieves more accurate attitude angle estimation for drones in highly dynamic environments. It also utilizes factor graph optimization technology to achieve short-term, high-precision, and long-term drift-free navigation information estimation for power inspection drones.

Description

Technical Field

[0001] The present invention belongs to the field of 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 inspections has increased significantly. Using drones for inspections not only improves efficiency and reduces grid maintenance costs, but also ensures personnel safety, offering broad application prospects. Navigation and positioning are essential core technologies for power drone inspections. When performing inspections, drones rely on accurate navigation and positioning information to ensure safety and efficiency. However, existing navigation and positioning technologies face numerous challenges in power inspection scenarios.

[0003] Power inspections are often conducted in complex terrain environments, which can easily block satellite signals. Long-term operation of a single inertial sensor can easily lead to cumulative errors and drift, necessitating multi-source information fusion technology to ensure navigation system accuracy. Traditional power inspection drones fuse inertial sensor information with satellite information through filtering. However, due to gyroscope bandwidth limitations and sampling delays, attitude angle estimation accuracy is insufficient in highly dynamic flight scenarios. Traditional methods do not consider the relative position between the drone and the power transmission line, resulting in large positioning errors due to electromagnetic signal interference when the drone approaches the power transmission line, increasing the risk of collision. Furthermore, traditional filtering-based methods only estimate the drone's navigation information at the current moment, resulting in suboptimal state estimation results. Summary of the Invention

[0004] This invention addresses the shortcomings of existing technologies and provides a navigation and positioning method, medium, and device for power inspection drones. To improve the navigation reliability and stability of power inspection drones, the invention combines the drone's dynamics model with inertial sensors to enhance attitude estimation accuracy in highly dynamic environments. By integrating visual information, inertial information, dynamics model information, and satellite information using factor graph optimization technology, the method achieves high-precision estimation of the drone's navigation state within a short period of time, as well as drift-free estimation of the drone's navigation state over long flight times.

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

[0006] A navigation and positioning method for a power inspection drone is provided. The drone is equipped with a visual sensor for sensing the drone's surrounding environment, and the drone is also equipped with a satellite receiver and an inertial sensor. The method is characterized by comprising 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 dynamic model-inertial augmented pre-integration calculations between adjacent visual keyframes. The dynamic model-inertial augmented pre-integration is an inertial pre-integration that incorporates the dynamic model.

[0011] Step 5: Combine the relative pose between the visual sensor and the transmission line with 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 calculate the absolute navigation information of the UAV relative to the global navigation coordinate system.

[0013] To optimize the above technical solutions, 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] Furthermore, in step 2, the process of determining whether the current visual frame is a visual key frame based on the visual feature point information is as follows:

[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 key frame; when the feature points tracked from the previous visual key frame 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] Furthermore, in step 3, the detection of the transmission line based on the image information of the visual keyframe 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. The two insulators are connected to form a line segment L3, which is perpendicular to L1 and L2.

[0020] The relative position between the visual sensor and the transmission line is solved based on the vanishing point. 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] Where 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 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 orthogonal to each other, and the vanishing point x2 is on the line L3. Solve x2 using the following system of equations:

[0025]

[0026] Where a, b, and c are the coefficients of the equation of line L3. Based on 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, we can solve the rotation matrix between the transmission line and the visual sensor.

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

[0028]

[0029] Where 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 vision 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 augmented pre-integration calculation between adjacent visual key frames is specifically as follows:

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

[0033]

[0034] Where, and Represent the true values ​​of acceleration and angular velocity, respectively. and 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 dynamic model angular acceleration and the gyroscope angular velocity through the Kalman filter at time i;

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

[0036]

[0037] Where, 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 prediction 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 is expressed as follows:

[0041]

[0042] Where, ε rx , ε ry , ε rz Represents 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] Where 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 identity matrix;

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

[0052]

[0053] Where, 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] Establish optimization variable S:

[0058]

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

[0060] Create an optimization function:

[0061]

[0062] Where, ||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 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 any two frames in the sliding window that observe the transmission line, is the covariance matrix of the visual transmission line factors;

[0063] in,

[0064]

[0065] Where, represents the dynamic model between time k-1 and time k - the inertia augmented pre-integration factor, Represent the positions of the drone at time k-1 and time k respectively, Represent the speed of the drone at time k-1 and time k respectively, Represent the UAV attitude quaternion at time k-1 and time k respectively, 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 Indicates taking the x, y, and z components of the quaternion γ. Represents quaternion multiplication operations;

[0066]

[0067] Where, 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 posture 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] Where, 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] Where, 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;

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

[0077]

[0078] Where, 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, 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] Where, 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 pose is estimated without drift. After each optimization is completed, the pose 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 above-mentioned power inspection drone navigation and positioning method.

[0086] Accordingly, 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 runnable on the processor. When the processor executes the computer program, it implements the above-mentioned power inspection drone navigation and positioning method.

[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 drone and satellite information in the absolute navigation coordinate system for global joint optimization, thereby realizing drift-free estimation of the drone 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 This is a simulated flight trajectory diagram for 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, and their data 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 UAV 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 key frame; when the feature points tracked from the previous visual key frame 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 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 in the figure, the line segment detector (LSD) algorithm is used to detect the wires, and two wires L1 and L2 are obtained. 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] Solve the relative position between the visual sensor and the transmission line based on the vanishing point, and assume that the direction vector of the wire 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 related to the camera pose and can be expressed as follows:

[0102]

[0103] Where 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 line corresponding to the vanishing point x. According to the properties of the vanishing points, 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 get r2 = K -1 x1. The column vectors of the rotation matrix are orthogonal to each other, and the vanishing point x2 is on the line L3. Solve x2 using the following system of equations:

[0106]

[0107] Where a, b, and c are the coefficients of the equation of line L3. Based on the coordinate value of x2, we can solve 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 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 calculation between adjacent visual key frames.

[0112] The dynamic model-inertia augmented pre-integration integrates the UAV dynamic model information on the basis of traditional inertial pre-integration. It can more accurately track the UAV motion state in a highly dynamic environment, improve the accuracy and reliability of navigation, and avoid repeated integration of dynamic model-inertial information during the factor graph optimization process, thereby improving 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 a dynamics model - an inertial augmented pre-integration model. The angular velocity and acceleration information can be modeled as follows:

[0114]

[0115] Where, 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 dynamic model angular acceleration and the gyroscope angular velocity through the Kalman filter at time i.

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

[0117]

[0118] Where, is the angular velocity estimated by fusing the angular acceleration of the dynamic 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 of the previous moment and the angular acceleration information of the dynamic model, and The Kalman filter is updated using the gyroscope output angular velocity. 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 prediction 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] Where, ε 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] Where 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 identity matrix.

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

[0130]

[0131] Where, 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] Where, [·] × The operator represents mapping a three-dimensional vector to a 3×3 antisymmetric matrix, as follows:

[0134]

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

[0136] Step 5: Combine vision, dynamic 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] Establish optimization variable S:

[0138]

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

[0140] Create an optimization function:

[0141]

[0142] Where, ||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 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 any two frames in the sliding window that observe the transmission line, is the covariance matrix of the visual transmission line factors.

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

[0144]

[0145] Where, 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, Represent the positions of the drone at time k-1 and time k respectively, Represent the speed of the drone at time k-1 and time k respectively, Represent the UAV attitude quaternion at time k-1 and time k respectively, 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 Indicates taking the x, y, and z components of the quaternion γ.

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

[0147]

[0148] Where, 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 posture 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 UAV's position and velocity 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 calculate the absolute navigation information of the UAV.

[0151] Create optimization variables:

[0152]

[0153] Where, 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] Where, 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] Where, 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 obtained by the fusion of dynamic model, inertia and vision information in step 5, 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] Where, 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's 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 the figure. 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 provides a computer-readable storage medium storing a computer program, which enables a computer to execute the navigation and positioning method for the 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 drone as described in Example 1 is implemented.

[0171] In the embodiments disclosed herein, computer storage media can be tangible media that can contain or store programs for use by or in conjunction with an instruction execution system, device, or apparatus. Computer storage media can include, but are not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or equipment, or any suitable combination of the foregoing. 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-ROMs), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0172] Those skilled 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 merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions based on the principles of the present invention are within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and modifications that do not depart from the principles of the present invention should be considered within the scope of protection 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, the measurement object being the drone, 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 dynamic model-inertial augmented pre-integration calculations between adjacent visual keyframes. The dynamic model-inertial augmented pre-integration is an inertial pre-integration that incorporates the dynamic model. Step 5: Combine the relative pose between the visual sensor and the transmission line with 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 calculate the absolute navigation information of the UAV relative to the global navigation coordinate system.

2. The navigation and positioning method for a 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. The navigation and positioning method for a power inspection drone according to claim 2, characterized in that: In step 2, the process of determining whether the current visual frame is a visual key frame based on the visual feature points is 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 key frame; when the feature points tracked from the previous visual key frame 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. The navigation and positioning method for a 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 keyframe 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. The two insulators are connected to form a line segment L3, which is perpendicular to L1 and L2. The relative position between the visual sensor and the transmission line is solved based on the vanishing point. 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: Where 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 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 orthogonal to each other, and the vanishing point x2 is on the line L3. Solve x2 using the following system of equations: Where a, b, and c are the coefficients of the equation of line L3. Based on 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, we can solve the rotation matrix between the transmission line and the visual sensor. Then the translation vector between the transmission line and the visual sensor is solved by the following equations: Where 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 vision sensor coordinate system.

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

6. The navigation and positioning method for a 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: Where, and Represent the true values ​​of acceleration and angular velocity, respectively. and 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 dynamic model angular acceleration and the gyroscope angular velocity through the Kalman filter at time i; The angular velocity prediction model based on angular acceleration is: Where, 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 prediction 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 is expressed as follows: Where, ε rx , ε ry , ε rz Represents 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 ; Where 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 identity matrix; Finally, the dynamic model-inertial augmented pre-integral between adjacent visual keyframes is calculated by the following formula: Where, 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. The navigation and positioning method for a 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 as follows: Establish optimization variable S: Where, Represents the position of the UAV relative to the local navigation coordinate system, Indicates the speed of the UAV relative to the local navigation coordinate system, represents the accelerometer bias, Denotes the estimated angular velocity deviation, n s is the number of key frames in the sliding window; Create an optimization function: Where, ||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 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 any two frames in the sliding window that observe the transmission line, is the covariance matrix of the visual transmission line factors; in, Where, represents the dynamic model between time k-1 and time k - the inertia augmented pre-integration factor, Represent the positions of the drone at time k-1 and time k respectively, Represent the speed of the drone at time k-1 and time k respectively, Represent the UAV attitude quaternion at time k-1 and time k respectively, 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 Indicates taking the x, y, and z components of the quaternion γ. Represents quaternion multiplication operation; Where, 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 posture 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. The navigation and positioning method for a 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: Where, 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: Where, 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; The relative pose residual between two adjacent moments k-1 and k is defined as: Where, 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, 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: Where, 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 pose is estimated without drift. After each optimization is completed, the pose 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 drone navigation and positioning method as described in any one of claims 1 to 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