Greenhouse robot multi-sensor fusion positioning method based on multiple interactive models

Abstract

The invention relates to a greenhouse robot multi-sensor fusion positioning method based on interactive multiple models, and the method comprises the following steps: obtaining the observation data of a greenhouse robot, including the linear velocity and displacement data provided by an ODOM, the angular velocity and angle data provided by an IMU, and the indoor positioning information provided by UWB; the observation data are substituted into an improved interactive multi-model algorithm, the improved interactive multi-model algorithm comprises a uniform-speed motion model and a uniform steering model, and a positioning result of the greenhouse robot is obtained by performing weighted fusion on state estimation of all the models; wherein according to the improved interactive multi-model algorithm, a filter is replaced by an improved extended Kalman filter, a sliding window mechanism is introduced into the improved extended Kalman filter to process the observation data, and a covariance matrix is dynamically adjusted. Compared with the prior art, the method has the advantages of high positioning precision, high real-time performance, low cost and the like.

Description

Technical Field

[0001] The present invention belongs to the technical field of greenhouse robot positioning, and in particular relates to a multi-sensor fusion positioning method for greenhouse robots based on an interactive multi-model. Background Art

[0002] In outdoor environments, the positioning of robots mostly uses the Global Navigation Satellite System (GNSS). However, in complex greenhouse environments, GNSS cannot obtain high-precision positioning information. Ultra-wideband (UWB) technology has accurate positioning, but errors will occur when there are obstructions. Inertial navigation (IMU) can measure motion in real time but has cumulative errors. The combination of the two can achieve complementary advantages and effectively improve positioning accuracy. Therefore, indoor fusion positioning based on UWB and IMU has received extensive attention. However, this technology still faces problems such as noise interference and insufficient accuracy in practical applications. Considering the complexity of the robot's motion state, a single motion model is difficult to accurately match its motion state, thus affecting the positioning accuracy. The Interactive Multi-Model (IMM) algorithm aims at multiple models that the target may have, and through model filtering estimation, probability update, and weighted calculation, realizes real-time positioning and tracking of the motion target state, and has been applied in fields such as vehicles and unmanned aerial vehicles.

[0003] Literature (Wang Fei. Research on Indoor Mobile Robot Navigation and Positioning Technology Based on UWB [D]. Harbin Engineering University, 2017.) designed a navigation and positioning method that fuses UWB / IMU for indoor service robots. It optimizes the positioning accuracy through two-way ranging, selects the best data using the HDOP value, and processes gyroscope data using the extended Kalman filter, greatly improving the positioning accuracy and stability. Literature (Long Zhenhuan. Research on Greenhouse Robot Positioning Based on Multi-Sensor Fusion [D]. Hunan Agricultural University, 2022. DOI: 10.27136 / d.cnki.ghunu.2022.000702.) proposed a positioning method for greenhouse robots based on the multi-sensor fusion of UWB / IMU / ODOM / LIDAR. This method effectively reduces the cumulative positioning error of the robot in the greenhouse and improves the positioning accuracy of the robot in the greenhouse environment. Both of the above two methods fuse radar data. The point cloud data volume of the radar is very large, and the cost of the radar is also relatively high. Therefore, the above methods have problems of relatively high computational cost and construction cost. Literature (Yang Xiujian, Ao Peng, Shen Shiquan, etc. UWB / LiDAR / IMU Combined Positioning Method for Complex Environments [J]. Journal of Chinese Inertial Technology, 2024, 32(07): 654-662. DOI: 10.13695 / j.cnki.12-1222 / o3.2024.07.003.) proposed a UWB / LiDAR / IMU combined positioning method for complex environments. The relocalization based on UWB / LiDAR is combined with IMU for position calculation through IMM-UKF. This method has relatively high positioning accuracy, but high cost and large computational amount. Therefore, it is necessary to design a multi-sensor fusion positioning method for greenhouse robots with high positioning accuracy, strong real-time performance, and low cost.) Summary of the Invention

[0004] The purpose of the present invention is to overcome the defects of the above-mentioned existing technologies and provide a multi-sensor fusion positioning method for greenhouse robots based on an interactive multi-model, which has the advantages of high positioning accuracy, strong real-time performance, and low cost.)

[0005] The purpose of the present invention can be achieved through the following technical solutions:

[0006] The present invention provides a multi-sensor fusion positioning method for greenhouse robots based on an interactive multi-model, including the following steps:

[0007] Obtain the observation data of the greenhouse robot, where the observation data includes the linear velocity and displacement data provided by ODOM, the angular velocity and angle data provided by IMU, and the indoor positioning information provided by UWB;

[0008] Substitute the observed data into the improved interactive multiple model algorithm, which includes a uniform motion model and a uniform turning model, and obtain the positioning result of the greenhouse robot by weighted fusion of the state estimates of each model;

[0009] Among them, the improved interactive multiple model algorithm replaces the filter with an improved extended Kalman filter, and a sliding window mechanism is introduced in the improved extended Kalman filter to process the observed data and dynamically adjust the covariance matrix.

[0010] Further, the specific process of processing the observed data based on the sliding window mechanism is as follows:

[0011] Obtain the current observed data, and judge whether it exceeds the preset window. If so, reconstruct the data cache, remove the oldest observed data from the data cache, write the latest observed data into the cache, and then perform subsequent data processing; if not, directly perform subsequent data processing;

[0012] Screen the valid values of the observed data;

[0013] Calculate the weighted average of the valid values of the observed data.

[0014] Further, the specific process of screening the valid values of the observed data is as follows:

[0015] Calculate the average value μ of the observed data:

[0016]

[0017] Among them, N is the preset window, and x i is the observed value;

[0018] Calculate the standard value σ of the observed data:

[0019]

[0020] Screen out the observed values whose absolute value of the difference from the average value μ is less than 2 times the standard deviation σ, which are the valid values y j :

[0021] y j ={x i ||x i -μ|<2σ}.

[0022] Further, the weighted average of the valid values is The specific calculation formula is as follows:

[0023]

[0024] Among them, ω jis the weight of the valid observation value y j , and M is the number of valid observation values.

[0025] Furthermore, the covariance matrix after dynamic adjustment is R adj , and the specific expression is as follows:

[0026]

[0027] where R is the initial covariance matrix.

[0028] Furthermore, the improved interactive multiple model algorithm includes an input interaction stage, a filtering estimation stage, a model probability update stage, and an estimation fusion stage. Among them,

[0029] the input interaction stage is used to calculate the mixed state estimation of each model at time (k - 1) according to the state estimation and probability of each model at time (k - 1);

[0030] the filtering estimation stage is used to perform filtering calculations through the improved extended Kalman filter, based on the observation data at time (k - 1) and the mixed state estimation of each model, to obtain the vector residual between the predicted value and the observed value of the state vector at time k, the information observation prediction covariance matrix, and the state estimation of each model, and then calculate the approximate function of each model;

[0031] the model probability update stage is used to update the probability of each model at time k according to the approximate function of each model;

[0032] the estimation fusion stage is used to perform weighted fusion on the probability of each model at time k and the state estimation of each model to obtain the positioning result of the greenhouse robot.

[0033] Furthermore, when calculating the mixed state estimation in the interaction stage, the probability conversion of each model conforms to the Markov model probability transition relationship.

[0034] Furthermore, the approximate function Λ j (k) of model j at time k has the following specific expression:

[0035]

[0036] where y j (k) is the vector residual between the predicted value and the observed value of the state vector at time k, and S j (k) is the information observation prediction covariance matrix at time k.

[0037] Furthermore, the observation data is received and processed through the ROS system, and after coordinate transformation, it is substituted into the improved interactive multiple model algorithm.

[0038] Further, the state vector X corresponding to the uniform motion model CV and the model state transition matrix F CV are respectively:

[0039] X CV = [x y θ v] T

[0040]

[0041] The state vector X corresponding to the uniform turning model CT and the model state transition matrix F CT are respectively:

[0042] X CT = [x y θ v ω] T

[0043]

[0044] where x is the abscissa, y is the ordinate, θ is the heading angle, v is the linear velocity, ω is the angular velocity, and dt is the sampling period.

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] 1. The present invention proposes a multi-sensor fusion positioning method for greenhouse robots based on an interactive multi-model. Through an improved interactive multi-model algorithm, the positioning result of the greenhouse robot is calculated according to the observation data of the greenhouse robot. Specifically, the observation data specifically includes the linear velocity and displacement data provided by ODOM, the angular velocity and angle data provided by IMU, and the indoor positioning information provided by UWB. Among them, the linear velocity and displacement data provided by ODOM can be used to monitor the motion state of the greenhouse robot and correct the cumulative error of IMU. The angular velocity and angle data provided by IMU can be used to predict the position of the greenhouse robot and correct the heading angle of ODOM. The indoor positioning information provided by UWB can be used for the global positioning of the greenhouse robot and correct the cumulative errors of ODOM and UWB. The above three types of data complement each other's advantages and provide data support for low-cost, high-precision, and high-robustness positioning. In the present invention, the improved interactive multi-model algorithm replaces the filter with an extended Kalman filter that can better handle nonlinear systems and has high computational efficiency and applicability, and further improves the extended Kalman filter. A sliding window mechanism is introduced in the improved extended Kalman filter to process the observation data, which can enhance the connection between data and reduce fluctuations. The improved extended Kalman filter also dynamically adjusts the covariance matrix, improving the filtering effect and the real-time performance and robustness of the system. Based on the above design, the present invention can, to a certain extent, smooth the state estimation result, reduce the state estimation fluctuations caused by abnormal measurement values, and improve the positioning accuracy and stability.

[0047] 2. The positioning method proposed by the present invention is not limited to the scenario of the present invention, and can also meet the positioning requirements in other application scenarios, and further functions such as mapping and navigation can be realized based on the positioning, which has high flexibility and application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 It is a system schematic diagram of an embodiment of the present invention.

[0049] Description of the reference numerals: 1. The first UWB positioning base station; 2. The second UWB positioning base station; 3. The third UWB positioning base station; 4. The fourth UWB positioning base station; 5. The chassis of the greenhouse agricultural robot; 6. IMU; 7. UWB positioning tag; 8. Industrial control computer; 9. Greenhouse vegetables.

[0050] Figure 2 It is a plan view of the chassis of the greenhouse agricultural robot according to an embodiment of the present invention.

[0051] Figure 3 It is a flowchart of the EKF filtering based on a sliding window (IEKF flowchart) according to an embodiment of the present invention.

[0052] Figure 4 It is a flowchart of the IMM-IEKF algorithm according to an embodiment of the present invention.

[0053] Figure 5 It is a probability transition diagram of the Markov model according to an embodiment of the present invention.

[0054] Figure 6 It is a comparison diagram of the simulation trajectories according to an embodiment of the present invention.

[0055] Figure 7 It is a comparison diagram of the RMSE of the simulation sampling points according to an embodiment of the present invention.

[0056] Figure 8 It is the robot positioning trajectory according to an embodiment of the present invention.

[0057] Figure 9 It is a comparison diagram of the RMSE of the experimental sampling points according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0058] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented on the premise of the technical solution of the present invention, and the detailed implementation manner and specific operation process are given, but the protection scope of the present invention is not limited to the following embodiments.

[0059] Embodiment:

[0060] This embodiment provides a multi-sensor fusion positioning method for a greenhouse agricultural robot based on an interactive multi-model. The overall process is as follows:

[0061] First, select appropriate sensors for the positioning of greenhouse agricultural robots. In this embodiment, three sensors, namely an odometer (ODOM), an inertial measurement unit (IMU), and an ultra-wideband positioning module (UWB), are selected. The ODOM is used to provide accurate linear velocity and displacement data, the IMU is used to provide accurate angular velocity and angle data, and the UWB is used to provide high-precision indoor positioning information. Build a UWB positioning system according to the actual working environment, install the IMU and calibrate the zero bias, then connect the three sensors of ODOM, IMU, and UWB to the greenhouse robot system, receive and process the sensor data in the ROS system, and perform coordinate transformation.

[0062] Then, design a kinematic model suitable for the greenhouse robot, replace the filter in the interactive multiple model algorithm with an extended Kalman filter (EKF), and introduce a sliding window mechanism based on the extended Kalman filter to design an improved extended Kalman filter (IEKF) to complete the design of the improved interactive multiple model algorithm (IMM-IEKF).

[0063] Finally, through the designed greenhouse robot motion model and the improved interactive multiple model algorithm, estimate the position of the greenhouse robot according to the observation data obtained by the three sensors.

[0064] The specific implementation method is as follows:

[0065] I. Calibration and data transmission of sensors

[0066] First, fix and calibrate the installation of the sensors, and ensure that the data can be connected to the same system to facilitate the acquisition of sensor data in subsequent fusion positioning, which mainly includes two parts: calibration and data transmission.

[0067] 1. Calibration of the sensor system

[0068] As shown in the system of the present invention Figure 1 as shown, fix four UWB positioning base stations at the four corners of the greenhouse respectively to ensure that the positioning range covers the entire greenhouse. Take the first UWB positioning base station (1) as the main base station, determine the position of the base station through measurement, and establish a UWB-based global coordinate system with the main base station as the origin. Install the IMU and the UWB positioning tag at the plane center position of the greenhouse agricultural robot. As Figure 2 shown, the IMU and the UWB positioning tag can be at different heights, which can reduce the measurement error caused by installation to a certain extent, and calibrate the IMU under static conditions to improve the measurement accuracy and reliability.

[0069] 2. Data transmission

[0070] The linear velocity and displacement data provided by ODOM can be used to monitor the motion state of the greenhouse agricultural robot and correct the cumulative error of the IMU. The acceleration, angular velocity, and angle data provided by the IMU can be used to predict the position of the greenhouse agricultural robot and correct the heading angle of ODOM. The high-precision indoor positioning data provided by UWB can be used for the global positioning of the greenhouse agricultural robot and correct the cumulative errors of ODOM and UWB. The three sensors of ODOM, IMU, and UWB complement each other through their respective advantages and collect data under the condition of the same frequency of 50hz, which can achieve low-cost, high-precision, and high-robustness positioning of the greenhouse agricultural robot in the greenhouse environment. Connect the UWB positioning tag, IMU, and the chassis of the greenhouse agricultural robot to the industrial control computer installed with the AutolaborOS system and the ROS-Melodic robot operating system through the serial port, and read the sensor data (observation data) through the topic at a frequency of 50hz for subsequent fusion.

[0071] II. Design of positioning algorithm

[0072] 1. Design of motion model

[0073] For the positioning problem of the greenhouse agricultural robot based on the motion model and motion state matching, the multi-model interaction of the present invention mainly includes two models: the constant velocity motion model (CV) and the constant turning model (CT). Under the measurement of a 50hz frequency, all motion states can match these two models.

[0074] The state vector X corresponding to the constant velocity motion model (CV) CV and the model state transition matrix F CV are respectively:

[0075] X CV = [x y θ v] T

[0076]

[0077] The state vector X corresponding to the constant turning model (CT) CT and the model state transition matrix F CT are respectively:

[0078] x CT = [x y θ v ω] T

[0079]

[0080] where x is the abscissa, y is the ordinate, θ is the heading angle, v is the linear velocity, ω is the angular velocity, and dt is the sampling period.

[0081] 2. Design of improved interactive multi-model algorithm (IMM-IEKF)

[0082] The IMM is mainly divided into four parts: input interaction, filtering estimation, model probability update, and estimation fusion. In the input interaction stage, the state and probability of the current model are input into the system; in the filtering estimation stage, the states of different models are estimated and predicted through a filter; in the model probability update stage, the probabilities of different models are updated according to the latest observation data; in the estimation fusion stage, the estimation results of different models are weighted and fused.

[0083] In this embodiment, the extended Kalman filter that can better handle nonlinear systems while maintaining high computational efficiency and applicability is selected in the filtering estimation stage, and improvements are made on this basis. By introducing a sliding window mechanism to process the observation data, enhancing the connection between data, reducing fluctuations, and dynamically adjusting the covariance matrix, the filtering effect and the real-time performance and robustness of the system are improved. The EKF filtering process based on the sliding window (IEKF process) is as Figure 3 shown: First, set the initial state and covariance matrix, predict the next state and covariance according to the motion model of the system, update the state and covariance according to the observed data, and calculate the Kalman gain. After receiving new observation data, first judge whether the data cache exceeds the preset window. If it exceeds the window, reconstruct the data cache, remove the oldest cached data, write the new observation data into the cache, and perform subsequent data processing. If it does not exceed the window, directly perform subsequent data processing. Then, enter the valid value screening stage. First, calculate the average value μ of the observation data, then calculate the standard value σ of the observation data, and then screen out the observed values whose absolute value of the difference from the average value μ is less than 2 times the standard deviation σ, which are the valid values y of the observation data j , and the specific formula is as follows:

[0084]

[0085] y j ={x i ||x i -μ|<2σ}

[0086] where N is the preset window, and x i is the observed value.

[0087] After the valid value screening stage is completed, calculate the weighted average of the valid values. When calculating the weighted average, this embodiment considers assigning different weights to new and old data according to data real-time performance to ensure that the weight of new data is higher; in the dynamic adjustment mechanism of the covariance matrix, if the number of valid observed values decreases, it indicates that the uncertainty and volatility of the current data set increase accordingly. At this time, the value of parameter R should be increased accordingly to reflect this increased uncertainty. The above process is as follows:

[0088]

[0089] Among them, is the weighted average of the effective values, ω j is the weight of the effective observation value y j , M is the number of effective observation values, and R is the initial covariance matrix.

[0090] By adding a sliding window mechanism to process the observation data and dynamically adjusting the covariance matrix to improve the extended Kalman filter algorithm, the state estimation result can be smoothed to a certain extent, the state estimation fluctuation caused by abnormal measurement values can be reduced, and the positioning accuracy and stability can be improved.

[0091] The flowchart of the improved interactive multiple model algorithm (IMM-IEKF) is as Figure 4 shown. The motion models are two types: constant velocity motion model (CV) and constant turn model (CT); the filter is the improved extended Kalman filter (IEKF); the observation inputs are ODOM, IMU, and UWB.

[0092] S1. Input interaction stage: First, initialize the probability of the model and the transition probability at the next moment. As Figure 5 shown, the probability conversion between motion models conforms to the Markov model probability transition relationship. Among them, M ij represents the probability that the robot transitions from motion model i to motion model j. In this embodiment, i = 1, 2 and j = 1, 2.

[0093] Let the model probability of model i at time k - 1 be μ i (k - 1), then the prediction probability normalization constant of each model is:

[0094]

[0095] Then the mixing probability from model i to model j is:

[0096]

[0097] According to the input state estimates x i1 (k - 1), x i2 (k - 1), calculate the mixing state estimate of model j:

[0098]

[0099] S2. Filtering estimation stage: Through the improved extended Kalman filter (IEKF), perform filtering calculations based on the above mixing state estimate x ij (k - 1) and the observation input z(k - 1) to obtain the vector residual y j (k) between the predicted value and the observed value of the state vector at time k, and the information observation prediction covariance matrix Sj (k), and the state estimate x of each model j (k):

[0100] y j (k) = z(k - 1) - H(k)x ij (k - 1)

[0101] S j (k) = H(k)P(k - 1)H(k) T + R(k)

[0102] x j (k) = x j (k - 1)+ K j (k)y j (k)

[0103] S3. Model probability update stage: Calculate the approximate function Λ j (k) of each model, and update the probability μ of each model at time k according to the approximate function of each model j :

[0104]

[0105] where c is the normalization constant, is the prediction probability normalization constant:

[0106]

[0107] S4. Estimation fusion stage: Perform weighted fusion on the probability μ j (k) of each model at time k and the state estimate x j (k) of each model to obtain the positioning result of the greenhouse robot:

[0108]

[0109] To verify the effectiveness of the above method, corresponding experimental simulations were carried out in this embodiment.

[0110] 1. Simulation analysis

[0111] Set the simulation target to start at (0, 0), and successively go through the processes of uniform motion, uniform turning, uniform motion, and uniform turning. The uniform linear velocity is 0.2 m / s, the uniform turning angular velocity is 0.2 rad / s, the sampling period is T = 0.02 s, and the initial state is [0 0 0 0.20] T , and the model transition matrix is μ = [0.8 0.2].[[]]

[0112] Figure 6This is a comparison graph of the real trajectory and the trajectories of three positioning algorithms (EKF, IEKF, IMM-IEKF) in the simulation experiment. From Figure 6 The partial enlarged view shows that the positioning effect of IEKF is better than that of the traditional EKF, and the positioning result of IMM-IEKF is the closest to the real trajectory.

[0113] Figure 7 This is a comparison graph of the root mean square error (RMSE) of the three positioning algorithms (EKF, IEKF, IMM-IEKF) at the sampling point positions in the simulation experiment. From Figure 7 it can be seen that overall, the root mean square error of IEKF at the sampling point positions is less than that of EKF, and the root mean square error of the IMM-IEKF positioning algorithm at the sampling point positions is less than that of the other two algorithms.

[0114] Table 1 Comparison of simulation performance

[0115]

[0116] From the average value of the positioning RMSE and the positioning accuracy given in Table 1, it can be seen more clearly that the IEKF algorithm has a better positioning effect than the traditional EKF algorithm, with a 21.2% improvement in positioning accuracy; the positioning accuracy of the IMM-IEKF algorithm can reach 0.0392 m, and compared with single UWB, traditional EKF, and IEKF, the positioning accuracy has increased by 57.2%, 27.3%, and 7.8% respectively.

[0117] 2. Experimental verification and analysis

[0118] This embodiment also conducts experimental tests. It is carried out in a laboratory equipped with a high-precision Vicon optical motion capture system. The infrared high-speed camera captures the light-emitting marker points installed on the robot as the true value of the robot's motion trajectory. The positions of the UWB base stations are calibrated through the Vicon optical motion capture system. Taking the UWB coordinate system as the reference coordinate system, the positioning tags are arranged at the geometric center of the robot, and the positioning base stations are arranged in a rectangular area of 2.5 m × 4 m.

[0119] Figure 8 and Figure 9 This is a comparison graph of the single-point positioning error of the robot positioning trajectory and the sampling points in the experiment. It can be seen from the graph that the IMM-IEKF positioning algorithm has the highest degree of fit between the final positioning trajectory and the real trajectory and the smallest positioning error at the sampling points compared with EKF and IEKF.

[0120] Table 2 Comparison of experimental performance

[0121]

[0122] Table 2 shows the mean RMSE and positioning accuracy of the robot in the experiment. It can be seen that the IEKF algorithm has better positioning effect than the traditional EKF algorithm, and the positioning accuracy is improved by 12.9%; the positioning accuracy of the IMM-IEKF algorithm can reach 0.0468 m, and compared with single UWB, traditional EKF and IEKF, the positioning accuracy is improved by 49.4%, 28.8% and 18.2% respectively.

[0123] In summary, the multi-sensor fusion positioning method for greenhouse agricultural robots proposed in the present invention, from theoretical analysis to verification of embodiments, can illustrate that the present invention has the characteristics of high positioning accuracy, strong real-time performance, low cost, etc. The designed sensor combination method makes full use of the advantages of various sensors for complementarity, making up for the disadvantages of single sensors; the designed fusion method of interactive multi-model fully considers the non-linear characteristics of the motion state of greenhouse agricultural robots, as well as the real-time performance and complexity of the system, so as to fully improve the utilization of sensor data and the system response, and can achieve better positioning effects.

[0124] The above description of the embodiments is to enable those of ordinary skill in the art to understand and use the invention. It is obvious that those skilled in the art can easily make various modifications to these embodiments and apply the general principles described herein to other embodiments without creative work. Therefore, the present invention is not limited to the above embodiments, and the improvements and modifications made by those skilled in the art without departing from the scope of the present invention according to the disclosure of the present invention should be within the protection scope of the present invention.

Claims

1. A greenhouse robot multi-sensor fusion positioning method based on interactive multi-model, characterized in that: The following steps are involved: Acquire observation data of the greenhouse robot, wherein the observation data includes linear velocity and displacement data provided by ODOM, angular velocity and angle data provided by IMU, and indoor positioning information provided by UWB; Substituting the observed data into an improved interactive multi-model algorithm, wherein the improved interactive multi-model algorithm includes a uniform motion model and a uniform steering model, and obtaining a positioning result of the greenhouse robot by weighted fusion of state estimates of each model; Among them, the improved interactive multi-model algorithm replaces the filter with an improved extended Kalman filter, and the improved extended Kalman filter introduces a sliding window mechanism to process the observation data and dynamically adjusts the covariance matrix.

2. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 1 is characterized in that: The specific process of processing the observation data based on the sliding window mechanism is: Get the current observation data and determine whether it exceeds the preset window. If so, reconstruct the data cache, remove the oldest observation data in the data cache, write the latest observation data into the cache, and then perform subsequent data processing; if not, directly perform subsequent data processing; Filter valid values ​​of observation data; Calculate the weighted average of the effective values ​​of the observed data.

3. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 2 is characterized in that: The specific process of screening the valid values ​​of observation data is as follows: Calculate the mean μ of the observed data: Among them, N is the preset window, x i is the observed value; Calculate the standard value σ of the observed data: Screen out the observations whose absolute value of the difference from the mean value μ is less than 2 times the standard deviation σ, which is the effective value y of the observation data j : y j ={x i ||x i -μ|<2σ}。 4. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 3 is characterized in that: The weighted average of the effective values ​​is The calculation formula is as follows: Among them, ω j is the effective observation value y j The weight of , M is the number of valid observations.

5. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 4 is characterized in that: The dynamically adjusted covariance matrix is ​​R adj , the expression is as follows: Among them, R is the initial covariance matrix.

6. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 1 is characterized in that: The improved interactive multi-model algorithm includes an input interaction stage, a filter estimation stage, a model probability update stage and an estimation fusion stage, wherein: The input interaction phase is used to calculate the mixed state estimate of each model at time (k-1) based on the state estimate and probability of each model at time (k-1); The filtering estimation stage is used to perform filtering calculations based on the observation data at time (k-1) and the mixed state estimation of each model through the improved extended Kalman filter, obtain the vector residual of the state vector prediction value and the observation value at time k, the information observation prediction covariance matrix, and the state estimation of each model, and then calculate the approximate function of each model; The model probability update stage is used to update the probability of each model at time k according to the approximate function of each model; The estimation fusion stage is used to perform weighted fusion on the probability of each model at time k and the state estimation of each model to obtain the positioning result of the greenhouse robot.

7. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 6 is characterized in that: When calculating the mixed state estimation in the interaction phase, the probability conversion of each model conforms to the probability transfer relationship of the Markov model.

8. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 6 is characterized in that: The approximate function Λ of model j at time k j The expression of (k) is as follows: Among them, y j (k) is the vector residual of the predicted value and the observed value of the state vector at time k, S j (k) is the information observation prediction covariance matrix at time k.

9. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 1 is characterized in that: The observation data is received and processed by the ROS system, and the coordinate transformation is performed before inputting into the improved interactive multi-model algorithm.

10. The greenhouse robot multi-sensor fusion positioning method based on interactive multi-model according to claim 1, characterized in that: The state vector X corresponding to the uniform motion model CV and the model state transfer matrix F CV They are: X CV =[xy θ v] T The state vector X corresponding to the uniform steering model CT and the model state transfer matrix F CT They are: X CT =[xy θ v ω] T Among them, x is the horizontal coordinate, y is the vertical coordinate, θ is the heading angle, v is the linear velocity, ω is the angular velocity, and dt is the sampling period.

Images