Real-time dynamic attitude stability control method and system based on multi-sensor fusion

Abstract

The invention relates to the technical field of sensor data fusion, in particular to a real-time dynamic attitude stability control method and system based on multi-sensor fusion, and the method comprises the following steps: collecting multi-source sensor data, and correcting to generate a synchronous sensor data set; calculating a stability coefficient, a signal quality coefficient and a consistency coefficient, weighting the signal quality coefficient and the consistency coefficient to generate a dynamic fusion weight coefficient, executing weighted fusion based on the weight, outputting a fusion attitude state estimation result through adaptive extended Kalman filtering, and generating an optimization control instruction sequence by combining with dynamic constraint model rolling optimization; according to the method, a dynamic weight mechanism based on noise and signal strength is introduced to achieve self-adaptive fusion, the synchronization precision and reliability are improved by combining consistency verification, abnormal signals are corrected through self-adaptive filtering, the attitude estimation stability is enhanced, and the performance of the system is improved. And a control instruction is generated through rolling optimization, so that rapid and accurate adjustment is realized, and attitude keeping and control consistency in a complex environment is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of sensor data fusion, in particular to a real-time dynamic attitude stabilization control method and system based on multi-sensor fusion. BACKGROUND

[0002] The technical field of sensor data fusion mainly involves the integration and processing of data collected by multiple different types of sensors, in order to improve the accuracy and reliability of information. This technology is widely used in automation, robotics, aerospace, intelligent transportation and other fields. Key issues include sensor data acquisition, data fusion algorithm, information processing method, data accuracy improvement, real-time guarantee and system optimization. Specifically, sensor data fusion aims to solve the problem of inconsistency, redundancy and error between multiple sensors, process data from different sources through fusion algorithm, obtain more accurate results, and provide more reliable decision support and control system.

[0003] Among them, the traditional real-time dynamic attitude stabilization control method based on multi-sensor fusion refers to using dynamic data collected by multiple sensors to control the attitude stability of the target system through real-time processing and fusion. This method uses sensor data fusion algorithm to fuse data from gyroscopes, accelerometers and other sensors, calculates the attitude information of the target object, and adjusts the stability of the system through control strategy. The traditional method mainly relies on Kalman filter, weighted average and other data fusion technologies to realize dynamic adjustment and stability of attitude through real-time processing of multi-dimensional sensor information.

[0004] The traditional multi-sensor attitude control method is easily affected by noise fluctuations in dynamic environment, and fixed fusion weight is difficult to reflect the real-time performance change of sensors, resulting in insufficient stability of attitude estimation results in complex motion. Data synchronization relies on single time trigger mechanism, and time sequence deviation between different sensors causes distortion of fusion results. The filtering process lacks real-time correction and adaptive adjustment of abnormal signals, causing estimation error accumulation and delay. The control link relies on fixed model output instruction, which is difficult to perform rolling prediction and dynamic optimization according to attitude change. The system has response lag in fast attitude adjustment, affecting control precision and execution continuity. The whole system is prone to attitude drift and control instability problems in complex dynamic working conditions. SUMMARY

[0005] In order to solve the technical problems that the traditional multi-sensor attitude control method is easy to be affected by noise fluctuation in a dynamic environment, the fixed fusion weight is difficult to reflect the real-time performance change of the sensor, the stability of the attitude estimation result is insufficient in complex motion, the data synchronization relies on a single time trigger mechanism, the time sequence deviation between different sensors causes the distortion of the fusion result, the filter process lacks real-time correction and adaptive adjustment of abnormal signals, the estimation error is accumulated and delayed, the control link relies on a fixed model to output instructions, it is difficult to perform rolling prediction and dynamic optimization according to the attitude change, the system response is delayed in fast attitude adjustment, the control precision and execution continuity are affected, and the whole system is prone to attitude drift and control instability in complex dynamic working conditions, the embodiment of the present application provides a real-time dynamic attitude stability control method based on multi-sensor fusion.

[0006] In order to achieve the above-mentioned purpose, the present application adopts a real-time dynamic attitude stability control method based on multi-sensor fusion, comprising the following steps: S1: collecting angular velocity data and acceleration data through an inertial measurement unit, collecting position coordinate data through a positioning module, collecting image feature point data through a visual perception module, collecting contact force data through a force sensor, performing synchronous correction processing based on a time stamp alignment method, and generating a synchronous sensor data set; S2: based on the synchronous sensor data set, calculating a noise stability coefficient based on noise level, calculating a signal quality coefficient through signal intensity ratio, calculating a consistency coefficient through consistency verification, and generating a dynamic fusion weight coefficient by weighted sum of the coefficients; S3: calling the dynamic fusion weight coefficient, weighting and fusing the sensor attitude related data based on the weight coefficient, and inputting an adaptive extended Kalman filter to perform prediction and update steps to calculate Kalman gain and output fusion attitude state estimation result; S4: based on the current attitude angle and angular velocity data in the fusion attitude state estimation result, selecting a corresponding dynamic constraint model according to the platform type, inputting the attitude state and constraint condition into the model predictive control algorithm for rolling optimization calculation to generate an optimized control instruction sequence.

[0007] As a further scheme of the present application, the synchronous sensor data set includes time synchronization information, space registration information and data integrity identification of multi-source sensors, the dynamic fusion weight coefficient includes noise weight factor, signal confidence coefficient and data consistency coefficient, the fusion attitude state estimation result includes attitude angle parameter, angular velocity parameter and covariance matrix parameter, and the optimized control instruction sequence includes target control quantity parameter, constraint boundary parameter and rolling optimization variable.

[0008] As a further scheme of the present application, the specific steps of S1 are: S101: Obtain inertial measurement unit angular velocity and acceleration data, positioning module coordinate data, visual perception module image feature point data, and force sensor contact force data, sort the sensor data frames according to the time stamps, calculate the time difference and correct the drift frame time, and generate a preliminary time alignment data set; S102: Based on the preliminary time alignment data set, extract the time stamp sequence of the angular velocity, acceleration, coordinate, image feature point and contact force data, perform interpolation resampling on the sequence, correct the offset points according to the unified time axis, and generate a time synchronization correction data set; S103: According to the time synchronization correction data set, aggregate the corresponding time frames of the sensors to form a synchronous recording frame of the angular velocity vector, acceleration vector, coordinate matrix, image feature point matrix and contact force vector, perform time index consistency verification, and generate a synchronous sensor data set.

[0009] As a further scheme of the present application, the specific steps of S2 are: S201: Based on the synchronous sensor data set, extract the instantaneous noise power density of the sensor signal, calculate the time sequence mean square error and compare it with the noise stability threshold, filter the channels less than the threshold and perform normalization transformation, and generate a noise stability coefficient; S202: Call the noise stability coefficient, calculate the signal intensity ratio vector according to the ratio of signal intensity to reference intensity benchmark value in the synchronous sensor data set, and perform weighted correction on the signal intensity ratio vector according to the noise stability coefficient, and extract the intensity difference to generate a signal quality coefficient; S203: Call the signal quality coefficient and noise stability coefficient, calculate the numerical change rate difference in adjacent time windows and compare it with the consistency benchmark value, perform weighted aggregation operation on the comparison result according to the consistency matching degree, and generate a dynamic fusion weight coefficient.

[0010] As a further scheme of the present application, the noise stability threshold is determined according to the mean square error distribution statistical characteristics of the channel signals in the synchronous sensor data set in multiple time windows; The reference intensity benchmark value is determined according to the long-term average intensity value of the channel signals in the synchronous sensor data set; The consistency benchmark value is determined according to the statistical deviation characteristics of the intensity change in the original signal data.

[0011] As a further scheme of the present application, the specific steps of S3 are: S301: Based on the dynamic fusion weight coefficient, perform weighted operation on the angular velocity data and acceleration data of the sensor attitude associated data, according to the time sequence index, compare the attitude sampling period, and according to the weight coefficient, generate a weighted attitude input data set. S302: call the weighted attitude input data set, perform prediction calculation according to state transition parameters and noise covariance parameters in an adaptive extended Kalman filter, calculate a predicted state vector using the state transition parameters, and obtain a Kalman gain parameter set according to the noise covariance and the observation data inverse ratio; S303: according to the Kalman gain parameter set, performing weighted update on the predicted state vector and the observation data, weighting and fusing the residual component and the gain parameter corresponding relationship to correct the state estimation result, and calculating the updated covariance data distribution to generate the fused attitude state estimation result.

[0012] As a further scheme of the present application, the specific steps of S4 are: S401: based on the current attitude angle and angular velocity data in the fused attitude state estimation result, detecting the platform type parameter, retrieving the dynamic constraint condition according to the type parameter, performing weighted calculation on the attitude angle change rate and angular velocity vector and extracting the matrix feature quantity to generate the attitude constraint feature set; S402: according to the attitude angle and angular velocity data of the attitude constraint feature set, inputting the constraint condition into the model predictive control algorithm for rolling optimization calculation, comparing the angular velocity increment of the time domain window and screening the constraint data frame to obtain the attitude prediction state set; S403: calling the attitude angle and angular velocity sequence data of the attitude prediction state set, calculating the angular velocity gain coefficient and the attitude angle deviation amount for the dynamic difference between sequences, linearly combining the gain coefficient and the deviation amount to extract the sequence vector, and generating the optimized control instruction sequence.

[0013] As a further scheme of the present application, the method further comprises the step of S5: S5: calling the first control instruction in the optimized control instruction sequence, differentiating and decomposing the control instruction according to the platform type, sending the control signal to the corresponding motor through the high-frequency actuator driving circuit to generate the actuator control output signal; The actuator control output signal includes a driving current signal, a driving voltage signal and a motor torque signal.

[0014] As a further scheme of the present application, the specific steps of S5 are: S501: based on the first control instruction in the optimized control instruction sequence, extracting the motor driving parameter, torque set value and target displacement coordinate, matching the platform execution channel parameter set and correcting the amplitude proportion deviation according to the control parameter reference set to generate the differentiated decomposition parameter set; S502: According to the difference decomposition parameter set, parse the high-frequency actuator driving circuit input signal channel, extract the voltage amplitude and driving frequency parameters, compare and calculate the phase error vector, correct the phase offset parameter, and obtain the timing synchronous driving signal group; S503: Call the timing synchronous driving signal group, perform selection and mapping on the motor signal path, calculate the current distribution proportion parameter matrix according to the phase and amplitude difference, adjust the amplitude normalization, output through the high-frequency actuator driving circuit, and generate the actuator control output signal.

[0015] The real-time dynamic attitude stabilization control system based on multi-sensor fusion comprises: A multi-source data synchronization module collects angular velocity data and acceleration data through an inertial measurement unit, collects position coordinate data through a positioning module, collects image feature point data through a visual perception module, collects contact force data through a force sensor, performs synchronization correction processing based on a timestamp alignment method, generates a synchronous sensor data set, and delivers it to a weight calculation and fusion module; The weight calculation and fusion module calls the synchronous sensor data set, calculates a noise stability coefficient based on noise level, calculates a signal quality coefficient through signal strength ratio, calculates a consistency coefficient through consistency verification, performs weighted summation of the coefficients, generates a dynamic fusion weight coefficient, and delivers it to an attitude estimation module; The attitude estimation module calls the dynamic fusion weight coefficient, performs weighted fusion on sensor attitude-related data based on the weight coefficient, inputs an adaptive extended Kalman filter, performs prediction and update steps to calculate Kalman gain, outputs a fusion attitude state estimation result, and delivers it to a predictive control module; The predictive control module selects a corresponding dynamic constraint model according to the platform type based on the current attitude angle and angular velocity data in the fusion attitude state estimation result, inputs the attitude state and constraint condition into the model predictive control algorithm for rolling optimization calculation, generates an optimized control instruction sequence, and delivers it to an execution driving module; The execution driving module calls the first control instruction in the optimized control instruction sequence, performs differential decomposition processing on the control instruction according to the platform type, sends the control signal to the corresponding motor through the high-frequency actuator driving circuit, and generates the actuator control output signal.

[0016] Compared with the prior art, the advantages and positive effects of the present application are: In the present application, by introducing a dynamic weight calculation mechanism based on noise level and signal strength, the multi-source data can be adaptively allocated weight in fusion according to real-time state, eliminating the error deviation caused by fixed proportion fusion, combining with the consistency checking process to improve the data synchronization accuracy and fusion reliability, the fusion output is adjusted by adaptive filtering to realize the immediate correction of abnormal signals, enhance the continuous stability of attitude estimation, the attitude and motion state are introduced into the prediction control model for rolling optimization calculation, the control instruction can be dynamically generated and decomposed according to the real-time state change, realizing the rapid response and accurate adjustment of attitude control under high frequency execution, improving the attitude holding ability and control consistency of the system in complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can be obtained without creative labor.

[0018] Figure 1 The step flowchart of the present application is shown in the figure. Figure 2 The S1 refinement schematic diagram of the present application is shown in the figure. Figure 3 The S2 refinement schematic diagram of the present application is shown in the figure. Figure 4 The S3 refinement schematic diagram of the present application is shown in the figure. Figure 5 The S4 refinement schematic diagram of the present application is shown in the figure. Figure 6 The S5 refinement schematic diagram of the present application is shown in the figure. Figure 7 The system module diagram of the present application is shown in the figure. DETAILED DESCRIPTION

[0019] The technical solutions in the present application will be described below in combination with the drawings.

[0020] In the embodiments of the present application, the words such as "example", "for example" are used to represent as an example, illustration or description. Any embodiment or design scheme described as "example" in the present application should not be interpreted as more preferred or more advantageous than other embodiments or design schemes. In fact, the word "example" is intended to present the concept in a specific way. In addition, in the embodiments of the present application, the meaning expressed by "and / or" can be both, or can be one of the two.

[0021] In the embodiments of the present application, "image" and "picture" can be used interchangeably, and it should be pointed out that the meanings expressed are consistent when the distinction is not emphasized.

[0022] In the embodiments of the present application, sometimes the subscript such as W1 is written in the form of non-subscript such as W1, and the meanings expressed are consistent when the distinction is not emphasized.

[0023] In order to make the technical problems, technical solutions and advantages to be solved by the present application more clear, the following will be described in detail in conjunction with the drawings and specific embodiments.

[0024] Please refer to Figure 1 The embodiments of the present application provide a real-time dynamic attitude stabilization control method based on multi-sensor fusion, comprising the following steps: S1: collecting angular velocity data and acceleration data through an inertial measurement unit, collecting position coordinate data through a positioning module, collecting image feature point data through a visual perception module, collecting contact force data through a force sensor, performing synchronous correction processing based on a timestamp alignment method, and generating a synchronous sensor data set; S2: based on the synchronous sensor data set, calculating a noise stability coefficient based on noise level, calculating a signal quality coefficient through signal strength ratio, calculating a consistency coefficient through consistency verification, and performing weighted summation of the coefficients to generate a dynamic fusion weight coefficient; S3: calling the dynamic fusion weight coefficient, performing weighted fusion on the sensor attitude related data based on the weight coefficient, and inputting the adaptive extended Kalman filter to perform prediction and update steps to calculate the Kalman gain and output the fusion attitude state estimation result; S4: based on the current attitude angle and angular velocity data in the fusion attitude state estimation result, selecting a corresponding dynamic constraint model according to the platform type, inputting the attitude state and constraint condition into the model predictive control algorithm for rolling optimization calculation to generate an optimized control instruction sequence; S5: calling the first control instruction in the optimized control instruction sequence, performing differential decomposition processing on the control instruction according to the platform type, sending the control signal to the corresponding motor through the high-frequency actuator driving circuit to generate the actuator control output signal; The synchronous sensor dataset includes time synchronization information, spatial registration information, and data integrity identifiers from multiple sensor sources. The dynamic fusion weighting coefficients include noise weighting factors, signal confidence coefficients, and data consistency coefficients. The fused attitude state estimation results include attitude angle parameters, angular velocity parameters, and covariance matrix parameters. The optimized control command sequence includes target control quantity parameters, constraint boundary parameters, and rolling optimization variables. The actuator control output signals include drive current signals, drive voltage signals, and motor torque signals.

[0025] Please see Figure 2 The specific steps of S1 are as follows: S101: Acquire angular velocity and acceleration data of the inertial measurement unit, coordinate data of the positioning module, image feature point data of the visual perception module, and contact force data of the force sensor. Sort the sensor data frames by timestamp, calculate the time difference and correct the drift frame time, and generate a preliminary time-aligned dataset. After acquiring data including triaxial angular velocity and triaxial acceleration from the inertial measurement unit (IMU), coordinate data including longitude, latitude, and altitude from the positioning module (such as GPS), pixel coordinate data of image feature points from the visual perception module (such as monocular or binocular cameras), and contact force data from force sensors (such as pressure sensors installed on the soles of the feet of a legged robot), the data frames from different sensors, each with its own timestamp, are first stored in a container with a capacity of [missing information]. In a first-in-first-out (FIFO) data buffer queue, for example, IMU data frames are... The frequency at which these values ​​enter the queue is approximately [time interval]. GPS data frames are The frequency of entry is approximately [timestamp interval]. The visual data frame frequency is The timestamp interval is approximately The force sensor data frame frequency is The timestamp interval is In the queue, data frames are sorted in ascending order based on their timestamp values. For example, a data frame with a timestamp of... The IMU frames will be arranged in the timestamp of Before the force sensor frame, to correct the accumulated error between different sensor hardware clock sources, a reference clock source is set, and the IMU with the highest sampling frequency is selected as the master clock. Then, the drift of the other sensor clocks relative to the master clock is calculated. Specifically, in Within a time window of seconds, the change in the timestamp difference between GPS and IMU is counted. If at the beginning of the window, the timestamp of a GPS frame... With the most recent IMU frame timestamp The difference is At the end of the window, another GPS frame's timestamp With the most recent IMU frame timestamp The difference is The drift rate is then calculated as follows: Set a drift correction threshold, which is determined based on the stability parameters of the sensor clock crystal oscillator, typically 20 parts per million (ppm). ),Right now Due to the calculated drift rate The absolute value is less than If the calculated absolute value of the drift rate is, for example, within the allowable range, then the drift is considered to be within the allowable range. If the threshold is exceeded, the excess portion will be... As a correction factor, the timestamps of subsequent sensors entering the queue (GPS in this example) are linearly compensated and adjusted. For example, for A new GPS timestamp received seconds later will be subtracted from its original value. After the sorting and drift correction processes described above, a preliminary time-aligned dataset is obtained.

[0026] S102: Based on the preliminary time-aligned dataset, extract the timestamp sequences of angular velocity, acceleration, coordinates, image feature points and contact force data, perform interpolation resampling on the sequences, correct the offset points according to the unified time axis, and generate a time synchronization correction dataset. Based on the initial time-aligned dataset, a unified and fixed time axis is first established for the entire fusion system. The frequency of this time axis is determined according to the execution frequency of the robot control loop. For example, for a quadcopter UAV that requires high-frequency attitude adjustment, the frequency of this unified time axis is set to [frequency value missing]. That is, the time interval on the time axis is millisecond( ), forming a target timestamp sequence (Unit: milliseconds) Next, extract multiple sensor-independent timestamp and data value sequences from the initial time-aligned dataset. For example, for a sampling frequency of... IMU angular velocity data, in and Between the synchronization moments of these two targets, there are multiple raw data points, such as in The angular velocity at that point is ,exist The value at that location is Wait, at this time, through the to Inside the window The corresponding value is calculated by taking the arithmetic mean of the original angular velocity values. The angular velocity value at this synchronization time point is relevant for sampling frequencies lower than [a certain value]. Sensors, such as The GPS, whose raw data points are located at (coordinates are) )and (coordinates are) To obtain the synchronization time point The coordinates at that location are calculated using linear interpolation: New coordinates = coordinates a +(coordinates) b -coordinate a ) Substitute the values ​​into the equation for calculation; longitude value = Latitude value = Height value = Thus obtain The interpolation coordinates at time are After resampling the data to a unified time axis, the offset points in the data sequence are corrected. The offset points are determined by whether the change in data within a single synchronization cycle exceeds the robot's physical kinematic limits. A threshold for acceleration change is set, which is based on the robot's maximum thrust and mass. If a drone's maximum acceleration is... ,but The upper limit of the reasonable change in acceleration within is set as follows: If in The acceleration calculated at that time is , and The value calculated at that time ,That The change in axis is Exceeded The threshold will then The value is adjusted to the average of the values ​​at the two consecutive time points, i.e. The offset points are processed in this way to ultimately form a time synchronization correction dataset.

[0027] S103: Based on the time synchronization correction dataset, aggregate the corresponding time frames of the sensors to form synchronized recording frames of angular velocity vector, acceleration vector, coordinate matrix, image feature point matrix and contact force vector, perform time index consistency check, and generate a synchronized sensor data set; Based on the time synchronization correction dataset, for each synchronization time point on the unified time axis, for example The data is aggregated into a structured data frame, specifically the IMU's three-axis angular velocity data, at each second. Combine into one angular velocity vector Transform triaxial acceleration data Combine into one acceleration vector GPS coordinate data Combine into one coordinate matrix The image feature points tracked by the visual perception module at that moment, such as The set of pixel coordinates of a feature point on the image plane forms a... Image feature point matrix As shown in Table 1, the contact force data of a single foot force sensor of the legged robot are presented. Form a Contact force vector In this way, the original, separate data streams with different frequencies are integrated into a series of synchronized recording frames with the same time index. Then, a time index consistency check is performed on the aggregated synchronized recording frames. This check involves traversing each synchronization time point and checking for any missing sensor data in the recording frame at that time point. A data missing tolerance is set for this purpose, and this tolerance is weighted according to the importance of different data to attitude estimation. The weight values ​​are set with reference to the control algorithm's dependence on the sensors; for example, the weight of IMU data is set to... The weight of GPS data is The weight of visual data is The force sensor weight is When a frame of data is missing, its corresponding weight value is accumulated into a missing data counter, and the missing data tolerance threshold is set to... If at a certain point in time If IMU data is present in a 1-second recording frame, but GPS and visual data are both missing, then the value of the missing data counter is [value missing]. This value is greater than If the threshold is reached, the data in this frame is determined to be inconsistent, and the system will discard the entire synchronized record frame at that time point. If only visual data is missing, the missing data counter value is [value missing]. If the data is less than the threshold, the frame is retained and filled with the visual data from the previous frame. After passing this test, the remaining recorded frames constitute the synchronous sensor data set.

[0028] Table 1: Example Table of Image Feature Point Matrix

[0029] As shown in Table 1, the table lists the specific pixel positions of the five image feature points tracked by the visual perception module in the image coordinate system at a certain synchronization moment, forming the image feature point matrix required for subsequent pose estimation.

[0030] Please see Figure 3The specific steps of S2 are as follows: S201: Based on the synchronous sensor data set, extract the instantaneous noise power density of the sensor signal, calculate the mean square error of the time series and compare it with the noise stability threshold, filter out the channels with less than the threshold and perform normalization transformation to generate the noise stability coefficient. Based on the synchronous sensor data set, data within a continuous time window is first extracted from multiple sensor channels. For example, for the Z-axis angular velocity channel of the inertial measurement unit, data within a continuous time window of length is extracted. The time window in milliseconds (inclusive) Within a set of data points, the extracted signal sequence is: Calculate the mean of the sequence. Then, calculate the average of the sum of squares of the differences between each data point and the mean, and obtain the root mean square error within that time window. Next, to determine the noise stability threshold, the system collected continuous noise data under static conditions. The mean squared deviation data of these time windows form a set of mean squared deviation distributions. The mean of this set is then calculated. and standard deviation Set the noise stabilization threshold to ,Right now Due to the current calculated mean square error Less than the threshold Therefore, the Z-axis angular velocity channel is judged to be noise-stable. For the other channel, such as feature points extracted by the vision module in low-light environments... The mean square error of the coordinate stream within the same time window is calculated as follows: greater than The threshold is set, therefore this channel is filtered as an unstable channel and does not participate in subsequent coefficient generation. For stable channels with values ​​below the threshold, a normalization transformation is performed. The transformation is calculated by... Subtracting the ratio of the current mean square error to the noise stability threshold, the initial coefficient value for the Z-axis angular velocity channel is... This calculation process is performed in parallel across sensor channels, such as accelerometers. The root mean square error of the axis is Its coefficient value is The mean square error of the force sensor is Its coefficient value is Finally, the coefficient values ​​calculated from the stable channel are combined to generate the noise stability coefficient.

[0031] S202: Call the noise stability coefficient, calculate the signal strength ratio vector based on the ratio of signal strength to reference strength in the synchronous sensor data set, and perform weighted correction on the signal strength ratio vector according to the noise stability coefficient. Then, perform amplitude normalization and layering on the ratio vector, extract the intensity difference to generate the signal quality coefficient. The generated channel noise stability coefficients are invoked first, and a reference strength baseline value is determined. This baseline value is calculated by examining the channel's past performance. Second( The signal strength is determined by the average signal strength within a data point (e.g., for a drone). For shaft accelerometers, the signal strength is defined as the absolute value of the measured value. The average absolute value of seconds is Then the reference strength benchmark value of this channel is... At the current moment, the measured The absolute value of the axis acceleration is Then calculate its signal strength ratio as The same calculation is performed on the remaining channels to form a signal strength ratio vector containing the channel ratios, for example, Next, magnitude normalization is performed on each element of the vector, with the upper limit of the normalization interval set to 1. Anything greater than The ratios were all set to Due to the ratio Less than Its value remains unchanged. Then, based on the normalized ratio, it is stratified into three strata, with the ratio in... The area between them is defined as a low-strength layer. It is a medium strength layer. It is a high-strength layer, therefore, The ratio of the axial accelerometer It belongs to the high-strength layer, and the ratio is The other channel belongs to the medium-intensity layer. Finally, the intensity difference is extracted based on the layer to generate signal quality coefficients. Fixed mapping coefficient values ​​are set for different layers, with the low-intensity layer mapped as... The medium-intensity layer is mapped as The high-strength layer is mapped as ,therefore, The signal quality coefficient of the shaft accelerometer was assigned a value. The signal quality coefficient of the other channel was assigned a value. In this way, a value between [value] and [value] is calculated for each sensor channel. and The values ​​between these ranges are used to generate signal quality coefficients.

[0032] S203: Call the signal quality coefficient and noise stability coefficient, calculate the difference in the rate of change of values ​​within adjacent time windows and compare it with the consistency benchmark value, perform a weighted aggregation operation on the comparison result according to the degree of consistency matching, and generate dynamic fusion weight coefficients; Call the signal quality coefficient and noise stability coefficient of the sensor channel, and select two adjacent ones. A millisecond time window (window A and window B) is used to calculate the difference in the rate of change of values ​​within the window to assess the consistency between different sensors. For example, comparing the rate of change of the Z-axis gyroscope of the IMU with the rate of change of the heading angle estimated from continuous GPS position, the average reading of the Z-axis gyroscope within window A is... The average reading in window B is Its numerical change is During the same period, the average rate of change of the heading angle calculated from the GPS position within window A was... Window B is Its numerical change is The difference between the two rates of change is Next, this difference is compared with a consistency benchmark value, which is set based on long-term (e.g., under normal robot operating conditions) conditions. The statistical standard deviation of the difference between these two rates of change collected (minutes) is given by the statistical standard deviation. Then the consistency benchmark value will be set to Double the standard deviation, i.e. Due to the difference currently being calculated Less than the benchmark value This indicates that the measurements from the two sensors at this moment have a high degree of consistency. Subsequently, the results are weighted and aggregated based on the degree of consistency matching. pass Calculated by subtracting the ratio of the difference to the benchmark value, i.e. Finally, the polymerization noise stability coefficient Signal quality coefficient (As shown in Table 2) and the degree of consistency matching The weighted aggregation operation is performed, and the weight values ​​are set based on experience, such as stability weights. Quality weight Consistency weight The final dynamic fusion weighting coefficient of the IMU Z-axis gyroscope The calculation process is as follows This calculation is applied to the sensor channels involved in the fusion to generate dynamic fusion weight coefficients.

[0033] Table 2: Channel Intermediate Coefficient Values

[0034] As shown in Table 2, this table displays the noise stability and signal quality coefficient values ​​calculated for different sensor channels before the final weight aggregation. The intermediate results, together with the consistency matching degree, will be used to generate the final dynamic fusion weights.

[0035] Please see Figure 4 The specific steps of S3 are as follows: S301: Based on dynamic fusion weighting coefficients, weighted operations are performed on the angular velocity and acceleration data of sensor attitude correlation data. The attitude sampling period is compared according to the time series index, and the components with the same index are weighted according to the weighting coefficients to generate a weighted attitude input dataset. Based on the generated dynamic fusion weight coefficients, for example, the weight of the Z-axis angular velocity channel is... The weight of the X-axis acceleration channel is First, extract the same time index from the synchronous sensor data set (e.g., Attitude correlation data at the second level, specifically, extracting raw angular velocity data provided by the inertial measurement unit (IMU). and acceleration data Simultaneously, the estimated angular velocity values ​​calculated using visual odometry (VO) are extracted. And the acceleration estimate calculated from GPS speed changes Next, the dynamic fusion weighting coefficients of the sensors are weighted according to their corresponding measurement data components. Assuming the Z-axis angular velocity weight of the IMU is... The Z-axis angular velocity weight of VO is The weighted Z-axis angular velocity components are then calculated as follows: Similarly, the same weighting operation is performed on the X-axis and Y-axis components of angular velocity and all three axis components of acceleration. For example, the X-axis acceleration weight of the IMU is... The X-axis acceleration weight of GPS is The weighted X-axis acceleration components are then... This process iterates through the attitude-related sensor data, fuses the similar data components from multiple sources at each time index point according to their dynamic weights, and finally recombines the weighted results of the components into a vector form to form the weighted attitude input dataset at that moment.

[0036] S302: Call the weighted attitude input dataset, perform prediction calculation in the adaptive extended Kalman filter based on the state transition parameters and noise covariance parameters, calculate the predicted state vector using the state transition parameters, and then obtain the Kalman gain parameter set by comparing the noise covariance with the observed data after inverse calculation. Call the weighted attitude input dataset generated in the previous time step, which contains the weighted angular velocity vector. and acceleration vector State prediction is performed in an adaptive extended Kalman filter. First, the quaternion state vector representing the robot's current pose is used. With weighted angular velocity input, through a time step-based State transition parameters per second (i.e., state transition matrix) The calculation is performed on this matrix, which is constructed based on the robot's kinematics model. Specifically, the operation involves... With a and The quaternion update values ​​are multiplied to obtain the predicted state vector. Simultaneously, update the error covariance matrix of the state. Multiply it by the state transition matrix on the left. Multiply by the right again The transpose of the matrix, plus a process noise covariance matrix. ,Should The diagonal elements of the matrix are based on the random walk noise parameters of the gyroscope (e.g., The prediction error covariance is set to reflect the increase in uncertainty during the prediction process. Next, using observational data (such as weighted acceleration vectors) ( ) and an observation matrix that maps the state vector to the observation space. Perform the calculations and add the observation noise covariance matrix. (Its diagonal elements are based on the noise density parameters of the accelerometer, such as...) (Set the parameters), then invert the result of the operation, and then use this inverse matrix to calculate the prediction error covariance. and the transpose of the observation matrix Perform multiplication to obtain the Kalman gain parameter set.

[0037] S303: Based on the Kalman gain parameter set, perform a weighted update on the predicted state vector and the observed data, perform a weighted fusion of the residual components and the correspondence of the gain parameters to correct the state estimation result, and calculate the updated covariance data distribution to generate the fused attitude state estimation result. Based on the calculated Kalman gain parameter set (one The state is updated using the matrix (of the observed state) and the observation residual data. First, the observation residual is calculated, which is the predicted state vector of the current attitude. By transforming the observation model, a predicted gravitational acceleration vector is obtained. For example, according to The calculated predicted gravity vector is Then compare it with the actual weighted acceleration observations. Subtracting them yields the observed residual vector. Next, this residual vector Each component with the Kalman gain matrix The corresponding rows are weighted and merged, specifically by... and Perform matrix multiplication to obtain a state correction value. For example, if The first line and The result of multiplication is Then the first component of the state vector will be added with this correction value, and this correction amount will be... With the predicted state vector Adding them together yields the final state estimate after correction based on the observed data. As shown in Table 3, at the same time, Kalman gain is utilized. and observation matrix For the prediction error covariance matrix The update is performed by subtracting the identity matrix from the value of the identity matrix. and The product of , then the result is multiplied by Multiplying them yields the updated covariance matrix. The diagonal elements of the matrix become smaller, and the final output is the corrected quaternion attitude and the updated error covariance, generating the fused attitude state estimation result.

[0038] Table 3: Comparison of Attitude State Update Before and After Update

[0039] See Table 3, which shows the numerical changes of the four components of the attitude quaternion before and after the update with observation data and Kalman gain weighting, reflecting the corrective effect of the fusion process on attitude estimation.

[0040] Please see Figure 5 The specific steps of S4 are as follows: S401: Based on the current attitude angle and angular velocity data in the fused attitude state estimation results, the platform type parameter is detected, the dynamic constraint conditions are retrieved according to the type parameter, the weighted calculation is performed on the attitude angle change rate and angular velocity vector and the matrix feature quantity is extracted to generate the attitude constraint feature set; The current attitude angle is extracted from the generated fused attitude state estimation results, for example, by quaternions. Calculated roll angle Pitch angle With yaw angle and angular velocity data First, the platform type parameter stored in the system memory is detected, and the parameter value is read as "quadcopter UAV". Based on this type parameter, the corresponding constraint conditions are retrieved from the preset dynamic constraint database, as shown in Table 4. These conditions specify the maximum attitude angle, maximum angular velocity, and maximum angular acceleration. Next, the rate of change of the current attitude angle, i.e., the angular velocity vector, is considered. , and attitude angle vector A weighted calculation is performed, with the weighting coefficients set according to the control priority, including the weights of the attitude angles. Set as Weight of angular velocity Set as Its setting takes into account that exceeding the attitude angle limit poses a greater threat to flight stability than exceeding the angular velocity limit. The calculation process involves a weighted sum of the squares of the ratios between the current state value and its constraint boundary. Specifically, the characteristic components of the roll angle are... The characteristic components of the pitch angle are The characteristic components of the roll angular velocity are The characteristic components of pitch angular velocity are The characteristic components of yaw angular velocity are The calculated results of the components are summed to obtain a comprehensive matrix eigenvalue. This value, together with the original data of attitude angle and angular velocity, constitutes the attitude constraint feature set.

[0041] Table 4: Dynamic Constraints of Quadrotor UAVs

[0042] As shown in Table 4, this table details the various dynamic constraint boundary values ​​set for the quadcopter UAV platform. These values ​​will serve as key constraints in subsequent predictive control calculations.

[0043] S402: Based on the attitude angle and angular velocity data of the attitude constraint feature set, input the constraint conditions into the model predictive control algorithm for rolling optimization calculation, compare the angular velocity increment of the time domain window and filter the constraint data frames to obtain the attitude prediction state set; Based on the current attitude angle in the attitude constraint feature set and angular velocity data The retrieved dynamic constraints, such as the maximum pitch angle, will be used to determine the dynamic constraints. The input is fed into a model-predictive-based control calculation process, which has a length of [length missing]. Each time step, with a step size of Scrolling optimization is performed within a future time-domain window of seconds. First, a series of candidate control input sequences are generated, each sequence corresponding to a future... A set of motor speed adjustments at each time step is obtained. Then, using the UAV's dynamic model, forward simulation is performed on each candidate control sequence to predict the speed from the current moment to the future. The attitude angle and angular velocity trajectories within a second are compared during the simulation process for each predicted attitude angle and angular velocity increment. For example, at the [second]th [second]... Step, the angular velocity increment predicted by a candidate sequence is Check whether this increment will lead to an increase in angular velocity at the next moment. Exceed Constraints, or attitude angles Exceed The constraint, if the predicted trajectory is in the th... At that time step, the pitch angle reached The data frame containing the predicted trajectory is then identified as a constrained data frame, by adding a constraint to a cost function. A penalty term proportional to the square of the sum is used to filter out candidate sequences. Through iterative calculation, the sequence that, while satisfying the constraints, makes the predicted trajectory closest to the target pose (e.g., hovering state) is selected. The candidate control sequence is used, and the predicted attitude trajectory corresponding to the sequence is used as the attitude prediction state set.

[0044] S403: Call the attitude angle and angular velocity sequence data of the attitude prediction state set, calculate the angular velocity gain coefficient and attitude angle deviation based on the dynamic difference between the sequences, extract the sequence vector by linearly combining the gain coefficient and the deviation, and generate the optimized control command sequence. The attitude angle and angular velocity sequence data from the attitude prediction state set are retrieved. For example, for the roll axis, the first five values ​​of the predicted optimal attitude angle sequence are... The corresponding angular velocity sequence is First, for the current time (i.e., the first time step of the predicted sequence) The dynamic difference between the current attitude angle and the target attitude angle is calculated, and the attitude angle deviation is defined as the difference between the current attitude angle and the target attitude angle. The difference between ) Converted to radians Next, the angular velocity gain coefficient is calculated. This coefficient consists of a proportional term and a differential term. The gain coefficient of the proportional term... Set as Gain coefficient of the differential term Set as The coefficients are set based on the step response analysis of the UAV's physical model, selecting values ​​that can produce a critical damped response. The angular velocity gain coefficient is calculated by... Deviation from current attitude angle Multiply, plus Compared with the current angular velocity value Multiply, we get Then, the calculated gain coefficient and attitude angle deviation are linearly combined. This linear combination is the aforementioned calculation process, and the result is... As the first component of a sequence vector, this vector represents the control torque command applied to the roll axis. The same calculation process is performed for the pitch and yaw axes; for example, the calculation result for the pitch axis is... yaw axis is Finally, these three components are combined into a three-dimensional vector. Generate an optimized control instruction sequence.

[0045] Please see Figure 6 The specific steps of S5 are as follows: S501: Based on the first control instruction in the optimized control instruction sequence, extract the motor drive parameters, torque setpoint and target displacement coordinates, match the platform execution channel parameter set and correct the amplitude ratio deviation according to the control parameter reference set, and generate a differentiated decomposition parameter set; Based on the first control command in the generated optimized control command sequence, namely the three-dimensional torque vector (unit: First, the motor drive parameters and torque setpoints contained in the command are extracted. The torque setpoints are the three components of a vector, corresponding to the desired torques on the roll, pitch, and yaw axes, respectively. Next, the torque command is matched with a preset platform execution channel parameter set. For an "X"-shaped quadcopter UAV, this parameter set defines the physical positions and rotation directions of the four motors (numbered 1 to 4, representing right front, left rear, left front, and right rear, respectively), and a control allocation matrix that maps the three-axis torques to the thrust variations of the four motors. Then, the command is corrected based on a control parameter reference set, which sets the base throttle value for the UAV when hovering. For example, for an operating voltage of [missing information - likely a voltage value], [missing information - likely a voltage value]. In this system, the pulse width modulation (PWM) signal for each motor is set to [value] when hovering. microseconds ( This value corresponds to approximately The throttle output is then adjusted, and subsequently, by left-multiplying the torque command vector by the inverse of the control allocation matrix, the thrust change of each motor relative to the hovering reference is calculated, as shown in Table 5. For example, the roll torque... This is mainly achieved by increasing the thrust of motors 2 and 4 and decreasing the thrust of motors 1 and 3. The calculated changes in the PWM signals of the four motors are as follows: Finally, the change is compared with... The hovering reference values ​​are added together to obtain the final differentiated decomposition parameter set, which is the target PWM command for the four motors: Motor 1 is Motor 2 is Motor 3 is Motor 4 is .

[0046] Table 5: Control Command Decomposition Calculation Table

[0047] As shown in Table 5, this table illustrates the detailed calculation process of decomposing the overall three-axis torque control command into four independent motors. By combining the components with the hovering reference value, specific control parameters for each actuator are generated.

[0048] S502: Based on the differential decomposition parameter set, analyze the input signal channel of the high-frequency actuator drive circuit, extract the voltage amplitude and drive frequency parameters, compare and calculate the phase error vector, correct the phase offset parameters, and obtain the timing synchronous drive signal group. Based on the generated differential decomposition parameter set, i.e. the target PWM commands for the four motors First, the signal channels that will be input to the high-frequency actuator drive circuit (electronic speed controller, ESC) are analyzed to extract the core parameters of each channel signal. For PWM signals, the drive frequency parameter is fixed, for example, set to... This means that every Send a control pulse, and the voltage amplitude parameter is also a fixed logic level, for example... The core parameter that changes is the pulse duration, i.e., the aforementioned PWM command value. Next, an internal high-precision timer compares the deviation between the actual output time and the theoretical synchronization time of the four channel pulse signals. At the start of the control cycle, theoretically, the pulse signals of all four channels should simultaneously go high. However, due to minute signal delays in the physical circuitry, for example, the actual measured delays of the channels are... The delay value is compared with an ideal zero-delay reference to calculate the phase error vector. Subsequently, the phase shift parameter of the internal signal generator is corrected. Specifically, in the next control cycle, the pulse generation time of each channel is advanced relative to the start of the cycle. The amount of advancement is equal to the phase error measured in the previous step for that channel. For example, for a delay of... Channel 2, its pulse generation will be advanced. For delays of Channel 4, its pulse generation will be advanced. With this correction, the pulse signals of the four channels can be physically closer to being emitted synchronously, resulting in a timing synchronization drive signal group.

[0049] S503: Calls the timing synchronous drive signal group, performs selection and mapping of motor signal path, calculates current distribution ratio parameter matrix based on phase and amplitude difference, adjusts amplitude normalization, and outputs through high frequency actuator drive circuit to generate actuator control output signal; Invoke the modified timing synchronization drive signal group, which contains the target pulse width for four channels. And the corresponding phase compensation parameters. First, a preset selection and mapping is performed on the motor signal path, that is, the signal of channel 1 is fixedly routed to motor 1 at the front right, the signal of channel 2 is routed to motor 2 at the rear left, and so on. This mapping relationship is determined according to the physical wiring of the UAV during system initialization. Then, the current distribution ratio parameter matrix is ​​calculated based on the amplitude difference in the signal group. Here, amplitude refers to pulse width. The calculation process is to subtract the zero point value of the throttle signal from the pulse width of each motor (e.g., ), then divide by the full travel of the throttle signal (e.g. - This yields a standardized thrust percentage. For motor 1, the thrust percentage is... or Perform this calculation on the motor to obtain the current distribution ratio parameter matrix. Subsequently, the amplitude is finally normalized. This step is a hardware-level operation to ensure that the high-level voltage of the pulse generated by the signal generator is accurate. Low level is Furthermore, the duration of the pulse is strictly equal to the calculated target value. Finally, the digital signal, after precise timing and amplitude adjustment, is output to four high-frequency actuator drive circuits through the GPIO pins of the flight controller chip, generating the final actuator control output signal that drives the brushless motor.

[0050] Please see Figure 7 A real-time dynamic attitude stabilization control system based on multi-sensor fusion includes: The multi-source data synchronization module collects angular velocity and acceleration data through the inertial measurement unit, position coordinate data through the positioning module, image feature point data through the visual perception module, and contact force data through the force sensor. Based on the timestamp alignment method, it performs synchronization correction processing to generate a set of synchronized sensor data, which is then transmitted to the weight calculation and fusion module. The weight calculation and fusion module calls the synchronous sensor data set, calculates the noise stability coefficient based on the noise level, calculates the signal quality coefficient through the signal strength ratio, performs consistency verification to calculate the consistency coefficient, and generates dynamic fusion weight coefficients by weighted summation, which are then passed to the attitude estimation module. The attitude estimation module calls the dynamic fusion weight coefficients, performs weighted fusion of sensor attitude correlation data based on the weight coefficients, inputs the data into the adaptive extended Kalman filter, performs prediction and update steps to calculate the Kalman gain, outputs the fused attitude state estimation result, and passes it to the prediction control module. The predictive control module, based on the current attitude angle and angular velocity data in the fused attitude state estimation results, selects the corresponding dynamic constraint model according to the platform type, inputs the attitude state and constraint conditions into the model predictive control algorithm for rolling optimization calculation, generates an optimized control command sequence, and passes it to the execution drive module; The execution drive module calls the first control instruction in the optimized control instruction sequence, performs differentiated decomposition processing on the control instruction according to the platform type, and sends the control signal to the corresponding motor through the high-frequency actuator drive circuit to generate the actuator control output signal.

[0051] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0052] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0053] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0054] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0055] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0056] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A real-time dynamic attitude stabilization control method based on multi-sensor fusion, characterized in that, Includes the following steps: S1: Angular velocity and acceleration data are collected through the inertial measurement unit, position coordinate data are collected through the positioning module, image feature point data are collected through the visual perception module, and contact force data are collected through the force sensor. Synchronization correction processing is performed based on the timestamp alignment method to generate a synchronization sensor data set. S2: Based on the synchronous sensor data set, calculate the noise stability coefficient based on the noise level, calculate the signal quality coefficient through the signal strength ratio, perform consistency verification to calculate the consistency coefficient, and sum the coefficients by weight to generate dynamic fusion weight coefficients; S3: Call the dynamic fusion weight coefficients, perform weighted fusion of sensor attitude correlation data based on the weight coefficients, input the adaptive extended Kalman filter, perform prediction and update steps to calculate the Kalman gain, and output the fused attitude state estimation result; S4: Based on the current attitude angle and angular velocity data in the fused attitude state estimation results, select the corresponding dynamic constraint model according to the platform type, input the attitude state and constraint conditions into the model predictive control algorithm for rolling optimization calculation, and generate an optimized control command sequence.

2. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 1, characterized in that, The synchronous sensor data set includes time synchronization information, spatial registration information, and data integrity identifiers from multiple sensor sources. The dynamic fusion weight coefficients include noise weighting factors, signal confidence coefficients, and data consistency coefficients. The fused attitude state estimation results include attitude angle parameters, angular velocity parameters, and covariance matrix parameters. The optimized control command sequence includes target control variable parameters, constraint boundary parameters, and rolling optimization variables.

3. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 1, characterized in that, The specific steps of S1 are as follows: S101: Acquire angular velocity and acceleration data of the inertial measurement unit, coordinate data of the positioning module, image feature point data of the visual perception module, and contact force data of the force sensor. Sort the sensor data frames by timestamp, calculate the time difference and correct the drift frame time, and generate a preliminary time-aligned dataset. S102: Based on the preliminary time-aligned dataset, extract the timestamp sequences of angular velocity, acceleration, coordinates, image feature points and contact force data, perform interpolation resampling on the sequences, correct the offset points according to the unified time axis, and generate a time synchronization correction dataset. S103: Based on the time synchronization correction dataset, aggregate the corresponding time frames of the sensors to form synchronized recording frames of angular velocity vector, acceleration vector, coordinate matrix, image feature point matrix and contact force vector, perform time index consistency check, and generate a synchronized sensor data set.

4. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 3, characterized in that, The specific steps of S2 are as follows: S201: Based on the synchronous sensor data set, extract the instantaneous noise power density of the sensor signal, calculate the mean square error of the time series and compare it with the noise stability threshold, filter out the channels that are less than the threshold and perform normalization transformation to generate the noise stability coefficient. S202: Call the noise stability coefficient, calculate the signal strength ratio vector based on the ratio of signal strength to reference strength benchmark value in the synchronous sensor data set, and perform weighted correction on the signal strength ratio vector according to the noise stability coefficient. Then, perform amplitude normalization and layering on the ratio vector, extract the intensity difference to generate the signal quality coefficient. S203: Call the signal quality coefficient and noise stability coefficient, calculate the difference in the rate of change of values ​​within adjacent time windows and compare it with the consistency benchmark value, perform a weighted aggregation operation on the comparison result according to the degree of consistency matching, and generate dynamic fusion weight coefficients.

5. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 4, characterized in that, The noise stability threshold is determined based on the statistical characteristics of the mean square error distribution of the channel signals in the synchronous sensor data set within multiple time windows. The reference strength benchmark value is determined based on the long-term average strength value of the channel signal in the synchronous sensor data set; The consistency benchmark value is determined based on the statistical deviation characteristics of intensity changes in the original signal data.

6. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 4, characterized in that, The specific steps for S3 are as follows: S301: Based on the dynamic fusion weighting coefficient, perform weighted operation on the angular velocity data and acceleration data of the sensor attitude correlation data, compare the attitude sampling period according to the time series index, and weight the components with the same index according to the weighting coefficient to generate a weighted attitude input dataset. S302: Call the weighted attitude input dataset, perform prediction calculation in the adaptive extended Kalman filter based on the state transition parameters and noise covariance parameters, calculate the predicted state vector using the state transition parameters, and then obtain the Kalman gain parameter set by comparing the noise covariance with the observed data after inversion. S303: Based on the Kalman gain parameter set, perform a weighted update on the predicted state vector and the observed data, perform a weighted fusion correction on the correspondence between the residual components and the gain parameters, calculate the updated covariance data distribution, and generate the fused attitude state estimation result.

7. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 6, characterized in that, The specific steps of S4 are as follows: S401: Based on the current attitude angle and angular velocity data in the fused attitude state estimation results, the platform type parameter is detected, the dynamic constraint conditions are retrieved according to the type parameter, and a weighted calculation is performed on the attitude angle change rate and angular velocity vector and the matrix feature quantity is extracted to generate an attitude constraint feature set; S402: Based on the attitude angle and angular velocity data of the attitude constraint feature set, input the constraint conditions into the model predictive control algorithm for rolling optimization calculation, compare the angular velocity increment of the time window and filter the constraint data frames to obtain the attitude prediction state set. S403: Call the attitude angle and angular velocity sequence data of the attitude prediction state set, calculate the angular velocity gain coefficient and attitude angle deviation based on the dynamic difference between the sequences, extract the sequence vector by linearly combining the gain coefficient and the deviation, and generate an optimized control command sequence.

8. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 1, characterized in that, The method also includes step S5: S5: Call the first control instruction in the optimized control instruction sequence, perform differentiated decomposition processing on the control instruction according to the platform type, send the control signal to the corresponding motor through the high-frequency actuator drive circuit, and generate the actuator control output signal; The actuator control output signals include drive current signals, drive voltage signals, and motor torque signals.

9. The real-time dynamic attitude stabilization control method based on multi-sensor fusion according to claim 8, characterized in that, The specific steps of S5 are as follows: S501: Based on the first control instruction in the optimized control instruction sequence, extract the motor drive parameters, torque setpoint and target displacement coordinates, match the platform execution channel parameter set and correct the amplitude ratio deviation according to the control parameter reference set, and generate a differentiated decomposition parameter set; S502: Based on the differentiated decomposition parameter set, analyze the input signal channel of the high-frequency actuator drive circuit, extract the voltage amplitude and drive frequency parameters, compare and calculate the phase error vector, correct the phase offset parameters, and obtain the timing synchronous drive signal group. S503: Call the timing synchronous drive signal group to select and map the motor signal path, calculate the current distribution ratio parameter matrix based on the phase and amplitude difference, adjust the amplitude normalization, and output through the high-frequency actuator drive circuit to generate the actuator control output signal.

10. A real-time dynamic attitude stabilization control system based on multi-sensor fusion, characterized in that, The system is used to implement the real-time dynamic attitude stabilization control method based on multi-sensor fusion as described in any one of claims 1-9, and the system comprises: The multi-source data synchronization module collects angular velocity and acceleration data through the inertial measurement unit, position coordinate data through the positioning module, image feature point data through the visual perception module, and contact force data through the force sensor. Based on the timestamp alignment method, it performs synchronization correction processing to generate a set of synchronized sensor data, which is then transmitted to the weight calculation and fusion module. The weight calculation and fusion module calls the synchronous sensor data set, calculates the noise stability coefficient based on the noise level, calculates the signal quality coefficient through the signal strength ratio, performs consistency verification to calculate the consistency coefficient, and generates a dynamic fusion weight coefficient by weighted summation of the coefficients, which is then passed to the attitude estimation module. The attitude estimation module calls the dynamic fusion weight coefficients, performs weighted fusion of sensor attitude correlation data based on the weight coefficients, inputs the data into the adaptive extended Kalman filter, performs prediction and update steps to calculate the Kalman gain, outputs the fused attitude state estimation result, and transmits it to the prediction control module. The predictive control module, based on the current attitude angle and angular velocity data in the fused attitude state estimation results, selects the corresponding dynamic constraint model according to the platform type, inputs the attitude state and constraint conditions into the model predictive control algorithm for rolling optimization calculation, generates an optimized control command sequence, and passes it to the execution drive module; The execution drive module calls the first control instruction in the optimized control instruction sequence, performs differentiated decomposition processing on the control instruction according to the platform type, and sends the control signal to the corresponding motor through the high-frequency actuator drive circuit to generate the actuator control output signal.

Images