Coordinate system conversion method, inertia and vision fused navigation method and unmanned aerial vehicle

Abstract

The invention relates to the technical field of unmanned aerial vehicle navigation, in particular to a coordinate system conversion method, a navigation method fusing inertia and vision and an unmanned aerial vehicle. According to the method for converting the IMU coordinate system into the visual coordinate system, firstly, IMU data and visual data with the same timestamp are obtained to form a combined sample, the IMU data are collected by an IMU sensor, and the visual data are collected by a visual sensor; and optimizing an objective function on the combined sample, and extracting a rotation matrix R and a translation vector c which meet an optimization objective to form a transfer matrix Tci from the IMU coordinate system to the visual coordinate system. According to the method, coordinate conversion is performed on the IMU data and the visual data based on the timestamp, a foundation is laid for fusion of IMU detection and visual detection, unmanned aerial vehicle positioning can be conveniently performed by combining the IMU data and the visual data, the unmanned aerial vehicle positioning precision is improved, and the problem that the unmanned aerial vehicle cannot be accurately positioned by adopting a single sensor in a complex environment is solved.

Description

Technical Field

[0001] The present application relates to the field of unmanned aerial vehicle navigation technology, and in particular to a coordinate system conversion method, a navigation method integrating inertia and vision, and a unmanned aerial vehicle. Background Art

[0002] Due to the narrow space and complex structure of the cable trench, traditional drone navigation technology such as GPS cannot be effectively used in underground environments, and navigation is easily interfered by obstacles. Existing drones cannot adapt to the narrow and complex special environment of the cable trench. It is difficult to ensure the safety of drones during flight, and they are not strong in dealing with obstacles such as potholes and bumps, and accidents are prone to occur during landing.

[0003] Currently, drone navigation mostly uses a single sensor, which cannot fully detect environmental information. Summary of the invention

[0004] In order to overcome the defect in the above-mentioned prior art that a single sensor cannot accurately locate a UAV in a complex environment, the present application proposes a method for converting an IMU coordinate system to a visual coordinate system, which lays a foundation for the fusion of IMU detection and visual detection, facilitates the combination of IMU data and visual data for UAV positioning, and improves the UAV positioning accuracy.

[0005] The present invention proposes a method for converting an IMU coordinate system to a visual coordinate system. First, IMU data and visual data with the same timestamp are obtained to form a combined sample. The IMU data is collected by an IMU sensor, and the visual data is collected by a visual sensor. The objective function is optimized on the combined sample, and a rotation matrix R and a translation vector c that meet the optimization objective are extracted to form a transfer matrix T from the IMU coordinate system to the visual coordinate system. ci ;

[0006]

[0007] The optimization goal is:

[0008] Among them, ΔR is the dynamic adjustment amount used to constrain the amplitude of rotation optimization; λ is the set regularization weight factor; N is the number of drone feature points in the visual data, p vision,i is the coordinate of the i-th feature point in the visual data in the camera coordinate system, p imu,i The coordinates of the ith feature point in the visual data calculated using the kinematic model in the IMU coordinate system; the kinematic model combines the motion state variables and the IMU data detected by the IMU sensor to predict the motion state variables at the next moment; the IMU data includes angular velocity and acceleration.

[0009] Preferably, the method of obtaining IMU data and visual data with the same timestamp to form a combined sample includes the following steps:

[0010] Construct a rough screening sample {t vision (n), t imu (m)}, t imu (m)≤t vision (n)≤t imu (m+k1), Δt vision <t imu (m+k1)-t imu (m)<2Δt vision ;t vision (n) is the timestamp of the nth frame of visual data, t imu (m) is the timestamp of the mth IMU data; Δt vision is the visual sensor acquisition time interval, t imu (m+k1) is the timestamp of the m+k1th IMU data;

[0011] When there is t imu (m+d)=t vision (n), 0≤d≤k; then t imu (m+d) corresponding IMU data and t vision (n) The corresponding visual data constitute a combined sample;

[0012] When there is no t imu (m+d)=t vision (n), 0≤d≤k; then the timestamp obtained by interpolation is t vision (n) IMU data, with timestamp t vision The IMU data and IMU data of (n) constitute the combined sample.

[0013] Preferably, the timestamp obtained by interpolation is t vision (n) IMU data d imu , the method formula of vn is as follows:

[0014]

[0015] Among them, M is the set order; c i is the i-th order polynomial interpolation coefficient; t0 is the timestamp of the known IMU data on which the interpolation calculation is based.

[0016] Preferably, t0 is the distance t based on which the interpolation calculation is based. vision (n) Timestamp of the most recent known IMU data.

[0017] The present invention proposes a navigation method integrating inertia and vision, comprising the following steps:

[0018] S1. Construct kinematic model, observation value calculation model and observation model;

[0019] The observation value calculation model is: Z = [Z p Z q ] T

[0020] Z p =C(q v )·(T ci ·p i +C(q i )·p c )+n p

[0021]

[0022] Among them, Z is the observed value, Z p is the position observation value, Z q is the attitude observation value; C(q v ) is the rotation matrix generated by the attitude quaternion provided by the visual sensor, C(q i ) is the rotation matrix generated by the attitude quaternion provided by the IMU sensor; T ci is the transformation matrix from the IMU coordinate system to the visual coordinate system; p i is the position transformation matrix of the UAV predicted by the kinematic model; p c is the position matrix of the visual feature points in the camera coordinate system; n p is the observation noise at the set position; q v is the rotation quaternion of the visual coordinate system relative to the IMU coordinate system, q i The pose predicted by the kinematic model; Represents quaternion multiplication; q c is the rotation quaternion of the visual coordinate system relative to the global coordinate system;

[0023] The observation model is: H = [H p H q ] T ; Z p is the position observation value, Z q is the attitude observation value; H p is the position observation matrix, H q is the attitude observation matrix;

[0024] Combine the following steps S2-S4 to obtain the UAV motion state variables at time k;

[0025] S2. Construct the Kalman gain K at time k by combining the state covariance matrix k ;

[0026] S3, obtain the motion state variables at time k-1 and the IMU data at time k-1, and predict the motion state variables x at time k through the kinematic model k|k-1 ; The motion state variables include the observation value Z, and the observation model H is calculated based on the observation value Z;

[0027] S4. Obtain the corrected motion state variable x at time k k|k ;

[0028] x k|k =x k|k-1 +K k (z k -Hx k|k-1 )

[0029] S5, calculating the state covariance matrix;

[0030] S6. Loop steps S2-S5 to continuously calculate the motion state variables of the UAV to complete navigation.

[0031] Preferably, the Kalman gain K k The calculation formula is:

[0032] K k =P k|k-1 H T (HP k|k-1 H T +R k ) -1

[0033] R k =R base +βΔR observation

[0034] Among them, P k|k-1 Represents the covariance matrix that measures the impact of the observed value at time k-1 on the predicted state at time k. When k = 1, P k | k-1 The identity matrix is ​​used; the superscript T indicates the matrix transpose;

[0035] R k is the observation noise covariance matrix; R base is the basic observation noise covariance matrix; ΔR observation is the dynamically adjusted observation noise correction value; β is the set weight factor.

[0036] Preferably, the covariance matrix P that measures the impact of the observation at time k on the predicted state at time k+1 is k+1|k The calculation formula is as follows:

[0037]

[0038] Q k =αQ base +(1-α)ΔQ motion

[0039] Among them, F k is the matrix representation of the kinematic model; P k is the modified state covariance matrix, which is used to evaluate the modified motion state variable x k|k The uncertainty of Q k is the noise covariance matrix at time k; Q base is the basic process noise covariance matrix; ΔQ motion is the correction amount of the set dynamic process noise; α is the set weighting coefficient.

[0040] Preferred:

[0041] P k =(IK k H)P k|k-1

[0042] Where I represents the identity matrix.

[0043] The present invention provides an unmanned aerial vehicle that uses the navigation method integrating inertia and vision to navigate.

[0044] Preferably, a variable landing gear is provided on the main body; the variable landing gear includes a landing gear, a landing leg, a deformable servo, a deformable crank and a deformable connecting rod; the deformable crank, the deformable connecting rod and the landing gear constitute a crank rocker mechanism; the deformable crank is connected to the body through the deformable servo, and the landing leg is arranged at the bottom end of the landing gear; when the deformable servo rotates and drives the deformable crank to rotate, the landing gear swings accordingly as a rocker to realize the folding and unfolding of the landing gear.

[0045] The advantages of this application are:

[0046] (1) This application proposes a method for converting an IMU coordinate system to a visual coordinate system. The method performs coordinate conversion on IMU data and visual data based on a timestamp and can adjust the calibration results in real time according to different device configurations, thereby ensuring the adaptability and flexibility of the system in a variety of deployment scenarios.

[0047] (2) This application constructs combined samples through timestamp matching, and configures IMU data corresponding to the timestamp for visual data through an interpolation algorithm, fully considering the advantage of higher acquisition frequency of IMU data, ensuring the time matching of the combined data, reducing errors, and improving the reliability of combining IMU data and visual data to describe the status of the drone.

[0048] (3) The navigation method that integrates inertia and vision proposed in this application realizes the deep dynamic fusion of visual data and IMU data through the dynamic weighting mechanism of Kalman filtering and feature point matching. This application uses the environmental feature points provided by the visual data to optimize and correct the prediction results, which significantly improves the robustness and accuracy of navigation positioning.

[0049] (4) This application combines kinematic models with coordinate transformations and introduces enhanced vision to extract environmental feature points and perform three-dimensional modeling, thereby ensuring dynamic tracking of key features. This is beneficial to improving adaptability and flexibility in a variety of deployment scenarios, especially in underground cable trench environments, achieving high-precision pose estimation and stable tracking of drones, providing reliable technical support for subsequent navigation and mission execution.

[0050] (5) This application combines dynamic covariance adaptive estimation technology and uses the high-frequency acceleration and angular velocity data provided by the IMU to predict the posture state of the drone in real time. Based on the traditional prediction method, a prediction strategy based on a multi-scale time window is proposed by matching the timestamp of the visual data, which can effectively reduce the impact of cumulative errors on system performance, thereby ensuring the system's responsiveness to rapid motion.

[0051] (6) The improved Kalman filter vision fusion inertial navigation algorithm based on multi-modal dynamic collaboration mechanism significantly improves the positioning accuracy and robustness of the UAV by making full use of the multi-source information advantages of visual sensors and inertial measurement units (IMUs).

[0052] (7) An anti-collision device that is easy to replace is used, and a movable landing gear is set up to improve flight efficiency and stability, increase the flexibility of the drone, improve safety and reliability, and increase the inspection angle. At the same time, a drone system that adapts to the complex cable trench environment is designed, and an automatic flight control system for drones that can stably, efficiently, and safely perform underground cable trench inspection tasks is developed. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 It is a flow chart of a method for converting an IMU coordinate system to a visual coordinate system;

[0054] Figure 2 It is a flow chart of a navigation method integrating inertia and vision;

[0055] Figure 3 For drone stereogram;

[0056] Figure 4 This is the front view of the drone;

[0057] Figure 5 This is the side view of the drone;

[0058] Figure 6 It is a local bulk map of the drone;

[0059] Figure 7 This is the structural diagram of the UAV's variable landing gear;

[0060] Figure 8 This is the structural diagram of the propeller and anti-collision circle;

[0061] Fig. 9 This is the camera gimbal structure diagram;

[0062] Illustration: 1. Airframe; 101. Propeller motor; 2. Camera gimbal; 201. Gimbal shock absorber; 202. Camera pitch motor; 203. Camera flip motor; 204. Gimbal connector; 3. Camera; 4. Variable landing gear; 401. Landing gear; 402. Landing legs; 403. Deformable servo; 404. Deformable crank; 405. Deformable connecting rod; 5. Anti-collision ring; 6. Propeller. DETAILED DESCRIPTION

[0063] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0064] Reference Figure 1 The present application proposes a method for converting an IMU coordinate system to a visual coordinate system, comprising the following steps:

[0065] SA1. First, construct a rough screening sample {t vision (n), t imu (m)}; t vision (n) is the timestamp of the nth frame of visual data, t imu (m) is the timestamp of the mth IMU data;

[0066] The coarse screening sample meets the conditions: t imu (m)≤t vision (n)≤t imu (m+k1)

[0067] Δt vision <t imu (m+k1)-t imu (m)<2Δt vision

[0068] Δt vision is the visual sensor acquisition time interval, t imu(m+k1) is the timestamp of the m+k1th IMU data;

[0069] SA2, determine whether there is t imu (m+d)=t vision (n), 0≤d≤k; if yes, then t imu (m+d) corresponding IMU data and t vision (n) The corresponding visual data constitute a combined sample;

[0070] If not, the timestamp is calculated by interpolation as t vision (n) IMU data, denoted as d imu，vn ; d imu，vn With timestamp t vision (n) The visual data constitutes a combined sample;

[0071]

[0072] Among them, M is the set order; c i is the i-th order polynomial interpolation coefficient, which is calculated based on the known data fitting; t0 is the timestamp of the known IMU data based on which the interpolation calculation is performed; in specific implementation, the known timestamp closest to tvision(n) is selected for interpolation calculation, that is:

[0073] |t vision (n)-t0|=min{|t vision (n)-t imu (m+d)|;0≤d≤k}.

[0074] SA3. Calculate the transformation matrix T from the IMU coordinate system to the visual coordinate system based on the combined samples ci ; First, optimize the objective function on the combined sample to obtain the rotation matrix R and translation vector c; the rotation matrix R is used to describe the rotation transformation relationship from the IMU coordinate system to the visual coordinate system, and the translation vector c is used to describe the translation relationship from the IMU coordinate system to the visual coordinate system; then combine the rotation matrix R and the translation vector c to construct the transfer matrix

[0075] The optimization goal is:

[0076] Among them, ΔΔR is the dynamic adjustment amount used to constrain the amplitude of rotation optimization; λ is the set regularization weight factor, which is used to balance the optimization weights of feature point error terms and rotation adjustment terms to improve calibration accuracy. The specific value can be in the interval [0.1, 10]; N is the number of drone feature points in the visual data, p vision，i is the coordinate of the i-th feature point in the visual data in the camera coordinate system, p imu，iis the coordinate of the i-th feature point in the visual data calculated using the kinematic model in the IMU coordinate system.

[0077] Reference Figure 2 , a navigation method integrating inertia and vision proposed in this application comprises the following steps:

[0078] S1, construct kinematic model, observation value Z = [Z p Z q ] T and the observation model H = [H p H q ] T ; Z p is the position observation value, Z q is the attitude observation value; H p is the position observation matrix, H q is the attitude observation matrix; the superscript T indicates the matrix transpose;

[0079] The kinematic model predicts the motion state variables at the next moment based on the motion state variables at the previous moment and the IMU data; the motion state variables include: position p, velocity v and attitude q, acceleration bias b a and angular velocity bias b ω ;IMU data includes angular velocity a and acceleration ω.

[0080] The kinematic model specifically includes a position prediction sub-model, a velocity prediction sub-model and a posture prediction sub-model;

[0081] Position prediction submodel: p k+1 =p k +v k Δt

[0082] According to the current speed v k and time interval Δt, estimate the position p at the next moment k+1 .

[0083] Speed ​​prediction submodel: v k+1 =v k +(R k (ab a )-g)Δt

[0084] Where: R k is the drone rotation matrix represented by quaternion; g is the gravitational acceleration.

[0085] Posture prediction sub-model:

[0086]

[0087] Where: Ω is the angular velocity matrix, which is composed of the rotation components in the directions of the three coordinate axes; Represents quaternion multiplication.

[0088] Z p =C(q v )·(T ci ·p i +C(q i )·p c )+n p

[0089]

[0090]

[0091]

[0092] Among them, C(q v ) is the rotation matrix generated by the attitude quaternion provided by the visual sensor, C(q i ) is the rotation matrix generated by the attitude quaternion provided by the IMU sensor; T ci is the transformation matrix from the IMU coordinate system to the visual coordinate system; p i is the position transformation matrix of the UAV predicted by the kinematic model; p c is the position matrix of the visual feature points in the camera coordinate system; n p is the position observation noise set empirically;

[0093] q v is the rotation quaternion of the visual coordinate system relative to the IMU coordinate system, q i The pose predicted by the kinematic model; Represents quaternion multiplication; q c is the rotation quaternion of the visual coordinate system relative to the global coordinate system; q c，j is the quaternion q c The j+1th component of , j=0, 1, 2, 3.

[0094] represents the derivation;

[0095] S2, combined with the observation noise covariance matrix R describing the uncertainty of the observations k Construct the Kalman gain K k , Kalman gain K k Used to measure the relative impact of predicted and observed values ​​on state correction;

[0096] K k =P k|k-1 H T (HP k|k-1 H T +R k )-1

[0097] R k =R base +βΔR observation

[0098] Among them, P k|k-1 Represents the covariance matrix that measures the impact of the observed value at time k-1 on the predicted state at time k. When k = 1, P k|k-1 Use the identity matrix;

[0099] H is the observation model matrix; R base is the basic observation noise covariance matrix, which is used to reflect the influence of the inherent noise of the observation equipment; ΔR observation It is a dynamically adjusted observation noise correction that reflects the fluctuation of observation noise caused by specific environment or data changes;

[0100] β is the set weight factor, which is used to control the ratio of basic noise and dynamic noise correction;

[0101] S3, obtain the motion state variables at time k-1 and the IMU data at time k-1, and predict the motion state variables x at time k through the kinematic model k|k-1 ;

[0102] S4. Obtain the corrected motion state variable x at time k k|k ;

[0103] x k|k =x k|k-1 +K k (z k -Hx k|k-1 );

[0104] S5. Calculate the state covariance matrix P k and P k+1|k ;

[0105]

[0106] Q k =αQ base +(1-α)ΔQ motion

[0107] P k =(IK k H)P k|k-1

[0108] P k+1|k F represents the covariance matrix that measures the impact of the observation value at time k on the predicted state at time k+1; k is the matrix representation of the kinematic model; Q kis the noise covariance matrix at time k; Q base is the basic process noise covariance matrix, which is used to describe the influence of fixed noise and is the set value determined by the IMU sensor; ΔQ motion is the correction amount of dynamic process noise, which is used to describe the additional uncertainty of the system under different motion states; ΔQ motion It is a conventional physical quantity, which is calculated by analyzing motion state variables (such as acceleration, angular velocity, etc.) and related statistical characteristics in existing methods;

[0109] P k is the corrected state covariance matrix, that is, to evaluate the corrected motion state variable x k|k The value of uncertainty; I represents the identity matrix;

[0110] α is the set weighting coefficient, which is used to control the influence ratio of basic noise and dynamic noise correction;

[0111] S6. Update k to k+1, then return to step S2, continue to predict the motion state variables, and realize drone navigation.

[0112] Reference Figure 3-Figure 9 The drone structure proposed in the present application includes a body 1, a camera platform 2, a camera 3, a variable landing gear 4, an anti-collision ring 5 and a propeller 6. The camera platform 2, the variable landing gear 4, the anti-collision ring 5 and the propeller 6 are all arranged on the body 1, and the body 1 is provided with a propeller motor 101 for driving the propeller 6 to rotate. The anti-collision ring 5 is located on the outer circle of the rotation track of the propeller 6 to protect the propeller 6 and avoid accidental collision damage.

[0113] The camera gimbal 2 includes a gimbal shock absorber 201, a camera pitch motor 202, a camera flip motor 203 and a gimbal connector 204; the gimbal shock absorber 201 is arranged on the gimbal connector 204, and the gimbal connector 204 is clamped on the body 1; the camera 3 is arranged on the gimbal connector 204 through the camera pitch motor 202 and the camera flip motor 203, the camera pitch motor 202 is used to adjust the pitch angle of the camera 3, and the camera flip motor 203 is used to adjust the rotation angle of the camera 3.

[0114] The variable landing gear 4 includes a landing gear 401, a landing leg 402, a deformable steering gear 403, a deformable crank 404 and a deformable connecting rod 405. The deformable crank 404, the deformable connecting rod 405 and the landing gear 401 constitute a crank rocker mechanism; the deformable crank 404 is connected to the body 1 through the deformable steering gear 403, and the landing leg 402 is arranged at the bottom end of the landing gear 401 to support the body. When the deformable steering gear 403 rotates to drive the deformable crank 404 to rotate, the landing gear 401 as a rocker swings accordingly, thereby realizing the movement of folding and unfolding the landing gear.

[0115] In this way, the variable landing gear 4 and the main body 1 of the drone form a four-bar linkage, and the deformable crank 404 is driven by the deformable servo 403 to rotate, thereby causing the landing gear to rise and fall; a dual-degree-of-freedom camera gimbal 2 is carried under the drone, and a gimbal shock absorber 201 is arranged between the camera gimbal 2 and the drone main body 1 to improve the stability of the gimbal; four sets of propeller assemblies are arranged on the main body 1, and the anti-collision ring 5 is installed on the outside of the propeller 6, and the anti-collision ring 5 and the main body 1 can be connected by a snap buckle.

[0116] When the drone is operated, the operator unfolds the drone, installs the anti-collision circle 5, and then lowers the drone into the cable trench. After the drone takes off, the landing gear is retracted to reduce the overall volume. During the inspection process, the drone realizes environmental perception through the laser radar and IMU sensors, and then realizes the navigation method that integrates inertia and vision proposed in this application for navigation and positioning. During the inspection process, the drone uses the camera 3 to monitor and analyze the inspection area in real time, identify cable hazards, and send the acquired data to the operator in real time through the wireless communication module, so that the operator can understand the inspection status in time and make necessary decisions. After the inspection is completed, the landing gear opens while the drone descends to achieve a smooth landing.

[0117] Of course, for those skilled in the art, the present application is not limited to the details of the above exemplary embodiments, but also includes the same or similar structures that can be implemented in other specific forms without departing from the spirit or basic characteristics of the present application. Therefore, no matter from which point of view, the embodiments should be regarded as exemplary and non-restrictive, and the scope of the present application is defined by the attached claims rather than the above description, so it is intended to include all changes that fall within the meaning and scope of the equivalent elements of the claims. Any figure mark in the claims should not be regarded as limiting the claim involved.

[0118] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

[0119] The technologies, shapes, and structural parts not described in detail in this application are all well-known technologies.

Claims

1. A method for converting an IMU coordinate system to a visual coordinate system, characterized in that: First, IMU data and visual data with the same timestamp are obtained to form a combined sample. IMU data is collected by the IMU sensor, and visual data is collected by the visual sensor. The objective function is optimized on the combined sample, and the rotation matrix R and translation vector c that meet the optimization objective are extracted to form the transfer matrix T from the IMU coordinate system to the visual coordinate system. ci ; The optimization goal is: Among them, ΔR is the dynamic adjustment amount used to constrain the amplitude of rotation optimization; λ is the set regularization weight factor; N is the number of drone feature points in the visual data, p vision，i is the coordinate of the i-th feature point in the visual data in the camera coordinate system, p imu，i The coordinates of the ith feature point in the visual data calculated using the kinematic model in the IMU coordinate system; the kinematic model combines the motion state variables and the IMU data detected by the IMU sensor to predict the motion state variables at the next moment; the IMU data includes angular velocity and acceleration.

2. The method for converting an IMU coordinate system to a visual coordinate system according to claim 1, wherein: The method of obtaining IMU data and visual data with the same timestamp to form a combined sample includes the following steps: Construct a rough screening sample {t vision (n), t imu (m)}, t imu (m)≤t vision (n)≤t imu (m+k1), Δt vision <t imu (m+k1)-t imu (m)<2Δt vision ;t vision (n) is the timestamp of the nth frame of visual data, t imu (m) is the timestamp of the mth IMU data; Δt vision is the visual sensor acquisition time interval, t imu (m+k1) is the timestamp of the m+k1th IMU data; When there is t imu (m+d)=t vision (n), 0≤d≤k; then t imu (m+d) corresponding IMU data and t vision (n) The corresponding visual data constitute a combined sample; When there is no t imu (m+d)=t vision (n), 0≤d≤k; then the timestamp obtained by interpolation is t vision (n) IMU data, with timestamp t vision The IMU data and IMU data of (n) constitute the combined sample.

3. The method for converting an IMU coordinate system to a visual coordinate system as claimed in claim 2, characterized in that: The timestamp obtained by interpolation is t vision (n) IMU data d imu , the method formula of vn is as follows: Where M is the set order; c i is the i-th order polynomial interpolation coefficient; t0 is the timestamp of the known IMU data on which the interpolation calculation is based.

4. The method for converting an IMU coordinate system to a visual coordinate system as claimed in claim 2, characterized in that: t0 is the distance t based on which the interpolation calculation is based vision (n) Timestamp of the most recent known IMU data.

5. A navigation method integrating inertia and vision using the method for converting the IMU coordinate system to the visual coordinate system as described in any one of claims 1 to 4, characterized in that: The following steps are involved: S1. Construct kinematic model, observation value calculation model and observation model; The observation value calculation model is: Z = [Z p Z q ] T Z p =C(q v )·(T ci ·p i +C(q i )·p c )+n p Among them, Z is the observed value, Z p is the position observation value, Z q is the attitude observation value; C(q v ) is the rotation matrix generated by the attitude quaternion provided by the visual sensor, C(q i ) is the rotation matrix generated by the attitude quaternion provided by the IMU sensor; T ci is the transformation matrix from the IMU coordinate system to the visual coordinate system; p i is the position transformation matrix of the UAV predicted by the kinematic model; p c is the position matrix of the visual feature points in the camera coordinate system; n p is the observation noise at the set position; q v is the rotation quaternion of the visual coordinate system relative to the IMU coordinate system, q i The pose predicted by the kinematic model; Represents quaternion multiplication; q c is the rotation quaternion of the visual coordinate system relative to the global coordinate system; The observation model is: H = [H p H q ] T ; Z p is the position observation value, Z q is the attitude observation value; H p is the position observation matrix, H q is the attitude observation matrix; Combine the following steps S2-S4 to obtain the UAV motion state variables at time k; S2. Construct the Kalman gain K at time k by combining the state covariance matrix k ; S3, obtain the motion state variables at time k-1 and the IMU data at time k-1, and predict the motion state variables x at time k through the kinematic model k|k-1 ; The motion state variables include the observation value Z, and the observation model H is calculated based on the observation value Z; S4. Obtain the corrected motion state variable x at time k k|k ; x k|k =x k|k-1 +K k (z k -Hx k|k-1 ) S5, calculating the state covariance matrix; S6. Loop steps S2-S5 to continuously calculate the motion state variables of the UAV to complete navigation.

6. The navigation method integrating inertia and vision as claimed in claim 5, characterized in that: Kalman gain K k The calculation formula is: K k =P k|k-1 H T (HP k|k-1 H T +R k ) -1 R k =R base +βΔR observation Among them, P k|k-1 Represents the covariance matrix that measures the impact of the observed value at time k-1 on the predicted state at time k. When k = 1, P k|k-1 The identity matrix is ​​used; the superscript T indicates the matrix transpose; R k is the observation noise covariance matrix; R base is the basic observation noise covariance matrix; ΔR observation is the dynamically adjusted observation noise correction value; β is the set weight factor.

7. The navigation method integrating inertia and vision as claimed in claim 6, characterized in that: The covariance matrix P that measures the impact of the observation value at time k on the predicted state at time k+1 k+1|k The calculation formula is as follows: Q k =αQ base +(1-α)ΔQ motion Among them, F k is the matrix representation of the kinematic model; P k is the modified state covariance matrix, which is used to evaluate the modified motion state variable x k|k The uncertainty of Q k is the noise covariance matrix at time k; Q base is the noise covariance matrix of the basic process set; ΔQ motion is the correction amount of the set dynamic process noise; α is the set weighting coefficient.

8. The navigation method integrating inertial and visual navigation as claimed in claim 6, characterized in that: P k =(I-K k H)P k|k-1 Where I represents the identity matrix.

9. A drone, characterized in that: Navigation is performed using the navigation method integrating inertia and vision as described in any one of claims 5 to 8.

10. The drone according to claim 9, characterized in that: A variable landing gear is provided on the main body; the variable landing gear comprises a landing gear, a landing leg, a deformable servo, a deformable crank and a deformable connecting rod; the deformable crank, the deformable connecting rod and the landing gear constitute a crank rocker mechanism; the deformable crank is connected to the body through the deformable servo, and the landing leg is arranged at the bottom end of the landing gear; when the deformable servo rotates to drive the deformable crank to rotate, the landing gear swings accordingly as a rocker to realize the folding and unfolding of the landing gear.

Images