Airborne closed cabin vehicle path dynamic navigation planning method

Abstract

The invention relates to the technical field of vehicle navigation, and discloses an airborne closed cabin vehicle path dynamic navigation planning method, which comprises the following steps: S1, navigation initialization, S2, real-time perception data acquisition and correction, and upward navigation reference based on a target established in the step S1, S3, map-free error correction and fusion, S4, dynamic path planning, and S5, accurate correction before landing. And S6, outputting a navigation control instruction based on the real-time navigation track in the step S4. Complete autonomous navigation of an airborne closed cabin vehicle in the whole process is achieved, dependence on external visual reference, a pre-built map or a ground guide signal is not needed, an absolute direction anchor point is provided through a target upward navigation benchmark, multi-source sensor decoupling correction and adaptive Kalman filtering fusion are combined, airflow disturbance compensation is synchronously brought in, and the navigation precision is improved. The inertial navigation accumulative error is effectively eliminated, the method can be adapted to an extreme airborne environment without reference, map and signal, and the independence and reliability of the navigation process are guaranteed.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of vehicle navigation, in particular to a dynamic navigation planning method for airdrop closed cabin vehicle path. BACKGROUND

[0002] The airdrop closed cabin vehicle navigation technology is a key technology to ensure that the vehicle can clearly show the position and direction after being dropped from high altitude, and is mainly applied to military equipment airdrop, remote area emergency rescue material delivery and other scenes. The core requirement is to efficiently express the direction and position on the in-vehicle display interface under the condition of full closure of the cabin and no visual reference of the external environment, and then realize the whole-process autonomous path planning and attitude control of the vehicle to ensure that the landing position and heading accuracy meet the task requirements.

[0003] In the prior art, for the vehicle navigation in a closed space or a dynamic scene, a mode of sensor perception combined with external information assistance is generally adopted: the ground closed environment relies on the pre-built environment map combined with inertial navigation to correct errors; the dynamic airdrop scene mainly relies on the positioning signal of the carrier, the airflow data transmitted by the ground weather station, or the ground base station signal temporarily established after airdrop, to assist the inertial navigation system to suppress errors, and some schemes also need to rely on visual sensors to capture external features to realize path calibration.

[0004] At the same time, the existing navigation direction expression mainly adopts two ways of "north up" or "I am up".

[0005] However, the existing technology has core defects in the airdrop closed cabin vehicle scene: it generally relies on external reference, pre-built map or ground / carrier auxiliary signal. Once these external conditions are removed, only the inertial navigation system cannot cope with the airflow disturbance in the airdrop process, and it is easy to produce significant position and attitude cumulative error, resulting in continuous decline of navigation accuracy, which cannot meet the high-precision navigation requirements of closed cabin vehicles without external assistance and full autonomy.

[0006] At the same time, due to the airdrop process and the no-reference ground environment, both "north up" and "I am up" ways produce a large cognitive load for the vehicle passengers to judge the direction of travel. SUMMARY

[0007] In view of the deficiencies of the prior art, the present application provides a dynamic navigation planning method for airdrop closed cabin vehicle path, solves the problem of autonomous high-precision navigation of airdrop closed cabin vehicle without external assistance, and provides a new navigation direction expression method, so that the vehicle passengers can more intuitively judge the direction of travel.

[0008] To achieve the above purpose, the present application realizes the following technical scheme: a dynamic navigation planning method for airdrop closed cabin vehicle path, comprising the following steps: S1, navigation initialization: obtain the initial geographic coordinates of the vehicle air drop and the geographic coordinates of the target point, establish a target upward navigation reference based on the initial geographic coordinates and the geomagnetic sensor data, and provide a directional reference for subsequent data collection and path planning; S2, real-time sensing data collection and correction: based on the target upward navigation reference established in step S1, continuously collect vehicle motion state data and environmental reference data through the vehicle-mounted multi-source sensor, and simultaneously perform decoupling correction of the vehicle body posture and the sensor data to obtain corrected sensing data; the motion state data includes three-axis acceleration, three-axis angular velocity and attitude angle, and the environmental reference data includes absolute heading angle and real-time height; S3, no map error correction and fusion: based on the corrected sensing data obtained in step S2, multi-source fusion processing is performed using an adaptive Kalman filter algorithm; the adaptive Kalman filter realizes by constructing a state equation containing position, velocity, attitude and sensor zero offset, and dynamically adjusting the filter noise covariance based on real-time observation residual; during the multi-source fusion processing, air flow disturbance compensation is simultaneously performed, and finally the state data corrected for eliminating the cumulative error of inertial navigation is obtained; S4, dynamic path planning: based on the initial geographic coordinates of step S1, the geographic coordinates of the target point and the corrected state data of step S3, hierarchical path planning is performed to generate a real-time navigation trajectory; S5, accurate correction before landing: based on the real-time navigation trajectory of step S4, the vehicle height is monitored by a barometer; when the vehicle height is lower than the preset height threshold, the update frequency of the multi-source fusion data in step S3 is increased, and the deviation threshold of the local path adjustment in step S4 is reduced, the heading angle correction accuracy is strengthened, and the landing position calibration is realized; S6, navigation control instruction output: based on the calibrated real-time navigation trajectory of step S5, vehicle attitude adjustment instructions and motion control instructions are converted and generated to guide the vehicle to move towards the target point.

[0009] Preferably, the establishment process of the target upward navigation reference in step S1 includes: acquiring initial geomagnetic components through a geomagnetic sensor, calibrating combined with the magnetic declination data in the initial geographic coordinates to determine the absolute heading reference pointing to the geographic north pole; calculating the target azimuth angle based on the initial geographic coordinates and the geographic coordinates of the target point, establishing a navigation coordinate system with the initial point as the origin, the direction pointing to the target point as the X axis, the horizontal tangent perpendicular to the X axis as the Y axis, and the sky direction as the Z axis, and taking the north direction of the horizontal plane as an auxiliary direction.

[0010] Preferably, the vehicle-mounted multi-source sensor in step S2 includes a three-axis inertial measurement unit, a three-axis geomagnetic sensor, and a barometer; the three-axis inertial measurement unit collects three-axis acceleration and three-axis angular velocity data, the three-axis geomagnetic sensor collects absolute heading angle data, and the barometer collects real-time height data; the decoupling correction process of the vehicle body posture and the sensor data includes: establishing a bias model through the pitch angle and the roll angle collected by the inertial measurement unit, and correcting the original acceleration data and the original geomagnetic data based on the bias model; the corrected acceleration data is obtained through the product of the cosine values of the pitch angle and the roll angle, and the corrected geomagnetic data is obtained through the product difference of the absolute values of the pitch angle and the roll angle and a preset coefficient; the sensor data is synchronized at a microsecond level through a phase-locked loop.

[0011] Preferably, the layered path planning in step S4 includes global path generation and local path adjustment; the global path generation generates a straight reference trajectory in the target upward navigation coordinate system established in step S1 based on the initial geographic coordinates and the target point geographic coordinates in step S1; the local path adjustment calculates the position deviation of the vehicle position in the corrected state data in step S3 from the global path, and the attitude deviation of the vehicle attitude angle from the preset attitude angle, and dynamically adjusts the local trajectory by correcting the heading angle and the speed component when any deviation exceeds a preset threshold.

[0012] Preferably, the position deviation threshold is dynamically configured according to the air drop height, the position deviation threshold is set to 3-5 meters in the high-altitude air drop stage, and the position deviation threshold is set to 1-2 meters in the low-altitude air drop stage; the attitude deviation threshold is set to 3-5 degrees, and the local path adjustment is triggered when the deviation exceeds the threshold for 3 sampling periods.

[0013] Preferably, the air flow disturbance compensation process in step S3 includes: calculating the disturbance acceleration from the residual of the actual acceleration collected by the inertial measurement unit, the theoretical motion acceleration, and the gravity acceleration, incorporating the disturbance acceleration into the state equation of the adaptive Kalman filter, and correcting the influence of the air flow on the vehicle motion state; the multi-source fusion processing in step S3 also includes dynamic weight distribution of the sensors, and the distribution process adjusts the weight proportion of each sensor according to the environmental disturbance intensity in the air drop stage; the weight proportion of the inertial measurement unit is increased to adapt to strong air flow disturbance in the high-altitude air drop stage, the weight proportions of the inertial measurement unit and the geomagnetic sensor are balanced to take into account dynamic response and heading accuracy in the low-altitude air drop stage, and the weight proportion of the geomagnetic sensor is increased to strengthen the heading calibration accuracy before landing.

[0014] Preferably, the navigation data update frequency of steps S2 to S4 is not less than 100 Hz, the state update period of the adaptive Kalman filter is consistent with the sensor data collection period, and the real-time collected data of the sensors is processed synchronously.

[0015] Preferably, in step S5, the preset height threshold is set to 50-100 meters, and when the vehicle height is lower than the threshold, the update frequency of the multi-source fusion data is increased to 200 Hz or more to increase the data processing density; the sensor dynamic weight distribution is simultaneously performed in the pre-landing accurate correction stage, the sensor data fusion priority is adjusted by increasing the weight proportion of the geomagnetic sensor, and the vehicle landing position is calibrated by the heading angle correction action.

[0016] The present application provides a method for dynamic navigation planning of airdrop closed cabin vehicle path. The method has the following advantages: 1. The present application realizes complete autonomous navigation of airdrop closed cabin vehicles throughout the journey, without relying on external visual reference, pre-built maps or ground guidance signals. The method provides absolute direction anchor points through target upward navigation reference, assisted by horizontal plane north direction indication, combines multi-source sensor decoupling correction and adaptive Kalman filter fusion, and synchronously incorporates airflow disturbance compensation, effectively eliminating inertial navigation cumulative error, and can adapt to extreme airdrop environments without reference, map or signal, ensuring the independence and reliability of the navigation process, and providing a new navigation direction expression method, enabling vehicle passengers to more intuitively determine the direction of travel.

[0017] 2. The present application meets the navigation accuracy requirements of different airdrop stages through hierarchical path planning and dynamic parameter adjustment. In the high-altitude stage, emphasis is placed on resisting airflow interference, in the low-altitude stage, dynamic response and heading accuracy are balanced, and in the pre-landing stage, accurate correction is strengthened. The weight distribution, deviation threshold and data update frequency of each stage are adapted as needed to avoid the problem that a single parameter cannot cover the entire airdrop process, ensuring stable navigation accuracy throughout the high-altitude drop and landing.

[0018] 3. The present application adapts to different environmental interference intensities through sensor dynamic weight distribution, realizes microsecond-level synchronization of multi-source data through phase-locked loop, controls the matching of data update frequency and filtering period, and ensures that sensor data can still be stably fused under complex conditions such as airflow disturbance and attitude fluctuation, providing accurate and real-time input for path planning and attitude adjustment, and ensuring that the vehicle always maintains a stable navigation state in a dynamically changing airdrop environment. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 The flowchart of the present application. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be described in detail below with reference to the drawings of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0021] The embodiments of the present application provide a dynamic navigation planning method for airdrop cabin vehicle path. Figure 1 The embodiments of the present application provide a dynamic navigation planning method for airdrop cabin vehicle path.

[0022] The simulation application object is a fully enclosed cabin airdrop vehicle, the initial airdrop height is set to 800 meters, the horizontal distance between the target point and the initial airdrop point is 3 kilometers, the vehicle-mounted sensor module is integrated in the front electronic cabin of the vehicle, and each module realizes data interaction through vehicle-mounted Ethernet.

[0023] S1, navigation initialization: The navigation initialization is executed by the aircraft navigation system and the vehicle-mounted navigation unit 30 seconds before airdrop, including: Initial geographic coordinate and target point coordinate acquisition: The initial geographic coordinate of the vehicle airdrop is acquired through the high-precision positioning module of the aircraft, and the geographic coordinate of the target point is imported into the vehicle-mounted storage unit through task preset, both types of coordinates are based on the geodetic coordinate system (WGS-84), and the positioning accuracy needs to meet the subsequent navigation calculation requirements.

[0024] Target upward navigation reference establishment: The initial geomagnetic component is collected by starting the three-axis geomagnetic sensor, the preset global magnetic declination database is called synchronously, the magnetic declination data in the region are extracted according to the initial geographic coordinate, the absolute heading reference pointing to the geographic north pole is determined through the calibration calculation of the geomagnetic component and the magnetic declination, then the target azimuth is calculated based on the initial geographic coordinate and the geographic coordinate of the target point, the X axis of the navigation coordinate system is rotated to coincide with the target azimuth through the coordinate rotation matrix, and the target upward navigation coordinate system is established: taking the initial point as the origin, the X axis points to the target point direction (i.e. the longitudinal direction), the Y axis is perpendicular to the target point direction (i.e. the transverse direction), the Z axis is perpendicular to the ground and upward, and the horizontal plane north direction is used as an auxiliary direction. The coordinate system directly takes the target line of sight as a reference, and the subsequent navigation calculation only needs to focus on the transverse deviation and the included angle of the vehicle relative to the X axis.

[0025] S2, real-time sensing data acquisition and correction: After the vehicle separates from the aircraft, the vehicle-mounted multi-source sensor module starts continuous data acquisition, and the acquisition period is set according to the navigation requirements to ensure the real-time data update, including: Vehicle-mounted multi-source sensor configuration: The sensors include a three-axis inertial measurement unit, a three-axis geomagnetic sensor and a barometer, the three-axis inertial measurement unit collects three-axis acceleration, three-axis angular velocity and attitude angle (roll angle, pitch angle and heading angle), the three-axis geomagnetic sensor collects absolute heading angle data, and the barometer collects real-time height data. The sensor type can be customized according to actual requirements, and needs to meet the anti-vibration and anti-impact performance requirements in the airdrop environment.

[0026] Sensor data synchronization: The phase-locked loop technology is used to realize synchronization of data of each sensor, and the synchronization error is controlled in the microsecond level, so as to ensure consistency of time stamps of data of each sensor and avoid fusion deviation caused by time difference.

[0027] Decoupling correction of vehicle body posture and sensor data: During the air drop process, the vehicle will produce roll and pitch, so that the original sensor data contains posture interference, and the interference needs to be eliminated through decoupling correction; The specific process is as follows: a deviation model is established by using the pitch angle and roll angle collected by the three-axis inertial measurement unit, and the original sensor data is corrected based on the model, wherein the corrected acceleration data is obtained by calculating the product of the cosine values of the pitch angle and the roll angle, and the corrected geomagnetic data is obtained by calculating the difference between the product of the absolute values of the pitch angle and the roll angle and a preset coefficient, the preset coefficient is determined according to the vehicle posture fluctuation range through the preliminary test, after the correction is completed, the corrected perception data which can be used for subsequent fusion processing is obtained and stored in the vehicle cache unit.

[0028] S3, no map error correction and fusion: Based on the corrected perception data, an adaptive Kalman filter algorithm is used to perform multi-source data fusion, and the filter period is consistent with the sensor collection period, including: State equation construction: The state vector is defined, the vector includes vehicle position (X-axis, Y-axis, Z-axis coordinates), velocity (X-axis, Y-axis, Z-axis velocity), attitude angle (roll angle, pitch angle, heading angle) and sensor zero offset (acceleration zero offset and angular velocity zero offset of the three-axis inertial measurement unit), according to the vehicle motion characteristics and sensor characteristics, the state equation is established, the equation includes state transition matrix, input matrix and process noise term, the state transition matrix is constructed based on the vehicle dynamics model, the input matrix takes the acceleration and angular velocity collected by the three-axis inertial measurement unit as input, and the initial variance of the process noise term is set according to the sensor accuracy.

[0029] Real-time observation residual adjusts filter noise covariance: An adaptive algorithm is used to estimate the process noise variance and observation noise variance in real time through the observation residual, the observation residual is the difference between the corrected perception data and the filter prediction value, when the observation residual exceeds the set threshold, the weight corresponding to the speed and attitude in the process noise variance is automatically adjusted to avoid filter divergence and ensure the accuracy of the fusion data.

[0030] Airflow disturbance compensation: The theoretical acceleration of the vehicle is calculated. The theoretical acceleration is determined based on the free fall model and includes the gravitational acceleration component. The residual between the actual acceleration collected by the three-axis inertial measurement unit and the theoretical acceleration and gravitational acceleration is used as the disturbance acceleration. The disturbance acceleration is incorporated into the state equation of the adaptive Kalman filter to correct the influence of airflow on the vehicle's motion state and reduce navigation errors caused by airflow disturbance.

[0031] Sensor dynamic weight allocation: The airdrop process is divided into different stages based on the airdrop altitude. The weight ratio of sensors in the fusion process is adjusted according to the intensity of environmental interference in each stage. In the high-altitude airdrop stage, the airflow interference is strong, so the weight ratio of the three-axis inertial measurement unit is increased to adapt to the strong interference. In the low-altitude airdrop stage, the airflow tends to be stable, so the weight ratio of the three-axis inertial measurement unit and the three-axis geomagnetic sensor is balanced to take into account dynamic response and heading accuracy. In the precise correction stage before landing, the weight ratio of the three-axis geomagnetic sensor is increased to enhance the heading calibration accuracy. The weight ratio is incorporated into the observation equation of the adaptive Kalman filter through the weighted least squares method to ensure that the fused data in different stages is optimal. After the fusion processing is completed, the corrected state data used to eliminate the cumulative error of inertial navigation is obtained.

[0032] In the dynamic weight allocation of sensors, the intensity of environmental interference is quantified and determined using acceleration data collected by the triaxial inertial measurement unit. The specific steps are as follows: Data sampling and preprocessing: Take triaxial acceleration data (ax, ay, az) within 5 consecutive sensor acquisition cycles (consistent with the sensor acquisition cycle; if the acquisition cycle is 10ms, then the time window is 50ms), and remove outliers that exceed the normal physical range (-10g to 10g, where g is the gravitational acceleration, taken as 9.81m / s²). Standard deviation calculation: Calculate the standard deviation σ of the preprocessed triaxial acceleration data. ax σ ay σ az The calculation formula is σ= , where x i For a single acceleration sample value, μ is the mean of the five sample values ​​for that axis. =5; Interference intensity level classification: Take σ ax σ ay σ az The maximum value among the three, σ max As a quantitative indicator of environmental disturbance intensity, it is calculated according to σ. max Numerical classification of interference levels: Strong interference: σ max >0.5m / s², corresponding to the high-altitude stage of airdrop altitude >500m; Moderate disturbance: 0.2 m / s² < σ max≤0.5m / s², corresponding to the low-altitude phase of airdrop altitude 100m < altitude ≤500m; Weak interference: σ max ≤0.2m / s² corresponds to the precise correction stage before landing when the airdrop height is ≤100m.

[0033] Based on the above interference intensity levels, the sensor weight ratio is calculated using a fixed model with a base weight and an interference level correction coefficient, and the specific rules are as follows: Basic weight setting: The basic weight ratio of the triaxial inertial measurement unit, triaxial magnetometer, and barometer is preset to 4:4:2, that is, the basic weight of the triaxial inertial measurement unit is 0.4, the basic weight of the triaxial magnetometer is 0.4, and the basic weight of the barometer is 0.2. Interference level correction factor configuration: Strong interference level (σ) max >0.5m / s², high-altitude stage): The weight of the triaxial inertial measurement unit is increased by 0.2 (correction factor +0.2), the weight of the triaxial geomagnetic sensor is decreased by 0.1 (correction factor -0.1), and the weight of the barometer is decreased by 0.1 (correction factor -0.1). After correction, the weight ratio is 0.6 for the triaxial inertial measurement unit, 0.3 for the triaxial geomagnetic sensor, and 0.1 for the barometer. Medium interference level (0.2m / s² < σ) max ≤0.5m / s², low-altitude stage): Do not adjust the basic weights, and maintain the weight ratio of 0.4 for the triaxial inertial measurement unit, 0.4 for the triaxial geomagnetic sensor, and 0.2 for the barometer; Weak interference level (σ) max ≤0.2m / s², pre-landing stage): The weight of the triaxial inertial measurement unit is reduced by 0.1 (correction factor -0.1), the weight of the triaxial geomagnetic sensor is increased by 0.1 (correction factor +0.1), and the weight of the barometer remains unchanged. After correction, the weight ratio is 0.3 for the triaxial inertial measurement unit, 0.5 for the triaxial geomagnetic sensor, and 0.2 for the barometer. Weighting is incorporated into the filtering process: The calculated weight proportions of each sensor are used as the diagonal elements (off-diagonal elements are 0) of the weight matrix W of the adaptive Kalman filter observation equation. The observation equation is Z=HX+v, where Z is the corrected sensing data vector, H is the observation matrix, and v is the observation noise vector. The weight matrix W is used to adjust the contribution of different sensor data in the observation update, ensuring that the weights of sensors with strong anti-interference capabilities are dynamically increased during high interference phases.

[0034] The update cycle of sensor weight ratio is consistent with the state update cycle of adaptive Kalman filter (i.e., synchronized with the sensor acquisition cycle; if the acquisition cycle is 10ms, then the weight update cycle is 10ms). Before each filtering calculation, σ_max and interference level are recalculated based on the acceleration data of the three-axis inertial measurement unit in the latest 5 sampling cycles, and then the weight ratio is updated according to the above rules to ensure that the weight adjustment is synchronized with the real-time changes of environmental interference during the airdrop process, and to avoid the problem that fixed weights cannot adapt to dynamic interference scenarios.

[0035] S4, Dynamic Path Planning: Based on the initial geographic coordinates, the target point's geographic coordinates, and the corrected state data, hierarchical path planning is performed, including: Global path generation: In the target-up navigation coordinate system established in S1, since the X-axis is already aligned with the target point, the global reference trajectory is directly defined as the X-axis of the coordinate system, that is, a straight line with a Y-axis coordinate value of 0. There is no need to perform complex latitude and longitude interpolation calculations. It is only necessary to set the target descent gradient along the X-axis direction (that is, the slope of the height as the distance along the X-axis) to generate a spatial reference line pointing directly to the target, ensuring that the vehicle always moves along the shortest line of sight path.

[0036] Local path adjustment: The system calculates in real time the position deviation between the vehicle's position and the global path in the corrected state data, as well as the attitude deviation between the vehicle's attitude angle and the preset attitude angle. The position deviation threshold is dynamically configured according to the airdrop altitude. The position deviation threshold is set to 3 to 5 meters in the high-altitude airdrop phase and 1 to 2 meters in the low-altitude airdrop phase. The attitude deviation threshold is set to 3 to 5 degrees. When the position deviation or attitude deviation exceeds the corresponding threshold for three consecutive sampling cycles, the local trajectory is dynamically adjusted by correcting the heading angle and velocity components. The heading angle correction is based on the proportional-integral control algorithm, which calculates the adjustment amount based on the difference between the target heading angle and the current heading angle. The velocity component adjustment adjusts the horizontal velocity within a preset range according to the magnitude of the position deviation to ensure that the vehicle returns to the global trajectory.

[0037] S5. Precise corrections before landing: By monitoring the vehicle's height in real time using a barometer, when the vehicle's height falls below a preset height threshold, a precise correction phase is initiated before landing. The preset height threshold is set to 50 to 100 meters, with the specific value determined based on the airdrop height and landing accuracy requirements.

[0038] The update frequency of multi-source fusion data has been improved: The multi-source fusion data update frequency of adaptive Kalman filtering is increased to over 200 Hz, increasing data processing density, quickly responding to airflow turbulence before landing, and reducing data processing latency.

[0039] Local path adjustment deviation threshold reduction: Simultaneously reduce the position deviation threshold and attitude deviation threshold to further control the accumulation of errors before landing and ensure that the vehicle's attitude and position accuracy meet the landing requirements.

[0040] Sensor weighting and heading angle correction optimization: Before landing, the precise correction phase simultaneously performs dynamic weight allocation of sensors, increases the weight ratio of the three-axis geomagnetic sensor, adjusts the priority of sensor data fusion, and enhances the accuracy of heading angle correction. Through high-frequency heading angle adjustments, it ensures that the deviation between the heading and the target direction is minimized when the vehicle lands, and achieves precise control of the landing position in conjunction with position calibration.

[0041] S6. Navigation control command output: Based on the real-time navigation trajectory that has been precisely corrected before landing, the trajectory data is converted into control commands that the vehicle can execute, including: The control commands include attitude adjustment commands and motion control commands. The attitude adjustment commands are generated based on the heading angle adjustment amount and are used to control the vehicle steering mechanism to adjust the vehicle heading. The motion control commands are generated based on the speed adjustment requirements and are used to control the vehicle power system and braking system to adjust the vehicle's horizontal speed and ensure a smooth landing.

[0042] Control commands are sent to the vehicle's ECU via the CAN bus. The ECU optimizes the commands based on the vehicle's dynamics model, drives the vehicle's actuators, and ultimately guides the vehicle to move toward the target point, achieving a precise landing.

[0043] S7. Navigation data update frequency control: The navigation data update frequency for real-time perception data acquisition, map-free error correction and fusion, and dynamic path planning is no less than 100 Hz. This ensures that the data update speed matches the dynamic changes of the vehicle during the airdrop process. At the same time, the state update cycle of the adaptive Kalman filter is kept consistent with the sensor data acquisition cycle. The data acquired by the sensor in real time is processed synchronously to avoid fusion errors caused by cycle mismatch and ensure the real-time performance and accuracy of navigation response in dynamic environments.

[0044] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for dynamic navigation planning of routes for airdropped closed-chamber vehicles, characterized in that, Includes the following steps: S1. Navigation Initialization: Obtain the initial geographic coordinates of the vehicle airdrop and the geographic coordinates of the target point. Based on the initial geographic coordinates and geomagnetic sensor data, establish an upward navigation reference for the target to provide a directional reference for subsequent data collection and path planning. S2. Real-time perception data acquisition and correction: Based on the target upward navigation reference established in step S1, vehicle motion state data and environmental reference data are continuously acquired through on-board multi-source sensors. Decoupling correction of vehicle attitude and sensor data is performed simultaneously to obtain corrected perception data. Motion state data includes three-axis acceleration, three-axis angular velocity and attitude angle. Environmental reference data includes absolute heading angle and real-time altitude. S3. Mapless Error Correction and Fusion: Based on the corrected sensing data obtained in step S2, an adaptive Kalman filter algorithm is used to perform multi-source fusion processing. The adaptive Kalman filter is achieved by constructing a state equation that includes position, velocity, attitude, and sensor zero bias, and dynamically adjusting the filter noise covariance based on real-time observation residuals. During the multi-source fusion processing, airflow disturbance compensation is performed simultaneously to finally obtain corrected state data used to eliminate inertial navigation cumulative errors. S4. Dynamic Path Planning: Based on the initial geographic coordinates of step S1, the geographic coordinates of the target point, and the corrected state data of step S3, perform hierarchical path planning to generate a real-time navigation trajectory. S5. Precise Correction Before Landing: Based on the real-time navigation trajectory in step S4, the vehicle altitude is monitored by the barometer. When the vehicle altitude is lower than the preset altitude threshold, the update frequency of the multi-source fusion data in step S3 is increased, while the deviation threshold of the local path adjustment in step S4 is reduced, the heading angle correction accuracy is enhanced, and the landing position is calibrated. S6. Navigation control command output: Based on the real-time navigation trajectory calibrated in step S5, convert and generate vehicle attitude adjustment commands and motion control commands to guide the vehicle to move towards the target point.

2. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 1, characterized in that, The process of establishing the target upward navigation reference in step S1 includes: collecting the initial geomagnetic components through a geomagnetic sensor, performing calibration by combining the magnetic declination data in the initial geographic coordinates, and determining the absolute heading reference pointing to the geographic North Pole; calculating the target azimuth angle based on the initial geographic coordinates and the geographic coordinates of the target point, and establishing a navigation coordinate system with the initial point as the origin, the direction pointing to the target point as the X-axis, the horizontal tangent perpendicular to the X-axis as the Y-axis, and the celestial direction as the Z-axis.

3. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 1, characterized in that, In step S2, the on-board multi-source sensors include a three-axis inertial measurement unit (IMU), a three-axis geomagnetic sensor, and a barometer. The IMU collects three-axis acceleration and three-axis angular velocity data, the geomagnetic sensor collects absolute heading angle data, and the barometer collects real-time altitude data. The decoupling and correction process between the vehicle attitude and the sensor data includes: establishing a deviation model using the pitch and roll angles collected by the IMU; correcting the original acceleration and geomagnetic data based on the deviation model; calculating the corrected acceleration data by multiplying the cosine values ​​of the pitch and roll angles; calculating the corrected geomagnetic data by multiplying the absolute values ​​of the pitch and roll angles by a preset coefficient; and achieving microsecond-level synchronization of the sensor data through a phase-locked loop.

4. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 1, characterized in that, The hierarchical path planning in step S4 includes global path generation and local path adjustment. Global path generation is based on the initial geographic coordinates and target point geographic coordinates in step S1, and a straight-line reference trajectory is generated in the target upward navigation coordinate system established in step S1. Local path adjustment calculates the positional deviation between the vehicle position and the global path and the attitude deviation between the vehicle attitude angle and the preset attitude angle in the state data after correction in step S3. When any deviation exceeds the preset threshold, the local trajectory is dynamically adjusted by correcting the heading angle and velocity component.

5. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 4, characterized in that, The position deviation threshold is dynamically configured based on the airdrop height. The position deviation threshold is set to 3 to 5 meters during the high-altitude airdrop phase and 1 to 2 meters during the low-altitude airdrop phase. The attitude deviation threshold is set to 3 to 5 degrees. When the deviation exceeds the threshold for 3 consecutive sampling cycles, a local path adjustment is triggered.

6. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 1, characterized in that, The airflow disturbance compensation process in step S3 includes: calculating the disturbance acceleration by using the residuals of the actual acceleration collected by the inertial measurement unit and the theoretical motion acceleration and gravitational acceleration, incorporating the disturbance acceleration into the state equation of the adaptive Kalman filter, and correcting the influence of airflow on the vehicle's motion state; the multi-source fusion processing in step S3 also includes dynamic weight allocation of sensors, in which the weight ratio of each sensor is adjusted according to the environmental interference intensity during the airdrop phase; during the high-altitude airdrop phase, the weight ratio of the inertial measurement unit is increased to adapt to strong airflow interference; during the low-altitude airdrop phase, the weight ratio of the inertial measurement unit and the geomagnetic sensor is balanced to take into account dynamic response and heading accuracy; and during the precise correction phase before landing, the weight ratio of the geomagnetic sensor is increased to enhance heading calibration accuracy.

7. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 1, characterized in that, The navigation data update frequency in control steps S2 to S4 is not less than 100 Hz, so that the state update cycle of the adaptive Kalman filter is consistent with the sensor data acquisition cycle, so as to synchronously process the data acquired by the sensor in real time.

8. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 1, characterized in that, In step S5, a preset height threshold is set to 50 to 100 meters. When the vehicle height is lower than the preset height threshold, the update frequency of the multi-source fusion data is increased to more than 200 Hz to increase the data processing density. During the precise correction stage before landing, the dynamic weight allocation of sensors is performed simultaneously. The priority of sensor data fusion is adjusted by increasing the weight ratio of the geomagnetic sensor, and the landing position of the vehicle is calibrated in conjunction with the heading angle correction action.

9. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 6, characterized in that, The quantitative determination process for the intensity of environmental interference includes: Data sampling and preprocessing: Take triaxial acceleration data from 5 consecutive sensor acquisition cycles and remove outliers that exceed the physical range of -10g to 10g; Standard deviation calculation: Calculate the standard deviation σ of the preprocessed triaxial acceleration data. ax σ ay σ az； Strong interference: σ max >0.5m / s², corresponding to the high-altitude stage of airdrop altitude >500m; Moderate disturbance: 0.2 m / s² < σ max ≤0.5m / s², corresponding to the low-altitude phase of airdrop altitude 100m < altitude ≤500m; Weak interference: σ max ≤0.2m / s² corresponds to the precise correction stage before landing when the airdrop height is ≤100m.

10. The method for dynamic navigation planning of air-dropped closed-chamber vehicle paths according to claim 9, characterized in that, The specific rules for the dynamic weight allocation of the sensors include: The default weights for the triaxial inertial measurement unit, triaxial geomagnetic sensor, and barometer are 0.4, 0.4, and 0.2, respectively. Apply a correction factor based on the interference level: Under strong interference levels, the weight of the triaxial inertial measurement unit is increased by 0.2, while the weights of the triaxial geomagnetic sensor and barometer are each decreased by 0.

1. Maintain the basic weights under moderate interference levels; Under weak interference levels, the weight of the triaxial inertial measurement unit is -0.1, and the weight of the triaxial geomagnetic sensor is +0.

1. The final calculated weight percentages are used as the diagonal elements of the weight matrix W in the adaptive Kalman filter observation equation.

Images