A CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots

Abstract

This invention discloses a CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots, comprising: acquiring IMU-UWB multi-source localization data of the tracked robot and constructing a system state vector; constructing a joint observation model based on UWB ranging observation information and the introduced nonholonomic constraint NHC observation information of the tracked robot; predicting the state of the tracked robot using IMU data, and jointly incorporating UWB observation information and nonholonomic constraint NHC observation information into the extended Kalman filter update process to form an IMU-UWB-NHC fusion localization model; establishing a CEGWO-based fusion filter parameter optimization model; iteratively optimizing the filter parameter vector to be optimized in the CEGWO-based fusion filter parameter optimization model using the Cooperative Enhanced Grey Wolf Optimization Algorithm (CEGWO), and inputting the optimized optimal filter parameters into the IMU-UWB-NHC fusion localization model to output the position, velocity, and attitude estimation results of the tracked robot in the target environment. This invention can effectively improve the localization accuracy, stability, and robustness of tracked robots in complex orchard environments.

Description

Technical Field

[0001] This invention relates to the field of intelligent positioning for tracked robots, specifically to a CEGWO-optimized IMU-UWB-NHC fusion positioning method for tracked robots. Background Technology

[0002] With the development of smart agriculture and orchard automation technologies, tracked transport robots are widely used in orchard transportation, harvesting assistance, and inspection due to their advantages such as strong terrain adaptability, high load-bearing capacity, and good maneuverability. When tracked robots autonomously drive and operate in orchard environments, they need to obtain their own position and motion status in real time. Therefore, the accuracy, stability, and robustness of the positioning system directly affect their path tracking, obstacle avoidance decisions, and operational efficiency.

[0003] To improve positioning performance, various UWB and IMU fusion positioning schemes have been proposed in the prior art. For example, prior art document 1 (authorization announcement number CN121540139B) and prior art document 2 (authorization announcement number CN111982102B) are mainly aimed at general UWB / IMU fusion positioning. Although they can utilize the short-term prediction capability of IMU and the absolute positioning capability of UWB to a certain extent, they do not establish a special model for the motion characteristics of tracked robots in orchard environments. In particular, they do not introduce the nonholonomic constraint that the lateral velocity of the tracked robot in the body coordinate system is approximately zero during normal driving into the fusion positioning process, resulting in insufficient utilization of the motion constraint information of the tracked robot. Prior art document 3 (authorization announcement number CN115031723B) and prior art document 4 (authorization announcement number CN112747747B) mainly focus on improving UWB positioning accuracy or indoor positioning scenarios. Their positioning models, environmental assumptions and application objects are different from the outdoor operation scenarios of tracked robots, and are difficult to be directly applied to orchard environments with tree obstruction, ground undulations and obvious non-line-of-sight propagation. Summary of the Invention

[0004] This invention provides a CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots. This method fuses IMU data, UWB data, and nonholonomic constraint NHC information of the tracked robot. It uses the extended Kalman filter algorithm to predict and update the motion state of the tracked robot, and adopts the cooperative enhanced gray wolf optimization algorithm (CEGWO) to adaptively optimize the filter parameters in the extended Kalman filter. This can reduce the impact of tree occlusion, multipath propagation, non-line-of-sight propagation, and IMU error accumulation on the localization results in outdoor orchard environments, thereby improving the localization accuracy of tracked robots in complex orchard environments.

[0005] The technical solution of this invention is:

[0006] According to a first aspect of the present invention, a CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots is provided, comprising:

[0007] Step S1: Obtain IMU-UWB multi-source localization data of the tracked robot and construct the system state vector; wherein, the IMU-UWB multi-source localization data of the tracked robot includes IMU data and UWB data;

[0008] Step S2: Construct a joint observation model based on UWB ranging observation information and the introduced nonholonomic constraint NHC observation information of the tracked robot; wherein, UWB ranging observation information includes UWB ranging information and UWB observation information;

[0009] Step S3: Use IMU data to predict the state of the tracked robot, and combine UWB observation information and nonholonomic constraint NHC observation information into the extended Kalman filter update process to form an IMU-UWB-NHC fusion localization model; the process noise covariance matrix in the extended Kalman filter update process is used to predict the state of the tracked robot. UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix As the filtering parameters to be optimized;

[0010] Step S4: Based on the filtering parameters to be optimized, establish a fusion filtering parameter optimization model based on CEGWO;

[0011] Step S5: The Cooperative Enhanced Grey Wolf Optimization Algorithm (CEGWO) is used to iteratively optimize the filter parameter vector in the fusion filter parameter optimization model based on CEGWO constructed in Step S4. The optimized filter parameters are then input into the IMU-UWB-NHC fusion localization model, and the position, velocity and attitude estimation results of the tracked robot in the target environment are output.

[0012] Further, step S1 involves: acquiring accelerometer bias and gyroscope bias during the robot's movement as IMU data using an IMU module installed on the tracked robot; acquiring the robot's position in the world coordinate system as UWB data using a UWB positioning module installed on the tracked robot; and constructing a system state vector. :

[0013] ;

[0014] in: For the first The position of the Time Robot in the world coordinate system. For the first The linear velocity of the robot at all times. For the first The pose quaternion of the time-lapse robot, and The first Accelerometer bias and gyroscope bias during the robot's movement.

[0015] Furthermore, the joint observation model established in step S2 includes:

[0016] Let the first The location of each UWB base station in the world coordinate system is as follows: Tracked robot The position of the time in the world coordinate system is Then the robot and the first The theoretical distance between UWB base stations for:

[0017] ;

[0018] After considering ranging noise, based on the robot and the first The theoretical distance between UWB base stations , obtained the UWB observation information ;

[0019] + ;

[0020] in, Indicates the first Measurement noise of a UWB base station;

[0021] Tracked robots based on received UWB base station ranging information is used to construct a UWB observation information vector. ;

[0022] Simultaneously, based on the motion characteristic of a tracked robot where the lateral velocity in the body coordinate system is approximately zero during normal operation, nonholonomic constraint NHC observation information is introduced. Represented as:

[0023] ;

[0024] in, Let be the lateral velocity of the robot in the body coordinate system. This is nonholonomic constraint observation noise.

[0025] Further, step S3 is as follows:

[0026] In the state prediction phase, the state of the tracked robot is propagated using IMU data. The extended Kalman filter state covariance propagation is expressed as:

[0027] ;

[0028] in, For the first The state covariance matrix at time step, For the first The state transition matrix at each time step, The process noise covariance matrix; Indicates transpose;

[0029] During the observation update phase, UWB observation information and non-holonomically constrained NHC observation information are combined to form the first... Time-unified observation vector :

[0030] ;

[0031] in, For UWB observation information vectors, This is non-holonomic constrained NHC observation information;

[0032] Unified observation vector corresponding to the observation noise covariance matrix Represented as:

[0033] ;

[0034] in, Measure the noise covariance matrix for UWB base stations. This is the nonholonomic constraint observation noise covariance matrix.

[0035] Further, step S4 includes:

[0036] Based on the process noise covariance matrix UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix Define the filter parameter vector to be optimized. for:

[0037] ;

[0038] The fitness function, optimized using the positioning error of the tracked robot as the filtering parameter, is represented by the root mean square error (RMSE) of the positioning.

[0039] ;

[0040] in, For fitness value, This represents the total number of positioning times. and The first The position and estimated position of the robot in the world coordinate system at any given time.

[0041] Furthermore, step S5 specifically includes the following steps:

[0042] Step S51: Initialize the gray wolf population, representing the position of each individual gray wolf as a combination of filter parameter vectors to be optimized. Let the i-th... During the nth iteration The location of each individual gray wolf:

[0043] ;

[0044] in, They represent the first During the nth iteration The process noise covariance matrix, UWB measurement noise covariance matrix, and nonholonomic constraint observation noise covariance matrix for each individual gray wolf;

[0045] Step S52: Input the combined filtering parameter vectors corresponding to each individual gray wolf into the IMU-UWB-NHC fusion localization model, run the extended Kalman filter localization process, and calculate the corresponding fitness value according to the fitness function in the CEGWO-based fusion filtering parameter optimization model in step S4.

[0046] Step S53: Sort the gray wolf individuals according to their fitness values ​​from smallest to largest (i.e., the smaller the fitness value, the higher the ranking), determine the superior gray wolf individuals in the current population, and update the position of the gray wolf individuals through the cooperative enhancement mechanism.

[0047] Step S54: Repeat steps S52 and S53 until the maximum number of iterations or the fitness value meets the preset convergence condition, to obtain the optimal combination of filter parameters. ;in, and These are the optimal process noise covariance matrix, the optimal UWB measurement noise covariance matrix, and the optimal nonholonomic constraint observation noise covariance matrix, respectively.

[0048] Step S55: Input the optimal filter parameter combination into the IMU-UWB-NHC fusion localization model, so that the extended Kalman filter performs state prediction and observation update according to the optimal filter parameters, and finally outputs the position, velocity and attitude estimation results of the tracked robot in the target environment.

[0049] Furthermore, the collaboration enhancement mechanism includes: a multi-wolf collaborative search mechanism, a group information sharing mechanism, and a dynamic collaboration weighting mechanism.

[0050] Furthermore, the multi-wolf cooperative search mechanism is used to select n superior gray wolves with high fitness rankings to jointly participate in the search guidance and obtain a cooperative search position, as shown below:

[0051] ;

[0052] in, Indicates the first Multi-wolf collaborative search for location in the next iteration; Indicates the first In the next iteration, the top-ranked fitness level is... A superior gray wolf individual position;

[0053] The population information sharing mechanism is used to introduce the average position of the population in each iteration, and its expression is:

[0054] ;

[0055] in, Indicates the first The average position of the gray wolf population in the next iteration;

[0056] The dynamic collaborative weighting mechanism is used to determine based on The fitness value of the gray wolf is adaptively assigned search weights; let... The fitness values ​​of the gray wolves are respectively First, calculate the reciprocal of its fitness:

[0057] ;

[0058] Then calculate the dynamic collaboration weights:

[0059] ;

[0060] in, Indicates the first The class guides the dynamic collaboration weights corresponding to the gray wolf. To prevent extremely small positive numbers with a denominator of zero;

[0061] Next, based on dynamic collaboration weights, for The candidate positions obtained by the gray wolf guidance are weighted and fused to obtain the optimal gray wolf guidance positions. :

[0062] ;

[0063] in, They represent the first In the nth iteration A gray wolf individual Candidate positions formed under the guidance of the gray wolf;

[0064] By incorporating multi-wolf collaborative search mechanism, group information sharing mechanism, and dynamic cooperation weight mechanism into the location update process, an improved formula for updating the location of individual gray wolves is obtained:

[0065] ;

[0066] in, This represents the adjustment coefficient for multi-wolf collaborative search. The group information sharing adjustment coefficient is shown.

[0067] According to a second aspect of the present invention, a CEGWO-optimized tracked robot IMU-UWB-NHC fusion localization system is provided, comprising a module of the CEGWO-optimized tracked robot IMU-UWB-NHC fusion localization method described in any one of the preceding embodiments.

[0068] According to a third aspect of the present invention, a terminal device is provided, comprising a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the CEGWO-optimized tracked robot IMU-UWB-NHC fusion localization method described in any one of the above descriptions.

[0069] According to a fourth aspect of the present invention, a computer-readable storage medium is provided having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the CEGWO-optimized tracked robot IMU-UWB-NHC fusion localization method described above.

[0070] The beneficial effects of this invention are as follows: By constructing an IMU-UWB-NHC fusion localization model, this invention collaboratively integrates IMU state prediction, UWB observation information, and nonholonomic constraints of the tracked robot. This enables the suppression of inertial error accumulation and UWB ranging anomalies in complex environments such as orchard tree occlusion, multipath propagation, and ground undulations, thereby improving the continuity, accuracy, and stability of tracked robot localization. Furthermore, this invention employs CEGWO to evaluate the process noise covariance matrix. UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix Joint optimization is performed to achieve a more reasonable parameter match between EKF state prediction, UWB observation update and NHC constraint update, thereby avoiding the problem of filter parameters relying on manual experience setting, and improving the adaptability of the fusion localization model to complex orchard environments.

[0071] Experimental results show that in a simulated orchard environment, the RMSE, MAE, STD, and P95 of the method of this invention are 0.0926 m, 0.0794 m, 0.0476 m, and 0.1748 m, respectively. Compared with UWB-LS, EKF, and GWO-EKF, the RMSE is reduced by 68.73%, 17.54%, and 5.51%, respectively. In a real sandy orchard environment, the RMSE, MAE, STD, and P95 of the method of this invention are 0.0929 m, 0.0831 m, 0.0416 m, and 0.1603 m, respectively. Compared with UWB-LS, EKF, and GWO-EKF, the RMSE is reduced by 67.22%, 12.93%, and 7.19%, respectively. These results indicate that the present invention can effectively improve the positioning accuracy, stability, and robustness of tracked robots in complex orchard environments. Attached Figure Description

[0072] Figure 1 The flowchart is provided for an embodiment of the present invention.

[0073] Figure 2 This is a schematic diagram illustrating the execution of the fusion positioning method provided in an embodiment of the present invention.

[0074] Figure 3 The tracked robot model provided in this embodiment of the invention.

[0075] Figure 4 The CEGWO pseudocode provided in this embodiment of the invention.

[0076] Figure 5 The simulation experiment results provided by the embodiments of the present invention.

[0077] Figure 6 The sensor field layout provided in this embodiment of the invention.

[0078] Figure 7 The field experimental results provided by the embodiments of the present invention. Detailed Implementation

[0079] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of this application can be arbitrarily combined with each other.

[0080] Example 1: As Figures 1-7As shown, according to a first aspect of the present invention, a CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots is provided, comprising:

[0081] Step S1: Obtain IMU-UWB multi-source localization data of the tracked robot and construct the system state vector; wherein, the IMU-UWB multi-source localization data of the tracked robot includes IMU data and UWB data;

[0082] Step S2: Construct a joint observation model based on UWB ranging observation information and the introduced nonholonomic constraint NHC observation information of the tracked robot; wherein, UWB ranging observation information includes UWB ranging information and UWB observation information;

[0083] Step S3: Use IMU data to predict the state of the tracked robot, and combine UWB observation information and nonholonomic constraint NHC observation information into the extended Kalman filter update process to form an IMU-UWB-NHC fusion localization model; and use the process noise covariance matrix in the extended Kalman filter update process. UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix As the filtering parameters to be optimized;

[0084] Step S4: Based on the filtering parameters to be optimized, establish a fusion filtering parameter optimization model based on CEGWO;

[0085] Step S5: The Cooperative Enhanced Grey Wolf Optimization Algorithm (CEGWO) is used to iteratively optimize the filter parameter vector in the fusion filter parameter optimization model based on CEGWO constructed in Step S4. The optimized filter parameters are then input into the IMU-UWB-NHC fusion localization model, and the position, velocity and attitude estimation results of the tracked robot in the target environment are output.

[0086] Further, step S1 is: setting in the target environment A UWB base station of uniform height and The UWB base stations are arranged in a rectangular pattern, with the tracked robot placed on them. Within a rectangular area enclosed by several UWB base stations, an IMU module and a UWB positioning module are installed on a tracked robot. The IMU module acquires accelerometer and gyroscope biases during robot movement as IMU data, while the UWB positioning module acquires the robot's position in the world coordinate system as UWB data (specifically, the position is obtained by acquiring distance measurement data between the robot and multiple UWB base stations in the target environment, then calculating the robot's position in the world coordinate system using Taylor expansion). A system state vector is then constructed. :

[0087] ;

[0088] in: For the first The position of the Time Robot in the world coordinate system. For the first The linear velocity of the robot at all times. For the first The pose quaternion of the time-lapse robot, and The first Accelerometer bias and gyroscope bias during the robot's movement; .

[0089] For example, the target environment can be an indoor environment or an outdoor environment (such as a garden environment or a Haloxylon ammodendron planting area). Considering that the location of the outdoor environment is more objective, in addition to conducting experiments in a simulated environment, the present invention will also conduct experiments in an outdoor Haloxylon ammodendron planting area to verify the performance of the present invention.

[0090] Furthermore, the joint observation model established in step S2 includes:

[0091] Let the first The location of each UWB base station in the world coordinate system is as follows: Tracked robot The position of the time in the world coordinate system is Then the robot and the first The theoretical distance between UWB base stations (i.e., UWB ranging information) is:

[0092] ;

[0093] After considering ranging noise, based on the robot and the first The theoretical distance between UWB base stations , obtained the UWB observation information ;

[0094] + ;

[0095] in, Indicates the first Measurement noise of a UWB base station;

[0096] Tracked robots based on received UWB base station ranging information is used to construct a UWB observation information vector. ;

[0097] Simultaneously, based on the motion characteristic of a tracked robot where the lateral velocity in the body coordinate system is approximately zero during normal operation, nonholonomic constraint NHC observation information is introduced. Represented as:

[0098] ;

[0099] in, Let be the lateral velocity of the robot in the body coordinate system. This is a nonholonomic constraint on observation noise. By introducing this constraint, the impact of IMU integral drift and UWB ranging anomalies on the positioning results can be suppressed.

[0100] Tracked mobile robots have good mobility in agricultural work environments, and their motion is typically determined by linear velocity. and angular velocity Description, such as Figure 3 As shown. Let and This represents the robot's position coordinates in the world coordinate system (the robot's position in the world coordinate system). ), Indicates the robot's heading angle. For the robot's linear velocity, Let be the angular velocity, then the kinematic model of the tracked robot can be expressed as:

[0101] ;

[0102] Unlike omnidirectional mobile platforms, tracked robots primarily move longitudinally along the vehicle body during normal operation, exhibiting relatively small lateral slippage. Therefore, they can approximate nonholonomic constraints. Let the robot's velocity in the body coordinate system be:

[0103] ;

[0104] in, These represent the robot's longitudinal, lateral, and vertical velocities in the body coordinate system, respectively. Based on the motion characteristics of tracked robots, the lateral velocity is set as follows in this invention: .

[0105] Further, step S3 is: using IMU data to predict the state of the tracked robot, and jointly introducing UWB observation information and non-holonomic constraint NHC observation information into the extended Kalman filter update process to form an IMU-UWB-NHC fusion localization model;

[0106] In the state prediction phase, the state of the tracked robot is propagated using IMU data. The extended Kalman filter state covariance propagation is expressed as:

[0107] ;

[0108] in, For the first The state covariance matrix at time step, For the first The state transition matrix at each time step, The process noise covariance matrix; Indicates transpose;

[0109] During the observation update phase, UWB observation information and non-holonomically constrained NHC observation information are combined to form the first... Time-unified observation vector :

[0110] ;

[0111] in, For UWB observation information vectors, This is non-holonomic constrained NHC observation information;

[0112] Unified observation vector corresponding to the observation noise covariance matrix Represented as:

[0113] ;

[0114] in, Measure the noise covariance matrix for UWB base stations. The nonholonomic constrained observation noise covariance matrix;

[0115] As can be seen from the above process, the fusion localization result of the extended Kalman filter is affected by the process noise covariance matrix. UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix The combined effects of these factors necessitate optimization of the aforementioned filtering parameters to improve the accuracy and stability of the fused positioning. This will influence the performance of the extended Kalman filter prediction and observation update based on the process noise covariance matrix. UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix As the filtering parameters to be optimized, a fusion filtering parameter optimization model based on CEGWO is constructed.

[0116] Further, step S4 includes:

[0117] Based on the process noise covariance matrix UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix Define the filter parameter vector to be optimized. for:

[0118] ;

[0119] The fitness function, optimized using the positioning error of the tracked robot as the filtering parameter, is represented by the root mean square error (RMSE) of the positioning.

[0120] ;

[0121] in, For fitness value, This represents the total number of positioning times. and The first The robot's position (i.e., true position) and estimated position in the world coordinate system are determined at any given time. In subsequent solutions, the optimal combination of filtering parameters that reduces localization errors based on IMU-UWB-NHC fusion is obtained by minimizing the fitness function.

[0122] Furthermore, step S5 specifically includes the following steps:

[0123] Step S51: Initialize the gray wolf population, representing the position of each individual gray wolf as a combination of filter parameter vectors to be optimized. Let the i-th... During the nth iteration The location of each individual gray wolf:

[0124] ;

[0125] in, They represent the first During the nth iteration The process noise covariance matrix, UWB measurement noise covariance matrix, and nonholonomic constraint observation noise covariance matrix are corresponding to individual gray wolves.

[0126] Step S52: Input the combined filtering parameter vectors corresponding to each individual gray wolf into the IMU-UWB-NHC fusion localization model, run the extended Kalman filter localization process, and calculate the corresponding fitness value according to the fitness function in the CEGWO-based fusion filtering parameter optimization model in step S4.

[0127] Step S53: Sort the gray wolf individuals according to their fitness values ​​from smallest to largest (i.e., the smaller the fitness value, the higher the ranking), determine the superior gray wolf individuals in the current population, and update the position of the gray wolf individuals through the cooperative enhancement mechanism.

[0128] Specifically, the traditional gray wolf optimization algorithm divides the gray wolf population into groups based on their fitness values. and Four levels. Among them, The gray wolf is the main leader in the current population, and the rest of the gray wolves are... Individual. Traditional GWO mainly relies on Three candidate positions are generated for the guiding gray wolves, and the individual positions of the gray wolves are updated using an equal-weighted average method. The update method is as follows:

[0129] ;

[0130] in, Indicates the first In the nth iteration The location of each individual gray wolf; They represent the first A gray wolf individual Candidate positions formed under the guidance of gray wolves. Although this method is structurally simple, it relies on only a few guiding individuals, and... The same guiding effect can easily lead to a single search direction, insufficient population cooperation, and getting trapped in local optima.

[0131] To overcome the above shortcomings, this invention introduces a cooperative enhancement mechanism on the basis of traditional GWO.

[0132] The collaboration enhancement mechanism includes: a multi-wolf collaborative search mechanism, a group information sharing mechanism, and a dynamic collaboration weight mechanism;

[0133] The multi-wolf cooperative search mechanism selects n superior gray wolves with high fitness rankings to participate in the search guidance and obtain the cooperative search location, as shown below:

[0134] ;

[0135] in, Indicates the first Multi-wolf collaborative search for location in the next iteration; Indicates the first In the next iteration, the top-ranked fitness level is... A superior gray wolf individual position; , For the size of the gray wolf population (this invention) Pick (10% of the total). This mechanism can integrate information from multiple excellent parameter combinations, avoiding the algorithm relying solely on... Three guiding individuals.

[0136] The population information sharing mechanism is used to introduce the average position of the population in each iteration, and its expression is:

[0137] ;

[0138] in, Indicates the first The average position of the gray wolf population in each iteration. This average position reflects the overall distribution of all filter parameter combinations in the current population. Introducing it into the position update process can enhance the population's information sharing ability and reduce the risk of gray wolves prematurely concentrating in local areas.

[0139] The dynamic collaborative weighting mechanism is used to determine based on The fitness value of the gray wolf is used to adaptively allocate search weights; let... The fitness values ​​of the gray wolves are respectively First, calculate the reciprocal of its fitness:

[0140] ;

[0141] Then calculate the dynamic collaboration weights:

[0142] ;

[0143] in, Indicates the first The class guides the dynamic collaboration weights corresponding to the gray wolf. To prevent extremely small positive numbers with a denominator of zero (this invention takes...) The smaller the fitness value, the better the combination of filtering parameters for that individual gray wolf, and the greater its dynamic cooperative weight.

[0144] Then, based on dynamic collaboration weights, for The candidate positions obtained by the gray wolf guidance are weighted and fused to obtain the optimal gray wolf guidance positions. :

[0145] ;

[0146] in, They represent the first In the nth iteration A gray wolf individual Candidate positions formed under the guidance of the gray wolf.

[0147] By incorporating a multi-wolf collaborative search mechanism, a group information sharing mechanism, and a dynamic cooperation weighting mechanism into the location update process, we obtain the improved gray wolf individual location update formula of this invention:

[0148] ;

[0149] in, This represents the adjustment coefficient for multi-wolf collaborative search. This invention takes the group information sharing adjustment coefficient. .

[0150] After the update is complete, for Boundary constraint checks are performed to ensure that the values ​​of the filtering parameters to be optimized are within the range (the values ​​in this invention are...). = [0.01, 0.01, 0.01], = [1, 1, 1]):

[0151] ;

[0152] in, , Indicates the upper and lower limits of the value.

[0153] Step S54: Repeat steps S52 and S53 until the maximum number of iterations or the fitness value meets the preset convergence condition, to obtain the optimal combination of filter parameters. ;in, and These are the optimal process noise covariance matrix, the optimal UWB measurement noise covariance matrix, and the optimal nonholonomic constraint observation noise covariance matrix, respectively.

[0154] Step S55: Input the optimal filter parameter combination into the IMU-UWB-NHC fusion localization model, so that the extended Kalman filter performs state prediction and observation update according to the optimal filter parameters, and finally outputs the position, velocity and attitude estimation results of the tracked robot in the target environment.

[0155] For example, the pseudocode of the CEGWO described in this invention is as follows: Figure 4 As shown.

[0156] Based on the above-described method of the present invention, the following is an optional experimental verification process for the present invention:

[0157] I. Simulation Environment Test

[0158] To further verify the applicability of the proposed method in complex environments, this paper constructs an orchard experimental environment that approximates the spatial layout characteristics of a medium-density citrus orchard. The fruit trees are modeled using a rectangular planting pattern, with a spacing of 5 m along the row spacing and tree arrangement direction, and an initial offset of 1 m is introduced to reduce the impact of boundary conditions on scene generation. The GWO population size is set to 30, and the maximum number of iterations is set to 50.

[0159] Figure 5 The localization results of this invention were compared with those of UWB-LS, EKF (fusion IMU-UWB), and GWO-EKF (fusion IMU-UWB) in Table 1. The results show that the RMSE, MAE, STD, and P95 of this invention are 0.0926 m, 0.0794 m, 0.0476 m, and 0.1748 m, respectively, all superior to the comparative methods. The RMSE is reduced by 68.73%, 17.54%, and 5.51% compared to UWB-LS, EKF, and GWO-EKF, respectively. Among these, UWB-LS has the largest error, indicating that relying solely on UWB ranging is susceptible to occlusion, multipath propagation, and noise. EKF significantly improves localization continuity by introducing high-frequency IMU prediction. GWO-EKF further improves the fusion effect by optimizing filter parameters. Based on this, the present invention introduces a nonholonomic constraint and cooperative enhancement gray wolf optimization mechanism, which uses the kinematic prior of the tracked robot's lateral velocity being approximately zero to suppress state drift and obtain a better combination of noise covariance parameters, thereby further improving positioning accuracy and robustness.

[0160] In addition, by Figure 5 (e) It can be seen that the CEGWO proposed in this invention has better convergence and stability than the traditional GWO in the parameter optimization process, and can obtain a better solution in fewer iterations. This shows that the multi-wolf collaborative search, group information sharing and dynamic cooperative weight mechanism effectively enhance the global search and local development capabilities and reduce the risk of getting trapped in local optima.

[0161] Table 1 Simulation Experiment Results

[0162]

[0163] II. Real-world environment testing

[0164] To verify the applicability of the proposed method in real-world, complex environments, this invention underwent field trials in a Haloxylon ammodendron planting area in the Hotan desert region of Xinjiang, China. The vegetation in this area is primarily planted on sandy surfaces, irrigated using drip irrigation, and the ground is soft and undulating. Due to the sandy adhesion conditions and surface characteristics, traditional wheeled orchard robots in this area are prone to slipping, sinking, and difficulty in passage. Tracked robots, on the other hand, have advantages such as low ground pressure, strong passability, and good adaptability to complex terrain, making them more suitable for operation in this environment. Furthermore, the remote location of the test area makes it difficult to reliably rely on high-precision satellite positioning services; therefore, the proposed IMU-UWB-NHC local fusion positioning method effectively demonstrates its advantages under weak satellite positioning conditions.

[0165] On-site test setup as follows Figure 6 As shown, the tracked robot used is a commercially available product (such as the WITE Intelligent ROS navigation robot). Next, the IMU module and UWB positioning module are installed, and four UWB base stations are installed in a rectangular arrangement. In the experiment, the method of this invention is compared with three other methods: UWB-LS, EKF, and GWO-EKF, to comprehensively evaluate the positioning accuracy, stability, and robustness of each method in a real environment. Due to uncertainties in the field environment, such as tree obstruction, multipath propagation, ground undulations, and robot motion disturbances, the field test results better reflect the comprehensive performance of different methods in actual orchard operations compared to simulation scenarios. Table 2 shows the RMSE, MAE, STD, and P95 of each method in the field test. Figure 7 The corresponding trajectory and error distribution results are given.

[0166] From Table 2 and Figures 6-7 As can be seen, the method of this invention achieves optimal performance across all indicators, with RMSE, MAE, STD, and P95 values ​​of 0.0929 m, 0.0831 m, 0.0416 m, and 0.1603 m, respectively. Compared to UWB-LS, EKF, and GWO-EKF, the RMSE is reduced by 67.22%, 12.93%, and 7.19%, respectively. This indicates that the single UWB method is significantly affected by occlusion and multipath propagation in a real sandy orchard environment, while EKF fusion can improve localization continuity, and GWO parameter optimization can further improve localization performance. Furthermore, the method of this invention, combined with nonholonomic constraints and a cooperative enhanced gray wolf optimization strategy, can more fully utilize the kinematic priors of the tracked robot and obtain better filtering parameters, thus exhibiting higher localization accuracy and stability in real complex environments. In summary, the field test results verify the effectiveness and engineering application potential of the proposed method in a sandy orchard scenario.

[0167] Table 2 Test Results

[0168]

[0169] According to a second aspect of the present invention, a CEGWO-optimized IMU-UWB-NHC fusion localization system for tracked robots is provided, comprising modules of the CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots described in any of the above embodiments. For parts of the modules not described in detail above, please refer to the relevant descriptions in the embodiments.

[0170] According to a third aspect of the present invention, a terminal device is provided, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the CEGWO-optimized tracked robot IMU-UWB-NHC fusion localization method described in any one of the above embodiments.

[0171] According to a fourth aspect of the present invention, a computer-readable storage medium is provided having a computer program / instructions stored thereon, which, when executed by a processor, implement the steps of the CEGWO-optimized tracked robot IMU-UWB-NHC fusion localization method described above.

[0172] The specific embodiments of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those skilled in the art, various changes can be made without departing from the spirit of the present invention.

Claims

1. A CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots, characterized in that, include: Step S1: Obtain IMU-UWB multi-source localization data of the tracked robot and construct the system state vector; wherein, the IMU-UWB multi-source localization data of the tracked robot includes IMU data and UWB data; Step S2: Construct a joint observation model based on UWB ranging observation information and the introduced nonholonomic constraint NHC observation information of the tracked robot; wherein, UWB ranging observation information includes UWB ranging information and UWB observation information; Step S3: Use IMU data to predict the state of the tracked robot, and combine UWB observation information and nonholonomic constraint NHC observation information into the extended Kalman filter update process to form an IMU-UWB-NHC fusion localization model; the process noise covariance matrix in the extended Kalman filter update process is used to predict the state of the tracked robot. UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix As the filtering parameters to be optimized; Step S4: Based on the filtering parameters to be optimized, establish a fusion filtering parameter optimization model based on CEGWO; Step S5: The Cooperative Enhanced Grey Wolf Optimization Algorithm (CEGWO) is used to iteratively optimize the filter parameter vector in the fusion filter parameter optimization model based on CEGWO constructed in Step S4. The optimized filter parameters are then input into the IMU-UWB-NHC fusion localization model, and the position, velocity and attitude estimation results of the tracked robot in the target environment are output.

2. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 1, characterized in that, Step S1 is as follows: Accelerometer bias and gyroscope bias during the robot's movement are acquired using an IMU module installed on the tracked robot as IMU data; the robot's position in the world coordinate system is acquired using a UWB positioning module installed on the tracked robot as UWB data; and a system state vector is constructed. : ； in: For the first The position of the Time Robot in the world coordinate system. For the first The robot's linear velocity at all times. For the first The pose quaternion of the time-lapse robot, and The first Accelerometer bias and gyroscope bias during the robot's movement.

3. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 1, characterized in that, The joint observation model established in step S2 includes: Let the first The location of each UWB base station in the world coordinate system is as follows: Tracked robot The position of the time in the world coordinate system is Then the robot and the first The theoretical distance between UWB base stations for: ； After considering ranging noise, based on the robot and the first The theoretical distance between UWB base stations , obtained the UWB observation information ; + ； in, Indicates the first Measurement noise of a UWB base station; Tracked robots based on received UWB base station ranging information is used to construct a UWB observation information vector. ; Simultaneously, based on the motion characteristic of a tracked robot where the lateral velocity in the body coordinate system is approximately zero during normal operation, nonholonomic constraint NHC observation information is introduced. Represented as: ； in, Let be the lateral velocity of the robot in the body coordinate system. This is nonholonomic constraint observation noise.

4. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 1, characterized in that, Step S3 is as follows: In the state prediction phase, the state of the tracked robot is propagated using IMU data. The extended Kalman filter state covariance propagation is expressed as: ； in, For the first The state covariance matrix at time step, For the first The state transition matrix at each time step, The process noise covariance matrix; Indicates transpose; During the observation update phase, UWB observation information and non-holonomically constrained NHC observation information constitute the first... Time-unified observation vector : ； in, For UWB observation information vectors, This is nonholonomically constrained NHC observation information; Unified observation vector corresponding to the observation noise covariance matrix Represented as: ； in, Measure the noise covariance matrix for UWB base stations. This is the nonholonomic constraint observation noise covariance matrix.

5. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 1, characterized in that, Step S4 includes: Based on the process noise covariance matrix UWB measurement noise covariance matrix Non-holonomic constrained observation noise covariance matrix Define the filter parameter vector to be optimized. for: ； The fitness function, optimized using the positioning error of the tracked robot as the filtering parameter, is represented by the root mean square error (RMSE) of the positioning. ； in, For fitness value, This represents the total number of positioning times. and The first The position and estimated position of the robot in the world coordinate system at any given time.

6. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 1, characterized in that, Step S5 specifically includes the following steps: Step S51: Initialize the gray wolf population, representing the position of each individual gray wolf as a combination of filter parameter vectors to be optimized. Let the i-th... During the nth iteration The location of an individual gray wolf : ； in, They represent the first During the nth iteration The process noise covariance matrix, UWB measurement noise covariance matrix, and nonholonomic constrained observation noise covariance matrix corresponding to each individual gray wolf; Step S52: Input the combined filtering parameter vectors corresponding to each individual gray wolf into the IMU-UWB-NHC fusion localization model, run the extended Kalman filter localization process, and calculate the corresponding fitness value according to the fitness function in the CEGWO-based fusion filtering parameter optimization model in step S4. Step S53: Sort the gray wolf individuals according to their fitness values ​​from smallest to largest (i.e., the smaller the fitness value, the higher the ranking), determine the superior gray wolf individuals in the current population, and update the position of the gray wolf individuals through the cooperative enhancement mechanism. Step S54: Repeat steps S52 and S53 until the maximum number of iterations or the fitness value meets the preset convergence condition, to obtain the optimal combination of filter parameters. ;in, and These are the optimal process noise covariance matrix, the optimal UWB measurement noise covariance matrix, and the optimal nonholonomic constraint observation noise covariance matrix, respectively. Step S55: Input the optimal filter parameter combination into the IMU-UWB-NHC fusion localization model, so that the extended Kalman filter performs state prediction and observation update according to the optimal filter parameters, and finally outputs the position, velocity and attitude estimation results of the tracked robot in the target environment.

7. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 6, characterized in that, The collaboration enhancement mechanisms include: a multi-wolf collaborative search mechanism, a group information sharing mechanism, and a dynamic collaboration weighting mechanism.

8. The CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to claim 7, characterized in that, The multi-wolf cooperative search mechanism selects n superior gray wolves with high fitness rankings to participate in the search guidance and obtain the cooperative search location, as shown below: ； in, Indicates the first Multi-wolf collaborative search for location in the next iteration; Indicates the first In the next iteration, the top-ranked fitness level is... A superior gray wolf individual position; The population information sharing mechanism is used to introduce the average position of the population in each iteration, and its expression is: ； in, Indicates the first The average position of the gray wolf population in the next iteration; The dynamic collaborative weighting mechanism is used to determine based on The fitness value of the gray wolf is adaptively assigned search weights; let... The fitness values ​​of the gray wolves are respectively First, calculate the reciprocal of its fitness: ； Then calculate the dynamic collaboration weights: ； in, Indicates the first The class guides the dynamic collaboration weights corresponding to the gray wolf. To prevent extremely small positive numbers with a denominator of zero; Next, based on dynamic collaboration weights, for The candidate positions obtained by the gray wolf guidance are weighted and fused to obtain the optimal gray wolf guidance positions. : ； in, They represent the first In the nth iteration A gray wolf individual Candidate positions formed under the guidance of the gray wolf; By incorporating multi-wolf collaborative search mechanism, group information sharing mechanism, and dynamic cooperation weight mechanism into the location update process, an improved formula for updating the location of individual gray wolves is obtained: ； in, This represents the adjustment coefficient for multi-wolf collaborative search. The group information sharing adjustment coefficient is shown.

9. A CEGWO-optimized IMU-UWB-NHC fusion positioning system for a tracked robot, characterized in that, The module includes the CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots as described in any one of claims 1-8.

10. A terminal device, comprising a memory, a processor, and a computer program stored in the memory, characterized in that, The processor executes the computer program to implement the steps of the CEGWO-optimized IMU-UWB-NHC fusion localization method for tracked robots according to any one of claims 1-8.

Images