An indoor mobile robot hierarchical dynamic obstacle avoidance method based on human-like decision mechanism

Abstract

The application provides an indoor mobile robot hierarchical dynamic obstacle avoidance method based on a human-like decision mechanism, which comprises the following steps: 1, the indoor mobile robot obtains observation and positioning information of obstacles through a sensor, and constructs a multi-model predictor to perform prediction; 2, hierarchical decision based on distance driving is performed, and step 3 or step 4 is executed; 3, global path planning, a global path is generated according to an improved APF method, and the prediction information of the motion state of the obstacle is fused to control the movement of the robot, and step 1 is re-executed; 4, local obstacle avoidance control, based on the latest global path, an improved dynamic window method (DWA) is used to determine the optimal speed, the movement of the robot is controlled, and step 1 is re-executed. The application can effectively improve the path planning efficiency of the robot in a complex dynamic indoor environment and the response ability of the robot to sudden dynamic obstacles, and has the advantages of low calculation amount, high planning efficiency, automatic response and the like.

Description

Technical Field

[0001] This invention relates to a robot obstacle avoidance method, and more particularly to a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism. Background Technology

[0002] Conscious autonomous navigation is a fundamental human skill for adapting to the environment. However, for robots, achieving autonomous navigation in dynamically changing or densely obstacle-filled external environments is a challenging and complex task. Dynamic environments are characterized by complex terrain structures, a lack of prior information, and the unknown motion states of moving obstacles. Robots must accurately avoid collisions with local dynamic or static obstacles while safely reaching the global target location in the shortest time or along the smoothest path. Therefore, it is necessary to integrate available environmental data, pose information, and task requirements to autonomously plan a safe collision-avoidance route from the starting point to the target point, satisfying specific optimal criteria within an acceptable timeframe. Based on the functional characteristics of mobile robot operation, path planning can be divided into global planning and local obstacle avoidance. Local obstacle avoidance algorithms focus on the mobile robot's navigation within its surrounding environment. Utilizing its onboard sensors, the robot autonomously determines or controls its direction of movement, ensuring safe and effective obstacle avoidance while moving towards the target. These methods are crucial for improving the autonomy and safety of indoor mobile robots.

[0003] Representative strategies and techniques include the Dynamic Window Approach (DWA), the Artificial Potential Field (APF) method, and related improvements and fusion algorithms. Due to its real-time performance and compatibility with dynamic constraints, the DWA method has become a classic approach for robot local path planning and real-time obstacle avoidance, and is widely used in robot operating systems and various improved versions. It samples feasible trajectories in the online velocity and angular velocity space through a dynamic velocity window and selects the optimal path by combining evaluation functions such as obstacle distance and target orientation. Improvements to this method mainly focus on addressing the problems of low operating efficiency, susceptibility to local optimization, and poor search capabilities in complex environments found in existing methods.

[0004] The basic idea of ​​the APF method is to model the robot's motion environment as a virtual potential field. The target point generates an attractive force, while obstacles generate a repulsive force. By calculating the resultant potential field in real time, the robot plans its path by moving along the gradient descent direction of the potential field, gradually avoiding obstacles and approaching the target. This method is computationally simple and has strong real-time performance, but it is prone to getting trapped in local minima or experiencing path oscillations that make the target unreachable, and it is not suitable for dynamic environments. To address these common problems, improved solutions focus on optimizing the algorithm architecture and adjusting local parameters.

[0005] In existing adaptive dynamic obstacle avoidance fusion methods, obstacle prediction research often relies on the assumption that the obstacle's motion state is known or conforms to a fixed motion state equation. However, such ideal conditions are rarely met in complex real-world environments. Local obstacle avoidance methods can handle unpredictable environmental changes, but as local solutions, they often fail to meet the requirements of global path planning efficiency. Furthermore, while existing dynamic obstacle avoidance methods demonstrate excellent performance in low-obstacle-density dynamic environments, high-density dynamic multi-obstacle environments present significant challenges to their practical application due to the more complex obstacle distribution and greater motion randomness. Moreover, current research on obstacle avoidance strategies in complex dynamic multi-obstacle environments largely relies on real-time sensor data for perception and control, making it difficult to predict the long-term trajectories of maneuvering obstacles and reducing the success rate. Alternatively, some studies rely solely on simulation systems for algorithm performance verification, neglecting parameter configuration constraints and errors in real-world applications, thus reducing applicability in practical scenarios. Additionally, while local obstacle avoidance methods can handle unpredictable environmental changes, their local solutions often fail to meet the requirements of global path planning efficiency. Therefore, there is an urgent need to strategically integrate various algorithms to improve and balance the overall performance of autonomous obstacle avoidance and path planning, and to enhance the feasibility and efficiency of the methods in practical application scenarios. Summary of the Invention

[0006] Technical objective: The technical problem to be solved by the present invention is to provide a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism, addressing the shortcomings of the existing technology.

[0007] To address the aforementioned technical problems, this invention discloses a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism, comprising the following steps:

[0008] Step 1: The indoor mobile robot acquires the observation and positioning information of obstacles through sensors, builds a multi-model predictor to make predictions, and obtains the predicted information of the obstacle's motion state.

[0009] Step 2: Make distance-driven hierarchical decisions based on obstacle motion state prediction information, and execute Step 3 global path planning or Step 4 local obstacle avoidance control.

[0010] Step 3: Global path planning. Generate a global path based on the improved APF method, integrate obstacle motion state prediction information, control the robot's movement, and repeat step 1.

[0011] Step 4, Local obstacle avoidance control: Based on the latest global path, the improved Dynamic Window Method (DWA) is used to determine the optimal speed, control the robot's movement, and repeat Step 1.

[0012] Furthermore, the construction of the multi-model predictor for prediction described in step 1 includes:

[0013] Step 1-1: Construct three types of motion models, using the approximate constant velocity NCV model, the approximate constant acceleration NCA model, and the random walk RM model to describe the various motion states of the obstacle;

[0014] Step 1-2: Based on the three types of motion models constructed in Step 1-1, interactive multi-model Kalman filtering is used. By running multiple filters in parallel and dynamically adjusting the weights according to the matching degree between each model and the current observation data, the obstacle motion state prediction information, i.e., state prediction, is obtained. Covariance Matrix .

[0015] Furthermore, the state vector of the approximately constant velocity NCV model described in step 1-1... and state transition matrix They are defined as follows:

[0016] ,

[0017] ,

[0018] in, These are the robot's position coordinates in a two-dimensional plane; and These are the robot's velocities in the x and y directions; It is the robot's heading angle, and T is the observation period duration;

[0019] The discrete-time state equation is expressed as follows:

[0020] ,

[0021] in, It is noise.

[0022] Furthermore, the state vector of the approximate constant-acceleration NCA model described in step 1-1... and state transition matrix They are defined as follows:

[0023] ,

[0024] ,

[0025] in, and It refers to the robot's acceleration in the x and y directions;

[0026] The discrete-time state equation is expressed as follows:

[0027] ,

[0028] in, It is noise.

[0029] Furthermore, the discrete state vector of the random walk RM model described in step 1-1... Defined as:

[0030] ,

[0031] in, and Let be the rate of change of the robot's acceleration in the x and y directions;

[0032] The discrete-time state equation is expressed as follows:

[0033] ,

[0034] in, The mean value of the acceleration. and These are the average values ​​of the acceleration in the two directions, respectively. Here is the state transition matrix. The control matrices are represented as follows:

[0035] ,

[0036] ,

[0037] in, This is the decay term of acceleration.

[0038] Furthermore, the distance-driven hierarchical decision-making based on obstacle motion state prediction information described in step 2 includes:

[0039] Centered on the robot's current position, and based on the set obstacle avoidance distance threshold, the robot's motion space is divided into a path planning area and a local obstacle avoidance area. The relative distance between the robot and the nearest obstacle is calculated. If step 3 has never been executed or the relative distance is greater than or equal to the obstacle avoidance distance threshold, then step 3 is executed to perform global path planning; otherwise, step 4 is executed to perform local obstacle avoidance control.

[0040] Furthermore, the improved APF method described in step 3 involves improving the repulsive and gravitational field functions in the APF method by introducing velocity and acceleration control factors into the gravitational and repulsive potential field functions, and adding the relative distance between the robot and the obstacle and the dynamic influence distance of the obstacle.

[0041] Furthermore, the improved APF method described in step 3 is as follows:

[0042] The gravitational field function is modified to a new gravitational field function. , means as follows:

[0043] ,

[0044] in, This is the gravitational gain coefficient. and These are the gravitational potential constants for velocity and acceleration, respectively. and As the control factor for velocity and acceleration in the gravitational potential field, For robot speed, Accelerate the robot Positioning of the robot relative to the position of the moving target The Euclidean distance between them;

[0045] The repulsive potential field function is modified to a new repulsive potential field function. , means as follows:

[0046] ,

[0047] in, The repulsive gain coefficient is... and These are the repulsive force constants for velocity and acceleration, respectively. and As the control factor for the velocity and acceleration of the repulsive potential field, and For the speed of obstacles and robots, and For the acceleration of obstacles and robots, The repulsive force of obstacles affects the distance. As the repulsive force influence factor, Positioning of the robot and obstacles The Euclidean distance between them.

[0048] Furthermore, the improved dynamic window method DWA in step 4 includes:

[0049] Step 4-1: Based on kinematic constraints, dynamic constraints, and environmental constraints, construct a velocity search space and combine it with the latest global path to sample and obtain the set of robot velocity values;

[0050] Step 4-2: Construct the evaluation function, that is, add the target evaluation sub-function of APF potential field gradient to the motion trajectory evaluation function of the dynamic window method DWA;

[0051] Step 4-3: Using the evaluation function constructed in Step 4-2, evaluate the speed in the set of values ​​obtained in Step 4-1 to obtain the optimal speed.

[0052] Furthermore, the construction of the evaluation function described in step 4-2 includes:

[0053] Using the heading angle evaluation function Trajectory distance evaluation function Speed ​​evaluation function Evaluation function for approaching the target in the potential field Construct the evaluation function, as follows:

[0054] ,

[0055] in, and These are the weighting coefficients. This represents the normalization function;

[0056] The heading angle evaluation function , means as follows:

[0057] ,

[0058] in, and These represent the robot's current heading angle and the angle at the corresponding speed. The predicted heading angle below, where For linear velocity, Angular velocity;

[0059] The trajectory distance evaluation function , means as follows:

[0060] ,

[0061] in, Let k be the robot's position coordinates at time k. Let the coordinates be the position coordinates of the i-th obstacle at time k; This represents the Euclidean distance between the robot and the nearest obstacle.

[0062] Velocity evaluation function , means as follows:

[0063] ,

[0064] in, This represents the robot's maximum linear velocity.

[0065] Potential field target approach evaluation function , means as follows:

[0066] ,

[0067] in, It is a positive real constant. This indicates taking the first derivative of the function.

[0068] Beneficial effects: 1. In the presence of dynamic obstacles in complex environments, the hierarchical dynamic obstacle avoidance navigation method proposed in this invention exhibits stable performance even with low-cost sensors on the robot, and can generate relatively smooth and short paths to the target point. Predicting the motion state of obstacles allows the mobile robot more time and space for maneuvering, effectively improving the performance of the overall path planning algorithm and meeting the dynamic obstacle avoidance requirements of mobile robots in complex indoor environments.

[0069] 2. The dynamic obstacle avoidance method described in this invention can effectively improve the path planning efficiency of autonomous mobile robots in complex dynamic indoor environments and their ability to respond to sudden dynamic obstacles. It combines the efficiency of global planning with the real-time performance of local obstacle avoidance, and has the advantages of low computational load, high planning efficiency, and automatic response. Attached Figure Description

[0070] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments, and the advantages of the present invention in the above and / or other aspects will become clearer.

[0071] Figure 1 This is a schematic diagram of the overall architecture of the present invention.

[0072] Figure 2 This is a schematic diagram of the adaptive interactive multi-model motion prediction algorithm framework in the embodiment.

[0073] Figure 3 This is a schematic diagram of the two-dimensional force distribution model of the improved APF method in the embodiment.

[0074] Figure 4 The image shown is of the actual Pioneer 3-AT wheeled robot platform used in this embodiment.

[0075] Figure 5 This is a schematic diagram of the motion model corresponding to the wheeled robot platform in the embodiment.

[0076] Figure 6 This is a schematic diagram of a multi-obstacle environment experimental scenario in the embodiment.

[0077] Figure 7 This is a schematic diagram of the resultant potential field elevation line and path planning trajectory in a multi-obstacle experimental scenario in the embodiment.

[0078] Figure 8 This is a schematic diagram of a narrow passage environment experiment scenario in the embodiment.

[0079] Figure 9 This is a schematic diagram of the resultant potential field elevation line and path planning trajectory in the narrow channel experimental scenario of the embodiment.

[0080] Figure 10 This is a schematic diagram of the predicted trajectory, observed trajectory, and actual trajectory of the maneuvering obstacle target in the embodiment.

[0081] Figure 11 This is a schematic diagram of the position observation error and prediction error curves in the embodiment.

[0082] Figure 12 This is a schematic diagram of the velocity observation error and prediction error curves in the embodiment. Detailed Implementation

[0083] This invention addresses the problem of autonomous obstacle avoidance in indoor environments with dense dynamic obstacles, proposing a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism. The specific implementation idea is as follows:

[0084] First, simulating the human cognitive logic of "experience prediction - global planning - local adjustment", a hierarchical obstacle avoidance strategy and algorithm model were designed in the dynamic obstacle avoidance process, which effectively improved the adaptability of the dynamic obstacle avoidance algorithm in complex dynamic scenarios.

[0085] Secondly, based on the motion characteristics of the mobile obstacle, three types of motion models were proposed, and the state prediction was achieved using the IMM-KF algorithm. This supported the constraint definition of the DWA velocity space and improved the robot's response to sudden obstacles.

[0086] Third, it defines the potential field gradient of APF in the DWA evaluation function, which narrows the search space of the DWA dynamic window and increases path smoothness through continuous direction correction.

[0087] Fourth, by optimizing the APF potential field function, the problems of dynamic environment incompatibility and optimal solution oscillation were solved, thus improving the efficiency of path planning.

[0088] This invention proposes a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism. This method simulates the human cognitive logic of "experience-based prediction - global planning - local adjustment," such as... Figure 1 As shown, it is divided into an obstacle motion prediction layer, a global path planning layer, and a local obstacle avoidance control layer, and specifically includes the following implementation steps:

[0089] Step 1: In the obstacle motion prediction layer, the IMM-KF algorithm is used to fuse the probability outputs of multiple motion models. Based on the observation and localization information of the obstacle, obstacle motion prediction is performed. Specifically, Nearly Constant Velocity (NCV), Nearly Constant Acceleration (NCA), and Random Motion (RM) models are used to describe the obstacle's motion state. First, each model is filtered in parallel using an extended Kalman filter. Based on the obstacle's localization information and the state update equation, motion state estimates are obtained. Then, the likelihood function and mixture probability are calculated. Finally, the estimation results of each model are fused to obtain the final obstacle state prediction. This prediction information is used for the subsequent DWA velocity space definition in the local obstacle avoidance control layer and the APF function optimization in the global path planning layer.

[0090] Step 2: Design a distance-driven decision-making mechanism. Centered on the robot's position, set an obstacle avoidance threshold distance based on scene characteristics and robot dynamic parameters. Divide the motion space into a global path planning area and a local obstacle avoidance area. By calculating the relative distance between the robot and the nearest obstacle in real time, determine the current area and activate the corresponding hierarchical control strategy.

[0091] Step 3: If the distance between the robot and all obstacles is greater than or equal to the threshold, then proceed to the global path planning layer. An initial global path is generated based on the improved APF method, and the obstacle motion prediction information obtained in Step 1 is integrated. The robot moves towards the target along the gradient descent direction of the resultant potential field.

[0092] Step 4: If an obstacle enters the threshold distance, the system enters the dynamic obstacle avoidance control layer. A velocity window is defined based on the robot's motion characteristics, dynamic constraints, and obstacle avoidance requirements. Candidate velocity pairs are generated within the feasible window. The trajectory evaluation function is optimized by combining the potential field gradient change to determine the optimal velocity control command. This command must simultaneously satisfy the requirements of approaching the target, avoiding dynamic / static obstacles, and complying with kinematic constraints (dynamic window limitations) to adapt to dynamic scenarios and improve path smoothness.

[0093] Step 5: The system updates the pose information in real time through the sensors and calculates the control commands. The system then enters the next decision cycle and can dynamically adjust parameters such as the gain coefficient, minimum safe distance, and obstacle avoidance threshold according to the actual operating conditions and environmental changes.

[0094] Through collaborative and iterative operation, this method can achieve efficient and safe robot navigation in complex dynamic obstacle environments, while taking into account both the efficiency of global path planning and the real-time performance of local obstacle avoidance.

[0095] Step 1, to achieve obstacle motion state prediction based on the IMM-KF algorithm in the motion prediction layer, includes the following steps:

[0096] Step 1-1: Define three types of motion state models based on the motion characteristics of the moving obstacles. This provides model definitions for the parallel filtering of the interactive multi-model algorithm. Each model outputs its own state prediction and covariance prediction, which will serve as inputs for subsequent steps. The specific definitions of the three models include:

[0097] (1) NCV Model. This model assumes that dynamic obstacles move at a constant speed, or that the speed change is much smaller than the actual speed. It is suitable for scenarios with small speed changes or simple dynamic environments, such as motion prediction for pedestrians or indoor mobile robots. The model assumes a state vector... At time k, it is defined as:

[0098] ,

[0099] in, These are the robot's position coordinates in a two-dimensional plane; and These are the robot's velocities in the x and y directions; This is the robot's heading angle. The discrete-time state equation of the system can be defined as:

[0100] ,

[0101] in, for The state vector of the dynamic obstacle in the NCV model at any given time. Here is the state transition matrix of the NCV model. The discrete process uses Gaussian white noise with zero mean and a covariance matrix of... .

[0102] Assuming the velocity change of the NCV model is negligible relative to the actual velocity, and the heading angle remains constant in the short term, then the state transition matrix of the NCV model is:

[0103] ,

[0104] Where T is the observation period duration.

[0105] (2) NCA Model. This model assumes that the dynamic obstacle moves with a constant acceleration, or that the change in acceleration is much smaller than the actual acceleration. It is suitable for scenarios with large velocity changes or for dynamic obstacle prediction that requires higher accuracy. In the NCA model, the state vector... At time k, it is defined as:

[0106] ,

[0107] in, and Let x be the robot's acceleration in the x and y directions; assuming the change in acceleration in the NCA model is negligible relative to the actual acceleration, and the heading angle remains constant in the short term, then the state transition matrix of this model is... for:

[0108] ,

[0109] The discrete-time state equation is expressed as follows:

[0110] ,

[0111] (3) The RM model is used to describe the motion of dynamic obstacles with randomly changing dynamic direction and velocity. It considers two-dimensional planar motion, and the state vector includes pose, velocity, acceleration, and rate of change of acceleration. In complex dynamic scenes, if the velocity and acceleration of obstacles change randomly, it is difficult to describe the motion characteristics using NCVM or NCAM models. This invention designs the RM model to describe more complex and unpredictable motion states. The real-time probability density of the model's acceleration is based on a modified Rayleigh distribution. Defined as:

[0112] ,

[0113] in, For scale parameters, For the random acceleration of the target, Let be the upper positive or lower negative limit of the known target acceleration, and exp be the exponent with base e. At that time, the probability density function is zero. The variance of the maneuvering acceleration is:

[0114] ,

[0115] in, The mean of random acceleration, Let V be the variance of the random acceleration.

[0116] For the mean value of the maneuvering acceleration, the discrete state variables and the corresponding state equations in the two-dimensional case are defined as follows:

[0117] ,

[0118] ,

[0119] in, and Let be the rate of change of the robot's acceleration in the x and y directions; for Update state variables at all times. Here is the state transition matrix. For the control matrix, The mean value of the acceleration is approximately constant within each sampling period; , and These are the average values ​​of the acceleration in the two directions, respectively. It has a mean of zero and a variance of Discrete-time white noise, based on the above definition, the state transition matrix under the real-time statistical model. Defined as:

[0120] ,

[0121] in, This is the decay term of the acceleration. This assumption is limited to the random maneuver acceleration conforming to a first-order time-dependent process in the time domain. The control matrix corresponding to the mean acceleration input is defined as:

[0122] ,

[0123] Step 1-2: Based on the three types of motion models constructed in Step 1-1, the adaptive interactive multi-model algorithm is used to complete the iterative fusion of information in a recursive manner, referring to... Figure 2 The algorithm architecture shown, and the basic principles of Interactive Multi-Model Kalman Filtering (IMM-KF), are described below, with the implementation steps for each iteration cycle as follows:

[0124] Step 1-2-1: Construct a multi-model predictor. At time k-1, the algorithm receives the state estimates of each model from the previous time step. Covariance matrix and model probability ,in The mixed probability is calculated as follows:

[0125] ,

[0126] in, Let be the elements of the model transition probability matrix, representing the probability of switching from model i to model j. The mixture state and covariance of model j are calculated as follows:

[0127] ,

[0128] Based on the measurement matrix, mixed states, and covariance, each model performs independent state predictions and updates the predicted states and filtered covariance:

[0129] For the NCV model:

[0130] ,

[0131] For the NCA model:

[0132] ,

[0133] For the RM model:

[0134] ,

[0135] in, These are the system noise covariance matrices for the three types of motion models;

[0136] The prediction results of each model form a multi-model prediction set:

[0137] ,

[0138] The Kalman gain can be calculated as follows:

[0139] ,

[0140] Where H is the observation matrix and R is the observation noise covariance matrix; then the predicted state update is:

[0141] ,

[0142] in, For Kalman gain, For the measurement matrix, For the observation matrix, Estimate the target from the previous moment;

[0143] For step 1-2-2, calculate the likelihood function and mixture probability. When the observations at each time step are obtained, calculate the degree of matching between the model predictions and the observed values. The innovation observations are calculated as follows:

[0144] ,

[0145] in, Let be the observation matrix of model j. The information covariance matrix is ​​calculated as follows:

[0146] ,

[0147] in, Let be the observation noise covariance matrix of model j. Then the likelihood function (model fit) can be calculated as:

[0148] ,

[0149] Where m is the dimension of the observation vector. Next, the probabilities of each model are updated based on the likelihood function:

[0150] .

[0151] For steps 1-2-3, weighted fusion and optimal prediction are performed. The prediction results of each model are weighted and fused according to the updated model probabilities to obtain the final motion state prediction of the obstacle. The corresponding predicted state and covariance matrix are expressed as follows:

[0152] .

[0153] By implementing the above interactive multi-model filtering algorithm, the pose state of both motorized and non-motorized obstacle targets during path planning can be effectively predicted, providing auxiliary prior information for obstacle avoidance decision-making.

[0154] In step 3, the improved APF method is used to pre-plan the motion target path of the global planning layer. The specific implementation process is described as follows:

[0155] For robot path planning applications in static environments with multiple obstacles, the principle of the traditional APF method is described as follows:

[0156] APF (Adaptive Path Programming) is a reactive local path planning algorithm that models the robot's environment as a potential potential field. Gravity pulls the robot towards the target point, while repulsion pushes it away from obstacles. The robot plans an unobstructed path to the target along the descent direction of the resultant potential field gradient. To highlight the analytical elements, the mobile robot is modeled as a point mass moving in a two-dimensional coordinate space. For the point mass's position... q The gravitational potential field function at that location can be expressed as:

[0157] ,

[0158] in, This is the gravitational gain coefficient, used to control the strength of gravity. Let the position vector of the mobile robot be... This is the target position vector. Current position of the robot To the target location The Euclidean distance. Gravity manifests as the negative gradient function of the gravitational potential field:

[0159] ,

[0160] Correspondingly, the repulsive potential field function can be expressed as:

[0161] ,

[0162] in, The repulsive gain coefficient is... The repulsive force representing an obstacle affects the distance. This repulsive force is represented as the negative gradient function of the repulsive potential field function.

[0163] ,

[0164] The robot was subjected to the first i The repulsive force of the obstacle is Beyond this range, the obstacle exerts no repulsive force on the robot. Therefore, the net force acting on the robot is the sum of the attractive and repulsive forces:

[0165] ,

[0166] The robot determines its real-time direction of movement under the combined force of the target and multiple obstacles.

[0167] To address the limitations of traditional APF methods in real-world applications with multiple obstacles and dynamic environments, an improved approach is proposed, described in detail below:

[0168] Traditional APF (Automatic Path Avoidance) methods exhibit excellent performance in environments with uniformly distributed obstacles and sufficient gaps for robot obstacle avoidance. However, they may be affected by local optima, where the net force is zero at non-target points, leading to robot deadlock or oscillating solutions that prevent the robot from reaching the target point. In addition, APF methods are adapted to static scenarios with stationary obstacles, ignoring the interrelationships of maneuverability such as velocity, acceleration, and relative direction between moving objects. This increases the probability of collisions due to insufficient avoidance or unnecessary obstacle avoidance behaviors, reducing the overall efficiency of path planning.

[0169] To address the aforementioned practical problems, this invention improves the APF in two aspects:

[0170] The first optimization addresses the issues of local minimum traps and optimal solution oscillations in multi-obstacle environments. It considers the relative distance between the robot and the target, as well as the dynamic influence distance of obstacles, by adding a coefficient term to the repulsive field function and modifying it as follows:

[0171] ,

[0172] in, This is the repulsive force influence factor, with a value that is a positive constant, determined by the experimental scenario and sensor type. (Introduction) This ensures that the total potential field reaches its global minimum after the robot reaches the target point. The path planned by APF can descend along the direction of the steepest slope and bypass obstacle areas. The gravity gain coefficient needs to be flexibly selected based on the characteristics of the actual application scenario. Repulsion gain coefficient Repulsive force affects distance .

[0173] When the robot has not reached the target point, the repulsive force can be expressed as:

[0174] ,

[0175] in:

[0176] ,

[0177] in, Let represent the gradient functions of the unit vectors pointing from the obstacle to the robot and from the robot to the moving target, respectively. Correspondingly, the resultant force of the repulsive force and the relationship between the two components are as follows: Figure 3 As shown, it can be seen that when the component Another component is when the robot is excluded from the obstacle. Pull the robot toward the target, and the total resultant force will cause the robot to bypass the obstacle and move toward the target.

[0178] The second optimization aspect addresses the impact of instantaneous changes in the velocity and acceleration of dynamic obstacles. Combining the motion state prediction results from the interactive multi-model approach, velocity and acceleration control factors are introduced into the gravitational and repulsive potential field functions to optimize the potential field functions. Assuming the target is at a fixed position in the absolute coordinate system, its gravitational field function is corrected as follows:

[0179] ,

[0180] in, and These are the gravitational potential constants for velocity and acceleration, respectively. and These are the gravitational potential field velocity and acceleration control factors, whose values ​​are based on the robot's velocity. acceleration Adjustments are made based on the order of magnitude difference in position. Accordingly, the repulsive potential field function is modified as follows:

[0181] ,

[0182] in, and These are the repulsive force constants for velocity and acceleration, respectively. and These are the repulsive potential field velocity and acceleration control factors, whose values ​​are based on the obstacle velocity, respectively. acceleration Adjustments are made based on the order of magnitude difference in position and size.

[0183] In step 4, the improved DWA-APF method is used to realize the autonomous obstacle avoidance behavior of the local obstacle avoidance control layer. Within a limited decision space, the system adjusts its own trajectory in real time to achieve obstacle avoidance functions such as speed change, turning, and detour. The specific implementation process is described as follows:

[0184] Step 4-1: Define the effective velocity search space based on kinematic constraints, dynamic constraints, and environmental constraints.

[0185] Based on the determined motion model of the mobile robot, the velocity space is discretized, candidate motion trajectories are sampled, and the optimal trajectory is determined using an evaluation function. According to the scene characteristics, the DWA (Dynamic Window Approach) method adopted in this invention mainly considers the velocity constraints from the following three aspects:

[0186] (1) Kinematic constraints: Kinematic constraints ensure that the robot's speed is within its motion capability range and is limited by the range of its maximum speed during motion. This can be expressed as:

[0187] ,

[0188] in, This represents the velocity space that the robot can reach. and These represent the robot's maximum linear velocity, minimum linear velocity, maximum angular velocity, and minimum angular velocity, respectively.

[0189] (2) Dynamic constraints: Dynamic constraints mainly consider the acceleration limitations experienced by the robot. For example, the acceleration limitations caused by the dynamic performance of the motor hardware and drive device of a wheeled robot affect the speed window that the robot can reach in the next observation period, which can be expressed as:

[0190] and These represent the maximum linear acceleration, minimum linear acceleration, maximum angular acceleration, and minimum angular acceleration that the robot can reach, respectively.

[0191] (3) Obstacle avoidance environment constraints: Obstacle avoidance environment constraints mainly consider the speed limit in a safe obstacle avoidance environment to ensure that the robot will not collide with obstacles and that the distance between the robot and the nearest obstacle is greater than the safe distance. This can be expressed as:

[0192] ,

[0193] This means that the distance between the robot and the nearest obstacle is greater than the safe distance. The safe distance is determined based on parameters such as the robot's size and maximum acceleration. Based on these three types of constraints, the velocity sampling space of the mobile robot can be defined as:

[0194] ,

[0195] The velocity state space determined by the above equation is sampled, and several candidate velocities with equal sampling intervals are uniformly selected within the sampling window to select the required discrete finite velocity. The set of values ​​is used to calculate the candidate trajectory for the next time interval, thereby determining the corresponding candidate trajectory within the current period.

[0196] Step 4-2: Construct an evaluation function to evaluate the predicted trajectory of the velocity sampling, determine the optimal solution, and then generate a velocity command to drive the robot to complete obstacle avoidance.

[0197] In order to select the optimal velocity trajectory within the velocity space defined by the above constraints, a target evaluation function is introduced to evaluate the trajectory. Considering the application scope of this invention, the evaluation criteria take into account the following four factors:

[0198] (1) Smooth trajectory: enables the robot to choose a smooth motion trajectory, avoid drastic changes, and improve the success rate of obstacle avoidance.

[0199] (2) Safety distance: Ensure that the distance between the robot and the nearest obstacle is greater than the safety distance;

[0200] (3) Arrive quickly: Choose the highest possible speed to improve route planning efficiency;

[0201] (4) Target orientation: Consider the changes in the gradient of the artificial potential field to guide the robot to move in the direction of the task target.

[0202] Based on the above principles, sub-objective functions are constructed separately, and a dynamic window evaluation function is formed by weighted combination, which can be expressed as:

[0203] ,

[0204] in, These represent the weight coefficients of different evaluation elements. By adjusting the priority of these elements, the different behavioral goals of the robot can be balanced. This is used to normalize the scores of all sub-objective functions. The definitions of each sub-objective function are described below:

[0205] This is the heading angle evaluation function, used to evaluate and filter the difference between the end-of-trajectory motion direction and the current robot motion direction. It is defined as:

[0206] ,

[0207] in, and These represent the robot's current heading angle and the angle at the corresponding speed. The predicted heading angle is calculated. The smaller the angle change, the higher the score, enabling the robot to move as straight as possible or complete the obstacle avoidance process with smaller angle changes.

[0208] This is the trajectory distance evaluation function, used to represent the distance between the motion trajectory and the nearest obstacle, defined as:

[0209] ,

[0210] in, Let k be the robot's position coordinates at time k. Let the coordinates be the position coordinates of the i-th obstacle at time k; This represents the Euclidean distance between the robot and the nearest obstacle; the farther the distance to the nearest obstacle, the higher the score, thus ensuring the safety of the obstacle avoidance process.

[0211] This is a velocity evaluation function used to assess the magnitude of relative velocity during robot motion, defined as:

[0212] ,

[0213] Where, v and These represent the linear velocity and maximum linear velocity of the robot's influence velocity pair, respectively. A higher normalized velocity results in a higher score, encouraging the robot to reach the target point quickly at a higher speed.

[0214] This is the potential field target approach evaluation function. Based on the analysis summarized above, in APF-based path planning, the robot's orientation control strategy is based on the gradient descent of potential energy. Mathematically, the potential energy gradient can be modeled as its influence on the robot's velocity, defined as:

[0215] ,

[0216] in, It is a positive real constant. This indicates taking the first derivative of the function.

[0217] Based on the above definitions, an improved APF method and a DWA method were combined, optimizing the objective function evaluation index on the basis of the traditional DWA control method. The faster the potential field gradient descent, the higher the score, thus enabling more precise control of the robot to move efficiently toward the target while completing obstacle avoidance. According to the optimized dynamic window evaluation function defined above, the velocity window combinations are scored, and the velocity combination with the highest score is selected as the control input for the next moment. The above process is repeated until the robot completes the task and reaches the target point.

[0218] Example 1: The hierarchical dynamic obstacle avoidance method for indoor mobile robots based on human-like decision-making mechanism provided in this embodiment of the invention has broad application prospects in robot tasks such as industrial manufacturing and inspection, warehousing and logistics, and commercial and public services in indoor environments without GNSS signals.

[0219] The embodiment selects the commonly used Pioneer 3-AT wheeled robot platform for method implementation. The robot platform is as follows: Figure 4 As shown, it is equipped with conventional SICK laser scanners, binocular vision cameras, IMUs, and other sensors to perceive environmental information, location information, and obstacle movement status information. An AmigoBot robot is used to simulate dynamic obstacles in the environment.

[0220] The corresponding motion model of the wheeled robot platform is as follows: Figure 5 As shown. Based on the kinematic characteristics, it cannot move laterally during movement, and its movements are mainly forward, backward, and rotational. Therefore, the centroid coordinates of the mobile robot are used. and Representing the robot's position and heading angle at time t, using and This represents the linear velocity and angular velocity of the robot at time t, which can be used to measure the robot's velocity from its initial position. arrive The state change at time t is represented as:

[0221] ,

[0222] If the time interval between adjacent sampling times If the value is sufficiently small, assuming the robot's velocity change is negligible in a very short time, the above equation can be discretized, i.e.:

[0223] ,

[0224] The above equations all assume that the robot's trajectory occurs within the sampling time interval Δ t The inner part is a straight line. In reality, when the linear velocity of the mobile robot is not zero and the angular velocity is zero, the trajectory is a straight line. When both the linear velocity and angular velocity are not zero, the trajectory of the mobile robot is a curve, more accurately represented by an arc. When the robot's angular velocity is not zero, the radius of a point on the robot's sampled trajectory is... Position at time k and heading angle They can be represented as:

[0225] ,

[0226] Based on the determination of the robot model and the completion of motion modeling, specific examples are listed below:

[0227] Example 2: Path planning in a static scene with multiple obstacles, described in detail below:

[0228] (1) Static obstacle avoidance experimental environment construction and robot platform modeling: The commonly used Pioneer 3-AT wheeled robot platform was selected and equipped with conventional SICK laser scanner, binocular vision camera, IMU and other sensors to perceive environmental information, position information and obstacle motion status information; based on the robot's motion characteristics and dynamic characteristics, a kinematic model was established, and the sampling methods of angular velocity and linear velocity and the motion trajectory calculation method were determined; the experimental data were combined with ROS, MATLAB Robotics Toolbox and MobileSim to build an experimental platform for verification and processing of experimental data.

[0229] (2) Static environment obstacle avoidance parameter debugging and configuration: Construct a two-dimensional plane coordinate system in an indoor environment, and on this basis determine the starting point and ending point of the autonomous mobile robot, the size of the motion area map, and constraints such as initial angle, maximum angular velocity, and maximum initial velocity. Configure the static environment obstacle avoidance experimental parameters of the APF method, including the gravity gain coefficient, repulsion gain coefficient, obstacle repulsion influence distance, position sampling frequency, and dynamic gravitational field constant modification factor.

[0230] (3) Path planning and obstacle avoidance in static scenes with multiple obstacles: The different size characteristics of multiple obstacles are considered, and multiple circular or polygonal obstacles are set in the scene (refer to Figure 6 The improved APF method is applied to calculate the repulsive force generated by static obstacles and the attractive force generated by the target point. If there are no obstacles on the path, the repulsive force is set to 0. If obstacles exist, the obstacle avoidance process is completed along the direction of gradient descent of the resultant potential field. The contour lines of the resultant potential field and the planned path trajectory are shown below. Figure 7 As shown.

[0231] (4) Path planning and obstacle avoidance in narrow static scenes: The environmental characteristics of narrow passages and the situation where the target point is blocked by obstacles are considered when setting obstacles (refer to Figure 8 In narrow passage environments, it is necessary to increase the repulsive force gain coefficient and reduce the distance at which the repulsive force of obstacles affects the robot, preventing it from getting too close to the obstacles. An improved APF method is applied to calculate the repulsive force generated by static obstacles and the attractive force generated by the target point, enabling the robot to complete the obstacle avoidance process along the direction of the gradient descent of the resultant potential field. The contour lines of the resultant potential field and the planned path trajectory are shown below. Figure 9 As shown.

[0232] (5) Application and effect analysis: When the obstacle density is high, by introducing the obstacle repulsion force to affect the distance parameter, the robot can complete the obstacle avoidance process along the direction of the gradient descent of the potential field in a complex environment with multiple obstacles of different sizes, and reach the destination with a smooth trajectory. The planned path trajectory does not show obvious oscillation. Under the action of the repulsion force component defined in the improved algorithm, the robot is pulled towards the target point and successfully avoids falling into a local minimum. Under the action of the resultant potential field, the robot can reach the target through narrow channel gaps and effectively avoid collisions with obstacles.

[0233] Example 3: Perform path planning for dynamic obstacle scenarios with multiple obstacles, as detailed below:

[0234] (1) Construction of dynamic obstacle avoidance experimental environment and modeling of robot platform: The commonly used Pioneer 3-AT wheeled robot platform was selected and equipped with conventional SICK laser scanner, binocular vision camera, IMU and other sensors to perceive environmental information, position information and obstacle motion status information; AmigoBot robot was used to simulate dynamic obstacles in the environment. According to the robot's motion characteristics and dynamic characteristics, a kinematic model was established, and the sampling methods of angular velocity and linear velocity and the motion trajectory calculation method were determined; the experimental data were combined with ROS, MATLAB Robotics Toolbox and MobileSim to build an experimental platform for verification and processing of experimental data.

[0235] (2) Debugging and configuration of obstacle avoidance parameters in dynamic environment: Construct a two-dimensional plane coordinate system in an indoor environment, and on this basis, determine the starting point and ending point of the autonomous mobile robot, the size of the motion area map, and constraints such as initial angle, maximum angular velocity, and maximum initial velocity. Configure multi-motion model parameters: including motion state parameters, sampling frequency, initial probability, transition probability matrix, and dynamic environment obstacle avoidance experimental parameters of APF and DWA methods: including gravity gain coefficient, repulsion gain coefficient, obstacle repulsion influence distance, velocity sampling frequency, trajectory prediction time, and weights of each evaluation function.

[0236] (3) Performance verification of the IMM-KF algorithm: An open indoor area was selected, and the random motion of dynamic obstacles was simulated according to the different states of the AmigoBot robot in each stage set in the previous steps. The mobile robot Pioneer 3-AT observed and predicted the motion. The IMM-KF algorithm based on three types of motion models was used to predict the motion state of the maneuvering target. The observation data at the corresponding time intervals were selected for analysis, and the actual trajectory, observation trajectory and predicted trajectory curve of the maneuvering target were calculated. The actual reference trajectory was obtained by the high-precision integrated navigation device H Guide N580 and post-processed by Novatel commercial software IE8.9. The observation trajectory was obtained by the robot using a lidar / binocular vision sensor and using the EKF fusion filtering described in the literature [A Low-Cost and Robust Multi-Sensor Data Fusion Scheme for Heterogeneous Multi-Robot Cooperative Positioning in Indoor Environments]. The predicted trajectory was obtained by fusing the system state and observation information according to the IMM-KF multi-model filtering algorithm proposed in this invention. The predicted trajectory, observation trajectory and actual trajectory of the maneuvering obstacle target are as follows: Figure 10 As shown, the corresponding observation errors and prediction errors for position and velocity are as follows: Figure 11 and Figure 12 As shown.

[0237] (4) Dynamic scene path planning and obstacle avoidance: Considering the characteristics of obstacles in complex obstacle avoidance environments, multiple static obstacles are set in the environment, and two AmigoBot robots are controlled to simulate dynamic obstacles that appear at different stages. The motion state of the obstacles at different stages is set. When an obstacle enters the obstacle avoidance range, the improved DWA-APF is applied to realize dynamic obstacle avoidance. Otherwise, the improved APF method is used for global path planning.

[0238] (5) Practical Implementation and Effect Analysis: During dynamic obstacle avoidance, the contour lines of the resultant potential field change dynamically with the changes in dynamic obstacles (refer to...). Figure 9 The robot exhibits the characteristics of layered obstacle avoidance, similar to human decision-making machines. By controlling changes in angular and linear velocity, the robot achieves speed variation and detour behavior during dynamic obstacle avoidance, successfully reaching the target point along a relatively smooth path. Compared to static obstacle avoidance in similar environments, the robot's path is adjusted in real time during dynamic obstacle avoidance, and the planned path does not exhibit significant oscillations.

[0239] In summary, when faced with complex dynamic obstacles in the environment, the hierarchical dynamic obstacle avoidance navigation method proposed in this invention exhibits stable performance even with low-cost optical sensors on the robot, and can generate relatively smooth paths to the target point that are not too far away. Predicting the motion state of obstacles allows the mobile robot more time and space for maneuvering, effectively improving the performance of the overall path planning algorithm and meeting the dynamic obstacle avoidance requirements of mobile robots in complex indoor environments.

[0240] In its specific implementation, this invention provides a computer storage medium and a corresponding data processing unit. The computer storage medium can store a computer program, which, when executed by the data processing unit, can run the invention's content regarding a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism, as well as some or all of the steps in various embodiments. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0241] Those skilled in the art will clearly understand that the technical solutions in the embodiments of the present invention can be implemented using computer programs and their corresponding general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of computer programs, i.e., software products. These computer program software products can be stored in a storage medium and include several instructions to cause a device containing a data processing unit (which may be a personal computer, server, microcontroller, MCU, or network device, etc.) to execute the methods described in various embodiments or certain parts of the embodiments of the present invention.

[0242] This invention provides a hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism. Many methods and approaches exist for implementing this technical solution; the above description is merely a preferred embodiment of the invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications should also be considered within the scope of protection of this invention. All components not explicitly stated in this embodiment can be implemented using existing technologies.

Claims

1. A hierarchical dynamic obstacle avoidance method for indoor mobile robots based on human-like decision-making mechanisms, characterized in that, Includes the following steps: Step 1: The indoor mobile robot acquires the observation and positioning information of obstacles through sensors, builds a multi-model predictor to make predictions, and obtains the predicted information of the obstacle's motion state. Step 2: Make distance-driven hierarchical decisions based on obstacle motion state prediction information, and execute Step 3 global path planning or Step 4 local obstacle avoidance control. Step 3: Global path planning. Generate a global path based on the improved APF method, integrate obstacle motion state prediction information, control the robot's movement, and repeat step 1. Step 4, Local obstacle avoidance control: Based on the latest global path, the improved Dynamic Window Method (DWA) is used to determine the optimal speed, control the robot's movement, and re-execute Step 1; Step 2, which involves performing distance-driven hierarchical decision-making based on obstacle motion state prediction information, includes: Centered on the robot's current position, and based on the set obstacle avoidance distance threshold, the robot's motion space is divided into a path planning area and a local obstacle avoidance area. The relative distance between the robot and the nearest obstacle is calculated. If step 3 has never been executed or the relative distance is greater than or equal to the obstacle avoidance distance threshold, step 3 is executed to perform global path planning; otherwise, step 4 is executed to perform local obstacle avoidance control. The improved APF method described in step 3 involves modifying the repulsive and gravitational field functions in the APF method by introducing velocity and acceleration control factors into the gravitational and repulsive potential field functions, and incorporating the relative distance between the robot and the obstacle and the dynamic influence distance of the obstacle. The specific details of the improved APF method are as follows: The gravitational field function is modified to a new gravitational field function. , means as follows: ， in, This is the gravitational gain coefficient. and These are the gravitational potential constants for velocity and acceleration, respectively. and As the control factor for velocity and acceleration in the gravitational potential field, For robot speed, Accelerate the robot For robot positioning relative to the position of the moving target The Euclidean distance between them; The repulsive potential field function is modified to a new repulsive potential field function. , means as follows: ， in, The repulsive force gain coefficient, and These are the repulsive force constants for velocity and acceleration, respectively. and The repulsive potential field velocity and acceleration control factors, and For the speed of obstacles and robots, and For the acceleration of obstacles and robots, The repulsive force of obstacles affects the distance. As the repulsive force influence factor, For robot positioning and obstacles The Euclidean distance between them.

2. The hierarchical dynamic obstacle avoidance method for indoor mobile robots based on human-like decision-making mechanisms according to claim 1, characterized in that, The construction of a multi-model predictor for prediction described in step 1 includes: Step 1-1: Construct three types of motion models, using the approximate constant velocity NCV model, the approximate constant acceleration NCA model, and the random walk RM model to describe the various motion states of the obstacle; Step 1-2: Based on the three types of motion models constructed in Step 1-1, interactive multi-model Kalman filtering is used. By running multiple filters in parallel and dynamically adjusting the weights according to the matching degree between each model and the current observation data, the obstacle motion state prediction information, i.e., state prediction, is obtained. Covariance Matrix .

3. The hierarchical dynamic obstacle avoidance method for indoor mobile robots based on human-like decision-making mechanisms according to claim 2, characterized in that, The state vector of the approximately constant velocity NCV model described in step 1-1 and state transition matrix They are defined as follows: ， ， in, These are the robot's position coordinates in a two-dimensional plane; and These are the robot's velocities in the x and y directions; It is the robot's heading angle, and T is the observation period duration; The discrete-time state equation is expressed as follows: ， in, It is noise.

4. The hierarchical dynamic obstacle avoidance method for indoor mobile robots based on human-like decision-making mechanisms according to claim 3, characterized in that, The state vector of the approximate constant-acceleration NCA model described in step 1-1 and state transition matrix They are defined as follows: ， ， in, and It refers to the robot's acceleration in the x and y directions; The discrete-time state equation is expressed as follows: ， in, It is noise.

5. A hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism according to claim 4, characterized in that, The discrete state vector of the random walk RM model described in step 1-1 Defined as: ， in, and Let be the rate of change of the robot's acceleration in the x and y directions; The discrete-time state equation is expressed as follows: ， in, The mean value of the acceleration. and These are the average values ​​of the acceleration in the two directions, respectively. Here is the state transition matrix. The control matrices are represented as follows: ， ， in, This is the decay term of acceleration.

6. The hierarchical dynamic obstacle avoidance method for indoor mobile robots based on human-like decision-making mechanisms according to claim 1, characterized in that, The improved dynamic window method DWA in step 4 includes: Step 4-1: Based on kinematic constraints, dynamic constraints, and environmental constraints, construct a velocity search space and combine it with the latest global path to sample and obtain the set of robot velocity values; Step 4-2: Construct the evaluation function, that is, add the target evaluation sub-function of APF potential field gradient to the motion trajectory evaluation function of the dynamic window method DWA; Step 4-3: Using the evaluation function constructed in Step 4-2, evaluate the speed in the set of values ​​obtained in Step 4-1 to obtain the optimal speed.

7. A hierarchical dynamic obstacle avoidance method for indoor mobile robots based on a human-like decision-making mechanism as described in claim 6, characterized in that, The construction of the evaluation function described in step 4-2 includes: Using the heading angle evaluation function Trajectory distance evaluation function Speed ​​evaluation function Evaluation function for approaching the target in the potential field Construct the evaluation function, as follows: ， in, and These are the weighting coefficients. This represents the normalization function; The heading angle evaluation function , means as follows: ， in, and These represent the robot's current heading angle and the angle at the corresponding speed. The predicted heading angle below, where Linear velocity, Angular velocity; The trajectory distance evaluation function , means as follows: ， in, Let k be the robot's position coordinates at time k. Let be the position coordinates of the i-th obstacle at time k; This represents the Euclidean distance between the robot and the nearest obstacle. Velocity evaluation function , means as follows: ， in, This represents the robot's maximum linear velocity. Potential field target approach evaluation function , means as follows: ， in, It is a positive real constant. This indicates taking the first derivative of the function.

Images