Autonomous mobile robot path planning obstacle avoidance method and system

Abstract

The invention discloses an autonomous mobile robot path planning obstacle avoidance method and system, relates to the technical field of autonomous mobile robot path planning and obstacle avoidance, and is used for solving the problems that in a deep mine multi-layer roadway, multiple environmental risks are difficult to quantify, and once a key bottleneck channel is blocked, task failure is easily caused, and the reliability of path planning is insufficient. After an autonomous mobile machine is powered on, sensor calibration parameters are loaded, pose fusion is completed through extended Kalman filtering based on a multi-source odometer, an inertia sensor and a laser vision sensor, and an occupation grid map with static obstacles, dynamic candidates and passable marks is constructed. The method comprises the following steps: constructing a simultaneous multi-cause environmental risk feature vector to obtain an environmental comprehensive risk cost, performing layered screening and comprehensive cost sorting on search nodes in combination with task blocking sensitivity of off-line calculation on a mine laneway traffic map, and then generating a time parameterized speed reference trajectory; the Mecanum wheel omni-directional drive mechanism is guided to pass through the high-risk area along the safe and smooth path.

Description

Technical Field

[0001] This invention relates to the field of path planning and obstacle avoidance technology for autonomous mobile robots, and more specifically, to a method and system for path planning and obstacle avoidance for autonomous mobile robots. Background Technology

[0002] Currently, autonomous mobile robots are used for path planning and automatic obstacle avoidance in scenarios such as warehousing and factory logistics. However, their application in deep mine multi-level roadway systems still faces challenges. On the one hand, this environment consists of spiral ramps, horizontal roadways, and intersections. The passage width is significantly affected by roadway supports and debris, the ground is chronically slippery, and heavy vehicles and workers frequently pass through, causing local passage conditions to change drastically over time. On the other hand, equipment on the roadway roof obstructs lidar and visual sensors, resulting in large, long-term unknown areas in the grid map. Cost functions constructed solely based on grid occupancy probabilities or local obstacle distances are insufficient to reflect the combined risks of geometric bottlenecks, deteriorating adhesion conditions, observation uncertainties, and dynamic traffic loads. Existing path planning methods are mostly based on heuristic search, and their cost design primarily considers travel distance and obstacle distance. For multi-factor environmental risks, they typically use simple weighting or fixed safety margins, making it difficult to distinguish between channels with similar occupancy probabilities but different levels of danger. In emergency material delivery missions, once a critical spiral intersection is blocked by vehicles or debris, the only recourse is often passive detour at the local replanning level, lacking the ability to quantitatively characterize the importance of channels and proactively avoid them at the traffic layer level. Furthermore, existing local obstacle avoidance algorithms mostly employ velocity sampling or artificial potential field methods, using obstacle distance and reference path deviation as the main indicators. They neither utilize safe passage samples to build environmental statistical models nor adequately consider the multi-degree-of-freedom constraints of omnidirectional motion mechanisms such as Mecanum wheels, easily leading to sudden stops and mission interruptions in bottleneck areas.

[0003] To address the above problems, this invention proposes a solution. Summary of the Invention

[0004] In order to overcome the above-mentioned defects of the prior art, embodiments of the present invention provide an autonomous mobile robot path planning and obstacle avoidance method and system to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: In a preferred embodiment, it includes: After the autonomous mobile robot is powered on, it performs multi-sensor fusion localization and constructs an occupied grid map with static obstacles, dynamic candidates and passable markers; An environmental risk feature vector is constructed based on the occupied grid map. The comprehensive environmental risk cost is calculated according to the safety sample statistical model. The task blocking sensitivity feature is calculated offline based on the mine roadway access map. During the search, sensitive channels are first screened out according to the task blocking sensitivity threshold, and then nodes are expanded according to the comprehensive environmental risk cost. Based on the cost, a path search is performed, and a velocity reference trajectory is generated within a local window through velocity sampling and trajectory evaluation to control the robot to move along a smooth path.

[0006] In a preferred embodiment, the autonomous mobile robot loads multi-sensor calibration parameters after power-on and synchronously collects data from the odometer, inertial measurement unit, lidar, and depth camera under a unified time reference. The autonomous mobile robot uses extended Kalman filtering combined with scan matching to continuously estimate its own pose and updates the observation results into an occupied grid map with static obstacles, dynamic candidates and passable markers.

[0007] In a preferred embodiment, after completing environmental perception and occupancy grid map construction, the autonomous mobile robot uses the aforementioned global occupancy grid map as a basis to crop a local window centered on the grid to be expanded during the global path planning process. It reads the occupancy probability, static obstacle markers, dynamic candidate markers, observation count, and pre-configured roadway geometric parameters and terrain parameters of each grid within the window. It then sequentially calculates static obstacle approach characteristics, dynamic disturbance characteristics, passage width bottleneck characteristics, ground passage risk characteristics, and occlusion uncertainty characteristics to form an environmental risk feature vector for deep mine multi-level roadway scenarios.

[0008] In a preferred embodiment, during the commissioning and daily operation phases, actual passage grids that have not experienced collisions, skidding, emergency stops, or task interruptions are used as safety samples. The mean and covariance of the five types of features—static obstacle approach characteristics, dynamic disturbance characteristics, passage width bottleneck characteristics, ground passage risk characteristics, and occlusion uncertainty—are statistically analyzed to form a multidimensional statistical model characterizing normal safe passage conditions. During online planning, the comprehensive environmental risk cost is calculated based on the degree of deviation of the current features from the distribution of safety samples. This comprehensive environmental risk cost is then explicitly replaced by the risk term constructed solely based on occupancy probability and dynamic candidate ratio in the global cost function.

[0009] In a preferred embodiment, the autonomous mobile robot invokes a channel skeleton extraction program based on the global accessible grid set, shrinks the connected region into the channel centerline, and automatically identifies topological nodes at roadway intersections and entrances / exits to construct a mine roadway access map. By running global planning under conditions of no obstruction and temporary deletion of individual channels, comparing changes in total task cost, and normalizing cost increments according to preset rules, task obstruction sensitivity features are pre-determined for each centerline segment and its covered grid and written into a grid obstruction sensitivity table. During online global search, candidate grids are first stratified and filtered according to preset obstruction sensitivity thresholds and whether there are alternative paths on the access map. Bottleneck channels that can be bypassed but have excessively high task obstruction sensitivity are directly marked as planned disabled grids. Then, on the remaining grids, nodes are sorted and expanded based on the comprehensive environmental risk cost, cumulative travel cost, and expected distance cost to the target, generating a global access path in a deep mine multi-layer roadway system that simultaneously considers multi-factor environmental risks and task accessibility.

[0010] In a preferred embodiment, during the search process, an open set and a visited set are maintained in ascending order of comprehensive cost. The autonomous mobile robot expands the grid node with the lowest current cost in sequence according to the aforementioned comprehensive cost. Based on the occupancy probability and static and dynamic marking to remove grids completely occupied by static obstacles, an initial global path from the starting point to the ending point is generated. On this basis, the path sparsification and corner optimization program is called to compress continuous co-current line segments and prioritize the path shape that relies on smaller heading changes and lateral translation to complete the turn. Subsequently, within each control cycle, local tracking anchor points are determined based on the current pose obtained from the extended Kalman filter and the global reference path. Local working windows with static obstacles and dynamic candidate markers are cropped from the global occupied grid map. A limited number of candidate velocity combinations are generated under velocity and acceleration constraints. The collision risk, the degree of fit to the global path, and the propulsion effect toward the target are evaluated by short-time trajectory prediction. The velocity command with the lowest overall cost is selected as the current execution result.

[0011] In a preferred embodiment, the speed command with the lowest overall cost is selected from candidate speed combinations in each control cycle. When the local obstacle avoidance deviates too much from the global reference path, the local grid occupancy status and actual trajectory are reported to trigger local replanning or reconnection of the global path. The autonomous mobile robot splices the remaining global path with the actually executed local trajectory to form a new path point sequence. The path is smoothed while keeping the path points from intruding into the static obstacle grid and dynamic buffer area. The path is then combined with the speed and acceleration constraints of the Mecanum wheel in the longitudinal, lateral and planar rotation directions for time parameterization. A continuous speed reference trajectory is generated by the underlying motion controller and converted into the rotational speed of each drive wheel for following control along the smooth path.

[0012] In a preferred embodiment, it includes: a multi-sensor localization and occupancy mapping module, an environmental risk assessment and blocking sensitivity module, a path search and local tracking control module, and signal connections between the modules; The multi-sensor localization and occupancy mapping module is used to perform multi-sensor fusion localization and build an occupancy grid map with static obstacles, dynamic candidates and passable markers after the autonomous mobile robot is powered on. The environmental risk assessment blocking sensitivity module is used to construct environmental risk feature vectors based on the occupied grid map, calculate the comprehensive environmental risk cost according to the safety sample statistical model, and calculate the task blocking sensitivity feature quantity offline based on the mine roadway access map. During the search, sensitive channels are first screened out according to the task blocking sensitivity threshold, and then nodes are expanded according to the comprehensive environmental risk cost. The path search local tracking control module is used to search for paths based on costs and generate a velocity reference trajectory within a local window through velocity sampling and trajectory evaluation to control the robot to move along a smooth path.

[0013] The technical effects and advantages of the autonomous mobile robot path planning and obstacle avoidance method and system of this invention are as follows: This invention introduces multi-factor environmental risk assessment and task obstruction sensitivity modeling within a unified planning framework to jointly quantify and layer the risks of passage in deep mine roadways. At the local level, an environmental risk vector is constructed using features such as obstacle approach, passage geometry, attachment conditions, and occlusion uncertainty. A statistical model is formed using safe passage samples, mapping the deviation between candidate grids and safe clusters to a comprehensive environmental risk cost. This allows path search to proactively avoid high-risk areas, rather than relying solely on fixed safety distances. At the passage map level, this invention offline evaluates the change in total task cost when each passage is blocked, incorporating task obstruction sensitivity as an independent feature in node selection. When alternative paths exist, key bottleneck passages are prioritized for bypassing, reducing the probability of task interruption and large-scale replanning from the planning source. Combining path sparsity, path smoothing, and velocity sampling trajectory evaluation for Mecanum wheel omnidirectional motion mechanisms, a velocity reference trajectory with smooth curvature and a reasonable time scale can be generated under safety margin constraints, significantly improving the continuous operation efficiency and stability of autonomous mobile robots in complex bottleneck areas. Attached Figure Description

[0014] Figure 1 This is a timing diagram of the autonomous mobile robot path planning and obstacle avoidance method and system of the present invention.

[0015] Figure 2 This is a schematic diagram of the autonomous mobile robot path planning and obstacle avoidance method and system modules of the present invention. Detailed Implementation

[0016] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] Example This invention discloses a path planning and obstacle avoidance method for autonomous mobile robots, such as... Figure 1 As shown, it includes: After power-on initialization, the autonomous mobile robot first calls the pre-stored sensor calibration parameters and external parameter calibration results, loads the internal and external parameters of the two-dimensional LiDAR, depth camera, inertial measurement unit and Mecanum wheel odometer installed on the body, and starts the multi-source data synchronous acquisition process under a unified time reference.

[0018] During each sampling period, the autonomous mobile robot obtains motion control quantities of the body in the robot coordinate system from the odometer, obtains angular velocity and linear acceleration observations from the inertial measurement unit, and obtains distance and depth observations around the tunnel wall and ground obstacles from the lidar and depth camera. After completing the acquisition of raw data from multiple sources, the autonomous mobile robot calls a pose fusion algorithm based on extended Kalman filtering to recursively estimate its own pose in the global coordinate system.

[0019] The pose fusion algorithm uses the robot's state vector xk at discrete time k as the estimation object. The specific calculations are as follows: ; in, and This represents the robot's position coordinates in the plane coordinate system of the narrow alleyway. Indicates the aircraft's heading angle. and These represent the longitudinal and transverse linear velocity components in the body coordinate system, respectively.

[0020] Next, based on the kinematic constraints of the Mecanum wheel omnidirectional motion mechanism, the predicted state of the autonomous mobile robot between time step k−1 and time step k is given by the nonlinear motion equation: ; Given, among which, This represents the prior state estimated at time k based on time k−1. Let represent the posterior state estimate at time k−1. The function represents the longitudinal velocity, lateral velocity, and angular velocity control quantities of the aircraft obtained by back-derived from the Mecanum wheel odometer. This represents the pose update relationship given by the Mecanum wheel kinematic model. To incorporate process noise vectors caused by uneven ground and wheel slippage, the covariance matrix was calibrated through repeated experiments during the prototype stage.

[0021] In a typical implementation, the function Based on the current sampling period, the longitudinal velocity, lateral velocity, and angular velocity control quantities in the body coordinate system are integrated into the narrow passage plane coordinate system and updated recursively. , and Those skilled in the art can select an appropriate discrete integral form based on the Mecanum wheel kinematic model to implement it. .

[0022] After obtaining the prior state, the autonomous mobile robot performs scan matching on the current LiDAR scan and depth image to obtain the relative pose increment between the current moment and the previous moment, and then superimposes this increment onto the absolute pose of the previous moment to construct the observation. And through the observation equation: ; Establishing a connection between the true pose and the observation, where the function This represents the pose relationship mapped from the scan matching results to the global coordinate system. This indicates observation noise caused by environmental occlusion, dust, and sensor quantization errors.

[0023] Subsequently, the autonomous mobile robot performs observations on the equations at the current working point. The observation matrix is ​​obtained by finding the Jacobian. And using the prior state covariance matrix and observation noise covariance matrix Calculate the extended Kalman filter gain: ; The current state estimate and covariance matrix are then updated using the following formula: ; ; in, It is an identity matrix.

[0024] Next, after completing the pose estimation for the current moment, the autonomous mobile robot invokes the occupancy grid mapping program to convert the current LiDAR and depth camera observations into the occupancy probabilities of each grid cell in the environment. To this end, the working area is discretized into a set of two-dimensional grids according to a preset spatial resolution. ,in Let j represent the grid cell located in the i-th row and j-th column. The autonomous mobile robot maintains an occupancy value for each grid cell, expressed as a logarithmic probability. And use the log-odds update formula: ; The grid occupancy state is recursively calculated, where, This represents the log-odds occupancy value of the raster since the last update. To grid in the current robot pose The conditional probability returned by the sensor model when the measured value is occupied. is the log-probability constant corresponding to the idle prior.

[0025] Subsequently, the autonomous mobile robot passed through ; Convert the logarithmic odds value into a raster at time k. Occupancy probability ,in The closer a value is to 1, the greater the likelihood that the grid cell is an obstacle; the closer a value is to 0, the greater the likelihood that the grid cell is an empty area.

[0026] Among them, the above conditional probabilities During the prototype calibration phase, statistical regression was performed on measured data at different distances and incident angles in a standard tunnel environment to establish a basic sensor model for both lidar measurement and depth camera measurement.

[0027] Furthermore, considering the distinct directional structure of the tunnel walls in narrow tunnel environments, autonomous mobile robots, in terms of computation... A weighting factor related to the ray incident angle is introduced to assign higher reliability to measurements close to the wall normal direction. Specifically, for each hit grid... The measurement ray is defined as the angle between its direction and the estimated wall normal vector. A weighted sensor model is used: ; in, For the probability of a basic sensor model that does not consider directionality, This refers to the magnification factor set during the prototype calibration phase based on different wall materials and laser reflection characteristics. When the measurement ray is closer to being perpendicular to the wall surface... A larger value increases the overall weight, thereby enhancing the occupancy confidence of the wall grid; when the measurement ray is approximately parallel to the wall, The effect of reducing noise levels is suppressed, thus mitigating the impact of isolated noise points caused by dust or water vapor.

[0028] While continuously performing the log-probability update and weighted sensor model correction described above, the autonomous mobile robot combines the time dimension to distinguish the static and dynamic attributes of each grid cell. To this end, the autonomous mobile robot performs the following for each grid cell: Maintain two counters: the number of times the grid is determined to be occupied within the sliding time window. and the total number of times the grid was observed by the sensor within that time window. After each sensor update, the autonomous mobile robot calculates the grid occupancy ratio: And according to Compared with the preset static threshold and dynamic threshold ρ The relationship between the grid Divided into static obstacle grids, dynamic candidate grids, or passable grids: when When the grid is in a static obstacle grid, mark it as a static obstacle grid; when When this occurs, the grid is marked as a dynamic candidate grid, and a larger safety margin is applied to this type of grid in subsequent local obstacle avoidance steps; when When this occurs, the grid is considered a passable area and participates in the construction of the local passage space.

[0029] After completing environmental perception and grid map construction, the autonomous mobile robot first reads the starting pose and target pose of the current task from the task scheduling unit, and sets the starting pose... With target pose Mapped to the corresponding raster cell index in a unified map coordinate system and .

[0030] Subsequently, the autonomous mobile robot uses the globally occupied grid map obtained above as the search space, sets the cells marked as static obstacle grids as impassable, sets the cells marked as dynamic candidate grids as high-risk areas, and regards the grid cells with an occupancy ratio lower than the dynamic threshold as basic passable areas, and constructs the search state space according to the preset grid resolution and the connection relationship between eight or sixteen neighbors.

[0031] After constructing the search space, the autonomous mobile robot invokes a global path planning program based on heuristic search, starting from the grid. As the initial state for the search, the target raster As a search termination condition, a comprehensive cost is calculated for each raster cell n to be expanded during the search process.

[0032] To balance travel distance, expected distance to the target, and safety costs when traversing high-risk areas, the autonomous mobile robot maintains a cumulative travel cost term G(n), a heuristic estimation term H(n), and a risk cost term R(n) constructed based on occupancy probability and dynamic labeling for each candidate grid cell, and sorts the search nodes according to the following total cost function: ; in, Let G(n) be the total cost of a grid cell n, representing the value of the grid cell starting from the origin. The cumulative travel cost to reach grid n along the currently selected path is obtained by summing the Euclidean distances between adjacent grids along the path by a step size; H(n) represents the distance from grid n to the target grid in the ideal case where obstacles are ignored. The expected distance cost is estimated using the Euclidean distance or diagonal distance between grid center points as a heuristic; R(n) represents the additional risk cost introduced when passing through high occupancy probability regions and dynamic candidate regions near grid n; and The weighting coefficients for the heuristic estimation term and the risk cost term are determined during the prototype calibration stage through simulation and real vehicle tests in multiple narrow alleyway scenarios, so that the robot can shorten the global path length as much as possible while ensuring safety.

[0033] It should be noted that when autonomous mobile robots are deployed in multi-level roadway systems in deep mines to perform emergency material delivery tasks in narrow spaces where production and emergency escape routes are intertwined, such environments often consist of spiraling or descending sloping roadways, connected horizontal working roadways, and multiple T-shaped or cross-shaped intersections. Belt conveyors, pipelines, and cable trays are suspended above these intersections, the ground below is chronically slippery, and heavy mining cars, railcars, and other mobile equipment frequently pass through during peak production periods.

[0034] At this point, the passage risk at the same location is not determined by the single factor of whether there are obstacles in the surrounding area, but is simultaneously affected by a variety of factors such as the effective passage width of the tunnel, the ground adhesion conditions, unknown areas caused by sensor obstruction, and local dynamic traffic load. On the one hand, there are often bottleneck sections where the effective passage width is suddenly reduced at the connection between the spiral ramp and the horizontal tunnel. Although they are marked as passable on the grid map, the actual remaining width is only slightly larger than the outer envelope of the autonomous mobile robot. On the other hand, the adhesion coefficient of the ground is significantly reduced due to water accumulation, slurry, or gravel accumulation. There is a risk of slipping in the downhill direction, and in the uphill direction, there may be secondary stagnation or even backward slip due to insufficient traction. Meanwhile, the belt conveyor supports and pipeline brackets form a large-scale, long-term obstruction zone above the intersection area, making it impossible for lidar to obtain effective echoes in certain sectors for extended periods, resulting in large areas of long-term unknown regions in the grid map. During shift changes or emergency drills, heavy mining trucks, railcars, and large numbers of workers will gather or pass through the intersection area rapidly, causing the dynamic candidate grid density in the local area to rise sharply in a short period of time and exhibit highly irregular movement trajectories.

[0035] If the risk cost term is constructed solely based on local occupancy probability and dynamic candidate ratio, it cannot distinguish between high-risk bottleneck passages that appear to have few obstacles on the map, have extremely narrow effective passage widths, are highly slippery, and are chronically obstructed by sensors, and safe passages with similar occupancy probabilities but larger lateral margins, better ground conditions, and ample visible space. Furthermore, it cannot reflect the comprehensive risks arising from geometric bottlenecks, deteriorating road surface adhesion, increased observation uncertainty, and the superposition of dynamic traffic loads in a single indicator. This will lead to global path planning confusing fundamentally different risk factors at the cost level, potentially guiding autonomous mobile robots through a spiral intersection area that appears to have a moderate occupancy probability but carries extremely high overall risk.

[0036] Therefore, in this embodiment, when constructing the risk cost term R(n), the autonomous mobile robot performs two types of technical actions in sequence for the above-mentioned deep mine multi-level roadway scenario: comprehensive risk assessment of multiple environmental factors and calculation of task obstruction sensitivity. During the global search process, the robot applies the two actions to the candidate grid n in different ways.

[0037] Specifically, in the process of comprehensive environmental risk assessment, when expanding candidate grid n each time, the autonomous mobile robot cuts out a fixed-size local window from the occupied grid map centered on grid n. It then reads the occupancy probability, static / dynamic markers, and observation count of all grid cells within the window from the maintained map data structure, and calculates the environmental risk feature vector of candidate grid n by combining it with pre-configured roadway geometric parameters and terrain parameters. Specifically, the autonomous mobile robot first searches for static obstacle grids with an occupancy probability greater than a preset threshold within the local window, selects the grid with the closest Euclidean distance to grid n as the nearest static obstacle point, and denotes its occupancy probability as... Distance is Through the and 1 / By performing normalized weighting, we obtain the static obstacle approximation features. .

[0038] Subsequently, the autonomous mobile robot calculates the ratio of all dynamic candidate grids within a local window to the total number of grids, and counts the number of times the dynamic markers within that window switch within a preset time window. The two are then linearly combined with weights to obtain the dynamic perturbation feature. To characterize the bottleneck at the connection between the spiral ramp and the horizontal tunnel, the autonomous mobile robot uses the expected path tangential direction of the current grid n as a reference, and sequentially searches for the nearest static obstacle grids on both sides along the grid rows and columns in the normal direction perpendicular to it. The effective passage width is obtained by multiplying the number of passable grids between two obstacles by the grid resolution. Then, compared with the preset baseline width The bottleneck characteristics of the passage width are calculated using the following formula: ; Next, to reflect the risks associated with ground adhesion and slope, the autonomous mobile robot retrieves passage records centered on this local window from its historical operation logs. It then counts the number of times the longitudinal acceleration peak exceeds a threshold, the wheel odometer slip error exceeds limits, or the actual braking distance is significantly greater than the theoretical value. The number of occurrences and the magnitude of the deviation are normalized to obtain the characteristics of ground passage risk. Simultaneously, the autonomous mobile robot utilizes the number of grid observations and unknown state counts to calculate the ratio of the number of grids within a window that remain in an unknown state and are traversed by multiple laser beams across multiple consecutive frames to the total number of grids in the window. This ratio is then combined with normalization based on the number of consecutive unknown frames to obtain the occlusion uncertainty characteristics. Based on the above calculations, the autonomous mobile robot generates a five-dimensional environmental feature vector each time it expands the candidate grid n. ; During system debugging and daily operation in the mine, the autonomous mobile robot, assuming no collisions, slippage, sudden stops, or mission interruptions occur, treats each grid it actually passes through as a safety sample and calculates the safety sample feature vector in the same way. The system accumulates a set of safe samples in the background. The control unit periodically performs statistical operations on the safe sample set to calculate the mean vector μ and covariance matrix of the five-dimensional features. This is used as a statistical model for normal and safe passage conditions in the current mine environment. During the online global planning process, the autonomous mobile robot calls this statistical model for each candidate grid n, according to: ; Calculate the deviation of candidate grid n from the safe sample cluster in the multi-factor feature space, and within a preset upper limit. With proportionality coefficient Under constraints, the deviation is mapped to the comprehensive environmental risk cost: ' Subsequently, the autonomous mobile robot in the total cost function... Replacing the original risk term, the total path cost is written as: ; In each round, the node with the smallest F(n) is selected from the open set for expansion. In this way, the environmental risk cost term is not a simple weighting of each factor, but a multi-dimensional feature statistical model automatically formed based on a large number of safety samples. This model can jointly determine the degree of anomaly in the passage environment under the complex conditions of slope bottlenecks, dynamic disturbances, and occlusion uncertainties.

[0039] In the calculation of task blocking sensitivity, this embodiment models and uses task blocking sensitivity as a special quantity independent of R(n). To this end, the autonomous mobile robot first uses the globally passable grid set obtained above to call the channel skeleton extraction program to shrink the connected passable areas into one-dimensional centerline segments. It then automatically identifies topological nodes at roadway intersections and entrances / exits, constructing a mine roadway access map with centerline segments as edges and topological nodes as endpoints. For each centerline segment e in the access map, the control terminal runs a global planning operation without blocking in the background, constrained by the start and end points of the emergency material delivery task, and records the minimum total task cost. Then, temporarily delete the center line segment 'e' from the travel graph while keeping other parameters unchanged, and run global planning again. If there is no feasible path between the start and end points at this time, then deduct the blocking sensitivity feature of the center line segment and all the grids it covers. Assign a value of 1; if a feasible path still exists, record the new minimum total task cost. Calculate the cost increment and according to the preset maximum allowable increment Will Normalization Mapping all grid indices covered by the channel centerline segment e to grid blocking sensitivity table entries allows the autonomous mobile robot to obtain the task blocking sensitivity feature corresponding to any candidate grid n during online planning by looking up the table. .

[0040] In the specific global search process, autonomous mobile robots incur comprehensive environmental risks. With task blocking sensitivity feature A layered usage strategy is adopted. Before each extraction of the grid n to be expanded from the open set, the autonomous mobile robot first determines the threshold based on a preset blocking sensitivity threshold. Filter the grid n: when Furthermore, when alternative paths exist in the path map that do not pass through the center line segment, the autonomous mobile robot directly marks grid n as a planned disabled grid, without calculating F(n) for it or adding it to the open set, thereby forcing the global path to bypass the bottleneck channel that is highly critical in the topology; when , or although An autonomous mobile robot is only considered to be based on a path that is close to 1, but path analysis shows that there are no alternative paths between the start and end points. Calculate F(n) and sort and expand according to the total cost. In this embodiment, the risk cost term R(n) is taken as the comprehensive environmental risk cost. And task blocking sensitivity It takes effect in the node selection process in the form of an independent number of features, and is not algebraically superimposed with R(n).

[0041] This enables autonomous mobile robots to conduct joint analysis of local environmental risks during the planning stage based on multiple factors such as static approximation, dynamic disturbance, passage width bottleneck, and uncertainty of ground attachment and occlusion. It also enables them to preemptively eliminate key spiral intersection channels at the passage map level, which would lead to a sharp increase in the total cost of the task or make the task unreachable if blocked. This avoids planning passage paths with moderate occupancy probability but extremely high comprehensive risks in deep mine multi-layer roadway systems.

[0042] Furthermore, throughout the search process, the autonomous mobile robot maintains an open set sorted in ascending order of F(n) and a visited set. At each iteration, the grid with the minimum total value is selected from the open set as the current expansion node, and it is checked whether the target grid has been reached. Alternatively, it can enter a preset target neighborhood; if the termination condition is met, it can backtrack the parent node pointer of the current node and restore the grid sequence from the start point to the end point in reverse order to form an initial global path; if the termination condition is not met, it can generate adjacent passable grids of the current node according to the preset neighborhood connection relationship, calculate the candidate cumulative travel cost G(n) and risk cost R(n) for each neighbor grid, update its total cost F(n), and perform specific operations such as insertion, updating parent node pointer or skipping depending on whether it is in the open set or the visited set.

[0043] It should be noted that the search process described above uses the occupancy probability and static / dynamic markers provided above as a basis in each iteration to judge the accessibility and risk of the grid, ensuring that any grid completely occupied by static obstacles will not be added to the candidate path.

[0044] After obtaining the initial global path from the start point to the end point, the autonomous mobile robot calls the path sparsification and corner optimization program to simplify and adjust the discrete grid sequence. Specifically, the autonomous mobile robot first traverses the grid path obtained by the search algorithm, compresses the continuous collinear grid segments, and retains only the key nodes where the path geometry changes significantly, in order to reduce the number of path points.

[0045] Subsequently, taking advantage of the Mecanum wheel omnidirectional motion mechanism's ability to independently adjust speed in the lateral and longitudinal directions, the autonomous mobile robot evaluates the impact of different turning angle combinations on subsequent control near each path turning point. It tends to select path patterns that can complete the turn with a smaller heading change combined with lateral translation, thereby avoiding the risk of collisions caused by large-scale stationary rotation in narrow alleyways.

[0046] Furthermore, in each control cycle, the autonomous mobile robot first determines its current pose based on the extended Kalman filter obtained above. And the global reference path output above, find the path point on the path that is closest to the current pose, and use the path point as the local tracking anchor point.

[0047] Based on this, the autonomous mobile robot, centered on its current position, cuts out a fixed-size local working window from the global occupancy grid map. Each grid cell within the window carries an occupancy probability, a static obstacle marker, or a dynamic candidate marker. For cells marked as static obstacle grids, the autonomous mobile robot directly treats them as hard constraints for local obstacle avoidance. For cells marked as dynamic candidate grids, the autonomous mobile robot adds a certain number of safety buffer grids around the cell, temporarily treating the grids within the buffer range as soft constraint areas to increase the safe distance from potentially moving obstacles.

[0048] Subsequently, the autonomous mobile robot, based on its current linear velocity... and angular velocity In addition to the maximum speed and maximum acceleration constraints of the Mecanum wheel drive system, a finite number of candidate velocity triplets are constructed in the velocity space. .

[0049] These candidate speeds are generated by limiting them to a dynamic window around the current speed. This means that only speed combinations that can be achieved through limited acceleration and deceleration within one control cycle are allowed. This ensures that the generated speed commands meet the capabilities of the actuators while avoiding the impact of sudden acceleration on vehicle stability.

[0050] For each candidate velocity triplet, the autonomous mobile robot performs short-time trajectory forward prediction within a local working window according to discrete time steps. That is, it sequentially accumulates position and attitude changes within a given prediction time domain, and applies the longitudinal translation, lateral translation and rotation generated by the omnidirectional motion of the Mecanum wheel to the robot's current pose to generate a candidate local trajectory.

[0051] After obtaining the predicted trajectory for each candidate velocity triplet, the autonomous mobile robot calls the trajectory evaluation program to comprehensively evaluate the trajectory's safety within the local working window, its fit with the global reference path, and its propulsion effect towards the target direction. Specifically, for any candidate velocity triplet u and its corresponding predicted trajectory, the autonomous mobile robot calculates the comprehensive cost: ; Where J(u) is the comprehensive cost of the candidate velocity triple u; This is the cost of the collision risk between the trajectory and the obstacle grid in the local working window within the prediction time domain; This is the cost of the deviation between the trajectory and the global reference path given above; This is the cost of the trajectory advancing towards the target along the global reference path; , and These are weighting coefficients for three types of costs: collision risk, path fit, and target advancement. These three factors were calibrated during the prototype stage through simulations and real vehicle tests under numerous narrow alleyway conditions.

[0052] Among them, the cost of collision risk The position of each trajectory sampling point within a locally occupied grid is obtained by checking the landing position of each sampling point on the predicted trajectory. When any sampling point falls into a static obstacle grid or its immediate neighboring grid, the candidate velocity is considered infeasible and... Assign a maximum value; when a sampling point falls into a dynamic candidate grid and its buffer area, calculate a risk value that increases rapidly with decreasing distance based on the occupancy probability of that area and the proportion of dynamic candidates, to reflect the tendency to actively avoid dynamic obstacles. Path fitting cost The cost is calculated by averaging the distances between each sampling point on the predicted trajectory and the nearest point on the global reference path, and then weighted and summing these distances over the prediction time domain. The larger the distance, the more severe the trajectory deviation from the global skeleton, and the higher the cost. Target advancement cost. This is defined based on the arc length increment between the predicted trajectory endpoint's projection position on the global reference path and the current anchor point. The larger the arc length the trajectory endpoint travels along the path direction, the more beneficial the speed command is for advancing towards the target. The smaller the value, the more likely the trajectory is to linger or even regress within a local area. A larger value is assigned to avoid unnecessary reciprocating motion of the robot in confined spaces.

[0053] After calculating the cost of all candidate velocity triplets, the autonomous mobile robot selects the velocity triplet with the minimum comprehensive cost J(u) from all feasible candidate sets. As the execution instruction for the current control cycle, the predicted trajectory segment corresponding to this speed is used as a local reference trajectory buffer for continuous comparison with the new prediction results in the next control cycle.

[0054] When a robot experiences significant yaw during local obstacle avoidance due to repeatedly selecting candidate velocity triplets that deviate from the global reference path, the autonomous mobile robot will also report the latest occupied grid state and the current local actual trajectory within the local working window to the global path management program, requesting local replanning or reconnection to be performed on the aforementioned global reference path, thereby restoring the consistency of the global path while ensuring safe obstacle avoidance.

[0055] Next, after completing several control cycles of local obstacle avoidance and dynamic path correction, the autonomous mobile robot first extracts the path segments that have not yet been traversed from the global reference path and splices them with the previously executed local trajectory segments to form a discrete path point sequence starting from the current pose and extending to the vicinity of the final target. This path point sequence is represented by a series of planar coordinates in the world coordinate system. This means that each path point retains both the global skeleton formed above to avoid static obstacle grids and dynamic candidate grids, and reflects the necessary offsets caused by local obstacle avoidance.

[0056] Meanwhile, in order to eliminate the jagged turns caused by local obstacle avoidance and reduce the frequent adjustments of subsequent omnidirectional motion control, the autonomous mobile robot performs smoothing processing on the discrete path point sequence, and moderately shrinks multiple adjacent inflection points towards the geometric center while ensuring that they do not intrude into the static obstacle grid and dynamic buffer area.

[0057] During path smoothing, the autonomous mobile robot constructs the following smoothing cost function for the path point sequence. Perform iterative adjustments: ; in, The smoothing cost for the entire path; , , These are the planar coordinates of three adjacent path points. Used to measure at waypoints The magnitude of the second difference at a point reflects the degree of curvature of the path at that point; the smaller the value, the smoother the path at that point. path point Compared to the obstacle cost of the nearest static obstacle grid and dynamic candidate grid, the cost increases as the path point is closer to the obstacle, and approaches zero when the path point is located in the center of the passable area. To smooth out the priority weights, To prioritize obstacle avoidance, the settings were adjusted through multiple roadway scenario tests during the prototype stage.

[0058] Meanwhile, in each iteration, the autonomous mobile robot makes small adjustments to the path point positions according to the cost function, gradually reducing the second-order difference term, and forcing all path points to always remain in the passable area where the occupancy probability is lower than the dynamic threshold, thereby forming a smooth path that is geometrically continuous and safely away from the obstacle boundary.

[0059] After obtaining the smoothed spatial path, the autonomous mobile robot further performs time parameterization on the path by combining the velocity and acceleration limits of the Mecanum wheel omnidirectional motion mechanism. To this end, the autonomous mobile robot first resamples the smoothed path according to the path arc length, uniformly dividing the path into several sampling points with approximately equal arc length intervals, and calculating the tangential and normal vectors at each sampling point. Then, combining the robot's maximum linear velocity and maximum acceleration constraints in the longitudinal, lateral, and planar rotational degrees of freedom, the autonomous mobile robot adopts a piecewise trapezoidal velocity planning method to allocate time scales to the path arc length: a moderate acceleration segment is allocated in the initial section of the path, allowing the robot to smoothly increase its longitudinal and lateral velocities within the allowable acceleration range; a constant velocity segment is allocated in the longer straight or gently curved sections in the middle of the path, allowing the robot to advance along the smooth path at a speed close to the maximum allowable speed; and a deceleration segment is allocated near the target point and in curved sections with large curvature, allowing the robot to gradually reduce its speed and lateral drift before reaching the target, avoiding overshoot at the end of narrow passages.

[0060] After time parameterization, each sampling point on the smooth path is assigned a unique timestamp, which enables the autonomous mobile robot to generate an executable velocity reference trajectory in the continuous time domain. .in, and Let represent the desired linear velocities along the longitudinal and lateral directions in the robot's body coordinate system, respectively. This represents the desired angular velocity around an axis perpendicular to the ground. For each time sampling point, the autonomous mobile robot decomposes the desired velocity along the path arc length into longitudinal and lateral components based on the unit vectors of the tangential and normal directions of the path at that moment, and converts the path direction change rate into the corresponding angular velocity reference, ensuring that the velocity reference trajectory satisfies the pre-set velocity and acceleration boundary conditions at any time.

[0061] Subsequently, the autonomous mobile robot will The command is sent to the underlying motion controller, which calculates the rotational speed of each drive wheel according to the Mecanum wheel kinematics model and performs closed-loop tracking to achieve accurate following of a smooth path under omnidirectional motion.

[0062] This invention also proposes an autonomous mobile robot path planning and obstacle avoidance system, such as... Figure 2 As shown, it includes: a multi-sensor localization and occupancy mapping module, an environmental risk assessment and blocking sensitivity module, a path search and local tracking control module, and signal connections between the modules; The multi-sensor localization and occupancy mapping module is used to perform multi-sensor fusion localization and build an occupancy grid map with static obstacles, dynamic candidates and passable markers after the autonomous mobile robot is powered on. The environmental risk assessment blocking sensitivity module is used to construct environmental risk feature vectors based on the occupied grid map, calculate the comprehensive environmental risk cost according to the safety sample statistical model, and calculate the task blocking sensitivity feature quantity offline based on the mine roadway access map. During the search, sensitive channels are first screened out according to the task blocking sensitivity threshold, and then nodes are expanded according to the comprehensive environmental risk cost. The path search local tracking control module is used to search for paths based on costs and generate a velocity reference trajectory within a local window through velocity sampling and trajectory evaluation to control the robot to move along a smooth path.

[0063] The above formulas are all dimensionless calculations. The formulas are derived from software simulations based on a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0064] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, in the form of a computer program product.

[0065] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and inventive constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0066] In addition, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0067] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0068] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A path planning and obstacle avoidance method for autonomous mobile robots, characterized in that, include: After the autonomous mobile robot is powered on, it performs multi-sensor fusion localization and constructs an occupied grid map with static obstacles, dynamic candidates and passable markers; An environmental risk feature vector is constructed based on the occupied grid map. The comprehensive environmental risk cost is calculated according to the safety sample statistical model. The task blocking sensitivity feature is calculated offline based on the mine roadway access map. During the search, sensitive channels are first screened out according to the task blocking sensitivity threshold, and then nodes are expanded according to the comprehensive environmental risk cost. Based on the cost, a path search is performed, and a velocity reference trajectory is generated within a local window through velocity sampling and trajectory evaluation to control the robot to move along a smooth path.

2. The path planning and obstacle avoidance method for autonomous mobile robots according to claim 1, characterized in that: After being powered on, the autonomous mobile robot loads multi-sensor calibration parameters and synchronously collects data from the odometer, inertial measurement unit, lidar, and depth camera under a unified time reference. The autonomous mobile robot uses extended Kalman filtering combined with scan matching to continuously estimate its own pose and updates the observation results into an occupied grid map with static obstacles, dynamic candidates and passable markers.

3. The path planning and obstacle avoidance method for autonomous mobile robots according to claim 1, characterized in that: After completing the environmental perception and occupation grid map construction, the autonomous mobile robot uses the aforementioned global occupation grid map as a basis to crop a local window centered on the grid to be expanded during the global path planning process. It reads the occupation probability, static obstacle markers, dynamic candidate markers, observation times, and pre-configured roadway geometric parameters and terrain parameters of each grid within the window. It then calculates the static obstacle approach characteristics, dynamic disturbance characteristics, passage width bottleneck characteristics, ground passage risk characteristics, and occlusion uncertainty characteristics in sequence to form an environmental risk feature vector for deep mine multi-level roadway scenarios.

4. The path planning and obstacle avoidance method for autonomous mobile robots according to claim 3, characterized in that... During the debugging and daily operation phases, actual passing grids that have not experienced collisions, skidding, emergency stops, or task interruptions are used as safety samples. The mean and covariance of the five types of characteristics mentioned above—static obstacle approach characteristics, dynamic disturbance characteristics, passage width bottleneck characteristics, ground passage risk characteristics, and occlusion uncertainty—are statistically analyzed to form a multidimensional statistical model representing normal safe passage conditions. During online planning, the comprehensive environmental risk cost is calculated based on the degree of deviation of the current characteristics from the distribution of safe samples. This comprehensive environmental risk cost is then explicitly replaced by the risk term constructed solely based on occupancy probability and dynamic candidate ratio and written into the global cost function.

5. The path planning and obstacle avoidance method for autonomous mobile robots according to claim 1, characterized in that: The autonomous mobile robot invokes a channel skeleton extraction program based on the global accessible grid set, shrinks the connected area into the channel centerline, and automatically identifies topological nodes at roadway intersections and entrances / exits to construct a mine roadway access map. By running global planning under conditions of no obstruction and temporary deletion of individual channels, comparing changes in total task cost, and normalizing cost increments according to preset rules, the robot pre-determines task obstruction sensitivity features for each centerline segment and its covered grid and writes them into a grid obstruction sensitivity table. During online global search, candidate grids are first stratified and filtered according to preset obstruction sensitivity thresholds and the existence of alternative paths on the access map. Bottleneck channels that can be bypassed but have excessively high task obstruction sensitivity are directly marked as planned prohibited grids. Then, on the remaining grids, nodes are sorted and expanded based on comprehensive environmental risk cost, cumulative travel cost, and expected distance cost to the target, generating a global access path in a deep mine multi-layer roadway system that simultaneously considers multi-factor environmental risks and task accessibility.

6. The path planning and obstacle avoidance method for autonomous mobile robots according to claim 1, characterized in that: During the search process, an open set and a visited set are maintained in ascending order of comprehensive cost. The autonomous mobile robot expands the grid node with the lowest current cost in sequence according to the aforementioned comprehensive cost. Based on the occupancy probability and static and dynamic marking to remove grids completely occupied by static obstacles, an initial global path from the starting point to the end point is generated. On this basis, the path sparsification and corner optimization program is called to compress continuous co-current line segments and prioritize the path shape that relies on smaller heading changes and lateral translation to complete the turn. Subsequently, within each control cycle, local tracking anchor points are determined based on the current pose obtained from the extended Kalman filter and the global reference path. Local working windows with static obstacles and dynamic candidate markers are cropped from the global occupied grid map. A limited number of candidate velocity combinations are generated under velocity and acceleration constraints. The collision risk, the degree of fit to the global path, and the propulsion effect toward the target are evaluated by short-time trajectory prediction. The velocity command with the lowest overall cost is selected as the current execution result.

7. The path planning and obstacle avoidance method for autonomous mobile robots according to claim 6, characterized in that: Within each control cycle, the speed command with the lowest overall cost is selected from candidate speed combinations. When the local obstacle avoidance deviates too much from the global reference path, the local grid occupancy status and actual trajectory are reported to trigger local replanning or reconnection of the global path. The autonomous mobile robot splices the remaining global path with the actual executed local trajectory to form a new path point sequence. The path is smoothed while ensuring that the path points do not intrude into the static obstacle grid and dynamic buffer area. The time parameterization is combined with the speed and acceleration constraints of the Mecanum wheel in the longitudinal, lateral and planar rotation directions to generate a continuous speed reference trajectory converted by the underlying motion controller into the rotational speed of each drive wheel for following control along the smooth path.

8. An autonomous mobile robot path planning and obstacle avoidance system, characterized in that, include: The system includes a multi-sensor positioning and mapping module, an environmental risk assessment and blocking sensitivity module, a path search and local tracking control module, and signal connections between the modules. The multi-sensor localization and occupancy mapping module is used to perform multi-sensor fusion localization and build an occupancy grid map with static obstacles, dynamic candidates and passable markers after the autonomous mobile robot is powered on. The environmental risk assessment blocking sensitivity module is used to construct environmental risk feature vectors based on the occupied grid map, calculate the comprehensive environmental risk cost according to the safety sample statistical model, and calculate the task blocking sensitivity feature quantity offline based on the mine roadway access map. During the search, sensitive channels are first screened out according to the task blocking sensitivity threshold, and then nodes are expanded according to the comprehensive environmental risk cost. The path search local tracking control module is used to search for paths based on costs and generate a velocity reference trajectory within a local window through velocity sampling and trajectory evaluation to control the robot to move along a smooth path.

Images