Unmanned aerial vehicle flight high-precision inertial navigation method and system

Abstract

The application relates to the technical field of unmanned aerial vehicles, in particular to a high-precision inertial navigation method and system for unmanned aerial vehicle flight. The method comprises the following steps: acquiring a GPS signal before an unmanned aerial vehicle enters a tunnel, determining an initial pose according to the GPS signal; acquiring motion parameters of the unmanned aerial vehicle in real time during inspection; analyzing the motion parameters to determine a real-time motion state; acquiring tunnel structure information, and correcting a real-time attitude of the unmanned aerial vehicle according to the real-time motion state and the initial pose based on the tunnel structure information. The core problem of lacking a reliable initial reference after the loss of a GPS signal is solved, the navigation accuracy of the unmanned aerial vehicle in the tunnel is ensured to start from a high-precision starting point, a stable anchor point is provided for correction, the pertinence and effectiveness of drift compensation are improved, the problems of ineffective utilization of tunnel structure information and insufficient drift compensation are solved, high-precision pose output is realized, and the requirements of obstacle avoidance and inspection in a tunnel environment are met.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicles, in particular to a high-precision inertial navigation method and system for unmanned aerial vehicle flight. BACKGROUND

[0002] In the field of unmanned aerial vehicle inspection, autonomous flight navigation in tunnel environments faces severe challenges. GPS signals may be available at the entrance of a tunnel, but once the unmanned aerial vehicle enters the interior of the tunnel, the GPS signals will rapidly decay or be completely lost, resulting in the failure of traditional satellite positioning.

[0003] However, existing navigation methods rely on a single and volatile GPS signal inside the tunnel, resulting in the failure of template matching relying on visual navigation, making it easy to produce mismatching in block splitting and pixel sampling, and accumulating positioning errors. SUMMARY

[0004] The present application provides a high-precision inertial navigation method and system for unmanned aerial vehicle flight to solve the above problems.

[0005] In a first aspect, the present application provides a high-precision inertial navigation method for unmanned aerial vehicle flight, the method comprising:

[0006] acquiring a GPS signal before the unmanned aerial vehicle enters a tunnel, and determining an initial pose based on the GPS signal;

[0007] acquiring motion parameters of the unmanned aerial vehicle in real time during the inspection process; analyzing the motion parameters to determine a real-time motion state;

[0008] acquiring tunnel structure information, and correcting a real-time attitude of the unmanned aerial vehicle based on the tunnel structure information, the real-time motion state, and the initial pose.

[0009] According to the present application, the GPS signal before the unmanned aerial vehicle enters the tunnel is acquired, and the initial pose is determined based on the GPS signal, solving the core problem of lacking a reliable initial reference after the GPS signal is lost, ensuring that the navigation accuracy of the unmanned aerial vehicle inside the tunnel starts from a high-precision point, and providing a stable anchor point for correction. In the inspection process, the motion parameters of the unmanned aerial vehicle are acquired in real time, reflecting the instantaneous dynamics state of the unmanned aerial vehicle, and the unmanned aerial vehicle dynamics is continuously tracked after the GPS signal is lost. The motion parameters are analyzed to determine the real-time motion state, solving the problems of inertial navigation drift accumulation and the lack of opportunity for error correction in the hovering state, dynamically providing a low-error window, and improving the pertinence and effectiveness of drift compensation. The tunnel structure information is acquired, and the real-time attitude of the unmanned aerial vehicle is corrected based on the tunnel structure information, the real-time motion state, and the initial pose, solving the problems of ineffective use of tunnel structure information and insufficient drift compensation, achieving high-precision pose output, and meeting the requirements of obstacle avoidance and inspection in a tunnel environment.

[0010] Optionally, the real-time motion state comprises a hovering state; the analyzing the motion parameters to determine the real-time motion state comprises:

[0011] analyzing the motion parameters to determine three-axis velocity data and three-axis acceleration data;

[0012] vector synthesizing the three-axis velocity data to calculate a resultant velocity value;

[0013] calculating a standard deviation of the acceleration data within a preset time period, and taking the standard deviation as an acceleration fluctuation index;

[0014] when the resultant velocity value is lower than a velocity determination threshold and the standard deviation is lower than a stability threshold, determining that a hovering state is entered.

[0015] By the scheme, the motion parameters are analyzed to determine the three-axis velocity data and the three-axis acceleration data, avoiding data confusion and ensuring the independence of state evaluation. Meanwhile, the data timeliness is ensured to support real-time processing. The three-axis velocity data is vector synthesized to calculate the resultant velocity value, providing an intuitive velocity measure to quickly judge whether the UAV is in a low motion state, reducing the calculation complexity and accelerating the decision-making process. The standard deviation of the acceleration data within a preset time period is calculated, and the standard deviation is taken as the acceleration fluctuation index to reflect the stability of the UAV motion. When the resultant velocity value is lower than the velocity determination threshold and the standard deviation is lower than the stability threshold, it is determined that the hovering state is entered, ensuring that the UAV performs error correction in a stable state and avoiding navigation drift or control failure caused by misjudgment.

[0016] Optionally, when the resultant velocity value is lower than the velocity determination threshold and the standard deviation is lower than the stability threshold, determining that the hovering state is entered, comprises:

[0017] when the resultant velocity value is lower than the velocity determination threshold and the standard deviation is lower than the stability threshold, establishing a sliding time window with a length of N seconds;

[0018] counting a proportion of the resultant velocity value being lower than the velocity determination threshold within the sliding time window;

[0019] when the proportion exceeds a preset proportion threshold, calculating a change rate of the standard deviation within the sliding time window, and when the change rate is less than a preset change rate threshold, confirming that a stable hovering state is entered.

[0020] This scheme establishes a sliding time window of length N seconds when the resultant velocity value is below the velocity judgment threshold and the standard deviation is below the stability threshold. This ensures that the state judgment is based on continuous observation over time, rather than single-point data, enhancing environmental adaptability. The percentage of resultant velocity values ​​below the velocity judgment threshold within the sliding time window is statistically analyzed to effectively exclude brief deceleration events, ensuring that the hovering state maintains a low speed for most of the time. This improves the robustness of the judgment and reduces misjudgments caused by instantaneous noise or environmental interference. When the percentage exceeds a preset percentage threshold, the rate of change of the standard deviation within the sliding time window is calculated. When the rate of change is less than a preset rate of change threshold, a stable hovering state is confirmed, eliminating the risk of misjudgment and ensuring that the state judgment is triggered only when both velocity and acceleration are highly stable.

[0021] Optionally, the step of correcting the real-time attitude of the UAV based on the tunnel structure information, the real-time motion state, and the initial pose includes:

[0022] Determine the tunnel entrance plane based on the latitude and longitude in the initial pose;

[0023] The tunnel extension direction axis is determined based on the orientation data in the tunnel structure information;

[0024] A tunnel spatial coordinate system is established with the tunnel entrance plane as the XY plane and the extension direction axis as the Z axis;

[0025] Based on the tunnel spatial coordinate system, the pose change in the real-time motion state is obtained;

[0026] Based on the tunnel spatial coordinate system, and according to the tunnel structural information, a spatial constraint boundary is established;

[0027] The pose change is matched with the spatial constraint boundary, and the pose correction is calculated based on the matching degree between the pose change and the spatial constraint boundary.

[0028] According to the initial position, the tunnel entrance plane is determined according to the latitude and longitude, and the position reference drift caused by GPS loss is eliminated. According to the tunnel structure information, the tunnel extension direction axis is determined according to the trend data, and the accumulated pose error caused by direction deviation is avoided. The tunnel space coordinate system is established with the tunnel entrance plane as the XY plane and the extension direction axis as the Z axis, reflecting the relative motion of the unmanned aerial vehicle in the tunnel, stripping irrelevant environmental interference, and improving the environmental adaptability of the pose solution. Based on the tunnel space coordinate system, the pose change quantity in the real-time motion state is obtained, and the direction drift accumulation speed is significantly reduced. Based on the tunnel space coordinate system, the space constraint boundary is established according to the tunnel structure information, and the collision risk problem is solved. Matching the pose change quantity and the space constraint boundary, according to the matching degree of the pose change quantity and the space constraint boundary, the attitude correction quantity is solved, the motion direction is consistent with the tunnel trend, the demand of high-precision inspection obstacle avoidance is met, and the high computational burden of the visual algorithm is avoided.

[0029] Optionally, the space constraint boundary is established according to the tunnel structure information based on the tunnel space coordinate system, comprising:

[0030] The cross-sectional geometric parameters in the tunnel structure information are extracted based on the tunnel space coordinate system;

[0031] A series of constraint sections are established along the tunnel extension direction;

[0032] The parameter difference between adjacent constraint sections is determined by analyzing the motion parameters;

[0033] The continuous constraint field function is constructed according to the parameter difference;

[0034] The space constraint boundary is established according to the continuous constraint field function.

[0035] According to the tunnel space coordinate system, the cross-sectional geometric parameters in the tunnel structure information are extracted, the actual physical structure of the tunnel is reflected, and the boundary definition error caused by the lack of geometric parameters is avoided. A series of constraint sections are established along the tunnel extension direction, the discrete representation of the tunnel structure is realized, the geometric changes can be processed in segments by the space constraint boundary, and the local adaptability of the boundary definition is enhanced. The parameter difference between adjacent constraint sections is determined by analyzing the motion parameters, the local change trend of the tunnel along the extension direction is quantified, the difference calculation is synchronized with the actual pose of the unmanned aerial vehicle, and the real-time performance of the boundary matching is improved. According to the parameter difference, the continuous constraint field function is constructed, the dynamic continuity of the space constraint is realized, the jumping of the discrete sections is eliminated, the boundary can seamlessly adapt to the gradual change of the tunnel geometry, and the continuity and calculability of the constraint are ensured. According to the continuous constraint field function, the space constraint boundary is established, the unmanned aerial vehicle motion is effectively constrained, the IMU integral drift is suppressed, and the navigation accuracy is improved.

[0036] Optionally, the pose change amount in the real-time motion state is obtained based on the tunnel space coordinate system, comprising:

[0037] A displacement vector relative to the initial pose is calculated by the dead reckoning algorithm.

[0038] The displacement vector is projected to the tunnel space coordinate system by a coordinate system conversion matrix to obtain the pose change amount in the real-time motion state.

[0039] By this scheme, the displacement vector relative to the initial pose is calculated by the dead reckoning algorithm, meeting the real-time demand for pose update when the UAV is flying at high speed, avoiding the problem of calculation burden of visual navigation. The displacement vector is projected to the tunnel space coordinate system by the coordinate system conversion matrix to obtain the pose change amount in the real-time motion state, ensuring that the high update rate is maintained in the GPS-free environment, solving the core pain point of insufficient real-time performance.

[0040] Optionally, after confirming that the UAV enters the stable hovering state when the change rate is less than the preset change rate threshold, the method further comprises:

[0041] A high-frequency shaking component is extracted from the three-axis acceleration data.

[0042] The high-frequency shaking component is analyzed to determine a horizontal plane shaking angle and a height direction shaking displacement.

[0043] A horizontal offset is determined by geometric projection based on the horizontal plane shaking angle and the XY plane.

[0044] A height change amount is determined based on the height direction shaking displacement and the height reference data in the tunnel structure information.

[0045] By this scheme, the high-frequency shaking component is extracted from the three-axis acceleration data, eliminating irrelevant noise and avoiding the influence of low-frequency deviation on accuracy. The high-frequency shaking component is analyzed to determine the horizontal plane shaking angle and the height direction shaking displacement, converting the high-frequency acceleration signal into usable geometric parameters to provide direct input for geometric projection, thereby capturing the instantaneous attitude offset during hovering. The horizontal offset is determined by geometric projection based on the horizontal plane shaking angle and the XY plane, providing real-time position update and correcting the horizontal direction drift. The height change amount is determined based on the height direction shaking displacement and the height reference data in the tunnel structure information, correcting the height direction drift and ensuring that the height data is consistent with the tunnel geometric constraints.

[0046] Optionally, the pose change amount is matched with the space constraint boundary, comprising:

[0047] The pose change amount is analyzed to obtain displacement vector and angle change data.

[0048] According to the displacement vector, the angle change data and the space constraint boundary, a pose matching degree is calculated;

[0049] The minimum distance between the displacement vector and the space constraint boundary is calculated as a position matching degree through the Euclidean distance formula;

[0050] The angle matching degree between the angle change data and the normal vector of the space constraint boundary is determined through angle difference calculation;

[0051] The pose matching degree, the position matching degree and the angle matching degree are weighted and summed to determine a comprehensive matching degree;

[0052] The pose matching degree, the position matching degree and the angle matching degree are weighted and summed to determine a comprehensive matching degree;

[0053] According to the comprehensive matching degree, a pose correction amount is calculated.

[0054] According to the displacement vector, the angle change data and the space constraint boundary, a pose matching degree is calculated;

[0055] According to the comprehensive matching degree, a pose correction amount is calculated.

[0056] When the comprehensive matching degree is lower than a preset matching threshold, a pose optimization function is constructed; the pose optimization function aims to minimize the deviation of the pose change amount from the space constraint boundary, and the constraint conditions include the speed continuity obtained by analyzing the real-time motion state;

[0057] When the real-time motion state is a hovering state, the constraint conditions further include the horizontal offset and the height change amount;

[0058] When the real-time motion state is a non-hovering state, the constraint conditions further include the space constraint boundary;

[0059] The pose optimization function is solved to output a pose correction amount.

[0060] By the scheme, when the comprehensive matching degree is lower than the preset matching threshold, a pose optimization function is constructed to compensate for inertial navigation drift, so that the pose is more fitted to the tunnel structure. The pose optimization function takes minimizing the deviation of the pose change quantity from the spatial constraint boundary as the target, and the constraint conditions include the velocity continuity obtained by analyzing the real-time motion state, so as to ensure that the pose correction generated in the optimization process will not cause a sudden change in velocity, improve flight stability, and avoid control oscillation caused by drastic adjustment. When the real-time motion state is a hovering state, the constraint conditions further include a horizontal offset and a height change quantity, so that the optimization process is forced to reduce the horizontal offset, ensure position stability during hovering, and at the same time, forced to keep the height constant. When the real-time motion state is a non-hovering state, the constraint conditions further include the spatial constraint boundary, so that the optimization process is forced to make the pose conform to the tunnel geometry. The pose optimization function is solved to output a pose correction quantity, correct the pose deviation, and improve tunnel navigation accuracy.

[0061] In a second aspect, the application provides a high-precision inertial navigation system for unmanned aerial vehicle flight, which comprises:

[0062] An initial pose determination module is configured to acquire a GPS signal before the unmanned aerial vehicle enters a tunnel, and determine an initial pose based on the GPS signal.

[0063] A motion parameter analysis module is configured to acquire motion parameters of the unmanned aerial vehicle in real time during the inspection process, and analyze the motion parameters to determine a real-time motion state.

[0064] A real-time pose correction module is configured to acquire tunnel structure information, and correct a real-time pose of the unmanned aerial vehicle based on the tunnel structure information, the real-time motion state, and the initial pose.

[0065] Optionally, the real-time motion state includes a hovering state, and the motion parameter analysis module, when analyzing the motion parameters to determine the real-time motion state, is configured to analyze the motion parameters to determine three-axis velocity data and three-axis acceleration data, perform vector composition on the three-axis velocity data to calculate a combined velocity value, calculate a standard deviation of the acceleration data within a preset time period and take the standard deviation as an acceleration fluctuation index, and determine that the hovering state is entered when the combined velocity value is lower than a velocity determination threshold and the standard deviation is lower than a stability threshold.

[0066] Optionally, when the combined speed value is lower than the speed determination threshold and the standard deviation is lower than the stability threshold, the motion parameter analysis module determines that the unmanned aerial vehicle enters the hovering state, and is configured to: when the combined speed value is lower than the speed determination threshold and the standard deviation is lower than the stability threshold, establish a sliding time window with a length of N seconds; count a proportion of the combined speed value being lower than the speed determination threshold in the sliding time window; when the proportion exceeds a preset proportion threshold, calculate a change rate of the standard deviation in the sliding time window; and when the change rate is less than a preset change rate threshold, confirm that the unmanned aerial vehicle enters the stable hovering state.

[0067] Optionally, when the real-time attitude correction module corrects the real-time attitude of the unmanned aerial vehicle based on the tunnel structure information and according to the real-time motion state and the initial pose, the module is configured to: determine a tunnel entrance plane according to the longitude and latitude in the initial pose; determine a tunnel extension direction axis according to the strike data in the tunnel structure information; establish a tunnel space coordinate system with the tunnel entrance plane as an XY plane and the extension direction axis as a Z axis; obtain a pose change amount in the real-time motion state based on the tunnel space coordinate system; establish a space constraint boundary according to the tunnel structure information based on the tunnel space coordinate system; and match the pose change amount with the space constraint boundary, and calculate an attitude correction amount according to a matching degree of the pose change amount and the space constraint boundary.

[0068] Optionally, when the real-time attitude correction module establishes the space constraint boundary based on the tunnel space coordinate system and according to the tunnel structure information, the module is configured to: extract cross-section geometric parameters in the tunnel structure information based on the tunnel space coordinate system; establish a series of constraint cross-sections along a tunnel extension direction; analyze the motion parameters to determine a parameter difference value of adjacent constraint cross-sections; construct a continuous constraint field function according to the parameter difference value; and establish the space constraint boundary according to the continuous constraint field function.

[0069] Optionally, when the real-time attitude correction module obtains the pose change amount in the real-time motion state based on the tunnel space coordinate system, the module is configured to: calculate a displacement vector relative to the initial pose by a dead reckoning method; and project the displacement vector to the tunnel space coordinate system by a coordinate system conversion matrix to obtain the pose change amount in the real-time motion state.

[0070] Optionally, the high-precision inertial navigation system for unmanned aerial vehicle flight further includes a change amount determination module configured to: extract a high-frequency shaking component according to the three-axis acceleration data; analyze the high-frequency shaking component to determine a horizontal plane shaking angle and a height direction shaking displacement; determine a horizontal offset amount by geometric projection according to the horizontal plane shaking angle and in combination with the XY plane; and determine a height change amount according to the height direction shaking displacement and in combination with height reference data in the tunnel structure information.

[0071] Optionally, when the real-time attitude correction module matches the pose change amount with the space constraint boundary, the method comprises the following steps of: analyzing the pose change amount to obtain a displacement vector and angle change data; calculating a pose matching degree according to the displacement vector, the angle change data and the space constraint boundary; calculating a minimum distance between the displacement vector and the space constraint boundary as a position matching degree by using a Euclidean distance formula; determining an angle matching degree between the angle change data and a normal vector of the space constraint boundary by angle difference calculation; and determining a comprehensive matching degree by weighted summation of the pose matching degree, the position matching degree and the angle matching degree; and when the real-time attitude correction module calculates the attitude correction amount according to the matching degree between the pose change amount and the space constraint boundary, the method comprises the following step of: calculating the attitude correction amount according to the comprehensive matching degree.

[0072] Optionally, when the real-time attitude correction module calculates the attitude correction amount according to the comprehensive matching degree, the method comprises the following steps of: constructing an attitude optimization function when the comprehensive matching degree is lower than a preset matching threshold; the attitude optimization function takes minimizing the deviation between the pose change amount and the space constraint boundary as a target, and the constraint conditions include velocity continuity obtained by analyzing the real-time motion state; when the real-time motion state is a hovering state, the constraint conditions further include the horizontal offset amount and the height change amount; when the real-time motion state is a non-hovering state, the constraint conditions further include the space constraint boundary; and solving the attitude optimization function to output the attitude correction amount. BRIEF DESCRIPTION OF DRAWINGS

[0073] In order to more clearly illustrate the technical solutions of the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without any creative labor.

[0074] Figure 1 An application scenario schematic diagram provided by an embodiment of the present application;

[0075] Figure 2 A flowchart of a high-precision inertial navigation method for unmanned aerial vehicle flight provided by an embodiment of the present application;

[0076] Figure 3 A structure schematic diagram of a high-precision inertial navigation system for unmanned aerial vehicle flight provided by an embodiment of the present application. DETAILED DESCRIPTION

[0077] In order to make the purposes, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative effort should fall within the protection scope of the present application.

[0078] In addition, the term "and / or" in the present application only describes the association relationship of the associated objects, and indicates that there can be three relationships, for example, A and / or B, which can represent the three cases of A existing alone, A and B existing simultaneously, and B existing alone. In addition, the character " / " in the present application generally represents an "or" relationship between the front and rear associated objects unless otherwise specified.

[0079] The embodiments of the present application will be described in further detail below with reference to the drawings of the specification.

[0080] The existing high-precision inertial navigation method relies on single and volatile tunnel GPS signals, and the light inside the tunnel is usually insufficient and the texture is single, resulting in that the real image lacks identifiable feature points, the template matching relied on by visual navigation is invalid, and then the block splitting and pixel sampling are prone to mismatch, the cumulative positioning error is accumulated, and a reliable navigation reference cannot be provided.

[0081] Based on this, the present application provides a high-precision inertial navigation method and system for unmanned aerial vehicle flight, acquires GPS signals before the unmanned aerial vehicle enters the tunnel, determines the initial pose according to the GPS signals, solves the core problem of lacking reliable initial reference after the GPS signals are lost, ensures the navigation accuracy of the unmanned aerial vehicle inside the tunnel starting from a high-precision starting point, provides a stable anchor point for correction, acquires the motion parameters of the unmanned aerial vehicle in real time during the inspection process, reflects the instantaneous dynamics state of the unmanned aerial vehicle, and continuously tracks the dynamic state of the unmanned aerial vehicle after the GPS signals are lost. Analyzing the motion parameters, determining the real-time motion state, solving the problems of inertial navigation drift accumulation and the defect of not utilizing the error correction opportunity of the hovering state, dynamically providing a low-error window, and improving the pertinence and effectiveness of drift compensation. Acquiring tunnel structure information, based on the tunnel structure information, correcting the real-time attitude of the unmanned aerial vehicle according to the real-time motion state and the initial pose, solving the problems of ineffective utilization of tunnel structure information and insufficient drift compensation, realizing high-precision pose output, and meeting the requirements of obstacle avoidance and inspection in the tunnel environment.

[0082] Figure 1 An application scenario provided by the present application is provided, and the method provided by the present application is applied when the high-precision inertial navigation of the unmanned aerial vehicle is performed.

[0083] Specifically, the method provided in the application is applied to any server, the server interacts with a UAV, the UAV contains a GPS receiver, an on-board inertial measurement unit and an on-board laser radar, the GPS signal before the UAV enters the tunnel is acquired through the GPS receiver carried by the UAV, and the initial pose is determined according to the GPS signal. In the inspection process, the motion parameters of the UAV are acquired in real time through the accelerometer and the gyroscope in the on-board inertial measurement unit (IMU). The motion parameters are analyzed to determine the real-time motion state, solve the problems of inertial navigation drift accumulation and the lack of error correction opportunity in the hovering state, dynamically provide a low-error window, and improve the pertinence and effectiveness of drift compensation. The tunnel structure information is acquired through the on-board laser radar, based on the tunnel structure information, the real-time attitude of the UAV is corrected according to the real-time motion state and the initial pose, the problems of ineffective use of tunnel structure information and insufficient drift compensation are solved, high-precision pose output is realized, and the requirements of obstacle avoidance and inspection in the tunnel environment are met. The specific implementation mode can refer to the following embodiments.

[0084] Figure 2 A flowchart of a UAV flight high-precision inertial navigation method provided for an embodiment of the application, the method of the embodiment can be applied to the server in the above scene. As shown in the figure, Figure 2 the method comprises:

[0085] S201, acquiring the GPS signal before the UAV enters the tunnel, and determining the initial pose according to the GPS signal;

[0086] The initial pose can be the initial position and the initial attitude of the UAV before entering the tunnel determined based on the GPS signal.

[0087] Specifically, the GPS signal before the UAV enters the tunnel is acquired through the GPS receiver carried by the UAV. The longitude, latitude and altitude in the GPS signal are converted into position coordinates (X, Y, Z) using a coordinate conversion algorithm; at the same time, the initial direction (yaw angle (the included angle between the longitudinal axis of the UAV and the geographical north direction), pitch angle (the included angle between the longitudinal axis of the UAV and the horizontal plane, the upward head is positive) and roll angle (the included angle between the transverse axis of the UAV and the horizontal plane, the right wing is downwardly inclined) is determined in combination with the UAV orientation information (absolute direction reference of the UAV) determined by the continuous multiple frames of GPS position changes.

[0088] The position coordinates and the initial direction determined by the above data are integrated to determine the initial pose.

[0089] S202, in the inspection process, the motion parameters of the UAV are acquired in real time; the motion parameters are analyzed to determine the real-time motion state;

[0090] The motion parameters can be physical quantity data acquired in real time by the on-board inertial measurement unit.

[0091] The real-time motion state can be a current behavior mode of the UAV obtained by analyzing the motion parameters.

[0092] Specifically, during the UAV inspection process, the motion parameters of the UAV are obtained in real time by an accelerometer and a gyroscope in an onboard inertial measurement unit (IMU), wherein the accelerometer measures the linear acceleration (in X, Y, and Z axes) of the UAV, and the gyroscope measures the angular velocity (yaw rate, pitch rate, and roll rate).

[0093] The motion parameters are analyzed, and the statistical characteristics (such as mean and variance) of the linear acceleration and the angular velocity are calculated; then, according to the statistical characteristics and the preset threshold, the real-time motion state (hovering or flying) is determined, for example, if the linear acceleration is continuously lower than the preset threshold set according to the industry standard (indicating that the acceleration is close to static) and the angular velocity change is less than the preset threshold set according to the IMU sensor specification (indicating that the fluctuation is small), the hovering state is determined; otherwise, if the linear acceleration is continuously higher than the preset threshold set according to the lower limit of dynamic motion (indicating that the acceleration changes significantly) and the angular velocity change is greater than the preset threshold set according to the lower limit of effective rotation recognition (indicating that the fluctuation is large), the flying state is determined.

[0094] S203, tunnel structure information is obtained, and based on the tunnel structure information, the real-time attitude of the UAV is corrected according to the real-time motion state and the initial pose.

[0095] The tunnel structure information can be tunnel geometric feature data, including cross-sectional shape and extension direction.

[0096] The real-time attitude can be the current pose data of the UAV in the inspection process.

[0097] Specifically, the tunnel structure information is obtained by the onboard laser radar. When the real-time motion state is the hovering state, the stability of hovering (manifested as a very small change in position and attitude) is utilized, the deviation of the motion parameters from the initial pose is compared, and a short-term drift compensation value (used to offset the cumulative error in a short time) is calculated; at the same time, the tunnel structure information is used as a spatial constraint (physical limitation condition of the tunnel structure on the UAV position, such as minimum or maximum distance of the wall surface), a geometric projection model is established by the perspective projection principle in computer vision, the tunnel structure information is projected to a two-dimensional image plane, and the position deviation is corrected by minimizing the re-projection error.

[0098] The short-term drift compensation value determined by the above data and the spatial constraint correction are fused to generate a real-time attitude correction amount, and the real-time attitude of the UAV is corrected.

[0099] By the scheme, the GPS signal before the unmanned aerial vehicle entering the tunnel is acquired, and the initial pose is determined according to the GPS signal, so that the core problem of lacking reliable initial reference after the GPS signal is lost is solved, and the navigation accuracy of the unmanned aerial vehicle in the tunnel is ensured to start from a high-precision starting point, so as to provide a stable anchor point for correction. In the inspection process, the motion parameters of the unmanned aerial vehicle are acquired in real time, the instantaneous dynamic state of the unmanned aerial vehicle is reflected, and the unmanned aerial vehicle dynamics is continuously tracked after the GPS signal is lost. The motion parameters are analyzed, the real-time motion state is determined, the problems of drift accumulation of inertial navigation and defects of missing error correction opportunity of the hovering state are solved, a low-error window is dynamically provided, and the pertinence and effectiveness of drift compensation are improved. The tunnel structure information is acquired, the real-time attitude of the unmanned aerial vehicle is corrected based on the tunnel structure information, the real-time motion state and the initial pose, the problems of ineffective utilization of the tunnel structure information and insufficient drift compensation are solved, high-precision pose output is realized, and the requirements of obstacle avoidance and inspection in the tunnel environment are met.

[0100] In some embodiments, the motion parameters are analyzed, three-axis velocity data and three-axis acceleration data are determined, vector composition is performed on the three-axis velocity data, a resultant velocity value is calculated, a standard deviation of the acceleration data in a preset time period is calculated, and the standard deviation is taken as an acceleration fluctuation index; when the resultant velocity value is lower than a velocity determination threshold and the standard deviation is lower than a stability threshold, it is determined that the hovering state is entered.

[0101] The three-axis velocity data can be a motion velocity component of the unmanned aerial vehicle along an X-axis, a Y-axis and a Z-axis in a three-dimensional space. The three-axis acceleration data can be a motion acceleration component of the unmanned aerial vehicle along the X-axis, the Y-axis and the Z-axis in the three-dimensional space. The resultant velocity value can be a resultant velocity of a velocity vector of the unmanned aerial vehicle calculated by vector composition of the three-axis velocity data.

[0102] The preset time period can be a continuous time window with a fixed length, which is stored in a server in advance and called when used.

[0103] The acceleration fluctuation index can be a parameter representing acceleration stability. The velocity determination threshold can be a preset resultant velocity boundary value for determining whether the unmanned aerial vehicle is in a low-speed state.

[0104] The stability threshold can be a preset acceleration fluctuation boundary value for determining whether the acceleration is stable.

[0105] The hovering state can be a stable attitude representing that the unmanned aerial vehicle is approximately stationary.

[0106] Specifically, the motion parameters are analyzed, and three-axis velocity data (i.e., X-axis velocity component, Y-axis velocity component and Z-axis velocity component) and three-axis acceleration data (i.e., X-axis acceleration component, Y-axis acceleration component and Z-axis acceleration component) are extracted at a fixed sampling frequency.

[0107] The extracted triaxial velocity data is input into a vector composition formula to calculate a resultant velocity value (representing the overall speed of the UAV). All triaxial acceleration data samples within a preset time period (for data collection) are collected; then, the standard deviation of each axis (X-axis standard deviation, Y-axis standard deviation, and Z-axis standard deviation) is calculated according to the triaxial acceleration data samples; and then the average of the standard deviations is taken as the acceleration fluctuation index.

[0108] The speed determination threshold value (for determining whether the UAV is in a low speed state) is calibrated through experiments; the resultant velocity value is compared with the speed determination threshold value, and when the resultant velocity value is lower than the speed determination threshold value, the speed condition is met. The stability threshold value (for determining the stability of the acceleration) is calibrated through experiments; the standard deviation is compared with the stability threshold value, and when the standard deviation is lower than the stability threshold value, the stability condition is met. When the speed condition and the stability condition are met at the same time, it is determined that the UAV enters the hovering state.

[0109] Through the scheme, the motion parameters are analyzed, the triaxial velocity data and the triaxial acceleration data are determined, data confusion is avoided, the independence of state evaluation is ensured, the data timeliness is ensured, and real-time processing is supported. The triaxial velocity data is subjected to vector composition to calculate the resultant velocity value, an intuitive speed measure is provided, it is quickly judged whether the UAV is in a low motion state, the calculation complexity is reduced, and the decision-making process is accelerated. The standard deviation of the acceleration data within a preset time period is calculated, and the standard deviation is taken as the acceleration fluctuation index to reflect the smoothness of the UAV motion. When the resultant velocity value is lower than the speed determination threshold value and the standard deviation is lower than the stability threshold value, it is determined that the hovering state is entered, the UAV is ensured to be in a stable state for error correction, and navigation drift or control failure caused by misjudgment is avoided.

[0110] In some embodiments, when the resultant velocity value is lower than the speed determination threshold value and the standard deviation is lower than the stability threshold value, a sliding time window with a length of N seconds is established; the proportion of the resultant velocity values lower than the speed determination threshold value within the sliding time window is counted; when the proportion exceeds a preset proportion threshold value, the change rate of the standard deviation within the sliding time window is calculated, and when the change rate is less than a preset change rate threshold value, it is confirmed that the stable hovering state is entered.

[0111] The sliding time window can be a dynamic time period with a fixed window length of N seconds, and each time a new point is added, the oldest point is removed, so that the samples within the latest time period are always maintained.

[0112] The preset proportion threshold value can be a boundary value preset for determining whether the proportion is high enough to trigger the change rate calculation, which is stored in the server in advance and called when used.

[0113] The change rate can be the change speed of the standard deviation within the sliding time window.

[0114] The preset change rate threshold can be a preset boundary value for defining whether the change rate is small enough to confirm the hovering state, which is pre-stored in the server and called when used.

[0115] Specifically, when the resultant velocity value is lower than the speed determination threshold and the standard deviation is lower than the stability threshold, the timestamp at this moment is immediately read as the current time; and then, a sliding time window with a length of N seconds is established by tracing back N seconds of data from the current time as a starting point.

[0116] The resultant velocity values at each time point in the sliding time window are traversed to count the number of samples satisfying the condition that the resultant velocity value is lower than the speed determination threshold; and the proportion is calculated according to the number of samples. When the proportion exceeds a preset proportion threshold (for determining whether the proportion is high enough to trigger the change rate calculation) set according to experimental calibration (for example, the proportion exceeds 90%), the acceleration fluctuation indexes in the sliding time window are extracted in chronological order; then, the difference between the acceleration fluctuation index at the starting point of the window and the acceleration fluctuation index at the ending point is calculated, and then divided by the window length N seconds to obtain the change rate of the standard deviation in the sliding time window.

[0117] When the change rate is less than a preset change rate threshold (for defining whether the change rate is small enough to confirm the hovering state) set according to experimental calibration, it is confirmed that the stable hovering state is entered.

[0118] Through the scheme, when the resultant velocity value is lower than the speed determination threshold and the standard deviation is lower than the stability threshold, a sliding time window with a length of N seconds is established to ensure that the state determination is based on continuous observations in the time dimension rather than single-point data, thereby enhancing environmental adaptability. The proportion of the resultant velocity values lower than the speed determination threshold in the sliding time window is counted to effectively exclude temporary deceleration events, ensure that the hovering state needs to maintain low speed for most of the time, thereby improving the robustness of the determination and reducing the risk of misjudgment caused by instantaneous noise or environmental interference. When the proportion exceeds the preset proportion threshold, the change rate of the standard deviation in the sliding time window is calculated, and when the change rate is less than the preset change rate threshold, it is confirmed that the stable hovering state is entered, thereby eliminating the risk of misjudgment and ensuring that the state determination is triggered only when the speed and acceleration are highly stable.

[0119] In some embodiments, the tunnel entrance plane is determined according to the latitude and longitude in the initial pose; the tunnel extension direction axis is determined according to the strike data in the tunnel structure information; the tunnel space coordinate system is established with the tunnel entrance plane as the XY plane and the extension direction axis as the Z axis; the pose change amount in the real-time motion state is obtained based on the tunnel space coordinate system; the spatial constraint boundary is established based on the tunnel structure information and the tunnel space coordinate system; and the attitude correction amount is calculated according to the matching degree of the pose change amount and the spatial constraint boundary.

[0120] The tunnel entrance plane can be a horizontal virtual plane defined based on the longitude and latitude in the initial pose and the direction of the gravitational acceleration when the UAV enters the tunnel. The strike data can be three-dimensional vector data representing the tunnel extension direction in the tunnel structure information. The tunnel extension direction axis can be a unit vector representing the main extension direction inside the tunnel. The XY plane can be a two-dimensional plane defined by the tunnel entrance plane in the tunnel space coordinate system. The Z axis can be a coordinate axis defined by the extension direction axis in the tunnel space coordinate system. The tunnel space coordinate system can be a local Cartesian coordinate system with the tunnel entrance point as the origin, the tunnel entrance plane as the XY plane, and the extension direction axis as the Z axis. The pose change amount can be the change amount of the position and attitude calculated in real-time motion state. The spatial constraint boundary can be a geometric boundary equation defined according to the tunnel structure information in the tunnel space coordinate system. The matching degree can be the spatial compliance between the pose change amount and the spatial constraint boundary. The attitude correction amount can be the attitude angle adjustment amount calculated according to the matching degree.

[0121] Specifically, according to the longitude and latitude in the initial pose, the tunnel entrance plane is defined in combination with the direction of the gravitational acceleration (vertically downward) provided by the inertial measurement unit, wherein the tunnel entrance point (the longitude and latitude corresponding point of the initial pose) is taken as the origin, and the direction of the gravitational acceleration is taken as the normal direction.

[0122] The strike vector (representing the extension direction of the tunnel from the entrance to the inside) is extracted from the tunnel structure information; the strike vector is normalized to a unit vector, i.e. the tunnel extension direction axis.

[0123] The tunnel entrance plane is defined as the XY plane (horizontal plane), wherein the X axis and the Y axis are orthogonal in the plane (for example, the X axis points to the east, and the Y axis points to the north); the extension direction axis is defined as the Z axis (representing the tunnel depth direction, the positive direction points to the inside of the tunnel), the direction points to the inside of the tunnel and extends; then, the tunnel space coordinate system is constructed with the XY plane as the reference surface, and the Z axis is aligned with the extension direction of the tunnel inside.

[0124] In the tunnel space coordinate system, the position offset of the UAV since the initial pose is calculated by integrating the IMU acceleration data in combination with the time stamp (fixed sampling frequency); the attitude angle change is calculated by integrating the IMU angular velocity data; the position change amount in the real-time motion state is determined by integrating the position offset and the attitude angle change.

[0125] In the tunnel space coordinate system, the spatial constraint boundary (i.e. the flyable region boundary inside the tunnel) is defined in combination with the cross-section geometric parameters (such as shape, size) and the extension range (tunnel total length parameter) extracted from the tunnel structure information, for example, if the tunnel cross-section is rectangular, the spatial constraint boundary is defined as a rectangular column along the Z axis direction, and the boundary coordinates are determined by the cross-section width and height; if it is circular, it is defined as a cylindrical boundary.

[0126] The pose change amount is matched with the space constraint boundary, if the matching degree is high (indicating that most of the pose changes are within the boundary), no correction is needed; if the matching degree is low (such as out of bounds), it indicates that the drift error is large, and correction is needed. According to the matching result, the attitude correction amount is calculated through proportional control, for example, if the X direction is out of bounds, the roll angle correction amount is generated; if the Y direction is out of bounds, the pitch angle correction amount is generated.

[0127] According to the present scheme, the tunnel entrance plane is determined according to the latitude and longitude in the initial pose, and the position reference drift caused by GPS loss is eliminated. According to the trend data in the tunnel structure information, the tunnel extension direction axis is determined to avoid the accumulation and amplification of pose error caused by direction deviation. The tunnel space coordinate system is established with the tunnel entrance plane as the XY plane and the extension direction axis as the Z axis, reflecting the relative motion of the UAV in the tunnel, stripping irrelevant environmental interference, and improving the environmental adaptability of the pose calculation. Based on the tunnel space coordinate system, the pose change amount in the real-time motion state is obtained, significantly reducing the accumulation speed of direction drift. Based on the tunnel space coordinate system, the space constraint boundary is established according to the tunnel structure information, solving the problem of collision risk. The pose change amount is matched with the space constraint boundary, and the attitude correction amount is calculated according to the matching degree of the pose change amount and the space constraint boundary, ensuring that the motion direction is consistent with the tunnel trend and meeting the needs of high-precision inspection and obstacle avoidance, and avoiding the high computational burden of visual algorithms.

[0128] In some embodiments, based on the tunnel space coordinate system, the cross-sectional geometric parameters in the tunnel structure information are extracted; a series of constraint sections are established along the tunnel extension direction; the motion parameters are analyzed to determine the parameter difference value of adjacent constraint sections; the continuous constraint field function is constructed according to the parameter difference value; and the space constraint boundary is established according to the continuous constraint field function.

[0129] The cross-sectional geometric parameters can be the cross-sectional shape and size parameters described in the tunnel structure information. The tunnel extension direction can be the axis direction extending from the entrance to the internal endpoint of the tunnel in the tunnel space coordinate system. The series of constraint sections can be a series of sections established at a fixed interval along the tunnel extension direction. The adjacent constraint sections can be two sections adjacent in position in the series of constraint sections. The parameter difference value can be the difference value of the geometric parameters between the adjacent constraint sections. The continuous constraint field function can be a mathematical function constructed based on the parameter difference value to describe the continuous change of the geometric parameters with the tunnel extension direction.

[0130] Specifically, in the tunnel space coordinate system, the cross-sectional geometric parameters related to the current tunnel segment are read from the tunnel structure information, such as the radius parameter when the tunnel is a circular cross-section, and the width and height parameters when the tunnel is a rectangular cross-section.

[0131] In the tunnel spatial coordinate system, multiple discrete cross-section points are defined at fixed intervals along the tunnel's extension direction. Each point corresponds to a constraint cross-section, thus establishing a series of constraint cross-sections. In the tunnel spatial coordinate system, the initial pose is superimposed with motion parameters to obtain the current coordinates. Based on the current position, adjacent cross-sections are searched within the series of constraint cross-sections. If the cross-section parameters in the tunnel structure information are constant (e.g., all cross-section dimensions are the same), the parameter difference is zero (i.e., no difference). If the cross-section parameters change (e.g., width or height gradually changes along the Z-axis), the parameter difference between adjacent constraint cross-sections is determined.

[0132] Based on the parameter differences between adjacent constraint sections, linear interpolation is used to construct a continuous constraint field function that describes the continuous variation of geometric parameters with the Z-axis. For example, for a circular section, a radius function is constructed; for a rectangular section, a width function and a height function are constructed.

[0133] The constructed continuous constraint field function is directly converted into the spatial constraint boundary of the flyable region.

[0134] This scheme extracts cross-sectional geometric parameters from the tunnel structure information based on the tunnel's spatial coordinate system, reflecting the actual physical structure of the tunnel and avoiding boundary definition errors caused by missing geometric parameters. A series of constraint sections are established along the tunnel's extension direction to achieve a discretized representation of the tunnel structure, enabling the spatial constraint boundary to handle geometric changes in segments and enhancing the local adaptability of the boundary definition. Motion parameters are analyzed to determine the parameter differences between adjacent constraint sections, quantifying the local change trend of the tunnel along its extension direction and ensuring that the difference calculation is synchronized with the actual pose of the UAV, improving the real-time performance of boundary matching. Based on the parameter differences, a continuous constraint field function is constructed to achieve dynamic continuity of spatial constraints, eliminating the abruptness of discrete sections and allowing the boundary to seamlessly adapt to the gradual changes in tunnel geometry, ensuring the continuity and computability of the constraints. Based on the continuous constraint field function, a spatial constraint boundary is established to effectively constrain the UAV's motion, suppress IMU integral drift, and improve navigation accuracy.

[0135] In some embodiments, the displacement vector relative to the initial pose is calculated using a trajectory extrapolation algorithm; the displacement vector is then projected onto the tunnel space coordinate system using a coordinate system transformation matrix to obtain the pose change in the real-time motion state.

[0136] The trajectory extrapolation algorithm can be an algorithm that calculates the changes in position and attitude of the UAV relative to its initial pose by time integration.

[0137] The displacement vector can be a three-dimensional vector generated by a trajectory extrapolation algorithm, representing the real-time changes of the UAV relative to its initial pose.

[0138] The coordinate system transformation matrix can be a preset mathematical transformation matrix used to map the displacement vector to the tunnel space coordinate system.

[0139] Specifically, the integral acceleration data obtains a speed change, and then the speed change is time-integrated to obtain a position change; meanwhile, the angular velocity data is time-integrated to obtain an attitude change; the position change and the attitude change are combined into a displacement vector (representing real-time changes relative to an initial pose).

[0140] A coordinate system conversion matrix (used to map the displacement vector to the tunnel space coordinate system, such as a combination of a rotation matrix and a translation matrix) is constructed according to the relationship between the initial pose and the tunnel space coordinate system; the displacement vector is projected to the tunnel space coordinate system through the coordinate system conversion matrix to obtain a projected position offset and an attitude change; the position offset and the attitude change are combined as a pose change in a real-time motion state.

[0141] Through the present scheme, the displacement vector relative to the initial pose is calculated through the dead reckoning algorithm, which meets the real-time demand for pose update when the unmanned aerial vehicle flies at high speed, and avoids the problem of computational burden of visual navigation. Through the coordinate system conversion matrix, the displacement vector is projected to the tunnel space coordinate system to obtain the pose change in the real-time motion state, which ensures maintaining a high update rate in the GPS-free environment and solves the core pain point of insufficient real-time performance.

[0142] In some embodiments, according to three-axis acceleration data, a high-frequency shaking component is extracted; the high-frequency shaking component is analyzed to determine a horizontal plane shaking angle and a height direction shaking displacement; according to the horizontal plane shaking angle, combined with the XY plane, a horizontal offset is determined through geometric projection; according to the height direction shaking displacement, combined with height reference data in the tunnel structure information, a height change is determined.

[0143] The high-frequency shaking component can be a high-frequency fluctuation component separated from the three-axis acceleration data, representing the acceleration change caused by instantaneous shaking of the unmanned aerial vehicle in the hovering state.

[0144] The horizontal plane shaking angle can be an angle change in the horizontal plane.

[0145] The height direction shaking displacement can be an instantaneous displacement change in the height direction.

[0146] The geometric projection can be a mathematical mapping method for converting the horizontal plane shaking angle into a linear displacement component in the XY plane.

[0147] The horizontal offset can be a position change in the XY plane.

[0148] The height reference data can be a reference height value for comparison with the height direction shaking displacement.

[0149] The height change can be a position change in the height direction.

[0150] Specifically, a high-pass filter is applied to filter the three-axis acceleration data to extract high-frequency shaking components, wherein the cutoff frequency of the high-pass filter (a boundary parameter of the filtering operation) is set to separate high-frequency components (representing shaking noise) and low-frequency components (representing stable motion).

[0151] Double integration is performed on the high-frequency shaking components (first integration to obtain velocity, and then integration to obtain displacement) to determine the horizontal plane shaking angle (representing the roll and pitch micro-vibration angle of the UAV in the XY plane) and the height direction shaking displacement (representing the up-down micro-displacement of the UAV in the Z-axis direction).

[0152] The horizontal shaking angle is projected onto the XY plane using geometric projection, and then the X-axis direction offset and the Y-axis direction offset are calculated using trigonometric relationships. The X-axis direction offset and the Y-axis direction offset are integrated to determine the horizontal offset.

[0153] According to the height direction shaking displacement and the height reference data in the tunnel structure information (such as the tunnel design height or the initial height value at the entrance), the actual height change is calculated by a subtraction operation algorithm.

[0154] According to the three-axis acceleration data, the high-frequency shaking components are extracted to eliminate irrelevant noise and avoid the influence of low-frequency deviation on precision. The high-frequency shaking components are analyzed to determine the horizontal plane shaking angle and the height direction shaking displacement, and the high-frequency acceleration signal is converted into usable geometric parameters to provide direct input for geometric projection, thereby capturing the instantaneous attitude offset during hovering. According to the horizontal plane shaking angle, the XY plane is combined to determine the horizontal offset through geometric projection, providing real-time position update and correcting the horizontal direction drift. According to the height direction shaking displacement, the height reference data in the tunnel structure information is combined to determine the height change, correcting the height direction drift and ensuring that the height data is consistent with the tunnel geometric constraints.

[0155] In some embodiments, the pose change is analyzed to obtain displacement vector and angle change data; the pose matching degree is calculated according to the displacement vector, the angle change data and the spatial constraint boundary; the minimum distance between the displacement vector and the spatial constraint boundary is calculated as the position matching degree through the Euclidean distance formula; the angle matching degree between the angle change data and the normal vector of the spatial constraint boundary is determined through angle difference calculation; the comprehensive matching degree is determined by weighted summation of the pose matching degree, the position matching degree and the angle matching degree; and the attitude correction amount is calculated according to the comprehensive matching degree.

[0156] The angle change data can be a pitch angle, a roll angle, and a yaw angle of the attitude change quantity parsed from the pose change quantity. The pose matching degree can be a scalar index indicating a degree of compliance of the displacement vector and the angle change data with the space constraint boundary. The minimum distance can be a geometrically shortest distance of the displacement vector to the space constraint boundary calculated by an Euclidean distance formula. The position matching degree can be a degree of compliance of the displacement vector with the space constraint boundary. The angle difference calculation can be a mathematical process of calculating an included angle between the angle change data and a normal vector of the space constraint boundary. The normal vector can be a unit vector representing a tunnel extension direction on the space constraint boundary. The angle matching degree can be a degree of coincidence of the angle change data with the normal vector of the space constraint boundary, an angle difference value for quantifying a degree of attitude drift. The comprehensive matching degree can be a final matching index comprehensively representing a degree of compliance of the current pose of the unmanned aerial vehicle with the space constraint boundary of the tunnel.

[0157] Specifically, the pose change quantity is parsed by a data decomposition operation, and a position component thereof is extracted as a displacement vector. Meanwhile, an attitude component is extracted from the pose change quantity as angle change data. The displacement vector is projected onto the space constraint boundary to generate a projection point. An Euclidean distance difference between the projection point and the displacement vector is calculated as a position influence factor. Meanwhile, the angle change data is compared with an average direction (such as a tunnel extension direction) of the space constraint boundary to generate an angle deviation as an attitude influence factor. The position influence factor and the attitude influence factor are combined to generate the pose matching degree by linear combination.

[0158] An Euclidean distance of the displacement vector to the space constraint boundary is calculated by an Euclidean distance formula. A minimum distance is searched by traversing a plurality of Euclidean distances. The minimum distance value is directly taken as the position matching degree (the smaller the distance, the higher the position matching degree). For example, in a tunnel environment, a small distance value indicates that the unmanned aerial vehicle is close to the center line, and a large distance value indicates that the unmanned aerial vehicle may collide with the wall or deviate from the path.

[0159] An attitude angle is extracted from the angle change data and converted into a direction vector. An angle difference between the direction vector and a normal vector (a normal direction vector of the tunnel wall) of the space constraint boundary is calculated by a dot product formula. The direction difference is directly mapped as the angle matching degree. For example, in a curved tunnel, a small angle difference value indicates that the attitude (such as roll or pitch) of the unmanned aerial vehicle is consistent with the tunnel direction, and a large angle difference value indicates that the attitude is out of control or the direction is wrong.

[0160] The pose matching degree, the position matching degree, the angle matching degree, and a preset weight coefficient (used to assign importance weights of different matching degree indexes in a weighted summation process) set according to experience are weighted and summed to determine the comprehensive matching degree. The comprehensive matching degree is mapped to an attitude correction quantity by a solving algorithm. For example, a larger correction quantity is generated when the comprehensive matching degree is low, and vice versa.

[0161] By the scheme, the pose change amount is analyzed to obtain displacement vector and angle change data, structured processing of the pose change data is realized, and matching errors caused by data confusion are avoided. According to the displacement vector, the angle change data and the space constraint boundary, the pose matching degree is calculated, and the overall degree of pose drift is identified. By the Euclidean distance formula, the minimum distance between the displacement vector and the space constraint boundary is calculated as the position matching degree, and the drift error of the position direction is effectively detected and identified. By angle difference calculation, the angle matching degree of the normal vector of the angle change data and the space constraint boundary is determined, the drift error of the attitude direction is effectively detected, and the attitude of the unmanned aerial vehicle is ensured to be aligned with the tunnel geometry. The pose matching degree, the position matching degree and the angle matching degree are weighted and summed to determine the comprehensive matching degree, avoid decision conflicts caused by multiple independent values, and improve the correction efficiency. According to the comprehensive matching degree, the attitude correction amount is calculated to ensure that the pose change matches the tunnel constraint, and the navigation accuracy is improved.

[0162] In some embodiments, when the comprehensive matching degree is lower than a preset matching threshold, a pose optimization function is constructed; the pose optimization function aims to minimize the deviation of the pose change amount from the space constraint boundary, and the constraint conditions include the velocity continuity obtained by analyzing the real-time motion state; when the real-time motion state is a hovering state, the constraint conditions further include the horizontal offset and the height change amount; when the real-time motion state is a non-hovering state, the constraint conditions further include the space constraint boundary; the pose optimization function is solved to output the attitude correction amount.

[0163] The preset matching threshold can be a constant scalar value used for comparison with the comprehensive matching degree. The pose optimization function can be a mathematical optimization function used to solve the attitude correction amount. The deviation can be a difference value between the pose change amount and the space constraint boundary. The constraint conditions can be mathematical limit conditions set in the pose optimization function, including the velocity continuity constraint condition, the horizontal offset constraint condition and the height change amount constraint condition, and the space constraint boundary constraint condition. The velocity continuity can be a smooth characteristic varying with time based on the analysis of the real-time motion state. The non-hovering state can be a non-stationary state of the unmanned aerial vehicle.

[0164] Specifically, the comprehensive matching degree is compared with a preset matching threshold set according to experiments, and if the comprehensive matching degree is lower than the preset matching threshold, a pose optimization function is constructed.

[0165] The position deviation of the displacement vector from the space constraint boundary is calculated; at the same time, the attitude deviation of the angle change data from the space constraint boundary (such as the extension direction of the tunnel) is calculated; and then the real-time motion state is analyzed to determine the velocity continuity. Therefore, the pose optimization function aims to minimize the sum of deviations (the sum of the position deviation and the attitude deviation), and adds the constraint condition of the velocity continuity.

[0166] By analyzing the speed and acceleration, it is judged whether the real-time motion state is a hovering state. When the real-time motion state is a hovering state, the position deviation of the pose change in the horizontal direction (XY plane) is calculated, and the deviation value is taken as a horizontal offset constraint condition; at the same time, the position change of the pose change in the vertical direction (Z axis) is calculated, and the change value is taken as a height change constraint condition (the change value needs to be kept stable).

[0167] When the real-time motion state is a non-hovering state, the spatial constraint boundary (tunnel wall geometry) is taken as a spatial constraint boundary constraint condition.

[0168] The constructed attitude optimization function is solved using a standard optimization algorithm, and an attitude correction amount (representing an attitude angle adjustment amount, such as a roll angle, a pitch angle, and a yaw angle correction value) is output.

[0169] Through the scheme, when the comprehensive matching degree is lower than the preset matching threshold, the attitude optimization function is constructed, the inertial navigation drift is compensated, and the pose is more fitted to the tunnel structure. The attitude optimization function takes minimizing the deviation of the pose change and the spatial constraint boundary as the target, and the constraint conditions include the speed continuity obtained by analyzing the real-time motion state, so as to ensure that the attitude correction generated in the optimization process will not cause a sudden change in speed, improve flight stability, and avoid control oscillation caused by violent adjustment. When the real-time motion state is a hovering state, the constraint conditions also include the horizontal offset and the height change, so as to force the optimization process to reduce the horizontal offset and ensure the position stability during hovering, and at the same time, force the optimization process to keep the height constant. When the real-time motion state is a non-hovering state, the constraint condition also includes the spatial constraint boundary, so as to force the optimization process to make the pose conform to the tunnel geometry. The attitude optimization function is solved, the attitude correction amount is output, the pose deviation is corrected, and the tunnel navigation precision is improved.

[0170] Figure 3 A structural schematic diagram of a high-precision inertial navigation system for unmanned aerial vehicle flight provided by an embodiment of the present application is shown in FIG. 1. Figure 3 As shown in FIG. 1, the high-precision inertial navigation system 300 for unmanned aerial vehicle flight of the present embodiment includes an initial pose determination module 301, a motion parameter analysis module 302, and a real-time attitude correction module 303.

[0171] The initial pose determination module 301 is configured to acquire a GPS signal before the unmanned aerial vehicle enters the tunnel, and determine an initial pose according to the GPS signal.

[0172] The motion parameter analysis module 302 is configured to acquire motion parameters of the unmanned aerial vehicle in real time during the inspection process, and analyze the motion parameters to determine a real-time motion state.

[0173] The real-time attitude correction module 303 is configured to acquire tunnel structure information, and correct a real-time attitude of the UAV based on the tunnel structure information, the real-time motion state and the initial pose.

[0174] Optionally, the real-time motion state includes a hovering state; when the motion parameter analysis module 302 analyzes the motion parameters to determine the real-time motion state, the module is configured to analyze the motion parameters to determine three-axis velocity data and three-axis acceleration data; perform vector synthesis on the three-axis velocity data to calculate a resultant velocity value; calculate a standard deviation of the acceleration data within a preset time period, and take the standard deviation as an acceleration fluctuation index; and when the resultant velocity value is lower than a velocity determination threshold and the standard deviation is lower than a stability threshold, determine that the hovering state is entered.

[0175] Optionally, when the motion parameter analysis module 302 determines that the hovering state is entered when the resultant velocity value is lower than the velocity determination threshold and the standard deviation is lower than the stability threshold, the module is configured to, when the resultant velocity value is lower than the velocity determination threshold and the standard deviation is lower than the stability threshold, establish a sliding time window with a length of N seconds; count a proportion of the resultant velocity value that is lower than the velocity determination threshold within the sliding time window; when the proportion exceeds a preset proportion threshold, calculate a change rate of the standard deviation within the sliding time window; and when the change rate is less than a preset change rate threshold, confirm that the stable hovering state is entered.

[0176] Optionally, when the real-time attitude correction module 303 corrects the real-time attitude of the UAV based on the tunnel structure information, the real-time motion state and the initial pose, the module is configured to: determine a tunnel entrance plane according to the longitude and latitude in the initial pose; determine a tunnel extension direction axis according to the strike data in the tunnel structure information; establish a tunnel space coordinate system with the tunnel entrance plane as an XY plane and the extension direction axis as a Z axis; acquire a pose change amount in the real-time motion state based on the tunnel space coordinate system; establish a space constraint boundary based on the tunnel structure information according to the tunnel space coordinate system; match the pose change amount and the space constraint boundary, and calculate an attitude correction amount according to a matching degree of the pose change amount and the space constraint boundary.

[0177] Optionally, when the real-time attitude correction module 303 establishes the space constraint boundary based on the tunnel structure information according to the tunnel space coordinate system, the module is configured to: extract cross-sectional geometric parameters in the tunnel structure information based on the tunnel space coordinate system; establish a series of constraint cross sections along the tunnel extension direction; analyze the motion parameters to determine a parameter difference value of adjacent constraint cross sections; construct a continuous constraint field function according to the parameter difference value; and establish the space constraint boundary according to the continuous constraint field function.

[0178] Optionally, when the real-time attitude correction module 303 acquires the position and posture change amount in the real-time motion state based on the tunnel space coordinate system, the method comprises the following steps: calculating a displacement vector relative to an initial position and posture by a dead reckoning method; and projecting the displacement vector to the tunnel space coordinate system by a coordinate system conversion matrix to obtain the position and posture change amount in the real-time motion state.

[0179] Optionally, the high-precision inertial navigation system of the unmanned aerial vehicle further comprises a change amount determination module 304, configured to: extract a high-frequency shaking component from the three-axis acceleration data; analyze the high-frequency shaking component to determine a horizontal plane shaking angle and a height direction shaking displacement; determine a horizontal offset amount by geometric projection based on the horizontal plane shaking angle and the XY plane; and determine a height change amount based on the height direction shaking displacement and the height reference data in the tunnel structure information.

[0180] Optionally, when the real-time attitude correction module 303 matches the position and posture change amount with the space constraint boundary, the method comprises the following steps: analyzing the position and posture change amount to obtain a displacement vector and an angle change data; calculating a position and posture matching degree based on the displacement vector, the angle change data and the space constraint boundary; calculating a minimum distance between the displacement vector and the space constraint boundary as a position matching degree by a Euclidean distance formula; determining an angle matching degree between the angle change data and a normal vector of the space constraint boundary by an angle difference calculation; and determining a comprehensive matching degree by weighted summation of the position and posture matching degree, the position matching degree and the angle matching degree; and when the real-time attitude correction module 303 calculates the attitude correction amount based on the matching degree between the position and posture change amount and the space constraint boundary, the method comprises the following step: calculating the attitude correction amount based on the comprehensive matching degree.

[0181] Optionally, when the real-time attitude correction module 303 calculates the attitude correction amount based on the comprehensive matching degree, the method comprises the following steps: constructing an attitude optimization function when the comprehensive matching degree is lower than a preset matching threshold; the attitude optimization function aims to minimize the deviation between the position and posture change amount and the space constraint boundary, and the constraint conditions include a velocity continuity obtained by analyzing the real-time motion state; when the real-time motion state is a hovering state, the constraint conditions further include the horizontal offset amount and the height change amount; when the real-time motion state is a non-hoovering state, the constraint conditions further include the space constraint boundary; and solving the attitude optimization function to output the attitude correction amount.

[0182] The system of the embodiment can be used to execute the method of any of the above embodiments, and has similar implementation principles and technical effects, which will not be described here again.

Claims

1. A high-precision inertial navigation method for unmanned aerial vehicle flight, characterized in that, The method comprises the following steps: acquiring a GPS signal before the UAV enters the tunnel, and determining an initial pose according to the GPS signal; acquiring motion parameters of the UAV in real time during the inspection process; analyzing the motion parameters to determine a real-time motion state; acquiring tunnel structure information, and correcting a real-time attitude of the UAV according to the real-time motion state and the initial pose based on the tunnel structure information; the real-time motion state comprises a hovering state; the analysis of the motion parameters to determine the real-time motion state comprises: analyzing the motion parameters to determine three-axis speed data and three-axis acceleration data; performing vector composition on the three-axis speed data to calculate a combined speed value; calculating a standard deviation of the acceleration data within a preset time period, and taking the standard deviation as an acceleration fluctuation index; when the combined speed value is lower than a speed determination threshold and the standard deviation is lower than a stability threshold, it is determined that the hovering state is entered; when the combined speed value is lower than the speed determination threshold and the standard deviation is lower than the stability threshold, the method comprises the following steps: when the combined speed value is lower than the speed determination threshold and the standard deviation is lower than the stability threshold, a sliding time window with a length of N seconds is established; statistics of a proportion of the combined speed value lower than the speed determination threshold within the sliding time window are obtained; when the proportion exceeds a preset proportion threshold, a change rate of the standard deviation within the sliding time window is calculated, and when the change rate is less than a preset change rate threshold, it is confirmed that the stable hovering state is entered.

2. The method of claim 1, wherein, the correction of the real-time attitude of the UAV according to the real-time motion state and the initial pose based on the tunnel structure information comprises the following steps: determining a tunnel entrance plane according to longitude and latitude in the initial pose; determining a tunnel extension direction axis according to strike data in the tunnel structure information; establishing a tunnel space coordinate system with the tunnel entrance plane as an XY plane and the extension direction axis as a Z axis; acquiring a pose change amount in the real-time motion state based on the tunnel space coordinate system; establishing a space constraint boundary according to the tunnel structure information based on the tunnel space coordinate system; matching the pose change amount with the space constraint boundary, and calculating an attitude correction amount according to a matching degree of the pose change amount and the space constraint boundary.

3. The method of claim 2, wherein, the establishment of the space constraint boundary according to the tunnel structure information based on the tunnel space coordinate system comprises the following steps: extracting cross-section geometric parameters in the tunnel structure information based on the tunnel space coordinate system; establishing a series of constraint cross sections along the tunnel extension direction; analyzing the motion parameters to determine a parameter difference value of adjacent constraint cross sections; constructing a continuous constraint field function according to the parameter difference value; establishing the space constraint boundary according to the continuous constraint field function.

4. The method of claim 2, wherein, the acquisition of the pose change amount in the real-time motion state based on the tunnel space coordinate system comprises the following steps: calculating a displacement vector relative to the initial pose by a dead reckoning method; projecting the displacement vector to the tunnel space coordinate system by a coordinate system conversion matrix to obtain the pose change amount in the real-time motion state.

5. The method of claim 2, wherein, after it is confirmed that the stable hovering state is entered when the change rate is less than the preset change rate threshold, the method further comprises the following steps: According to the three-axis acceleration data, a high-frequency shaking component is extracted; The high-frequency shaking component is analyzed to determine a horizontal plane shaking angle and a height direction shaking displacement; According to the horizontal plane shaking angle, a horizontal offset is determined by geometric projection in combination with the XY plane; According to the height direction shaking displacement, a height change is determined in combination with height reference data in the tunnel structure information.

6. The method of claim 5, wherein, The matching of the pose change and the space constraint boundary includes: The pose change is analyzed to obtain displacement vector and angle change data; According to the displacement vector, the angle change data and the space constraint boundary, a pose matching degree is calculated; The minimum distance between the displacement vector and the space constraint boundary is calculated as a position matching degree by using the Euclidean distance formula; The angle matching degree between the angle change data and the normal vector of the space constraint boundary is determined by angle difference calculation; The comprehensive matching degree is determined by weighted summation of the pose matching degree, the position matching degree and the angle matching degree. The matching of the pose change and the space constraint boundary includes: According to the comprehensive matching degree, a pose correction amount is calculated.

7. The method of claim 6, wherein, The matching of the pose change and the space constraint boundary includes: When the comprehensive matching degree is lower than a preset matching threshold, a pose optimization function is constructed; the pose optimization function takes minimizing the deviation between the pose change and the space constraint boundary as the target, and the constraint conditions include the velocity continuity obtained by analyzing the real-time motion state; When the real-time motion state is a hovering state, the constraint conditions further include the horizontal offset and the height change; When the real-time motion state is a non-hovering state, the constraint conditions further include the space constraint boundary; The pose optimization function is solved to output the pose correction amount.

8. A high-precision inertial navigation system for unmanned aerial vehicles, characterized in that, Applied to the method of any one of claims 1-7, comprising: An initial pose determination module is configured to acquire a GPS signal before the UAV enters the tunnel, and determine an initial pose according to the GPS signal; A motion parameter analysis module is configured to acquire motion parameters of the UAV in real time during the inspection process, and analyze the motion parameters to determine a real-time motion state; A real-time pose correction module is configured to acquire tunnel structure information, and correct a real-time pose of the UAV based on the tunnel structure information, the real-time motion state and the initial pose.

Images