An unknown environment intelligent robot autonomous path planning method

Abstract

The application provides an unknown environment intelligent robot autonomous path planning method, relates to the technical field of robot navigation and path planning, and is used for path planning of an unknown environment, and comprises the following steps: constructing an environment map containing dynamic obstacles and accurate positioning of a robot, trajectory prediction of the dynamic obstacles, adopting an improved monarch butterfly optimization algorithm to optimize a FastSLAM mapping method of particle filtering, realizing environment map construction and robot positioning, performing trajectory prediction based on a CNN-BiLSTM network of a self-attention mechanism, and performing path planning by improving an A* algorithm, so that the robot positioning accuracy is improved, and the safety of path planning is ensured.

Description

Technical Field

[0001] This invention relates to the field of robot navigation and path planning technology, specifically to an autonomous path planning method for intelligent robots in unknown environments. Background Technology

[0002] Central air conditioning systems have become an indispensable infrastructure in people's work and life, with high-efficiency and energy-saving ground source heat pump systems widely used in large public buildings. However, their operation and maintenance face a core challenge: the system typically consists of multiple complex machine rooms distributed underground or in remote corners. Monitoring and inspection of these machine rooms rely heavily on manual labor, resulting in high labor costs, low efficiency, and the risk of missed inspections.

[0003] To achieve intelligent operation and maintenance, deploying intelligent inspection robots with autonomous path planning capabilities has become a key objective. The core of applying robots to ground source heat pump machine room environments lies in real-time perception, precise positioning, and path planning in unknown environments. The robot needs to build accurate environmental maps in real time and accurately locate itself. Simultaneously, dynamic elements within the machine room, especially moving personnel, can affect its obstacle avoidance capabilities. Therefore, accurately predicting the movement trajectories of these dynamic obstacles within a limited space is a prerequisite for safe obstacle avoidance. Finally, the robot must plan an optimized path in this complex and constrained environment that absolutely guarantees safety and efficiently covers key inspection points. Traditional planning methods based on static maps are insufficiently adaptable to such dynamic scenarios; planned paths are easily invalidated by personnel movement or temporary obstacles, and they lack real-time response and rapid replanning capabilities. An ideal solution requires the planner to seamlessly integrate real-time high-precision positioning information and dynamic situational awareness including predicted trajectories, instantly generating or adjusting safe and efficient paths. Summary of the Invention

[0004] To address the above problems, this invention proposes an autonomous path planning method for intelligent robots in unknown environments, including an improved FastSLAM map building and localization stage, a dynamic obstacle trajectory repair and prediction stage, and a robot path planning stage.

[0005] In the improved FastSLAM map building and localization stage, the improved Monarch Butterfly optimization algorithm is used to optimize the particle filter FastSLAM mapping method to realize environmental map building and robot localization. The improved Monarch Butterfly algorithm introduces adaptive genetic parameters and uses migration operators and adjustment operators to enhance the particle filter resampling process to obtain an improved FastSLAM map.

[0006] In the dynamic obstacle trajectory repair and prediction stage, the original obstacle trajectory is first repaired using the median absolute deviation and cubic spline difference method. Then, a dynamic obstacle trajectory prediction model based on a one-dimensional convolutional neural network, a bidirectional long short-term memory network, and a self-attention mechanism is used to extract the spatial-temporal features of the obstacle trajectory for multi-step prediction of the obstacle trajectory, thus obtaining the predicted obstacle trajectory.

[0007] In the robot path planning stage, the improved FastSLAM map and predicted obstacle trajectories are input into the path planning module, and the path is generated by combining the path representation based on B-spline curves and the improved A* algorithm; the environment is perceived in real time during execution.

[0008] Preferably, the improved FastSLAM map building and localization stage includes:

[0009] Initialize the monarch butterfly population and divide the population into region 1 (Land1) and region 2 (Land2);

[0010] The migration operator updates the position of the monarch butterflies in region 1;

[0011] Adjust the operator to update the individual positions of Monarch butterflies in region 2;

[0012] Adaptive adjustment of genetic parameters :

[0013] ;

[0014] The total number of monarch butterflies is [number missing]. t represents the number of iterations, and ceil is the floor function.

[0015] The updated monarch butterfly population is embedded into a particle filtering process to output robot pose estimation and map building results.

[0016] Preferably, the FastSLAM mapping method using particle filtering optimized by the improved Monarch Butterfly optimization algorithm has the following steps:

[0017] Step 1: Randomly sample N particles and initialize them so that each particle has the robot's initial parameters;

[0018] Step 2: Replace the particle filter individuals with individual monarch butterflies to divide the monarch butterfly population into two regions. and ;

[0019] Step 3: Read the sensor data on the robot to obtain the system observations at the current moment. Based on the robot's pose at the previous moment, simulate the iterative behavior of the monarch butterfly population, and integrate the adjustment and migration operators of the monarch butterfly population into the particle filter algorithm. and Adjustments are made to individual particles;

[0020] Step 4: Substitute the adaptive genetic parameters into the particle filtering algorithm, and perform optimal processing on the individual particles of each generation as the number of iterations changes;

[0021] Step 5: Determine whether the number of iterations G has reached the set value. If yes, proceed to Step 6; otherwise, go to Step 3 and continue execution.

[0022] Step 6: Perform linear optimization combination resampling on the particles;

[0023] Step 7: Output the estimated state of the system at the current moment;

[0024] Step 8: Update the system observations and repeat the above steps until the robot system has finished running.

[0025] Preferably, in the dynamic obstacle trajectory repair and prediction stage, the preprocessing stage uses median absolute deviation to detect trajectory outliers, identifies and removes outliers, and repairs the trajectory through cubic spline interpolation.

[0026] A CNN-BiLSTM-Self-Attention prediction model is constructed. Local spatiotemporal features are extracted through 1D-CNN layers, temporal dependencies are modeled through BiLSTM layers, and key trajectory segments are weighted through a self-attention mechanism.

[0027] The position error and acceleration smoothing constraint are fused using a loss function, and the total loss is calculated.

[0028] Preferably, the position loss function L pos for:

[0029] ;

[0030] In the formula: This is the model's position estimate for the b-th sample at time step t, with dimension . Each prediction step contains ; This represents the actual location value of the model for the b-th sample at the t-th time step. and Dimensionality is consistent; B is the batch size of the sample; T is the prediction step size.

[0031] Preferably, the predicted coordinate positions P at time steps t and t+1 are... t P t+1 Perform a difference operation to obtain the velocity at the t-th time step. :

[0032] ;

[0033] in Then, the velocity is differentially analyzed again to obtain the acceleration. :

[0034] ;

[0035] Final acceleration smoothing loss for:

[0036] ;

[0037] in, Let be the acceleration at the t-th time step of the b-th sequence. The velocity at time step (t+1) forms the total loss function. :

[0038] Adjustable parameters are: .

[0039] Preferably, a dynamic search step size strategy is adopted to gradually decrease the step size from a large step size to the optimal step size. :

[0040] ;

[0041] Maximum search step size is , It is a decreasing search step size.

[0042] Preferably, a third-order B-spline curve is used in the path smoothing stage. Assume there are a total of n+1 control points defining the boundary range of the B-spline curve, and the k-th order B-spline curve P(u) is defined as follows:

[0043] ;

[0044] In the formula, It is the i-th control point. It is a k-th order basis function of index i.

[0045] ;

[0046] ;

[0047] in, It is the 0th-order basis function of index i, u i u i+1 u i+k u k These represent the i-th, i+1-th, i+k-th, and k-th nodes in the node vector.

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

[0049] This invention introduces a path planning method for intelligent robots in unknown environments. First, a map of the unknown environment is constructed, including a dynamic obstacle map. The constructed map can be used for the robot's precise positioning. Then, path planning is performed to enable the robot to safely avoid all obstacles and reach the target location. This method not only improves the robot's positioning accuracy but also ensures the safety of path planning. Attached Figure Description

[0050] Figure 1 A comparison of the Monarch Butterfly algorithm before and after the improvement under different parameters;

[0051] Figure 2 The tracking status and absolute error of the four algorithms are given when the number of particles is 50.

[0052] Figure 3 The tracking status and absolute error of the four algorithms are given when the number of particles is 100.

[0053] Figure 4 The tracking status and absolute error of the four algorithms are given when the number of particles is 150.

[0054] Figure 5 Figure showing the simulation results of the PF-FastSLAM algorithm;

[0055] Figure 6 Figure showing the simulation results of the BA-FastSLAM algorithm;

[0056] Figure 7 Figure showing the simulation results of the MBO-FastSLAM algorithm;

[0057] Figure 8 Figure showing the simulation results of the IMBO-FastSLAM algorithm;

[0058] Figure 9 The result after trajectory data detection and repair;

[0059] Figure 10 For trajectory prediction using the CNN-BilSTM-Self-Attention algorithm;

[0060] Figure 11 For trajectory prediction using the lossless CNN-BiLSTM-Self-Attention algorithm;

[0061] Figure 12 For trajectory prediction using the CNN-BiLSTM algorithm;

[0062] Figure 13 For trajectory prediction using the BiLSTM algorithm;

[0063] Figure 14 The simulation results of the standard ARA* algorithm in a dynamic indoor environment;

[0064] Figure 15 The simulation results of the RWA* algorithm in a dynamic indoor environment;

[0065] Figure 16 The simulation results of the BS-IARA* algorithm in a dynamic indoor environment;

[0066] Figure 17 This invention provides an overall flowchart of an autonomous path planning method for intelligent robots in unknown environments.

[0067] Figure 18 It is a CNN-BiLSTM-Self-Attention trajectory prediction model. Detailed Implementation

[0068] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the scope of protection of the present invention.

[0069] See Figure 17 As shown, this invention proposes an autonomous robot path planning system for unknown dynamic environments, which is an intelligent robot navigation system with real-time perception, autonomous decision-making, and dynamic path correction capabilities.

[0070] The system comprises four core modules: 1) FastSLAM map building and robot localization based on IMBO-PF; 2) obstacle trajectory prediction based on CNN-BiLSTM and self-attention mechanism; 3) dynamic path planning integrating B-splines and improved A algorithms; and 4) human-robot master path planning based on environmental judgment. These four modules are coupled to form a complete closed loop, ensuring the robot can achieve highly robust path generation and real-time obstacle avoidance in unknown and dynamic environments. This invention realizes a complete autonomous path planning chain from environmental mapping and target perception to path generation, exhibiting significant advantages, particularly in handling unknown scenarios and dynamic changes.

[0071] The autonomous path planning method adopted by the system includes: an improved FastSLAM map building and localization stage; a dynamic obstacle trajectory prediction stage; and a robot path planning stage.

[0072] Improved FastSLAM map building and localization phase

[0073] To address the problems of severe weight degradation, insufficient particle diversity, and susceptibility to local optima in traditional particle filtering when applied to real-time localization and mapping tasks, this paper proposes a particle filtering method that integrates the improved Monarch Butterfly Optimization algorithm, abbreviated as IMBO-PF (Improved Monarch Butterfly Optimization enhanced Particle Filter).

[0074] This method simulates particle behavior using individual monarch butterflies, embeds the migration and adjustment operators from the monarch butterfly algorithm into the PF update process, and introduces an adaptive genetic parameter adjustment strategy to effectively improve particle diversity and filtering accuracy, significantly enhancing the robot's localization robustness and map construction accuracy in unknown environments.

[0075] Improve the migration operator part by dividing the region where the monarch butterfly individual is located into region 1 ( ) and Area 2 ( The purpose of the migration operator is to update... The location of an individual monarch butterfly within the range. The total population size of the monarch butterfly is... ,exist The quantity is set to Then in The number of middle , To Rounding down positive integers. for The proportion of monarch butterflies in China, and respectively referred to as and The Middle Monarch butterfly population is and Generate random numbers. , These are random numbers generated from a uniform distribution [0,1]. For the monarch butterfly migration cycle, if The newly born monarch butterfly is represented as:

[0076] (1);

[0077] in, Indicates the first Emperor Butterfly of the Middle Ages The One element; Indicates the first Emperor Butterfly of the Middle Ages The Each element, and the monarch butterfly from Randomly selected from [the list]. If [the list is]... The newly born monarch butterfly is represented as:

[0078] (2);

[0079] Among them, the monarch butterfly from Randomly selected from, Indicates the first Emperor Butterfly of the Middle Ages The Each element.

[0080] Improve the operator section; the purpose of adjusting the operator is to update... The location of individual monarch butterflies in the data was selected by random number. ,like ,but The Neogene Monarch butterfly is represented as:

[0081] (3);

[0082] in Indicates the first Emperor Butterfly of the Middle Ages The One element; Indicates the first The best monarch butterfly individual in the generation Each element. If ,but The Neogene Monarch butterfly is represented as:

[0083] (4);

[0084] Among them, the monarch butterfly from Randomly selected from, Indicates the first Emperor Butterfly of the Middle Ages The Each element.

[0085] Under this premise, if further judgment is obtained Then the newly born monarch butterfly is further updated as follows:

[0086] (5);

[0087] in, This represents the k-th element of Monarch butterfly j in generation t+1. Adjustment rate for Monarch butterflies; The stride length of the Monarch butterfly can be calculated using Levy (representing Levyflight, a random walking model):

[0088] (6);

[0089] Weighting factors:

[0090] (7);

[0091] in, This indicates the maximum stride length of the Monarch butterfly. This represents the current iteration number. This represents the monarch butterfly j in generation t.

[0092] Based on the above migration and adjustment operators and their update strategies for monarch butterfly individuals, it can be seen that MBO has a flexible and fast optimal solution search capability and rich population diversity.

[0093] The improved Monarch Butterfly algorithm incorporates genetic parameters into the classic Monarch Butterfly algorithm. ,and ( Its function is to retain the first... The best in the generation Individual, and replace the first The worst of the generations Individuals, to ensure that at least one in each generation Each individual is superior to or equal to the previous generation, ensuring the stability of the algorithm's optimization process. In the early stages of the algorithm's operation, smaller... The value ensures the algorithm's global search capability and avoids premature convergence leading to local optima. In the later stages of the algorithm's execution, to ensure stability and prevent performance degradation in individual monarch butterflies, the value should be increased. Values. Therefore, fixed genetic parameters. The overall optimization performance of the algorithm cannot be guaranteed. To address this issue, a nonlinear function is constructed, as shown in the following equation, which increases the overall stability of the algorithm while ensuring its convergence. The value is adaptively adjusted.

[0094] (8);

[0095] The number of monarch butterfly populations is t represents the number of iterations, and ceil is the floor function.

[0096] To verify that the proposed improved monarch butterfly optimization algorithm IMBO has better optimization performance compared to the standard monarch butterfly optimization algorithm MBO, this invention uses the Ackley benchmark function to conduct comparative simulation experiments on the standard MBO and IMBO. The following is the formula for the Ackley function f(x):

[0097] (9);

[0098] Where a, b, and c are constants, d is the dimension of vector x, and x i Let represent the i-th component of the input vector X, and exp(1) refers to e itself, which is used to maintain the adjustability of the function value.

[0099] The Ackley function is solved using IMBO, with a function dimension of 20. The parameter settings in the algorithm are as follows: Percentage of the population of Monarch butterflies migration cycle Monarch butterfly adjustment rate The population size of Monarch butterflies was counted separately. Simulation comparison experiments were conducted for different iteration numbers when the number of iterations was 50 and 100. The experimental results are as follows: Figure 1 As shown.

[0100] Figure 1 The horizontal axis represents the number of iterations of the MBO algorithm, and the vertical axis represents the function value corresponding to the optimal solution obtained by the tested Ackley baseline function under the two algorithms. When the number of iterations is the same, the smaller the value of the vertical axis, the stronger the optimization ability of the algorithm. Figure 1 In the first example, (a) the population size NP=50 and the number of iterations G=50; (b) the population size NP=50 and the number of iterations G=100; (c) the population size NP=100 and the number of iterations G=50; and (d) the population size NP=100 and the number of iterations G=100. Figure 1 As can be seen, IMBO outperforms traditional MBO overall under different population sizes and iteration counts. In the early stages of iteration, IMBO demonstrates higher optimization ability compared to traditional MBO, and the optimal solution found by IMBO is superior to that of traditional MBO for the same number of iterations. However, in the later stages of the algorithm, due to its adaptive... The existence of parameters greatly improves the stability of IMBO.

[0101] To address the problem of decreased prediction accuracy in particle filtering algorithms due to the loss of particle diversity during particle resampling, this invention proposes a resampling method based on linear combination optimization. The core idea of ​​linear combination resampling is to linearly combine the particles removed by standard particle filtering resampling with the repeatedly selected particles, while keeping the total number of particles unchanged. This allows the removed particles to have a certain influence on the newly generated particles, and the particle set after linear combination resampling has no duplicate particles, thus solving the problem of particle diversity loss that still exists after standard particle filtering resampling.

[0102] The linear combination method is:

[0103] (10);

[0104] in These are the new sampling points generated after linear combination; These are the sampling points that were selected repeatedly; These are the sampling points that were rejected. For the step size coefficient, select an appropriate one. The value can be reduced The effect of Euclidean distance; T is If the appropriate step size in this direction results in a new sampling point with a weight smaller than the original sampling point, then... The value is set to half of the original value, and new sampling points are generated.

[0105] This algorithm performs a secondary screening on the particles eliminated by the standard resampling algorithm. If the particles... weight w i satisfy:

[0106] (11);

[0107] If the particle is removed, it is added to the elimination group; otherwise, it is completely eliminated and not considered.

[0108] set up The total number of particles; The dimension of the sampling space; Let T be the probability distribution of any sampling point in its neighborhood space, then T takes the value of:

[0109] (12);

[0110] For the excluded sampling points This invention represents the sum of the position information of all particles located in the elimination group, assuming that there are particles in the elimination group. If there are 100 particles, then

[0111] (13);

[0112] Among them, w i For particle weights, x i It is in a particle state;

[0113] State estimate of the system after linear optimization combined resampling for:

[0114] (14);

[0115] Where k is the time step, w k To optimize the weights, x k Let i be the particle state, N be the number of particles, and i be the particle index.

[0116] Based on the Improved Monarch Butterfly Algorithm Optimized Particle Filter (IMBO-PF), the improved Monarch Butterfly Algorithm is integrated into the particle filter algorithm FastSLAM. By utilizing the adjustment and migration operators in the Monarch Butterfly Algorithm, particles are guided to move towards the high likelihood region, thus more closely approximating the true posterior probability distribution of the system.

[0117] To verify the prediction accuracy of the studied IMBO-PF, simulation experiments were conducted. To increase the experimental contrast, simulations were performed comparing four algorithms: Standard Particle Filter (PF), Bat Algorithm Optimized Particle Filter (BA-PF), Standard Monarch Butterfly Algorithm Optimized Particle Filter (MBO-PF), and Improved Monarch Butterfly Algorithm Optimized Particle Filter (IMBO-PF) with particle numbers of 50, 100, and 150. The standard univariate model selected for the simulation is as follows:

[0118] (15);

[0119] (16);

[0120] in They are respectively The system's state value and observed value at time t-1, where x(t-1) is the system's state value at time t-1. All noise is Gaussian noise with a mean of 0 and a variance of 1 at time t. Let the initial state of the system be... The tracking step size is 40. Percentage of the population of Monarch butterflies migration cycle =1.2, Monarch Butterfly Adjustment Rate Step size coefficient =0.4. The tracking status and absolute error values ​​of the four algorithms when the number of particles is 50, 100, and 150 are as follows: Figures 2-4 As shown, Figure 2 Four algorithms track the state when the number of particles is 50. Figure 2 (a) and absolute value of error ( Figure 2 (b) Figure 3 For each of the four algorithms tracking the state when the number of particles is 100 ( Figure 3 (a) and absolute value of error ( Figure 3 (b) Figure 4 For tracking the state of 150 particles using 4 different algorithms ( Figure 4 (a) and absolute value of error ( Figure 4(b)). Simulation results show that, under different particle numbers, IMBO-PF has the best overall tracking performance and the smallest error, followed by BA-PF and MBO-PF. Standard PF has the worst overall tracking performance, with several moments showing excessively large absolute errors, which can easily lead to inaccurate positioning and reduced accuracy when applied to robot localization. BA-PF and MBO-PF combine the bat algorithm and monarch butterfly algorithm with particle filtering algorithms, respectively, improving the overall particle filtering accuracy, but still exhibiting problems inherent in both biomimetic algorithms and standard particle filtering algorithms. IMBO-PF not only addresses the genetic parameters of the traditional monarch butterfly algorithm... Adaptive adjustments were made, and linear combination optimization was performed on the standard particle filter resampling stage. Simulation results showed that the algorithms significantly outperformed other algorithms. To more intuitively demonstrate the accuracy and performance of the four algorithms, the root mean square error (RMSE) and running time were calculated for each algorithm. The calculation formula is:

[0121] (17);

[0122] Where t is the total number of time steps, N is the number of particles, and n is the particle number. This is the state estimate of the nth particle at time step t. Let be the true value of the nth particle at time step t.

[0123] Tables 1 and 2 compare the root mean square error and running time of the four algorithms, respectively.

[0124] Table 1. Comparison of root mean square error of the four algorithms:

[0125]

[0126] Table 2 Comparison of running time (s) of the four algorithms:

[0127]

[0128] Analysis of Table 1 shows that increasing the number of particles improves the algorithm's estimation accuracy. The root mean square error of all four algorithms gradually decreases with increasing particle count. Comparing the four algorithms with the same number of particles, BA-PF and MBO-PF show improvement over the standard PF, but still have algorithmic shortcomings. The IMBO-PF algorithm improves particle diversity and the optimization performance of the standard MBO. Compared to the standard MBO-PF, the filtering accuracy of IMBO-PF is improved by 35.8%, 29.7%, and 38.6% for particle counts of 50, 100, and 150, respectively. Table 2 compares the average execution time of the four algorithms after running 50 times. Analysis of the data shows that the standard PF algorithm has the shortest execution time, while BA-PF and MBO-PF have similar execution times. Compared to MBO-PF, IMBO-PF improves execution efficiency by 9.1%, 8.5%, and 5.6% for particle counts of 50, 100, and 150, respectively. It can be seen that the adaptive genetic parameters proposed in this chapter based on the standard MBO algorithm... Keep This reduces the impact of fixed parameter values ​​on the algorithm's running speed to a certain extent. Considering both the filtering accuracy and running time of the algorithm, the IMBO-PF proposed in this invention has high application value.

[0129] IMBO-PF was applied to the FastSLAM algorithm for experimental verification. Utilizing the optimization mechanism of the Monarch Butterfly algorithm's adjustment and migration operators, particles in the particle filter were encouraged to move towards the high-likelihood region, further increasing the robot's localization estimation accuracy. The optimized FastSLAM algorithm flow is as follows:

[0130] Step 1: Initialization. Random sampling. Each particle is initialized, and each particle is given the robot's initial parameters.

[0131] Step 2: Replace the particle filter individuals with individual monarch butterflies to divide the monarch butterfly population into two regions. and .

[0132] Step 3: State Prediction. Read sensor data from the robot to obtain the current system observations. Based on the robot's pose from the previous moment, simulate the iterative behavior of the monarch butterfly population. Integrate the monarch butterfly population adjustment and migration operators into the particle filter algorithm. The individual particles are adjusted according to equations (1)-(2). The individual particles are adjusted according to formulas (3)-(7).

[0133] Step 4: Substitute the adaptive genetic parameters from equation (8) into the algorithm, and perform optimal selection on the individual particles in each generation as the number of iterations changes.

[0134] Step 5: Determine the number of iterations If the set value has been reached, proceed to Step 6; otherwise, proceed to Step 3.

[0135] Step 6: Perform linear optimization combination resampling of particles according to equations (10)-(13).

[0136] Step 7: Output the estimated state of the system at the current moment according to formula (14).

[0137] Step 8: Update the system observations and repeat the above steps until the robot system has finished running.

[0138] Comparative experiments were conducted on four FastSLAM algorithms. For ease of distinction, the four algorithms were named as follows: PF-FastSLAM (FastSLAM based on standard particle filter), MBO-FastSLAM (FastSLAM based on particle filter optimized by standard Monarch Butterfly algorithm), IMBO-FastSLAM (FastSLAM based on particle filter optimized by improved Monarch Butterfly algorithm), and BA-FastSLAM (FastSLAM based on particle filter optimized by Bat algorithm). Simulation results are shown below. Figures 5-8 As shown, Figure 5 In the middle (a), the localization and map construction results of the PF-FastSLAM algorithm are shown. Figure 5 (b) shows the positioning error results of the PF-FastSLAM algorithm; Figure 6 In the middle (a), the localization and map construction results of the MBO-FastSLAM algorithm are shown. Figure 6 (b) shows the positioning error results of the MBO-FastSLAM algorithm; Figure 7 In the middle (a), the localization and map building results of the IMBO-FastSLAM algorithm are shown. Figure 7 (b) shows the positioning error results of the IMBO-FastSLAM algorithm; Figure 8 In the middle (a), the localization and map construction results of the BA-FastSLAM algorithm are shown. Figure 8 In the middle (b), the positioning error result of the BA-FastSLAM algorithm is shown.

[0139] Depend on Figures 5-8It is evident that IMBO-FastSLAM performs best in predicting robot trajectories and landmarks, followed by PF-FastSLAM, BA-FastSLAM, and MBO-FastSLAM. The localization error graphs for each algorithm show that PF-FastSLAM exhibits significant fluctuations in localization error, with a maximum error of approximately 1m. BA-FastSLAM and MBO-FastSLAM show more stable error values, remaining around 0.5m and 0.25m respectively. This indicates that the introduction of biomimetic algorithms has, to some extent, addressed the issue of low prediction accuracy in the standard PF algorithm. Figure 8 It is evident that the improved Monarch Butterfly algorithm optimized by particle filtering proposed in this invention can effectively reduce robot localization errors when applied to robot localization, and maintain stable and accurate localization results in the later stages of localization. Looking at the average running times of the four algorithms after 30 runs each, PF-FastSLAM has the shortest running time at 33.94s; BA-FastSLAM has a running time of 50.59s; MBO-FastSLAM has a running time of 51.85s; and the proposed IMBO-FastSLAM has a running time of 46.39s. The data shows that the algorithm of this invention improves the running speed by 10.5% compared to the standard MBO algorithm. This is because the MBO-PF algorithm introduces the Monarch Butterfly algorithm into the standard PF algorithm, increasing the iteration time. However, the adaptive genetic parameters in IMBO-PF improve the speed at which the algorithm converges to the optimal solution, thus reducing the running time to some extent. The above data demonstrates that this paper incorporates the improved Monarch Butterfly algorithm into the standard PF algorithm and achieves better localization results and running speed. The estimation results of the landmarks can reflect the map construction results during the robot's movement. The estimation results and estimation error data of the three landmarks by the four algorithms are shown in Tables 3 to 5.

[0140] Table 3. Location estimation and error of landmark 1 by four algorithms:

[0141]

[0142] Table 4. Location estimation and error of landmark 2 using four algorithms:

[0143]

[0144] Table 5. Location estimation and error of landmark 3 by four algorithms:

[0145]

[0146] As shown in Tables 3-5, PF-FastSLAM exhibits the largest estimation error for landmarks in the simulation experiments, followed by BA-FastSLAM and MBO-FastSLAM. The IMBO-FastSLAM proposed in this invention demonstrates the best performance, improving the estimation accuracy of the three landmarks by 40%, 48%, and 65% respectively compared to the standard MBO-FastSLAM. In conclusion, IMBO-FastSLAM can ensure high accuracy and low time consumption when applied to localization and map building simulations based on robot motion models.

[0147] Dynamic obstacle trajectory repair and prediction stage

[0148] This paper presents a deep learning-based method for predicting the trajectory of dynamic obstacles. This method integrates convolutional neural networks, bidirectional long short-term memory networks, and self-attention mechanisms to improve the accuracy of predicting the future movement trends of obstacles in complex dynamic environments. This method is particularly suitable for scenarios such as autonomous driving and robot obstacle avoidance, where the trajectory exhibits significant nonlinearity, frequent interactions, or incomplete information.

[0149] In the specific implementation process, the trajectory data of dynamic obstacles is preprocessed. The collected raw trajectories often include continuous position records of obstacles over a period of time, and may also contain information such as speed and direction. To improve the efficiency of model training and the accuracy of prediction, MAD (Median Absolute Deviation), a robust statistic based on the median, is used and widely applied for outlier detection.

[0150] First, the raw trajectory data is preprocessed using Median Absolute Deviation (MAD), a robust statistic based on the median, widely used for data containing outliers. Compared to detection methods based on the mean and standard deviation, MAD is more robust to extreme values, making it particularly suitable for high-noise environments such as trajectory prediction.

[0151] In trajectory prediction tasks, what is typically dealt with is a continuous sequence of object positions, using position differences. Characterize velocity and motion trends. To detect potential anomalies, the increment for each step must first be calculated:

[0152] (18);

[0153] (19);

[0154] Where t represents the time step, and the subscript t is used for... This represents the increment at a specific time step t; the MAD method is applied to these increments for anomaly detection. First, the median of the increment sequence is calculated, then the absolute deviation of each increment from the median is calculated, and finally, the median of all deviations is taken as the benchmark for anomaly detection.

[0155] (20);

[0156] (twenty one);

[0157] , These are the increments of X and Y, respectively. The value obtained by applying the MAD method, where median is the median.

[0158] The value of the above formula measures the degree to which most increments in the dataset deviate from the median. For a specific time step t, if the difference between the increment at that step and the overall median exceeds 3 times the MAD, then that point is considered an outlier.

[0159] (twenty two);

[0160] (twenty three);

[0161] Then, a set of low-order polynomial functions are used to smoothly connect the data segments, thereby achieving a natural transition for missing points or outliers and maintaining the continuity and high-order smoothness of the entire trajectory.

[0162] Non-outlier points in the trajectory are extracted as interpolation nodes to construct a spline function. Typically, a cubic polynomial is fitted between every two adjacent control points, ensuring the function remains continuous on its first and second derivatives. That is, for the interval... Construct a cubic polynomial:

[0163] (twenty four);

[0164] Among them, S i (x) represents the interval [x] a cubic spline function on; i For node x=x i The value of the cubic polynomial at b i The first derivative of the cubic polynomial at the node x=x i The value at c i For node x=x i Half the value of the second derivative of the cubic polynomial at a given point determines the curvature of the curve, d. i These are the coefficients of the cubic term, used to describe nonlinear changes within the interval.

[0165] Changes in the function value of a cubic spline interpolation node only affect the sub-interval segments on either side of that point, with little impact on other, more distant sub-interval segments.

[0166] The dataset was used to detect and repair manually inserted outliers in the trajectory. The results are as follows: Figure 9 As shown, it can accurately identify and repair outliers, and ensure the smoothness of the repaired trajectory.

[0167] The preprocessed trajectory data is first input into a one-dimensional convolutional neural network (1D-CNN) module to extract local spatiotemporal features. The convolutional module captures local pattern changes in the trajectory over time, such as acceleration, deceleration, and sharp turns, using a sliding window approach. Through multiple layers of convolution and pooling operations, the model can extract multi-scale features from low-order position changes to high-order motion patterns, effectively enhancing its ability to perceive complex trajectory behaviors. The activation functions and kernel parameters in the convolutional layers can be adaptively adjusted according to the behavioral characteristics of different types of obstacles. The output of this stage is an enhanced trajectory feature sequence, which retains the spatial structure information in the trajectory evolution and compresses redundant features, reducing the computational burden on subsequent models.

[0168] Local features are input into a Bidirectional Long Short-Term Memory (Bi-LSTM) network for temporal modeling. The Bi-LSTM network consists of two LSTMs operating in opposite directions, modeling the feature sequence in forward and reverse order along the time axis, respectively. The hidden states from both directions are then concatenated as the output. This enhances the ability to model the overall evolutionary trend of the trajectory. During training, the network learns long-term dependencies in the temporal dimension, enabling it to identify potential behavioral patterns in obstacle movement and effectively address challenges posed by non-linear changes such as turning back, U-turns, and detours. The temporal-series hidden states output by the Bi-LSTM module retain rich dynamic behavioral semantic information, forming the input for subsequent attention mechanisms.

[0169] The structure of this dynamic obstacle trajectory prediction model is as follows: Figure 18 As shown, it consists of an input layer, two bidirectional LSTM layers, two Dropout layers, two fully connected layers, and a regression output layer. The input layer has a receive length of... n The model uses two-dimensional time series data. After training, it can output predicted sequences for future time periods.

[0170] In the model structure design, experiments show that setting the number of hidden units in the LSTM layer to 128 yields better prediction performance, and the Dropout probability is set to 0.2 to prevent overfitting. Fully connected layers are responsible for non-linearly mapping the features of the previous layer, improving the model's expressive power. Each neuron is fully connected to the previous layer, enhancing the feature fusion capability. The ReLU activation function is used in the BiLSTM layer and some fully connected layers to accelerate the network's convergence speed.

[0171] During training, the Adam optimizer is used to update parameters, balancing convergence efficiency and stability. As training progresses, the model gradually optimizes the network weights through error backpropagation, thereby obtaining more accurate prediction results.

[0172] To further emphasize the importance of key time points in predicting the overall motion trend, a self-attention mechanism is introduced to dynamically weight the output of the Bi-LSTM. This mechanism first maps the output vector at each time step to query, key, and value representations, as shown in the following equation. Then, it calculates an attention score based on the similarity between the query and key, normalizes it using a softmax function, and uses the score as a weighting coefficient. Finally, these weights are used to weight the corresponding value vectors, thereby generating the attention representation for each time step. This mechanism allows the model to autonomously determine which historical trajectory segments are most critical to the current prediction and assign them higher weights, while suppressing the influence of redundant or irrelevant information. Through this step, the model can maintain the stability and accuracy of its predictions even when facing irregular trajectories or sudden changes in motion.

[0173] (25);

[0174] To ensure trajectory continuity and smoothness, this invention can also introduce position smoothing terms or velocity constraint terms to further improve prediction stability and physical consistency. In practical deployment, the system supports single-target or multi-target prediction tasks, can perform parallel modeling of various types of dynamic obstacle trajectories, and has good scalability and real-time performance.

[0175] The custom loss function integrates the position prediction error and velocity smoothness constraint from traditional regression tasks, creating a trajectory prediction objective function that balances prediction accuracy and physical plausibility. The design philosophy of the loss function is to introduce an additional velocity smoothness loss term on top of position prediction accuracy. This is achieved by calculating the "acceleration" at each step using second-order differencing and applying a penalty. This acceleration calculation simulates the changing velocity trend of an object in real motion, enabling the model to not only fit the position but also predict reasonable and continuous motion changes. The model output includes the velocity smoothness at each step. The loss function extracts continuous position differences from the predicted X,Y sequence to construct velocity, then calculates velocity changes and introduces weighted penalties. In this way, the model automatically tends to generate prediction results with smooth velocity changes and natural trajectory shapes during training.

[0176] Location loss function L pos for:

[0177] (26);

[0178] In the formula: This is the model's position estimate for the b-th sample at time step t, with dimension . Each prediction step contains ; This represents the actual location value of the model for the b-th sample at the t-th time step. and Dimensionality is consistent; B is the batch size of the sample; T is the prediction step size.

[0179] The velocity smoothness loss component calculates acceleration through two differencing steps, first by calculating the predicted coordinate positions P at time steps t and t+1. t P t+1 Perform a difference operation to obtain the velocity at the t-th time step. :

[0180] (27);

[0181] in Then, the velocity is differentially analyzed again to obtain the acceleration. :

[0182] (28);

[0183] Final acceleration smoothing loss for:

[0184] (29);

[0185] in, Let be the acceleration at the i-th time step of the b-th sequence; The velocity at time step (t+1) forms the total loss function. :

[0186] (30);

[0187] The loss function provides adjustable parameters. This parameter controls the weighting ratio between position error and velocity smoothness. In practical applications, this parameter can be adjusted to achieve a dynamic trade-off between prediction accuracy and smoothness. For example, in scenarios with high accuracy requirements and short trajectories, the smoothness constraint can be weakened, while in scenarios with long-term predictions or higher continuity requirements, the weight of acceleration penalty can be appropriately increased to achieve better robustness and trajectory naturalness.

[0188] To verify the performance of the dynamic obstacle trajectory prediction model proposed in this invention, data from 70 consecutive moments in the trajectory data were used as input for historical trajectories, and trajectory data delayed by 80 moments was predicted. This invention conducts qualitative comparative experiments on the proposed CNN-BiLSTM-Self-Attention model with BiLSTM models, CNN-BiLSTM models, and lossless CNN-BiLSTM-Self-Attention models. The effectiveness of the models is analyzed by comparing the predicted trajectory curves. The actual trajectories and predicted trajectories of the four models are as follows: Figures 10-13 As shown in Table 6, the prediction errors are as follows. It can be seen that with the addition of the model improvement module, the model's prediction accuracy continuously improves.

[0189] Table 6. Comparison of prediction results between the CNN-BiLSTM-Self-Attention model and other models:

[0190]

[0191] Robot path planning stage

[0192] This paper proposes a hybrid approach for intelligent robot path planning in dynamic environments, aiming to meet the high real-time performance, high feasibility, and high smoothness requirements of robots in complex and frequently changing environments. This method combines an improved arbitrary-time repair A* algorithm with a B-spline curve optimization mechanism, balancing global planning with local dynamic adjustment capabilities, to achieve stable, continuous, and controllable path generation and real-time repair during robot motion.

[0193] The improved arbitrary-time repair A* algorithm proposed in this invention is based on the ARA* algorithm. In the ARA* algorithm, the value of the expansion factor is closely related to the actual situation and can directly affect the algorithm's results and time. During the search process, the initial expansion factor determines the algorithm's efficiency and solution quality. A smaller initial expansion factor means a large number of node expansions, leading to reduced search efficiency; a more significant initial expansion factor represents higher search efficiency but may produce suboptimal search results for feasible paths. The choice in the evaluation function is a prerequisite for finding the optimal path within a finite time, and the value of the expansion factor is usually set according to the different scales and complexities of the mobile robot's working environment. In the improved algorithm proposed in this invention, the initial expansion factor can vary with the scale of the working environment, which can balance the algorithm's requirements for search efficiency and optimization results, as shown in the following equation:

[0194] (31);

[0195] The function ceil ensures the expansion factor. , length is the length of the environment map, width is the width of the environment map, and c represents the complexity of the working map, i.e. the density of static obstacles.

[0196] The search step size of the standard ARA* algorithm is set according to actual needs to ensure the safe operation of the mobile robot. If the search step size is too large, the quality of the planned path can be better; however, if the step size is too small, it will affect the solution efficiency and easily lead to getting trapped in local searches. To balance the initial path planning speed and the optimality of the path, this algorithm adopts a dynamic step size strategy during iteration. That is, a larger search step size is used at the beginning of the iteration. After obtaining the initial path, the search step size is decreased iteratively to obtain a more optimal path. Furthermore, the ARA* algorithm consumes a significant amount of computation time in cost calculation and OPEN list node sorting. Using a larger search step size can effectively reduce the number of nodes in the OPEN list, improve initial planning efficiency, and prevent the algorithm from getting trapped in local searches in complex working areas. In the simulation environment, the maximum search step size... and minimum search step size This is determined by considering threat information and the planning performance of the mobile robot. This paper uses... The initial step size is used to improve the speed of the algorithm in planning possible paths. As the number of algorithm iterations increases, the search step size decreases linearly to improve the quality of path planning. When the step size decreases to a certain value... When the time no longer decreases, as shown in the following formula:

[0197] (32);

[0198] In the above equation, It is a decreasing search step size.

[0199] In real-world indoor work scenarios, the generated path should conform to the kinematic and dynamic constraints of the mobile robot. Therefore, the planned path should meet smoothness requirements, ensuring that it matches the actual motion trajectory as closely as possible. When using the ARA* algorithm for path planning, sharp points can occur at turns. The currently planned optimal path output needs to be smoothed across the entire route to facilitate smooth robot operation and reduce unnecessary energy loss at path sharp points. B-spline curves are the most commonly used trajectory smoothing method and a strategy in engineering practice, effectively improving path smoothness and safety.

[0200] Assuming there are a total of n+1 control points used to define the orientation, i.e., the boundary range of the B-spline curve, then the k-th degree B-spline curve P(u) is defined as follows:

[0201] (33);

[0202] In the above formula, It is the i-th control point. It is a k-order basis function indexed i, used to calculate the value of the curve at parameter u, and can be calculated by the following formula.

[0203] (34);

[0204] (35);

[0205] It is the 0th-order basis function of index i, u i u i+1 u i+k u k These represent the i-th, i+1-th, i+k-th, and k-th nodes in the node vector.

[0206] This invention selects a third-order B-spline curve, i.e., k=3, to balance the smoothness of the robot trajectory and the computational complexity.

[0207] The following experiments were conducted to verify the real-time path planning performance of the BS-IARA* algorithm in a dynamic indoor environment. First, the following assumptions were made regarding the mobile robot's operating environment: 1. Both dynamic and static obstacles exist simultaneously in the indoor working environment. 2. Moving obstacles in the working environment are simplified to circumcircles. 3. Obstacles move in straight lines and reverse direction upon reaching the boundary of the working environment. 4. Moving obstacles move at a known constant speed in the available directions.

[0208] Assume the mobile robot moves at a speed of 5 m / s, with initial coordinates of (0.5 m, 0.5 m) and ending coordinates of (48.5 m, 48.5 m), and the algorithm's planning time is limited to 1 second. Information on all dynamic obstacles in the working environment is shown in Table 7.

[0209] Simulation results are as follows Figure 14 , Figure 15 , Figure 16 As shown in Table 8. Figure 14 Figures (a), (b), and (c) illustrate the dynamic path planning of the mobile robot at three different time points using the standard ARA* algorithm. Figure 15 (a), (b), and (c) in the figure show the dynamic path planning of the mobile robot at three different time points under the RWA* algorithm; Figure 16 Figures (a), (b), and (c) illustrate the dynamic path planning of the mobile robot at three different time points using the BS-IARA* algorithm. Table 8 records the path cost, corner cost, and number of expansion nodes.

[0210] Table 8 shows that when using the BS-IARA* algorithm for dynamic path planning, the robot successfully avoided all obstacles while reaching the moving target. Compared with the standard ARA* and RWA* algorithms, the path length was shortened by 1.9%, the path turns were reduced by 64.9% and 53.19% respectively, and the number of extended nodes was reduced by 11.41% and 17.68% respectively. The above numerical simulation results show that the BS-IARA* algorithm proposed in this invention can find a feasible and optimal path within a specified time limit, meeting the path planning requirements in dynamic environments. Path smoothness is greatly improved, and the planning efficiency is high and the feasibility is strong. Compared with the standard ARA* and RWA* algorithms, the BS-IARA* algorithm can plan a feasible path in a very short time and make full use of the planning time to continuously optimize the path. In static environments, it can search for better feasible paths more efficiently, while also adapting to the strict requirements of timeliness and optimality in dynamic environments, exhibiting good smoothness.

[0211] Table 7 Dynamic Obstacle Information Table:

[0212]

[0213] Table 8. Comparison of Path Planning Algorithms:

[0214]

[0215] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for autonomous path planning of an intelligent robot in an unknown environment, characterized in that, This includes the improved FastSLAM map building and localization stage, the dynamic obstacle trajectory repair and prediction stage, and the robot path planning stage; In the improved FastSLAM map building and localization stage, the improved Monarch Butterfly optimization algorithm is used to optimize the particle filter FastSLAM mapping method to realize environmental map building and robot localization. The improved Monarch Butterfly optimization algorithm introduces adaptive genetic parameters and uses migration operators and adjustment operators to enhance the particle filter resampling process to obtain an improved FastSLAM map. In the dynamic obstacle trajectory repair and prediction stage, the original obstacle trajectory is first repaired using the median absolute deviation and cubic spline difference method. Then, a dynamic obstacle trajectory prediction model based on a one-dimensional convolutional neural network, a bidirectional long short-term memory network, and a self-attention mechanism is used to extract the spatial-temporal features of the obstacle trajectory for multi-step prediction of the obstacle trajectory, thus obtaining the predicted obstacle trajectory. In the robot path planning stage, the improved FastSLAM map and predicted obstacle trajectories are input into the path planning module, and the path is generated by combining the path representation based on B-spline curves and the improved A* algorithm; the environment is perceived in real time during the execution process. During the search process, the initial expansion factor is as follows: The function ceil ensures that the expansion factor ε≥1, length is the length of the environment map, width is the width of the environment map, and c represents the complexity of the working map; A dynamic search step size strategy is adopted to gradually decrease the step size from a large step size to the optimal step size l. l=l max -Δl; The maximum search step size is l max Δl is the decreasing search step size; The path smoothing stage uses a third-order B-spline curve. Assume there are a total of n+1 control points defining the boundary range of the B-spline curve. The k-th order B-spline curve P(u) is defined as follows: In the formula, P i B is the i-th control point. i，k (u) is a k-order basis function for index i. Among them, B i，0 (u) is the 0th-order basis function of index i, u i u i+1 u i+k u k These represent the i-th, i+1-th, i+k-th, and k-th nodes in the node vector.

2. The autonomous path planning method for intelligent robots in unknown environments according to claim 1, characterized in that, The improved FastSLAM map building and localization phases include: Initialize the monarch butterfly population and divide the population into region 1 (Land1) and region 2 (Land2); The migration operator updates the position of the monarch butterflies in region 1; Adjust the operator to update the individual positions of Monarch butterflies in region 2; Adaptive adjustment of genetic parameters Keep: Where the total population size of the Monarch butterfly is NP; t is the number of iterations, and ceil is the floor function; The updated monarch butterfly population is embedded into a particle filtering process to output robot pose estimation and map building results.

3. The autonomous path planning method for intelligent robots in unknown environments according to claim 2, characterized in that, The steps of the FastSLAM mapping method using particle filtering optimized by the improved Monarch Butterfly optimization algorithm are as follows: Step 1: Randomly sample N particles and initialize them so that each particle has the robot's initial parameters; Step 2: Replace the particle filter individuals with individual monarch butterflies to divide the monarch butterfly population into two regions, land1 and land2. Step 3: Read the sensor data on the robot to obtain the system observation value at the current moment. Based on the robot's pose at the previous moment, simulate the iterative behavior of the monarch butterfly population. Integrate the adjustment operator and migration operator of the monarch butterfly population into the particle filtering algorithm to adjust the individual particles in land1 and land2. Step 4: Substitute the adaptive genetic parameters into the particle filtering algorithm, and perform optimal processing on the individual particles of each generation as the number of iterations changes; Step 5: Determine whether the number of iterations G has reached the set value. If yes, proceed to Step 6; otherwise, go to Step 3 and continue execution. Step 6: Perform linear optimization combination resampling on the particles; Step 7: Output the estimated state of the system at the current moment; Step 8: Update the system observations and repeat the above steps until the robot system has finished running.

4. The autonomous path planning method for intelligent robots in unknown environments according to claim 1, characterized in that, In the dynamic obstacle trajectory repair and prediction stage, the preprocessing stage uses median absolute deviation to detect trajectory outliers, identify and remove outliers, and repair the trajectory through cubic spline interpolation. A CNN-BiLSTM-Self-Attention prediction model is constructed. Local spatiotemporal features are extracted through 1D-CNN layers, temporal dependencies are modeled through BiLSTM layers, and key trajectory segments are weighted through a self-attention mechanism. The position error and acceleration smoothing constraint are fused using a loss function, and the total loss is calculated.

5. The autonomous path planning method for intelligent robots in unknown environments according to claim 4, characterized in that, Location loss function L pos for: In the formula: Y represents the model's position estimate for the b-th sample at time step t, with dimensions [B×4T]. Each prediction step contains [X,Y,ΔX,ΔY]; b，t This represents the actual location value of the model for the b-th sample at the t-th time step. With Y b，t Dimensionality is consistent; B is the batch size of the sample; T is the prediction step size.

6. The autonomous path planning method for intelligent robots in unknown environments according to claim 5, characterized in that, For the predicted coordinate positions P at time steps t and t+1 t P t+1 Perform a difference operation to obtain the velocity at the t-th time step. Where P t =[X t ,Y t Then, the velocity is differentially divided again to obtain the acceleration. Final acceleration smoothing loss L smooth for: in, Let be the acceleration at the t-th time step of the b-th sequence. The velocity at time step (t+1) forms the total loss function L. total : L total =L pos +λL smooth The adjustable parameter is λ.

Images