Indoor navigation method and system for complex environment

Abstract

This application proposes an indoor navigation method and system for complex environments. Belonging to the field of indoor navigation, the method includes constructing a perception map, establishing an extended Kalman filter, predicting the pose state using IMU measurement data as input, and correcting the predicted pose using positioning data output from a laser SLAM algorithm as observations. On a dynamically updated environmental map, a path search is performed using discretized control actions based on the current starting point and target point. Based on the machine kinematics model and the current state, the pose and control input sequence within the next N steps are predicted. An objective function is constructed, and the optimal control input sequence is obtained by solving the minimization problem of the objective function, and control commands are issued to the actuator. It can improve the overall operational performance and reliability of AGVs in complex and dynamic indoor environments by combining real-time computation, global information fusion, strong resistance to local optima, and efficient dynamic obstacle avoidance.

Description

Technical Field

[0001] This application relates to the field of indoor navigation, and more specifically, to an indoor navigation method and system for complex environments. Background Technology

[0002] Automated Guided Vehicles (AGVs) are key equipment in modern intelligent manufacturing, warehousing and logistics, and their navigation accuracy and operational efficiency directly depend on the performance of the path planning and motion control algorithms employed. In indoor environments, the navigation methods commonly used for differential drive chassis mainly include the Dynamic Window (DWA) method and the Time-Flexible Band (TEB) algorithm.

[0003] The DWA algorithm is a local planner based on velocity sampling. It simulates feasible velocity combinations within a short time and selects the optimal solution based on an evaluation function. However, this method has significant limitations: First, because it relies solely on local environmental information for decision-making, it is prone to getting trapped in local optima, such as failing to effectively escape from U-shaped obstacle areas or complex corridors; second, the algorithm often fails to fully integrate or utilize prior information provided by the global path, leading to local trajectories potentially deviating from the globally optimal direction; third, its ability to handle dynamic obstacles is weak, and its reaction mode is relatively rigid, easily resulting in frequent starts and stops or insufficient avoidance in dense dynamic environments.

[0004] The TEB algorithm improves upon DWA by transforming the path planning problem into a time-based optimization problem, generating trajectories by adjusting the poses and timing of nodes in the "elastic band" along the path. This method generates smoother trajectories that better conform to kinematic constraints and improves adaptability to dynamic obstacles. However, the TEB algorithm introduces higher computational complexity, leading to decreased real-time performance, especially in complex scenarios or when frequent replanning is required, potentially failing to meet the stringent real-time control requirements of AGVs. Furthermore, it remains essentially a local optimization method, failing to fundamentally address the problem of getting trapped in local optima, and still exhibits shortcomings in global environmental perception and long-term decision-making. Summary of the Invention

[0005] The purpose of this application is to provide an indoor navigation method and system for complex environments, which can improve the overall operational performance and reliability of AGVs by taking into account real-time calculation, global information fusion, strong resistance to local optima and efficient dynamic obstacle avoidance in complex and dynamic indoor environments.

[0006] This application is implemented as follows: In a first aspect, this application provides an indoor navigation method for complex environments, comprising the following steps: S1. Perception Map Construction: A two-dimensional grid map is constructed based on the laser SLAM algorithm. According to the real-time laser radar scanning information and positioning information, obstacle mapping and updating are performed on the two-dimensional grid map in real time to obtain a dynamically updated environment map. S2. Multi-source fusion localization and anomaly recovery: S2-1. Establish an extended Kalman filter, whose system model is an IMU kinematic model; S2-2. In each filtering cycle, perform the prediction step: using IMU measurement data as input, predict the pose state of the previous cycle through the system model to obtain the predicted pose, which includes the planar position and heading angle. S2-3, Execute update steps: Use the positioning data output by the laser SLAM algorithm as the observation value to correct the predicted pose and obtain the fused estimated pose for the current period; S2-4. Calculate the residuals in distance and angle between the current periodic positioning data and the estimated pose after fusion. If the residuals exceed the preset threshold, trigger the recovery behavior. The recovery behavior includes: judging the environmental safety based on real-time lidar information, and performing in-situ turning calibration under safe conditions until the residuals between the pose output by the lidar SLAM algorithm and the estimated pose after fusion meet the preset threshold. S3. Heuristic Path Search and Generation: On a dynamically updated environment map, path search is performed based on the current starting point and target point using discretized control actions; the path search also employs two heuristic functions to prune search branches: First heuristic function: Under the assumption of no obstacles, calculate the shortest feasible path length from the current node to the target point based on machine kinematic constraints; The second heuristic function ignores machine kinematic constraints and calculates the shortest geometric path distance from each grid to the target point based on a two-dimensional grid map using a two-dimensional spatial dynamic programming algorithm. During the search process, the maximum value between the calculation results of the first and second heuristic functions is taken as the comprehensive heuristic value; the paths obtained by the search are optimized using Reeds-Shepp curves to obtain continuous paths; S4. Path processing and tracking control: S4-1. Perform smoothing optimization on the continuous path. The optimization objectives include the distance to obstacles and the path curvature to obtain a reference path. S4-2. Model Predictive Control (MPC) is used to track the reference path: In each control cycle, the nearest point on the reference path with the current fused estimated pose is selected as the tracking starting point. S4-3. Based on the machine kinematics model and the current state, predict the pose and control input sequence within the next N steps; construct an objective function, which includes: the tracking error between the predicted pose sequence and the reference path segment starting from the tracking point, the change in control input, and the terminal error between the predicted state and the target state at step N; obtain the optimal control input sequence by solving the minimization problem of the objective function, and issue the first control command in the sequence to the actuator.

[0007] Secondly, this application provides an indoor navigation system for complex environments, comprising: Perception map construction module: Based on the laser SLAM algorithm, a two-dimensional grid map is constructed, and obstacle mapping and updating are performed on the two-dimensional grid map in real time according to the real-time laser radar scanning information and positioning information to obtain a dynamically updated environment map; The multi-source fusion localization and anomaly recovery module includes: Filter submodule: Establish an extended Kalman filter, whose system model is an IMU kinematic model; Pose prediction submodule: In each filtering cycle, IMU measurement data is used as input to predict the pose state of the previous cycle through the system model to obtain the predicted pose, which includes the planar position and heading angle. The update submodule uses the localization data output by the laser SLAM algorithm as the observation value to correct the predicted pose and obtain the fused estimated pose for the current period. Trigger recovery submodule: Calculate the residuals in distance and angle between the current periodic positioning data and the fused estimated pose; if the residuals exceed the preset thresholds, trigger recovery behavior, which includes: judging environmental safety based on real-time LiDAR information, and performing in-situ turning calibration under safe conditions until the residuals between the pose output by the LiDAR SLAM algorithm and the fused estimated poses meet the preset thresholds; Heuristic pathfinding and generation module: On a dynamically updated environment map, based on the current starting point and target point, pathfinding is performed using discretized control actions; the pathfinding also employs two heuristic functions to prune search branches: First heuristic function: Under the assumption of no obstacles, calculate the shortest feasible path length from the current node to the target point based on machine kinematic constraints; The second heuristic function ignores machine kinematic constraints and calculates the shortest geometric path distance from each grid to the target point based on a two-dimensional grid map using a two-dimensional spatial dynamic programming algorithm. During the search process, the maximum value between the calculation results of the first and second heuristic functions is taken as the comprehensive heuristic value; the paths obtained by the search are optimized using Reeds-Shepp curves to obtain continuous paths; The path processing and tracking control module includes: Reference path acquisition submodule: Performs smooth optimization on continuous paths, with optimization objectives including distance from obstacles and path curvature, to obtain a reference path; Reference path tracking submodule: In each control cycle, select the nearest point on the reference path with the current fused estimated pose as the tracking starting point; The control command issuance submodule predicts the pose and control input sequence within the next N steps based on the machine's kinematics model and current state. It constructs an objective function, which includes the tracking error between the predicted pose sequence and the reference path segment starting from the tracking origin, the change in control input, and the terminal error between the predicted state and the target state at step N. By solving the minimization problem of the objective function, it obtains the optimal control input sequence and issues the first control command in the sequence to the actuator.

[0008] Thirdly, this application provides an electronic device, comprising: Memory, used to store one or more programs; processor; The above method is implemented when one or more programs are executed by the processor.

[0009] Fourthly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method.

[0010] Compared with the prior art, this application has at least the following advantages or beneficial effects: This invention provides an indoor navigation method and system for complex environments. It uses positioning data as an observation input to an extended Kalman filter, which is then fused with the predicted pose. Through Kalman gain calculation, a superior fused estimated pose is output, combining the high-frequency smoothness of an IMU with the absolute accuracy of laser SLAM. By incorporating residual detection, safety judgment, and in-situ turning calibration, it possesses intelligent fault tolerance. Combining the advantages of a first and second heuristic function, the search process converges quickly while finding a globally optimal and more reasonable path. By receiving dynamic maps, reliable poses, and optimized paths, it outputs control commands to drive the entity, ultimately completing a fully autonomous navigation closed loop of perception-localization-decision-control. It can improve the overall operational performance and reliability of AGVs in complex and dynamic indoor environments by simultaneously achieving real-time computation, global information fusion, strong resistance to local optima, and efficient dynamic obstacle avoidance. Attached Figure Description

[0011] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0012] Figure 1 This is a flowchart illustrating an indoor navigation method for complex environments according to this application; Figure 2 This is a schematic diagram of the structure of an indoor navigation system for complex environments according to this application; Figure 3 This is a schematic diagram of the structure of an electronic device according to this application.

[0013] icon: 1. Perceptual map construction module; 2. Multi-source fusion localization and anomaly recovery module; 21. Filter submodule; 22. Predicted pose submodule; 23. Execution update submodule; 24. Triggered recovery submodule; 3. Heuristic path search and generation module; 4. Path processing and tracking control module; 41. Reference path acquisition submodule; 42. Reference path tracking submodule; 43. Control command issuance submodule; 5. Processor; 6. Memory; 7. Communication interface. Detailed Implementation

[0014] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0015] The following detailed description of some embodiments of this application is provided in conjunction with the accompanying drawings. Unless otherwise specified, the various embodiments and features described below can be combined with each other. Example

[0016] This application provides an indoor navigation method and system for complex environments, which can improve the overall operational performance and reliability of AGVs by taking into account real-time computing, global information fusion, strong resistance to local optima, and efficient dynamic obstacle avoidance in complex and dynamic indoor environments.

[0017] Please refer to Figure 1 This indoor navigation method for complex environments includes the following steps: S1. Perception Map Construction: A two-dimensional grid map is constructed based on the laser SLAM algorithm. According to the real-time laser radar scanning information and positioning information, obstacle mapping and updating are performed on the two-dimensional grid map in real time to obtain a dynamically updated environment map. Specifically, during machine initialization, an initial two-dimensional grid map is constructed using a laser SLAM algorithm (e.g., the open-source algorithms Cartographer or Hector SLAM). The laser radar preferably acquires real-time scan data continuously at a frequency of 10Hz, though the specific frequency is not limited. Based on the real-time laser radar scan information and positioning information, obstacle mapping and updates are performed in real-time on the two-dimensional grid map, resulting in a dynamically updated environmental map. This setup solves the problem of static maps becoming ineffective indoor environments (such as warehouses and factories) due to changes in the environment caused by personnel movement and goods handling, providing an accurate and reliable real-time environmental model for navigation.

[0018] S2. Multi-source fusion localization and anomaly recovery: S2-1. Establish an extended Kalman filter, whose system model is an IMU kinematic model; S2-2. In each filtering cycle, perform the prediction step: using IMU measurement data as input, predict the pose state of the previous cycle through the system model to obtain the predicted pose, which includes the planar position and heading angle. Specifically, the system state of the extended Kalman filter is the pose state vector [x,y,θ]^T. The system model is an IMU kinematic model. In each filtering cycle (e.g., 100Hz), the estimated pose from the previous moment and the angular velocity and linear acceleration measured by the current IMU are used as inputs, and the current predicted pose [x_pred,y_pred,θ_pred]^T is predicted through integration.

[0019] S2-3, Execute update steps: Use the positioning data output by the laser SLAM algorithm as the observation value to correct the predicted pose and obtain the fused estimated pose for the current period; Specifically, the laser SLAM front-end preferably outputs localization data [x_slam, y_slam, θ_slam]^T at a frequency of 5-10Hz. This localization data is used as the observation input to an extended Kalman filter and fused with the predicted pose. Through Kalman gain calculation, a better fused estimated pose [x_fused, y_fused, θ_fused]^T is finally output. This pose combines the high-frequency smoothness of an IMU with the absolute accuracy of laser SLAM. By using extended Kalman filtering fusion, combining the high-frequency dynamic response advantage of an IMU with the absolute accuracy advantage of laser SLAM, a more stable and accurate fused estimated pose can be output than that of a single sensor, providing continuous estimation even when the laser is temporarily unavailable (e.g., by strong light or glass interference).

[0020] S2-4. Calculate the residuals in distance and angle between the current periodic positioning data and the estimated pose after fusion. If the residuals exceed the preset threshold, trigger the recovery behavior. The recovery behavior includes: judging the environmental safety based on real-time lidar information, and performing in-situ turning calibration under safe conditions until the residuals between the pose output by the lidar SLAM algorithm and the estimated pose after fusion meet the preset threshold. Specifically, the residuals (including distance and angle residuals) between [x_slam, y_slam, θ_slam]^T and [x_fused, y_fused, θ_fused]^T are calculated. A preset threshold is set (e.g., distance residual > 0.15 meters or angle residual > 0.1 radians). When the residuals do not exceed the threshold, the positioning is considered reliable, and the process loops normally. When the residuals exceed the threshold (indicating that in harsh perception environments such as glass reflection or sparse features in long corridors, the positioning results based on laser SLAM may be inaccurate or undergo abrupt changes), recovery behavior is immediately triggered: Safety assessment: Based on the current frame of LiDAR data, quickly analyze whether there are obstacles around the machine (especially within the rotation radius).

[0021] In-situ turning calibration: After confirming safety, the machine is slowly rotated in place at a fixed angular velocity (e.g., 15 degrees per second). During the rotation, new LiDAR scan frames are continuously acquired, and the machine's pose is calculated in real time using the SLAM algorithm, while simultaneously calculating the residual between the residual and the predicted value of the extended Kalman filter.

[0022] Iterative convergence: When the residual falls back to within a preset threshold, the rotation stops immediately. At this point, the pose output by the SLAM algorithm is realigned with the IMU prediction, and the system localization is restored to a reliable state, thus realizing the system's self-diagnosis and self-healing, and significantly improving its robustness in harsh perception environments.

[0023] By incorporating residual detection, safety assessment, and on-the-spot turning calibration, the system possesses intelligent fault tolerance capabilities. When positioning becomes inaccurate due to environmental interference, it can proactively trigger calibration actions to regain the correct pose, greatly improving the system's long-term operational reliability and unmanned operation level in complex and unstructured environments, and preventing complete navigation task failure due to positioning divergence.

[0024] S3. Heuristic Path Search and Generation: On a dynamically updated environment map, path search is performed based on the current starting point and target point using discretized control actions; the path search also employs two heuristic functions to prune search branches: Specifically, when the machine receives a new target point instruction, it initiates path search on the dynamic map generated in step S1. This embodiment employs an improved Hybrid A search framework, the innovation of which lies in the collaborative guidance of dual heuristic functions.

[0025] First heuristic function: Under the assumption of no obstacles, calculate the shortest feasible path length from the current node to the target point based on machine kinematic constraints; Specifically, the Reeds-Shepp path model is used to calculate the shortest feasible path length from the current search node (x,y,θ) to the target point (x_g,y_g,θ_g). This function considers the machine's minimum turning radius and effectively evaluates kinematic costs. It guides the search towards a kinematically reachable target direction, avoiding the generation of bizarre, unexecutable paths. The second heuristic function ignores machine kinematic constraints and calculates the shortest geometric path distance from each grid to the target point based on a two-dimensional grid map using a two-dimensional spatial dynamic programming algorithm. Specifically, dynamic programming (Dijkstra's algorithm) is used to pre-calculate the shortest geometric path distance from the target point to each obstacle-free grid cell on the map. This function ignores direction and only considers obstacle avoidance distance in two-dimensional space. The second heuristic function can identify dead ends and U-shaped obstacles in the map, guiding the search to avoid these inefficient areas.

[0026] During the search process, the maximum value between the calculation results of the first and second heuristic functions is taken as the comprehensive heuristic value; the paths obtained by the search are optimized using Reeds-Shepp curves to obtain continuous paths; Specifically, the combined heuristic value (H) is calculated as follows: When searching each node, H = max(h1, h2), where h1 and h2 are the first and second heuristic functions, respectively. The purpose of setting the maximum value is that h2 helps the search avoid U-shaped obstacles and dead ends, preventing it from falling into local traps. h1 guides the search towards a kinematically accessible target posture. Combining the advantages of both allows the search process to converge quickly and find a globally optimal and more reasonable path, significantly outperforming a single heuristic function.

[0027] S4. Path processing and tracking control: S4-1. Perform smoothing optimization on the continuous path. The optimization objectives include the distance to obstacles and the path curvature to obtain a reference path. Specifically, gradient descent smoothing optimization is performed on the continuous path generated in step S3 to minimize the path curvature while ensuring a safe distance, resulting in a reference path that is more conducive to tracking.

[0028] S4-2. Model Predictive Control (MPC) is used to track the reference path: In each control cycle, the nearest point on the reference path with the current fused estimated pose is selected as the tracking starting point. S4-3. Based on the machine kinematics model and the current state, predict the pose and control input sequence within the next N steps; construct an objective function, which includes: the tracking error between the predicted pose sequence and the reference path segment starting from the tracking point, the change in control input, and the terminal error between the predicted state and the target state at step N; obtain the optimal control input sequence by solving the minimization problem of the objective function, and issue the first control command in the sequence to the actuator.

[0029] Specifically, a Model Predictive Controller (MPC) is used for path tracking with a control cycle of 50ms. Based on the current fused estimated pose, the nearest point on the reference path is found as the tracking starting point. The machine differential kinematics model is used as the prediction model. The preferred prediction time domain is N=10, and the time step is T=0.05s. An objective function J is constructed, which includes: a tracking error term: penalizing the deviation between the predicted pose sequence and the reference path segment (starting from the tracking starting point); a control variable change term: penalizing drastic changes in linear and angular velocities to ensure smooth motion; and a terminal error term: penalizing the deviation between the predicted state at step N and the desired final state. Rolling optimization and execution: In each control cycle, the future N-step control sequence [v0,ω0;v1,ω1;...] that minimizes the objective function J is solved, and the first control command (v0,ω0) in the sequence is issued to the chassis actuator. This process is repeated to achieve accurate, smooth, and forward-looking path tracking. By predicting future multi-step states, the tracking error is minimized by optimizing the control sequence. This setup not only considers the current deviation but also anticipates the path's direction (such as curves), allowing for smoother adjustments in advance and more accurate and fluid tracking. By constructing an objective function that comprehensively considers tracking error, the rate of change of control variables (comfort / energy consumption), and terminal error, multiple objectives such as accuracy, stability, and efficient arrival are simultaneously met through a single optimization, achieving high-performance closed-loop control. Furthermore, this step receives the dynamic map from step S1, the reliable pose from step S2, and the optimized path from step S3, and outputs control commands to drive the entity, ultimately completing the fully autonomous navigation closed loop of perception-localization-decision-control.

[0030] Preferably, the system model of the extended Kalman filter is an IMU kinematic model, and its process noise covariance matrix and measurement noise covariance matrix are configured based on the calibration parameters of the IMU sensor.

[0031] Furthermore, the shortest feasible path length based on machine kinematic constraints in the first heuristic function is calculated using the Dubins path or Reeds-Shepp path model. The calculation steps include: inputting the pose states of the starting and target points and the minimum turning radius; enumerating all possible path type combinations; calculating the total length of each path combination, and taking the minimum value as the first heuristic function value. Since the evaluation criteria include steering costs, the ultimately searched path is inherently superior in smoothness and executability, laying a solid foundation for subsequent tracking control.

[0032] Furthermore, during the search process, when the Euclidean distance from the search node to the target point is less than a set threshold, Reeds-Shepp curve optimization is triggered. Reeds-Shepp curve optimization includes: calculating all possible Reeds-Shepp paths from the current node state to the target state; performing collision detection; and selecting the shortest collision-free path to replace the original discrete search path segment. By switching to an analytical solution (Reeds-Shepp curve) that can accurately connect arbitrary poses, it ensures that the planned path can seamlessly reach the target position and pose, achieving a closed loop from discrete approximation to continuous accuracy.

[0033] Furthermore, the specific steps of collision detection are as follows: the continuous Reeds-Shepp path is discretized into multiple detection points; each detection point's grid is determined to be an obstacle in the dynamically updated environment map. Through the dynamically updated environment map, new obstacles that appear after planning can be identified and avoided, enabling the planning system to adapt to real-time environmental changes and significantly improving safety.

[0034] Furthermore, in step S3, the second heuristic function is calculated using Dijkstra's algorithm or dynamic programming. The specific steps include: initializing the distance value to 0 starting from the grid cell containing the target point; updating the shortest geometric distance from each obstacle-free grid cell in the map to the target point via backpropagation; where the movement cost between adjacent grid cells is set according to the movement direction. This function constructs a global distance field, explicitly knowing the shortest geometric distance from each location in the map to the target point while avoiding all obstacles. Therefore, it effectively guides the search around complex traps such as dead ends and U-shaped obstacles, preventing the search from wandering in local areas and ensuring the global optimality of the search.

[0035] This embodiment also provides an indoor navigation system for complex environments, including: Perception Map Construction Module 1: Based on the laser SLAM algorithm, a two-dimensional grid map is constructed, and obstacle mapping and updating are performed on the two-dimensional grid map in real time according to the real-time laser radar scanning information and positioning information to obtain a dynamically updated environment map; Multi-source fusion localization and anomaly recovery module 2 includes: Filter submodule 21: Establish an extended Kalman filter, whose system model is an IMU kinematic model; Predicted pose submodule 22: In each filtering cycle, IMU measurement data is used as input to predict the pose state of the previous cycle through the system model to obtain the predicted pose, which includes the planar position and heading angle. Execute update submodule 23: Use the positioning data output by the laser SLAM algorithm as the observation value to correct the predicted pose and obtain the fused estimated pose for the current period; Trigger recovery submodule 24: Calculate the residuals in distance and angle between the current periodic positioning data and the fused estimated pose; if the residuals exceed the preset thresholds, trigger recovery behavior, which includes: judging environmental safety based on real-time lidar information, and performing in-situ turning calibration under safe conditions until the residuals between the pose output by the laser SLAM algorithm and the fused estimated poses meet the preset thresholds; Heuristic Path Search and Generation Module 3: On a dynamically updated environment map, based on the current starting point and target point, path search is performed using discretized control actions; the path search also employs two heuristic functions to prune search branches: First heuristic function: Under the assumption of no obstacles, calculate the shortest feasible path length from the current node to the target point based on machine kinematic constraints; The second heuristic function ignores machine kinematic constraints and calculates the shortest geometric path distance from each grid to the target point based on a two-dimensional grid map using a two-dimensional spatial dynamic programming algorithm. During the search process, the maximum value between the calculation results of the first and second heuristic functions is taken as the comprehensive heuristic value; the paths obtained by the search are optimized using Reeds-Shepp curves to obtain continuous paths; Path processing and tracking control module 4 includes: Reference path acquisition submodule 41: Performs smooth optimization on continuous paths, with optimization objectives including distance from obstacles and path curvature, to obtain a reference path; Reference path tracking submodule 42: In each control cycle, select the nearest point on the reference path after the current fused estimated pose as the tracking starting point; Control command issuance submodule 43: Based on the machine kinematics model and the current state, predict the pose and control input sequence within the next N steps; construct an objective function, which includes: the tracking error between the predicted pose sequence and the reference path segment starting from the tracking point, the change in control input, and the terminal error between the predicted state and the target state at step N; obtain the optimal control input sequence by solving the minimization problem of the objective function, and issue the first control command in the sequence to the actuator.

[0036] For a detailed implementation of the indoor navigation system for complex environments, please refer to the above-mentioned implementation of the indoor navigation method for complex environments, which will not be elaborated upon here.

[0037] This embodiment also provides an electronic device, including: Memory 6 is used to store one or more programs; Processor 5; Processor 5 is connected to memory 6 via communication interface 7; When one or more programs are executed by processor 5, all or some of the above methods are implemented.

[0038] This embodiment also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by the processor 5, implements all or part of the methods described above.

[0039] It will be apparent to those skilled in the art that this application is not limited to the details of the exemplary embodiments described above, and that this application can be implemented in other specific forms without departing from the spirit or essential characteristics of this application. Therefore, the embodiments should be considered illustrative and non-limiting in all respects, and the scope of this application is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within this application. No reference numerals in the claims should be construed as limiting the scope of the claims.

Claims

1. An indoor navigation method for complex environments, characterized in that, Includes the following steps: S1. Perception Map Construction: A two-dimensional grid map is constructed based on the laser SLAM algorithm. According to the real-time laser radar scanning information and positioning information, obstacle mapping and updating are performed on the two-dimensional grid map in real time to obtain a dynamically updated environment map. S2. Multi-source fusion localization and anomaly recovery: S2-1. Establish an extended Kalman filter, whose system model is an IMU kinematic model; S2-2. In each filtering cycle, a prediction step is performed: using IMU measurement data as input, the pose state of the previous cycle is predicted through the system model to obtain the predicted pose, which includes the planar position and heading angle. S2-3. Execute update step: Use the positioning data output by the laser SLAM algorithm as the observation value to correct the predicted pose and obtain the fused estimated pose for the current period. S2-4. Calculate the residuals in distance and angle between the current period's positioning data and the fused estimated pose; If the residual exceeds a preset threshold, a recovery action is triggered. The recovery action includes: judging the environmental safety based on real-time lidar information, and performing in-situ turning calibration under safe conditions until the residual between the pose output by the lidar SLAM algorithm and the estimated pose after fusion meets the preset threshold. S3. Heuristic Path Search and Generation: On the dynamically updated environment map, path search is performed based on the current starting point and target point using discretized control actions; the path search simultaneously employs two heuristic functions to prune search branches: First heuristic function: Under the assumption of no obstacles, calculate the shortest feasible path length from the current node to the target point based on machine kinematic constraints; The second heuristic function: Ignoring machine kinematic constraints, based on the two-dimensional grid map, calculates the shortest geometric path distance from each grid to the target point using a two-dimensional spatial dynamic programming algorithm; During the search process, the maximum value between the calculation results of the first heuristic function and the second heuristic function is taken as the comprehensive heuristic value; the searched path is optimized by using Reeds-Shepp curves to obtain a continuous path; S4. Path processing and tracking control: S4-1. Perform smoothing optimization on the continuous path, with optimization objectives including the distance to obstacles and path curvature, to obtain a reference path; S4-2. Model Predictive Control (MPC) is used to track the reference path: In each control cycle, the nearest point on the reference path with the current fused estimated pose is selected as the tracking starting point; S4-3. Based on the machine kinematics model and the current state, predict the pose and control input sequence within the next N steps; construct an objective function, which includes: the tracking error between the predicted pose sequence and the reference path segment starting from the tracking starting point, the change in control input, and the terminal error between the predicted state and the target state at the Nth step; obtain the optimal control input sequence by solving the minimization problem of the objective function, and issue the first control command in the sequence to the actuator.

2. The indoor navigation method for complex environments according to claim 1, characterized in that, In step S2, the system model of the extended Kalman filter is an IMU kinematic model, and its process noise covariance matrix and measurement noise covariance matrix are configured based on the calibration parameters of the IMU sensor.

3. The indoor navigation method for complex environments according to claim 1, characterized in that, In step S3, the shortest feasible path length based on machine kinematic constraints in the first heuristic function is calculated using the Dubins path or Reeds-Shepp path model. The calculation steps include: inputting the pose state of the starting point and the target point and the minimum turning radius; enumerating all possible path type combinations; calculating the total length of each path combination, and taking the minimum value as the value of the first heuristic function.

4. The indoor navigation method for complex environments according to claim 1, characterized in that, In step S3, during the search process, when the Euclidean distance from the search node to the target point is less than a set threshold, Reeds-Shepp curve optimization is triggered. The Reeds-Shepp curve optimization includes: calculating all possible Reeds-Shepp paths from the current node state to the target state; performing collision detection; and selecting the shortest collision-free path to replace the original discrete search path segment.

5. The indoor navigation method for complex environments according to claim 4, characterized in that, The specific steps of the collision detection are as follows: discretize the continuous Reeds-Shepp path into multiple detection points; determine one by one whether the grid where each detection point is located is an obstacle in the dynamically updated environment map.

6. The indoor navigation method for complex environments according to claim 1, characterized in that, In step S3, the second heuristic function is calculated using Dijkstra's algorithm or dynamic programming. The specific steps include: starting from the grid cell where the target point is located, initializing the distance value to 0; backpropagating to update the shortest geometric distance from each obstacle-free grid cell in the map to the target point; wherein, the movement cost between adjacent grid cells is set according to the movement direction.

7. An indoor navigation system for complex environments, characterized in that, include: Perception map construction module: Based on the laser SLAM algorithm, a two-dimensional grid map is constructed, and obstacle mapping and updating are performed on the two-dimensional grid map in real time according to the real-time laser radar scanning information and positioning information to obtain a dynamically updated environment map; The multi-source fusion localization and anomaly recovery module includes: Filter submodule: Establish an extended Kalman filter, whose system model is an IMU kinematic model; Pose prediction submodule: In each filtering cycle, IMU measurement data is used as input to predict the pose state of the previous cycle through the system model to obtain the predicted pose, which includes the planar position and heading angle. The update submodule executes the following: using the positioning data output by the laser SLAM algorithm as the observation value, the predicted pose is corrected to obtain the fused estimated pose for the current period. Trigger recovery submodule: Calculate the residuals in distance and angle between the current period's positioning data and the fused estimated pose; if the residuals exceed a preset threshold, trigger recovery behavior, which includes: judging environmental safety based on real-time lidar information, and performing in-situ turning calibration under safe conditions until the residuals between the pose output by the laser SLAM algorithm and the fused estimated pose meet the preset threshold; Heuristic path search and generation module: On the dynamically updated environment map, based on the current starting point and target point, path search is performed using discretized control actions; the path search simultaneously employs two heuristic functions to prune search branches: First heuristic function: Under the assumption of no obstacles, calculate the shortest feasible path length from the current node to the target point based on machine kinematic constraints; The second heuristic function: Ignoring machine kinematic constraints, based on the two-dimensional grid map, calculates the shortest geometric path distance from each grid to the target point using a two-dimensional spatial dynamic programming algorithm; During the search process, the maximum value between the calculation results of the first heuristic function and the second heuristic function is taken as the comprehensive heuristic value; the searched path is optimized by using Reeds-Shepp curves to obtain a continuous path; The path processing and tracking control module includes: Reference path acquisition submodule: performs smooth optimization on the continuous path, with optimization objectives including the distance to obstacles and path curvature, to obtain a reference path; Reference path tracking submodule: In each control cycle, select the nearest point on the reference path after the current fused estimated pose as the tracking start point; The control command issuance submodule predicts the pose and control input sequence within the next N steps based on the machine kinematics model and the current state; it constructs an objective function, which includes: the tracking error between the predicted pose sequence and the reference path segment starting from the tracking starting point, the change in control input, and the terminal error between the predicted state and the target state at the Nth step; by solving the minimization problem of the objective function, it obtains the optimal control input sequence and issues the first control command in the sequence to the actuator.

8. An electronic device, characterized in that, include: Memory, used to store one or more programs; processor; When the one or more programs are executed by the processor, the method as described in any one of claims 1-6 is implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-6.

Images