Limited space body-equipped intelligent unmanned aerial vehicle position constraint method, system and device

Abstract

The invention provides a position constraint method, system and device for an intelligent unmanned aerial vehicle with a body in a limited space, and the method comprises the steps: constructing a main and standby instance thread for flight control, and determining the odometer position information of the main and standby instance thread according to the obtained external odometer data and the three-dimensional position state information of the main and standby instance thread filtering estimation; standardizing the position information and then determining whether the measurement is updated or not; recording the position information of the standby instance thread passing the last odometer detection, and performing polynomial trajectory modeling; if the odometer information measurement of the standby instance thread does not need to be updated, calculating fitted odometer position information; if the fitted odometer position information passes the detection, carrying out measurement updating; and switching the main and standby instance threads according to the odometer information measurement updating mark. Based on the method, the invention further provides an unmanned aerial vehicle position restraint system and equipment. According to the invention, the stability and safety of the intelligent unmanned aerial vehicle for executing the route task in the limited space are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle position constraints, and in particular relates to a method, system and device for constraining the position of an embodied intelligent unmanned aerial vehicle in a limited space. Background Art

[0002] With the active promotion of the low-altitude economic industry, demonstration application projects for indoor drone inspections continue to be implemented and promoted in various confined spaces. The so-called confined space refers to those closed or semi-closed areas with relatively narrow and restricted passages, such as urban rail transit systems, various pipeline facilities, box-type bridge structures, and hollow piers and other key infrastructure. In a confined space environment, there are significant differences and challenges in determining the precise location of a drone compared to an open area outdoors. Since traditional GPS positioning signals cannot penetrate such closed or semi-closed areas, we must rely on lidar or visual SLAM (simultaneous localization and mapping) technology to achieve positioning and navigation in space. In this process, the SLAM system that integrates multiple sensors generates accurate odometer data and transmits this data to the drone's flight control system in real time. The flight control system further integrates this odometer information to ensure that the drone can achieve accurate positioning and autonomous flight in a complex and confined space.

[0003] At present, indoor drones mainly rely on Kalman filtering technology for positioning, which effectively integrates the multi-dimensional data from the external odometer released by SLAM (covering key information such as three-dimensional spatial position, speed and heading) to achieve accurate measurement and update of the drone status. When the odometer data released by the SLAM system diverges or jumps, it first needs to go through the new information detection process of the Kalman filter estimation and control module to determine whether the data is available. If the new information detection fails, it indicates that the data is abnormal, and the system will not include the data in the fusion process. If this situation persists for a period of time, resulting in the data not being successfully fused, the system will consider continuing to use the current odometer data to try to reset the state of the Kalman filter. If in the above case, the external odometer data still fails to return to normal, this will directly cause a violent fluctuation in the position state in the drone flight control system, which will cause the drone to lose control and eventually lead to a safety accident. Summary of the invention

[0004] In order to solve the above technical problems, the present invention proposes a method, system and device for position constraint of an embodied intelligent UAV in a limited space, which are used to improve the stability and safety of the UAV when performing route missions in a limited space scene.

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

[0006] A method for constraining the position of an embodied intelligent drone in a limited space comprises the following steps:

[0007] Constructing a master and a standby instance thread for UAV flight control; determining the master and standby instance thread odometer position updates according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the master and standby instance thread; after standardizing the master and standby instance thread odometer position updates, determining whether to perform odometer information measurement update of the master and standby instance thread;

[0008] Record the location information of the last odometer detection of the standby instance thread; use the location information of the last odometer detection of the standby instance thread, the location information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight to perform polynomial trajectory modeling;

[0009] If the odometer information measurement of the standby instance thread does not need to be updated, the polynomial trajectory point generation time is recorded, and the polynomial trajectory point generation time is substituted into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update;

[0010] The master and standby instance threads are switched according to the master instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

[0011] The present invention also proposes a limited space embodied intelligent unmanned aerial vehicle position constraint system, including: a thread construction module, a model construction module, an update module and a switching module.

[0012] The thread construction module is used to construct the main and standby instance threads and the standby instance threads for UAV flight control; determine the main and standby instance thread odometer position updates according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main and standby instance threads; after standardizing the main and standby instance thread odometer position updates, determine whether to perform odometer information measurement update of the main and standby instance threads;

[0013] The model building module is used to record the position information of the last odometer detection of the standby instance thread; the polynomial trajectory modeling is performed using the position information of the last odometer detection of the standby instance thread, the position information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight;

[0014] The update module is used for recording the generation time of the polynomial trajectory point if the odometer information measurement of the standby instance thread does not need to be updated, and substituting the generation time of the polynomial trajectory point into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update;

[0015] The switching module is used to switch the main instance thread to the standby instance thread according to the main instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

[0016] The present invention also proposes a limited space embodied intelligent drone position constraint device, comprising:

[0017] Memory for storing computer programs;

[0018] A processor is used to implement the method steps described when executing the computer program.

[0019] The effects provided in the content of the invention are only the effects of the embodiments, not all the effects of the invention. One of the above technical solutions has the following advantages or beneficial effects:

[0020] The present invention proposes a limited space embodied intelligent unmanned aerial vehicle position constraint method, system and device, the method comprising: constructing a main and standby instance thread and a standby instance thread for unmanned aerial vehicle flight control; determining the main and standby instance thread odometer position updates according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main and standby instance thread; determining whether to perform odometer information measurement update of the main and standby instance thread after standardizing the odometer position updates of the main and standby instance thread; recording the position information of the last odometer detection passed by the standby instance thread; using the position information of the last odometer detection passed by the standby instance thread, the position information of the next target waypoint to be reached in the route mission, and the cruising speed information set for the route flight to perform polynomial trajectory modeling; if the odometer information measurement of the standby instance thread does not need to be updated, recording the polynomial trajectory point generation time, substituting the polynomial trajectory point generation time into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the detection, using the fitted odometer position information to perform measurement update; switching the main and standby instance threads according to the main instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag. Based on a limited space embodied intelligent drone position constraint method, a limited space embodied intelligent drone position constraint system and device are also proposed. The present invention significantly enhances the redundancy performance of the system by introducing two parallel and independent running threads, the main instance thread and the alternative instance thread of state estimation. Once the main instance thread encounters a failure, it can immediately and seamlessly switch to the alternative instance thread, thereby ensuring the continuous and effective constraint of the drone position.

[0021] In the present invention, the main instance thread focuses on executing the new information detection task and does not introduce the polynomial fitting control strategy, so as to ensure that when the external odometer data returns to normal, the system can smoothly switch back to the main instance thread, ensuring that the state estimation process is not interfered by the polynomial fitting control strategy, thereby maintaining the consistency and accuracy of the main instance thread in state measurement updates.

[0022] The state estimation alternative instance thread of the present invention incorporates a polynomial fitting control strategy to deal with situations where the external odometer data diverges or jumps and cannot be used for measurement updates. At this time, the alternative instance thread will use the polynomial trajectory model to accurately fit the odometer position information that meets the requirements, thereby replacing the abnormal external odometer data and performing measurement updates, thereby ensuring that the alternative instance thread can maintain a high degree of stability when performing position estimation.

[0023] The polynomial trajectory model in the present invention deeply considers the application of 5th-order polynomials, that is, it incorporates the snap-level analysis, aiming to ensure that the constructed trajectory model can show higher smoothness and better fit the actual situation of route flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 This is a schematic diagram of a flow chart of a method for constraining the position of an embodied intelligent drone in a limited space proposed in Example 1 of the present invention;

[0025] Figure 2 A detailed flow chart of a method for constraining the position of an embodied intelligent drone in a limited space proposed in Example 1 of the present invention;

[0026] Figure 3 This is a schematic diagram of a limited space embodied intelligent drone position constraint system proposed in Example 2 of the present invention;

[0027] Figure 4 This is a schematic diagram of a limited space embodied intelligent drone position constraint device proposed in Example 3 of the present invention. DETAILED DESCRIPTION

[0028] In order to clearly illustrate the technical features of the present solution, the present invention is described in detail below through specific implementation methods and in conjunction with the accompanying drawings. The disclosure below provides many different embodiments or examples for realizing different structures of the present invention. In order to simplify the disclosure of the present invention, the components and settings of specific examples are described below. In addition, the present invention may repeat reference numbers and / or letters in different examples. This repetition is for the purpose of simplification and clarity, and does not itself indicate the relationship between the various embodiments and / or settings discussed. It should be noted that the components illustrated in the accompanying drawings are not necessarily drawn to scale. The present invention omits the description of known components and processing techniques and processes to avoid unnecessary limitations on the present invention.

[0029] Example 1

[0030] Embodiment 1 of the present invention proposes a limited space embodied intelligent drone position constraint method, which is used to solve the technical problems of drone position constraint in the prior art. This application introduces a position constraint algorithm to ensure that when the external odometer data diverges abnormally or mutates, the position and speed of the drone can be effectively controlled to avoid sudden loss of control or even crash accidents, thereby significantly improving the safety of the system.

[0031] Embodied intelligent drones refer to drones that achieve intelligent behaviors through the interaction between the drone’s body and the environment.

[0032] Figure 1 This is a schematic diagram of a flow chart of a method for constraining the position of an embodied intelligent drone in a limited space proposed in Example 1 of the present invention;

[0033] In step S100, the process starts.

[0034] In step S110, external odometer data released by SLAM is obtained;

[0035] In step S120, a main instance thread for drone flight control is constructed, specifically: determining the main instance thread odometer position update information according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main instance thread; after standardizing the main instance thread odometer position update information, determining whether to perform the main instance thread odometer information measurement update;

[0036] The position update is the difference between the observed measurement and the predicted measurement by the Kalman filter.

[0037] In step S130, a standby instance thread for UAV flight control is constructed, specifically: determining the standby instance thread odometer position update according to the acquired external odometer data and the three-dimensional position state information estimated by the standby instance thread Kalman filter; after standardizing the standby instance thread odometer position update, determining whether to perform standby instance thread odometer information measurement update.

[0038] And use the location information of the last odometer detection of the standby instance thread, the location information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight to perform polynomial trajectory modeling;

[0039] If the odometer information measurement of the standby instance thread does not need to be updated, the polynomial trajectory point generation time is recorded, and the polynomial trajectory point generation time is substituted into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update;

[0040] In step S140, the master instance thread and the standby instance thread are switched according to the master instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

[0041] In step S150, the process ends.

[0042] Figure 2 A detailed flow chart of a method for constraining the position of an embodied intelligent drone in a limited space proposed in Example 1 of the present invention;

[0043] Start working on the process.

[0044] Get the external odometer data released by SLAM; the three-dimensional position information of the external odometer data is (X odo , Y odo , Z odo );

[0045] Construct the main instance thread for UAV flight control, specifically: determine the main instance thread odometer position update according to the acquired external odometer data and the three-dimensional position state information estimated by the main instance thread Kalman filter; after standardizing the main instance thread odometer position update, determine whether to perform the main instance thread odometer information measurement update;

[0046] The three-dimensional position state information estimated by the main instance thread is The main instance thread odometer position update calculation process is:

[0047]

[0048] Among them, X odo is the X-axis position information of the external odometer data; Y odo is the Y-axis position information of the external odometer data; Z odo It is the Z-axis position information of the external odometer data; The X-axis position information estimated by the Kalman filter of the main instance thread; Y-axis position information estimated by Kalman filter of the main instance thread; The Z-axis position information estimated by the Kalman filter of the main instance thread; The X-axis component of the main instance thread odometer position update; The Y-axis component of the main instance thread odometer position update; The Z-axis component of the main instance thread odometer position update;

[0049] The formula for normalizing the main instance thread odometer position updates is:

[0050]

[0051] in, The X-axis component of the main instance thread odometer position innovation variance; The Y-axis component of the main instance thread odometer position innovation variance; The Z-axis component of the main instance thread odometer position innovation variance, which is obtained by summing the state variance and the odometer measurement variance; The X-axis component of the normalized main instance thread odometer position update; The Y-axis component of the normalized main instance thread odometer position update; The Z-axis component of the normalized main instance thread odometer position update;

[0052] The formula for determining whether to update the odometer information measurement of the main instance thread is:

[0053]

[0054] Where λ is the limit value, expressed as λ times the standard deviation; Γ m is the main instance thread odometer information measurement update flag; if one of the conditions in the formula is not satisfied, then Γ m If the flag is false, the main instance thread does not use the odometer data for measurement updates and accumulates the continuous failure time. Otherwise, the flag is true, and the main instance thread uses the odometer data to measure and update, and accumulates the continuous success time

[0055] Construct a standby instance thread for UAV flight control, specifically: determine the standby instance thread odometer position update according to the acquired external odometer data and the three-dimensional position state information estimated by the standby instance thread Kalman filter; after standardizing the standby instance thread odometer position update, determine whether to update the standby instance thread odometer information measurement.

[0056] And use the location information of the last odometer detection of the standby instance thread, the location information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight to perform polynomial trajectory modeling;

[0057] Similarly, the three-dimensional position information of the external odometer data is obtained as (X odo , Y odo , Z odo ), the three-dimensional position state information estimated by the standby instance thread is The process of calculating the odometer position information of the standby instance thread is as follows:

[0058]

[0059] in, The X-axis position information estimated by the Kalman filter of the standby instance thread; The Y-axis position information estimated by the Kalman filter of the standby instance thread; The Z-axis position information estimated by the Kalman filter of the standby instance thread; The X-axis component of the instance thread odometer position update; The Y-axis component of the instance thread odometer position update; The Z-axis component of the instance thread odometer position update;

[0060] The formula for normalizing the standby instance thread odometer position updates is:

[0061]

[0062] in, The X-axis component of the novelty variance of the odometer position of the standby instance thread; The Y-axis component of the odometer position innovation variance of the standby instance thread; The Z-axis component of the instance thread odometer position innovation variance; The X-axis component of the standardized standby instance thread odometer position update; The Y-axis component of the standardized standby instance thread odometer position update; The Z-axis component of the standardized standby instance thread odometer position update;

[0063] The formula for determining whether to perform an update of the standby instance thread odometer new information measurement is:

[0064]

[0065] Among them, Γ s is the update flag for the odometer information measurement of the standby instance thread; if one of the conditions in the formula is not satisfied, then Γ s If the flag is false, the standby instance thread does not use the odometer data for measurement update and records the polynomial trajectory point generation time t (relative to the time when the standby instance thread last passed the odometer update detection); otherwise, the flag is true and measurement update is performed.

[0066] Record the location information of the last odometer detection of the standby instance thread; specifically including: the X, Y and Z components of the location of the last odometer detection of the standby instance thread and The X, Y, and Z components of velocity information The X, Y, and Z components of the acceleration information and

[0067] In the process of polynomial trajectory modeling, the location information of the last odometer detection of the standby instance thread, the location information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight are used:

[0068] Determine the flight time T from the current position to the target waypoint:

[0069]

[0070] in, The X-axis component of the last odometer position information of the standby instance thread; The Y-axis component of the last odometer position information of the standby instance thread; The Z-axis component of the last odometer position information of the standby instance thread; V max Information on the cruise speed set for route flight; The X-axis component of the position information of the next target waypoint i to be reached in the route mission; The Y-axis component of the position information of the next target waypoint i to be reached in the route mission; The Z-axis component of the position information of the next target waypoint i to be reached in the route mission;

[0071] Construct the mapping matrix from the location where the standby instance thread last detected the odometer update to the target waypoint i:

[0072]

[0073] Construct linear equations for the three components X, Y, and Z at time T:

[0074]

[0075] in,

[0076]

[0077] Among them, C X is the X-axis component of the polynomial trajectory coefficient; C Y is the Y-axis component of the polynomial trajectory coefficient, C Z is the Z-axis component of the polynomial trajectory coefficient, and the vector size is 6×1;

[0078] By solving the linear equations of the three components X, Y, and Z at time T, we can get C X , C Y and C Z ,,Complete the polynomial trajectory modeling of the candidate instance thread relative to the position passed by the last odometer innovation detection to the waypoint i;

[0079] If the odometer information measurement of the standby instance thread does not need to be updated, the polynomial trajectory point generation time is recorded, and the polynomial trajectory point generation time is substituted into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update;

[0080] Substituting the polynomial trajectory point generation time into the polynomial trajectory model, the fitted odometer position information is as follows:

[0081]

[0082] Where A(t) = [t 5 t 4 t 3 t 2 t 1]; is the X-axis component of the fitted odometer position information, is the Y-axis component of the fitted odometer position information; is the Z-axis component of the fitted odometer position information.

[0083] The odometer position information of the candidate instance thread at the current moment of the polynomial fitting Perform new information detection. If the detection passes, use the fitted odometer position information for measurement update.

[0084] The master and standby instance threads are switched according to the master instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

[0085] Select the thread for the EKF instance and determine the odometer information measurement update flag of the main instance thread and the odometer information measurement update flag of the standby instance thread. m If the time exceeds the time threshold δ, it will be false. Then switch to the standby instance thread for estimation, which is used as the final location information of the limited space embodied intelligent drone. If during the use of the standby instance thread, the main instance thread odometer information measurement update flag Γ m If the time threshold δ is exceeded, it is always true, that is, This indicates that the external odometer data has returned to normal, and the thread is switched from the standby instance to the main instance.

[0086] The process ends.

[0087] Embodiment 1 of the present invention proposes a method for constraining the position of an embodied intelligent drone in a limited space. By introducing two parallel and independent running threads, the main instance thread and the alternative instance thread of state estimation, the redundancy performance of the system is significantly enhanced. Once the main instance thread encounters a failure, it can be seamlessly switched to the alternative instance thread immediately, thereby ensuring the continuous and effective constraint on the drone position.

[0088] Embodiment 1 of the present invention proposes a method for position constraint of an embodied intelligent drone in a limited space, in which the main instance thread focuses on executing the new information detection task and does not introduce a polynomial fitting control strategy, thereby ensuring that when the external odometer data returns to normal, the system can smoothly switch back to the main instance thread, ensuring that the state estimation process is not interfered by the polynomial fitting control strategy, thereby maintaining the consistency and accuracy of the main instance thread in state measurement updates.

[0089] Embodiment 1 of the present invention proposes a state estimation alternative instance thread in a limited space embodied intelligent drone position constraint method that incorporates a polynomial fitting control strategy, which is intended to deal with the situation where the external odometer data diverges or jumps and cannot be used for measurement updates. At this time, the alternative instance thread will use the polynomial trajectory model to accurately fit the odometer position information that meets the requirements, thereby replacing the abnormal external odometer data and performing measurement updates, thereby ensuring that the alternative instance thread can maintain a high degree of stability when performing position estimation.

[0090] Embodiment 1 of the present invention proposes a polynomial trajectory model in a limited space embodied intelligent drone position constraint method, which deeply considers the application of 5th-order polynomials, that is, it incorporates the snap-level analysis, aiming to ensure that the constructed trajectory model can show higher smoothness and better fit the actual situation of route flight.

[0091] Example 2

[0092] Based on the limited space embodied intelligent drone position constraint method proposed in Example 1 of the present invention, Example 2 of the present invention further proposes a limited space embodied intelligent drone position constraint system. Figure 3 This is a schematic diagram of a limited space embodied intelligent drone position constraint system proposed in Example 2 of the present invention; the system includes: a thread construction module, a model construction module, an update module and a switching module.

[0093] The thread construction module is used to construct the main and standby instance threads and the standby instance threads for UAV flight control; determine the main and standby instance thread odometer position updates according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main and standby instance threads; after standardizing the main and standby instance thread odometer position updates, determine whether to perform odometer information measurement update of the main and standby instance threads;

[0094] The model building module is used to record the position information of the last odometer detection of the standby instance thread; the polynomial trajectory modeling is performed using the position information of the last odometer detection of the standby instance thread, the position information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight;

[0095] The update module is used for recording the generation time of the polynomial trajectory point if the odometer information measurement of the standby instance thread does not need to be updated, and substituting the generation time of the polynomial trajectory point into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update;

[0096] The switching module is used to switch the main instance thread to the standby instance thread according to the main instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

[0097] During the execution of the thread construction module: the main instance thread odometer position update is determined based on the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main instance thread; after standardizing the main instance thread odometer position update, it is determined whether to perform the main instance thread odometer information measurement update; the standby instance thread odometer position update is determined based on the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the standby instance thread; after standardizing the standby instance thread odometer position update, it is determined whether to perform the standby instance thread odometer information measurement update.

[0098] The three-dimensional position information of the external odometer data is (X odo , Y odo , Z odo ); the three-dimensional position state information estimated by the main instance thread is The main instance thread odometer position update calculation process is:

[0099]

[0100] Among them, X odo is the X-axis position information of the external odometer data; Y odo The Y-axis position information of the external odometer data; Z odo It is the Z-axis position information of the external odometer data; The X-axis position information estimated by the Kalman filter of the main instance thread; Y-axis position information estimated by Kalman filter of the main instance thread; The Z-axis position information estimated by the Kalman filter of the main instance thread; The X-axis component of the main instance thread odometer position update; The Y-axis component of the main instance thread odometer position update; The Z-axis component of the main instance thread odometer position update;

[0101] The formula for normalizing the main instance thread odometer position updates is:

[0102]

[0103] in, The X-axis component of the main instance thread odometer position innovation variance; The Y-axis component of the main instance thread odometer position innovation variance; The Z-axis component of the main instance thread odometer position innovation variance; The X-axis component of the normalized main instance thread odometer position update; The Y-axis component of the normalized main instance thread odometer position update; The Z-axis component of the normalized main instance thread odometer position update;

[0104] The formula for determining whether to update the odometer measurement of the main instance thread is:

[0105]

[0106] Where λ is the limit value, expressed as λ times the standard deviation; Γ m is the main instance thread odometer information measurement update flag; if one of the conditions in the formula is not satisfied, then Γ m If the flag is false, the main instance thread does not use the odometer data for measurement updates and accumulates the continuous failure time. Otherwise, the flag is true, and the main instance thread uses the odometer data to measure and update, and accumulates the continuous success time

[0107] Determine the standby instance thread odometer position update information based on the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the standby instance thread; wherein the three-dimensional position state information estimated by the standby instance thread is The process of calculating the odometer position information of the standby instance thread is as follows:

[0108]

[0109] in, The X-axis position information estimated by the Kalman filter of the standby instance thread; The Y-axis position information estimated by the Kalman filter of the standby instance thread; The Z-axis position information estimated by the Kalman filter of the standby instance thread; The X-axis component of the instance thread odometer position update; The Y-axis component of the instance thread odometer position update; The Z-axis component of the instance thread odometer position update;

[0110] The formula for normalizing the standby instance thread odometer position updates is:

[0111]

[0112] in, The X-axis component of the novelty variance of the odometer position of the standby instance thread; The Y-axis component of the novelty variance of the odometer position of the standby instance thread; The Z-axis component of the instance thread odometer position innovation variance; The X-axis component of the standardized standby instance thread odometer position update; is the Y-axis component of the innovation of the odometer position of the spare instance thread for standardization; is the Z-axis component of the innovation of the odometer position of the spare instance thread for standardization;

[0113] The formula for determining whether to perform the measurement update of the innovation of the odometer of the spare instance thread is:

[0114]

[0115] where, Γ s is the flag for the measurement update of the odometer information of the spare instance thread; if one of the conditions in this formula is not met, then Γ s is flagged as false, and the spare instance thread does not use the odometer data for measurement update, and records the generation time t of the polynomial trajectory point; otherwise, it is flagged as true and the measurement update is performed.

[0116] The process executed by the model construction module includes: The position information where the last odometer detection of the spare instance thread passed includes: the X, Y, and Z components of the position where the last odometer detection of the spare instance thread passed The X, Y, and Z components of the speed information The X, Y, and Z components of the acceleration information

[0117] The process of performing polynomial trajectory modeling includes: determining the flight time T from the current position to the target waypoint:

[0118]

[0119] where, is the X-axis component of the position information of the last odometer of the spare instance thread; is the Y-axis component of the position information of the last odometer of the spare instance thread; is the Z-axis component of the position information of the last odometer of the spare instance thread; V max is the cruise speed information set for the route flight; is the X-axis component of the position information of the next target waypoint i in the route mission; is the Y-axis component of the position information of the next target waypoint i in the route mission; is the Z-axis component of the position information of the next target waypoint i in the route mission;

[0120] Construct the mapping matrix from the position where the last innovation detection of the odometer of the spare instance thread passed to the target waypoint i:

[0121]

[0122] Construct linear equations for the X, Y, and Z components at time T respectively:

[0123]

[0124] in,

[0125]

[0126] Among them, C X is the X-axis component of the polynomial trajectory coefficient; C Y is the Y-axis component of the polynomial trajectory coefficient, C Z is the Z-axis component of the polynomial trajectory coefficient;

[0127] By solving the linear equations of the three components X, Y, and Z at time T, we can get C X , C Y and C Z Complete polynomial trajectory modeling.

[0128] During the execution of the update module, the polynomial trajectory point generation time is substituted into the polynomial trajectory model to obtain the fitted odometer position information:

[0129]

[0130] Where A(t) = [t 5 t 4 t 3 t 2 t 1]; is the X-axis component of the fitted odometer position information, is the Y-axis component of the fitted odometer position information; is the Z-axis component of the fitted odometer position information.

[0131] The process of switching module execution includes: if the main instance thread odometer information measurement update flag Γ m If the time exceeds the time threshold δ, it will be false. Then switch to the standby instance thread for estimation;

[0132] If during the use of the standby instance thread, the main instance thread odometer information measurement update flag Γ m If the time threshold δ is exceeded, it is always true, that is, The thread of the standby instance is switched to the thread of the main instance.

[0133] Embodiment 2 of the present invention proposes a limited space embodied intelligent drone position constraint system, which significantly enhances the redundancy of the system by introducing two parallel and independent running threads, the main instance thread and the alternative instance thread of state estimation. Once the main instance thread encounters a failure, it can immediately and seamlessly switch to the alternative instance thread, thereby ensuring the continuous and effective constraint on the drone position.

[0134] Embodiment 2 of the present invention proposes a limited space embodied intelligent drone position constraint system in which the main instance thread focuses on executing new information detection tasks and does not introduce a polynomial fitting control strategy, thereby ensuring that when the external odometer data returns to normal, the system can smoothly switch back to the main instance thread, ensuring that the state estimation process is not interfered with by the polynomial fitting control strategy, thereby maintaining the consistency and accuracy of the main instance thread in state measurement updates.

[0135] Embodiment 2 of the present invention proposes a state estimation alternative instance thread in a limited space embodied intelligent drone position constraint system that incorporates a polynomial fitting control strategy, which is intended to deal with the situation where the external odometer data diverges or jumps and cannot be used for measurement updates. At this time, the alternative instance thread will use the polynomial trajectory model to accurately fit the odometer position information that meets the requirements, thereby replacing the abnormal external odometer data and performing measurement updates, thereby ensuring that the alternative instance thread can maintain a high degree of stability when performing position estimation.

[0136] Embodiment 2 of the present invention proposes a polynomial trajectory model in a limited space embodied intelligent drone position constraint system, which deeply considers the application of 5th-order polynomials, that is, it incorporates the snap-level analysis, aiming to ensure that the constructed trajectory model can show higher smoothness and better fit the actual situation of route flight.

[0137] Example 3

[0138] The present invention also proposes a device, Figure 4 This is a schematic diagram of a limited space embodied intelligent drone position constraint device proposed in Example 3 of the present invention, the device comprising:

[0139] Memory for storing computer programs;

[0140] The processor is used to implement the following method steps when executing the computer program:

[0141] In step S100, the process starts.

[0142] In step S110, external odometer data released by SLAM is obtained;

[0143] In step S120, a main instance thread for drone flight control is constructed, specifically: determining the main instance thread odometer position update information according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main instance thread; after standardizing the main instance thread odometer position update information, determining whether to perform the main instance thread odometer information measurement update;

[0144] In step S130, a standby instance thread for UAV flight control is constructed, specifically: determining the standby instance thread odometer position update according to the acquired external odometer data and the three-dimensional position state information estimated by the standby instance thread Kalman filter; after standardizing the standby instance thread odometer position update, determining whether to perform standby instance thread odometer information measurement update.

[0145] And use the location information of the last odometer detection of the standby instance thread, the location information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight to perform polynomial trajectory modeling;

[0146] If the odometer information measurement of the standby instance thread does not need to be updated, the polynomial trajectory point generation time is recorded, and the polynomial trajectory point generation time is substituted into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update;

[0147] In step S140, the master instance thread and the standby instance thread are switched according to the master instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

[0148] In step S150, the process ends.

[0149] Embodiment 3 of the present invention proposes a limited space embodied intelligent drone position constraint device, which significantly enhances the redundancy of the system by introducing two parallel and independent running threads, the main instance thread and the alternative instance thread of state estimation. Once the main instance thread encounters a failure, it can immediately and seamlessly switch to the alternative instance thread, thereby ensuring the continuous and effective constraint on the drone position.

[0150] Embodiment 3 of the present invention proposes a limited space embodied intelligent drone position constraint device in which the main instance thread focuses on executing new information detection tasks and does not introduce a polynomial fitting control strategy, thereby ensuring that when the external odometer data returns to normal, the system can smoothly switch back to the main instance thread, ensuring that the state estimation process is not interfered by the polynomial fitting control strategy, thereby maintaining the consistency and accuracy of the main instance thread in state measurement updates.

[0151] Embodiment 3 of the present invention proposes a state estimation alternative instance thread in a limited space embodied intelligent drone position constraint device that incorporates a polynomial fitting control strategy, which is intended to deal with the situation where the external odometer data diverges or jumps and cannot be used for measurement updates. At this time, the alternative instance thread will use the polynomial trajectory model to accurately fit the odometer position information that meets the requirements, thereby replacing the abnormal external odometer data and performing measurement updates, thereby ensuring that the alternative instance thread can maintain a high degree of stability when performing position estimation.

[0152] Embodiment 3 of the present invention proposes a polynomial trajectory model in a limited space embodied intelligent drone position constraint device, which deeply considers the application of 5th-order polynomials, that is, it incorporates the snap-level analysis, aiming to ensure that the constructed trajectory model can show higher smoothness and better fit the actual situation of route flight.

[0153] For the description of the relevant parts of a limited space embodied intelligent drone position constraint device provided in Example 3 of the present application, please refer to the detailed description of the corresponding parts of a limited space embodied intelligent drone position constraint method provided in Example 1 of the present application, and will not be repeated here.

[0154] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "include", "comprise" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment that includes a series of elements are inherent to the elements. In the absence of more restrictions, the elements limited by the sentence "comprise one..." do not exclude the presence of other identical elements in the process, method, article or equipment that includes the elements. In addition, the above-mentioned technical solution provided in the embodiment of the present application is consistent with the corresponding technical solution in the prior art in principle, and the part is not described in detail, so as not to repeat too much.

[0155] Although the above describes the specific implementation of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. For those skilled in the art, other different forms of modifications or deformations can be made on the basis of the above description. It is not necessary and impossible to list all the implementation methods here. On the basis of the technical solution of the present invention, various modifications or deformations that can be made by those skilled in the art without creative work are still within the scope of protection of the present invention.

Claims

1. A method for position constraint of embodied intelligent drone in limited space, characterized in that: The following steps are involved: Construct the master and standby instance threads for UAV flight control; determine the master and standby instance thread odometer position updates based on the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the master and standby instance threads; After standardizing the odometer position updates of the master and standby instance threads, determining whether to perform odometer information measurement update of the master and standby instance threads; Record the location information of the last odometer detection of the standby instance thread; use the location information of the last odometer detection of the standby instance thread, the location information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight to perform polynomial trajectory modeling; If the odometer information measurement of the standby instance thread does not need to be updated, the polynomial trajectory point generation time is recorded, and the polynomial trajectory point generation time is substituted into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update; The master and standby instance threads are switched according to the master instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

2. A method for position constraint of an embodied intelligent drone in a limited space according to claim 1, characterized in that: Determining the new odometer position information of the master and standby instance threads according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the master and standby instance threads; After the odometer position updates of the master and standby instance threads are standardized, determining whether to perform odometer information measurement update of the master and standby instance threads specifically includes: Determine the main instance thread odometer position update information according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main instance thread; after standardizing the main instance thread odometer position update information, determine whether to perform the main instance thread odometer information measurement update; Determine the standby instance thread odometer position update according to the acquired external odometer data and the three-dimensional position state information estimated by the standby instance thread Kalman filter; after standardizing the standby instance thread odometer position update, determine whether to perform standby instance thread odometer information measurement update.

3. A method for position constraint of an embodied intelligent drone in a limited space according to claim 2, characterized in that: The main instance thread odometer position update information is determined according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main instance thread; wherein the three-dimensional position information of the external odometer data is (X odo , Y odo , Z odo ); the three-dimensional position state information estimated by the main instance thread is The main instance thread odometer position update calculation process is: Among them, X odo is the X-axis position information of the external odometer data; Y odo is the Y-axis position information of the external odometer data; Z odo It is the Z-axis position information of the external odometer data; The X-axis position information estimated by the Kalman filter of the main instance thread; Y-axis position information estimated by Kalman filter of the main instance thread; The Z-axis position information estimated by the Kalman filter of the main instance thread; The X-axis component of the main instance thread odometer position update; The Y-axis component of the main instance thread odometer position update; The Z-axis component of the main instance thread odometer position update; The formula for normalizing the main instance thread odometer position updates is: in, The X-axis component of the main instance thread odometer position innovation variance; The Y-axis component of the main instance thread odometer position innovation variance; The Z-axis component of the main instance thread odometer position innovation variance; The X-axis component of the normalized main instance thread odometer position update; The Y-axis component of the normalized main instance thread odometer position update; The Z-axis component of the normalized main instance thread odometer position update; The formula for determining whether to update the odometer information measurement of the main instance thread is: Where λ is the limit value, expressed as λ times the standard deviation; Γ m is the main instance thread odometer information measurement update flag; if one of the conditions in the formula is not satisfied, then Γ m If the flag is false, the main instance thread does not use the odometer data for measurement updates and accumulates the continuous failure time. Otherwise, the flag is true, and the main instance thread uses the odometer data to measure and update, and accumulates the continuous success time 4. A method for position constraint of a limited space embodied intelligent drone according to claim 3, characterized in that: The method determines the standby instance thread odometer position update information according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the standby instance thread; wherein the three-dimensional position state information estimated by the standby instance thread is The process of calculating the odometer position information of the standby instance thread is as follows: in, The X-axis position information estimated by the Kalman filter of the standby instance thread; The Y-axis position information estimated by the Kalman filter of the standby instance thread; The Z-axis position information estimated by the Kalman filter of the standby instance thread; The X-axis component of the instance thread odometer position update; The Y-axis component of the instance thread odometer position update; The Z-axis component of the instance thread odometer position update; The formula for normalizing the standby instance thread odometer position updates is: in, The X-axis component of the novelty variance of the odometer position of the standby instance thread; The Y-axis component of the odometer position innovation variance of the standby instance thread; The Z-axis component of the instance thread odometer position innovation variance; The X-axis component of the standardized standby instance thread odometer position update; The Y-axis component of the standardized standby instance thread odometer position update; The Z-axis component of the standardized standby instance thread odometer position update; The formula for determining whether to update the odometer information measurement of the standby instance thread is: Among them, Γ s is the update flag for the odometer information measurement of the standby instance thread; if one of the conditions in the formula is not satisfied, then Γ s If the flag is false, the standby instance thread does not use the odometer data for measurement update and records the polynomial trajectory point generation time t; otherwise, the flag is true and measurement update is performed.

5. A method for position constraint of a limited space embodied intelligent drone according to claim 4, characterized in that: The location information of the last odometer detection of the standby instance thread includes: the X, Y and Z components of the location of the last odometer detection of the standby instance thread and The X, Y, and Z components of velocity information and The X, Y, and Z components of the acceleration information and 6. A method for position constraint of an embodied intelligent drone in a limited space according to claim 5, characterized in that: The process of performing polynomial trajectory modeling includes: Determine the flight time T from the current position to the target waypoint: in, The X-axis component of the last odometer position information of the standby instance thread; The Y-axis component of the last odometer position information of the standby instance thread; The Z-axis component of the last odometer position information of the standby instance thread; V max Information on the cruise speed set for route flight; The X-axis component of the position information of the next target waypoint i to be reached in the route mission; The Y-axis component of the position information of the next target waypoint i to be reached in the route mission; The Z-axis component of the position information of the next target waypoint i to be reached in the route mission; Construct the mapping matrix from the location where the standby instance thread last detected the odometer update to the target waypoint i: Construct linear equations for the three components X, Y, and Z at time T: A(T)·C X =B X (T) A(T)·C Y =B Y (T); A(T)·C Z =B Z (T) in, Among them, C X is the X-axis component of the polynomial trajectory coefficient; C Y is the Y-axis component of the polynomial trajectory coefficient, C Z is the Z-axis component of the polynomial trajectory coefficient; By solving the linear equations of the three components X, Y, and Z at time T, we can get C X , C Y and C Z Complete polynomial trajectory modeling.

7. A method for position constraint of a limited space embodied intelligent drone according to claim 6, characterized in that: Substituting the polynomial trajectory point generation time into the polynomial trajectory model to obtain the fitted odometer position information is specifically: Where A(t) = [t 5 t 4 t 3 t 2 t 1]; is the X-axis component of the fitted odometer position information, is the Y-axis component of the fitted odometer position information; is the Z-axis component of the fitted odometer position information.

8. A method for position constraint of a limited space embodied intelligent drone according to claim 4, characterized in that: The process of switching the master and standby instance threads according to the master instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag comprises: If the main instance thread odometer information measurement update flag Γ m If the time exceeds the time threshold δ, it will be false. Then switch to the standby instance thread for estimation; If during the use of the standby instance thread, the main instance thread odometer information measurement update flag Γ m If the time threshold δ is exceeded, it is always true, that is, The thread of the standby instance is switched to the thread of the main instance.

9. A limited space embodied intelligent drone position constraint system, characterized in that: include: Thread building module, model building module, update module and switch module; The thread construction module is used to construct the main and standby instance threads and the standby instance threads for UAV flight control; determine the main and standby instance thread odometer position updates according to the acquired external odometer data and the three-dimensional position state information estimated by the Kalman filter of the main and standby instance threads; After standardizing the odometer position updates of the master and standby instance threads, determining whether to perform odometer information measurement update of the master and standby instance threads; The model building module is used to record the position information of the last odometer detection of the standby instance thread; use the position information of the last odometer detection of the standby instance thread, the position information of the next target waypoint to be reached in the route task, and the cruising speed information set for the route flight to perform polynomial trajectory modeling; The update module is used for recording the generation time of the polynomial trajectory point if the odometer information measurement of the standby instance thread does not need to be updated, and substituting the generation time of the polynomial trajectory point into the polynomial trajectory model to obtain the fitted odometer position information; if the fitted odometer position information passes the test, the fitted odometer position information is used for measurement update; The switching module is used to switch the main instance thread to the standby instance thread according to the main instance thread odometer information measurement update flag and the standby instance thread odometer information measurement update flag.

10. A limited space embodied intelligent drone position constraint device, characterized in that: include: Memory for storing computer programs; A processor, configured to implement the method steps according to any one of claims 1 to 8 when executing the computer program.

Images