Method for a robot indoor navigation system to adapt to narrow environments
Patent Information
- Application Number
- CN202410028012.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-01-08
- Publication Date
- 2026-10-27
- Estimated Expiration
- 2044-01-08
AI Technical Summary
然而，根据实际部署经验，机器人往往在本可通行的狭窄环境前因无法生成可行的轨迹而出现振荡、停止行为或在狭窄环境中陷入致命区域导致导航任务失败
[0030]1、本发明不依赖于特定特征的检测来识别门或走廊等特殊狭窄环境，适用于场景中的任何狭窄环境，具备更好的泛用性。
Smart Images
Figure CN117928545B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the technical field of indoor robot navigation, and in particular to a method for an indoor robot navigation system to adapt to narrow environments. Background Technology
[0002] The complexity of indoor environments and the presence of obstacles pose challenges to robot navigation technology. Currently, there are still some issues with robots reliably and autonomously performing navigation tasks indoors. One practical problem that needs to be addressed is that robots cannot safely and stably pass through narrow environments slightly wider than their body width, such as corridors, corners, or doorways. Ideally, if a feasible global path exists, a local trajectory planner can find a suitable trajectory to allow the robot to navigate through these narrow environments. However, based on actual deployment experience, robots often fail to generate a feasible trajectory in narrow environments, resulting in oscillations, stopping, or getting stuck in fatal areas, leading to navigation task failure.
[0003] Regarding this issue, the task of detecting and passing through doors has been extensively studied in the robotics community as a special example of narrow environments that robots should be able to pass through. However, it has limitations in specific scenarios, namely the mandatory presence of doors or corridors (requiring the detection and identification of specific features). We hope that robots can safely pass through any narrow environment in the task scenario. Methods such as adjusting obstacle avoidance weights and other related parameters in motion control can enable robots to enter narrow environments, but this method also increases the robot's collision risk and degrades its navigation performance in other environments. There are also methods that propose setting auxiliary navigation points at both ends of the narrow passage to help the robot enter the passage in a better posture to improve the probability of passage. However, auxiliary navigation points need to be manually marked on the map in advance, which cannot enable the robot to pass through narrow environments completely autonomously. Summary of the Invention
[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a method for an indoor robot navigation system to adapt to narrow environments. This method enables the robot to automatically switch navigation strategies to adapt to narrow environments without human intervention, thereby improving the success rate and smoothness of the robot's autonomous passage through narrow environments where the width is slightly larger than the robot's body width.
[0005] To achieve the above objectives, the technical solution provided by this invention is: a method for an indoor robot navigation system to adapt to narrow environments, comprising:
[0006] Navigation strategy information generation stage: During the robot's indoor navigation task, it collects navigation failure poses (NFPs) caused by narrow environments, generates appropriate passage pose pairs (STPs) corresponding to the NFPs, and forms navigation strategy information for use in the navigation strategy switching stage.
[0007] Navigation strategy switching phase: During the robot's indoor navigation task, the timing for switching the narrow environment navigation strategy is determined based on the navigation strategy information provided in the navigation strategy information generation phase, and the strategy is executed. This includes switching the narrow environment navigation parameter configuration and executing multi-point navigation tasks to assist the robot in safely passing through the narrow environment. After passing through the narrow environment, the robot switches to the normal navigation strategy, including restoring the normal navigation parameter configuration and resuming the original navigation task.
[0008] Furthermore, the indoor navigation system assisted by this method is the NavigationStack navigation system under the ROS robot operating system, which consists of five modules: a map service module, an environment perception module, an autonomous localization module, a path planning module, and a motion control module. The workflow of the indoor navigation system is as follows: the map service module loads the global cost map; the environment perception module acquires data of the surrounding environment through the robot's sensors; the autonomous localization module calculates the robot's current pose at a set frequency based on the data acquired by the environment perception module, and when a target navigation point is received, it begins to execute the navigation task; the global path planner in the path planning module calculates the optimal global path from the current pose to the target navigation point at a set frequency based on the global cost map; the motion control module updates the local cost map based on the data acquired by the environment perception module, and the local trajectory planner calculates a trajectory that conforms as closely as possible to the global path based on the local cost map at a set frequency and issues corresponding speed control commands to control the robot to move along the trajectory; the above five modules work together until the robot reaches the target navigation point.
[0009] Furthermore, the navigation strategy information is a set consisting of n NFPs and STPs generated from them, denoted as set. strategy ={(NFP i STPs i )} i＝1,2,3,...,n Among them, NFP i STPs represent the i-th failed navigation pose found. i According to NFP i The generated appropriate passage pose pairs, STPs are generated from the appropriate passage start pose STP. start and appropriate passage end position STP end Composition, represented as STPs = (STP start STP end The pose (x, y, θ) is composed of the robot's horizontal coordinate x, vertical coordinate y, and orientation θ in the map.
[0010] Furthermore, the navigation strategy information generation process is implemented through navigation recovery behavior, as detailed below:
[0011] 1.1) Pre-set the triggering time for navigation recovery behavior, which includes the following three situations: continuous failure of global path planning, continuous failure of local trajectory planning, and oscillation phenomenon in which the robot moves back and forth between two positions;
[0012] 1.2) When the robot encounters one of the three situations described in step 1.1) during the indoor navigation task, the navigation task is paused and the navigation recovery behavior is executed: the appropriate passage pose pair generation method is executed. If STPs are successfully generated, the current pose is considered to be a navigation failure pose caused by the narrow environment. The NFP and STPs are written into the navigation strategy information, and the updated navigation strategy information is loaded into the navigation strategy switching stage. Then, the robot continues to try to execute the original navigation task. If the generation of STPs fails, it is considered that the robot's three situations described in step 1.1) are not caused by the narrow environment, and the navigation task fails.
[0013] Furthermore, in step 1.2), the suitable passage pose generation method refers to the current global cost map and pose, and attempts to generate an STP at the entrance of a potentially narrow environment ahead. start STP is generated at the exit. end STP start and STP end To get closer to the middle of the road, follow these steps:
[0014] 1.2.1) Modify the expansion radius of the global cost map in the navigation parameters to the preset narrowing threshold γ. narrow It then invokes the global path planner to replan a global path path = {p'} from the current pose to the target navigation point, containing m path points. j} j＝1,2,3,...,a,...,b,...,m , where p′ j This represents the pose of the j-th path point in the global path. The replanned global path will be closer to the area in the narrow environment that is relatively central to the obstacles on both sides.
[0015] 1.2.2) Optimize the path using b-spline curve fitting to obtain a smoother global path. opt ={p j} j＝1,2,3,...,a,...,b,...,m , where p j This represents the pose of the j-th path point in the optimized global path;
[0016] 1.2.3) Calculate set width ={width j} j＝1,2,3,...,a,...,b,...,m , where set width for path opt The set of widths of the path points, widthj It is a path opt p in j The width of the surrounding environment is calculated by finding the width. j First definition point o j1 With the second definition point o j2 And calculate the distance between the two points: in the global cost map, at map resolution r map Given the radius step size and initial radius, γ narrow For the maximum search radius, p j The search for obstacle points is performed by expanding the circle outwards from the center step by step. The search stops after the first obstacle point is found, and that point is used as the width. j First definition point o j1 If no obstacle point is found within the circular area of the maximum search radius, then the width will be adjusted. j Set to the maximum value max; search for the first defined point o. j1 Then, along the vector Direction with r map Find the first obstacle encountered for the step size and use that point as the width. j The second definition point o j2 ;calculate in and They represent o respectively j1 x and y coordinates and They represent point o respectively j2 x and y coordinates;
[0017] 1.2.4)set width After the calculation is complete, search the set in order. width When the search reaches the a-th width a First time the width is satisfied a <γ narrow When, the width a First definition point o a1 Second definition point o a2 The midpoint of the position is used as the STP start The position, perpendicular to the vector and the direction vector The direction as STP start Direction; find width a Then from set width The search begins at the (a+1)th item, and continues until the bth width is found. b First time the width is satisfied b >γ narrow At that time, p bPosition as STP end The position, perpendicular to the vector and the direction vector The direction as STP end The direction, where o b1 It is width b The first definition point; if set width The search was completed but no width matching the criteria was found. a or width b If so, the appropriate passage pose is considered to have failed to generate STPs;
[0018] 1.2.5) If STPs are successfully generated, their locations are locally optimized based on the global cost map: using STPs start Centered on the circle, vector The line in question serves as the dividing line, STP start The opposite direction is the orientation, and the robot chassis radius r robot A semi-circular search range is formed with a radius, and the point with the minimum cost is searched within this range. After the search is completed, n′ points c that satisfy the conditions are found. i′ Form a point set cost ={c i′} i′＝1,2,3,...,n′ Calculate the vectors sequentially direction and angular difference, of which It is STP start The direction will be used to determine the location of the point that minimizes the angle difference, which will be the STP. start Optimized location; using STP end Centered on the circle, vector The line in question serves as the dividing line, STP end The direction is the orientation, r robot A semi-circular search range is formed with a radius, and the point with the minimum cost is searched within this range. After the search is completed, n” points c that satisfy the conditions are found. i” Form a point set cost ′={c i”} i”＝1,2,3,...,n” Calculate the vectors sequentially direction and angular difference, of which It is STP end The direction will be used to determine the position of the point that minimizes the angle difference, which will be the STP. end The optimized location.
[0019] Furthermore, the specific steps executed during the navigation strategy switching process are as follows:
[0020] 2.1) When the robot performs indoor navigation tasks, it operates at a preset frequency (rate).judg Implement a method for determining entry into confined environments;
[0021] 2.2) When the judgment result of the narrow environment entry judgment method is true, the narrow environment entry judgment method is paused and the navigation strategy is switched to the narrow environment navigation strategy. This strategy includes: switching the navigation parameters to the narrow environment navigation parameter configuration, and changing the original navigation task to a multi-point navigation task that navigates sequentially to the appropriate starting point pose and the appropriate ending point pose corresponding to the navigation failure pose of the judgment result.
[0022] 2.3) Upon reaching the destination of the appropriate travel pose pair, the navigation strategy switches to the normal navigation strategy. This strategy includes: switching the navigation parameters to the normal navigation parameter configuration, resuming the original navigation task, i.e., navigating from the current pose to the original target navigation point, and continuing according to the rate. judg Frequency-based narrow environment entry judgment method.
[0023] Furthermore, in step 2.1), the method for determining whether a robot has entered a narrow environment is based on the distance between the current pose and the navigation failure pose, as well as the degree of matching between the current global path and the suitable passage pose corresponding to the navigation failure pose. The specific steps are as follows:
[0024] 2.1.1) Calculate the navigation strategy information set sequentially. strategy The distance between all failed navigation poses (NFPs) and the current pose is calculated, and this distance is set to be less than a preset threshold γ. dist This condition is used to add NFP to the candidate point set. candidate ={NFP k} k＝1,2,3,...,q NFP k Let q represent the k-th NFP that satisfies the above conditions, and let q represent the total number of NFPs that satisfy the above conditions.
[0025] 2.1.2) Calculate set sequentially candidate Each NFP k The corresponding appropriate passage pose pairs for STPs k The current global path containing m′ path points matching degree d k ,in Indicates path cur The first in The path points are calculated as follows: i) sequentially in path... cur Finding the first satisfaction and Distance less And the angle difference is less than Path points Let the distance and angle difference be denoted as . and in STPs k The corresponding appropriate starting point pose; if the path cur If none of the path points in the equation satisfy the conditions described in i), then the matching degree d k =0; ii) If it exists Then from Continue searching for the first satisfaction and Distance less And the angle difference is less than Path points in STPs k The corresponding appropriate endpoint pose is denoted as follows: the distance and angle difference are recorded as follows. and If path cur If none of the path points in the equation satisfy the conditions described in ii), then the matching degree d k =0;iii) If and If both exist, then
[0026] 2.1.3) Find the maximum matching degree d e =max{d k} k＝1,2,3,...,q If d e If the result is greater than 0, the result is true, and (NFP) is returned. e STPs e ), where STPs e It is used to calculate the maximum matching degree d e Appropriate passage pose pair, NFP e It is STPs e The corresponding navigation failure pose; all other cases will result in a false judgment.
[0027] Furthermore, in step 2.2), the specific configuration of the navigation parameters in the narrow environment is as follows:
[0028] Navigation parameter configuration in confined environments includes local trajectory planner parameter configuration and global cost map parameter configuration; the confined environment navigation parameter configuration reduces the minimum obstacle distance to the robot chassis radius r. robot Reducing the sampling point density of the trajectory feasibility check makes the collision detection of the local trajectory planner more lenient, and setting the expansion radius of the global cost map to γ narrow To reduce the maximum length of the global path subset to less than 1 meter, making the trajectory generation closer to the center of the road, the linear velocity and linear acceleration are reduced, while the angular velocity and angular acceleration are increased, making the robot more sensitive to adjusting angles in narrow environments.
[0029] Compared with the prior art, the present invention has the following advantages and beneficial effects:
[0030] 1. This invention does not rely on the detection of specific features to identify special narrow environments such as doors or corridors, and is applicable to any narrow environment in the scene, thus having better versatility.
[0031] 2. This invention enables the robot to automatically generate appropriate passage pose pairs for entrances and exits in narrow environments based on environmental information through the navigation strategy information generation process. Compared with manually marked additional navigation waypoints used to assist the robot in passing through narrow environments, this invention has better accuracy and feasibility, and eliminates the step of manual marking.
[0032] 3. This invention enables the robot to automatically adapt to narrow environments by changing its navigation strategy during navigation through a navigation strategy switching process, and it can pass through even narrower environments.
[0033] 4. The method of this invention uses a more lenient collision detection standard for navigation parameters in narrow environments, enabling the robot to ignore collision misjudgments caused by error sources such as map accuracy errors and sensor noise when encountering narrow environments, and to drive more smoothly. The risk of increased collision probability due to this parameter configuration is avoided by generating a local trajectory closer to the middle of the road and following appropriate passage poses for segmented navigation when the robot passes through narrow environments. Experimental results in real-world environments show that the success rate of the robot navigating through narrow environments using the method of this invention is significantly improved compared to methods such as simple parameter adjustment and manual marking of auxiliary navigation points. Attached Figure Description
[0034] Figure 1 This is an overall flowchart of the method of the present invention.
[0035] Figure 2 A schematic diagram of the robot, experimental scenario, and navigation task provided in the visualization tool rviz for an embodiment of the method of the present invention.
[0036] Figure 3 This is a schematic diagram illustrating the process of generating suitable travel pose pairs in the navigation strategy information generation stage of the present invention.
[0037] Figure 4 This is a flowchart of the narrow environment entry judgment method in the navigation strategy switching stage of the present invention. Detailed Implementation
[0038] The present invention will be further described below with reference to specific embodiments.
[0039] This embodiment provides a method for an indoor robot navigation system to adapt to confined environments. The overall process is as follows: Figure 1As shown, this embodiment includes a navigation strategy information generation stage for adaptive narrow environments and a navigation strategy switching stage. A two-wheeled differential drive robot with a body radius of 13cm is used to construct an experimental scenario containing a narrow passage C with a width of 35cm. The navigation task is for the robot to move from starting point A through the narrow passage to the target navigation point B. The robot, experimental scenario, and navigation task are as follows: Figure 2 As shown.
[0040] In the method for adapting the robot's indoor navigation system to a narrow environment, the auxiliary indoor navigation system is the Navigation Stack navigation system under the ROS robot operating system, which consists of five modules: a map service module, an environment perception module, an autonomous localization module, a path planning module, and a motion control module. The workflow of the navigation system is as follows: the map service module loads the global cost map; the environment perception module acquires data of the surrounding environment through the robot's sensors; and the autonomous localization module calculates the robot's current pose at a certain frequency based on the data acquired by the environment perception module. When a target navigation point is received, the navigation task begins: the global path planner in the path planning module calculates the optimal global path from the current pose to the target navigation point at a certain frequency based on the global cost map; the motion control module updates the local cost map based on the data acquired by the environment perception module, and the local trajectory planner calculates the trajectory that best conforms to the global path based on the local cost map at a certain frequency and issues corresponding speed control commands to control the robot to move along the trajectory; the above five modules work together until the robot reaches the target navigation point.
[0041] After the navigation task begins, the method for the robot's indoor navigation system to adapt to narrow environments runs in the background. The navigation strategy information generation stage awaits a trigger opportunity, and the navigation strategy switching stage operates at a preset frequency. judg In this embodiment, a rate is set to determine the entry into a confined environment. judg =2Hz; When the robot travels from starting point A to the vicinity of the narrow passage C intersection, it exhibits oscillating behavior. At this time, the navigation strategy information generation stage starts working, collecting navigation failure poses (NFPs) caused by the narrow environment, generating appropriate passage pose pairs (STPs) corresponding to the NFPs, and forming navigation strategy information for use by the navigation strategy switching stage. The specific details of the navigation strategy information generation stage are as follows:
[0042] Navigation strategy information is a set consisting of n NFPs and STPs generated from them, denoted as set. strategy ={(NFP i STPs i )} i＝1,2,3,...,n Among them, NFP i STPs represent the i-th failed navigation pose found.i According to NFP i The generated appropriate passage pose pairs, STPs are generated from the appropriate passage start pose STP. start and appropriate passage end position STP end Composition, represented as STPs = (STP start STP end The pose (x, y, θ) is composed of the robot's horizontal coordinate x, vertical coordinate y, and orientation θ in the map.
[0043] The navigation strategy information generation process is implemented through a custom navigation recovery behavior, as detailed below:
[0044] 1.1) Pre-set the triggering time for navigation recovery behavior, which includes the following three situations: continuous failure of global path planning, continuous failure of local trajectory planning, and oscillation phenomenon in which the robot moves back and forth between two positions;
[0045] 1.2) When the robot encounters one of the three situations described in step 1.1) during the indoor navigation task, the navigation task is paused and the navigation recovery behavior is executed: the appropriate passage pose pair generation method is executed. If STPs are successfully generated, the current pose is considered to be a navigation failure pose caused by the narrow environment. The NFP and STPs are written into the navigation strategy information, and the updated navigation strategy information is loaded into the navigation strategy switching stage. Then, the robot continues to try to execute the original navigation task. If the generation of STPs fails, it is considered that the robot's three situations described in step 1.1) are not caused by the narrow environment, and the navigation task fails.
[0046] The method for generating suitable passage pose pairs references the current global cost map and pose, and attempts to generate STPs at the entrance of potentially narrow environments ahead. start STP is generated at the exit. end STP start and STP end To get closer to the middle of the road, follow these steps:
[0047] 1.2.1) Modify the expansion radius of the global cost map in the navigation parameters to the preset narrowing threshold γ. narrow In this embodiment, γ narrow Set the value to 50cm, and call the global path planner to replan a global path path = {p'} from the current pose to the target navigation point, containing m path points. j} j＝1,2,3,...,a,...,b,...,m , where p′ j This represents the pose of the j-th path point in the global path. The replanned global path will be closer to the area in the narrow environment that is relatively central to the obstacles on both sides.
[0048] 1.2.2) Optimize the path using b-spline curve fitting to obtain a smoother global path. opt ={p j} j＝1,2,3,...,a,...,b,...,m , where p j This represents the pose of the j-th path point in the optimized global path;
[0049] 1.2.3) Calculate set width ={width j} j＝1,2,3,...,a,...,b,...,m , where set width for path opt The set of widths of the path points, width j It is a path opt p in j The width of the surrounding environment is calculated by finding the width. j First definition point o j1 With the second definition point o j2 And calculate the distance between the two points: in the global cost map, at map resolution r map Given the radius step size and initial radius, γ narrow For the maximum search radius, p j The search for obstacle points is performed by expanding the circle outwards from the center step by step. The search stops after the first obstacle point is found, and that point is used as the width. j First definition point o j1 If no obstacle point is found within the circular area of the maximum search radius, then the width will be adjusted. j Set to the maximum value max; search for the first defined point o. j1 Then, along the vector Direction with r map Find the first obstacle encountered for the step size and use that point as the width. j The second definition point o j2 ;calculate in and They represent o respectively j1 x and y coordinates and They represent point o respectively j2 x and y coordinates;
[0050] 1.2.4)set width After the calculation is complete, search the set in order. width When the search reaches the a-th width a First time the width is satisfied a <γ narrow When, the widtha First definition point o a1 Second definition point o a2 The midpoint of the position is used as the STP start The position, perpendicular to the vector and the direction vector The direction as STP start Direction; find width a Then from set width The search begins at the (a+1)th item, and continues until the bth width is found. b First time the width is satisfied b >γ narrow At that time, p b Position as STP end The position, perpendicular to the vector and the direction vector The direction as STP end The direction, where o b1 It is width b The first definition point, the generated STPs are as follows Figure 3 As shown in c; if set width The search was completed but no width matching the criteria was found. a or width b If so, the appropriate passage pose is considered to have failed to generate STPs;
[0051] 1.2.5) If STPs are successfully generated, their locations are locally optimized based on the global cost map: using STPs start Centered on the circle, vector The line in question serves as the dividing line, STP start The opposite direction is the orientation, and the robot chassis radius r robot A semi-circular search range is formed with a radius, and the point with the minimum cost is searched within this range. After the search is completed, n′ points c that satisfy the conditions are found. i′ Form a point set cost ={c i′} i′＝1,2,3,...,n′ Calculate the vectors sequentially direction and angular difference, of which It is STP start The direction will be used to determine the location of the point that minimizes the angle difference, which will be the STP. start Optimized location; using STP end Centered on the circle, vector The line in question serves as the dividing line, STP end The direction is the orientation, r robotA semi-circular search range is formed with a radius, and the point with the minimum cost is searched within this range. After the search is completed, n” points c that satisfy the conditions are found. i” Form a point set cost ′={c i”} i”＝1,2,3,...,n” Calculate the vectors sequentially direction and angular difference, of which It is STP end The direction will be used to determine the position of the point that minimizes the angle difference, which will be the STP. end Optimized positions, optimized STPs, such as Figure 3 As shown in d.
[0052] As soon as the navigation strategy information generation phase is completed, the navigation strategy information is updated and loaded into the navigation strategy switching phase. The robot continues to attempt to execute the original navigation task, and the navigation strategy switching phase continues at a set frequency. judg The specific steps of the navigation strategy switching process in the narrow environment entry determination method are as follows:
[0053] 2.1) When the robot performs indoor navigation tasks, according to the frequency (rate) judg Implement a method for determining entry into confined environments;
[0054] 2.2) When the judgment result of the narrow environment entry judgment method is true, the narrow environment entry judgment method is paused and the navigation strategy is switched to the narrow environment navigation strategy. This strategy includes: switching the navigation parameters to the narrow environment navigation parameter configuration, and changing the original navigation task to a multi-point navigation task that navigates sequentially to the appropriate starting point pose and the appropriate ending point pose corresponding to the navigation failure pose of the judgment result.
[0055] 2.3) Upon reaching the destination of the appropriate travel pose pair, the navigation strategy switches to the normal navigation strategy. This strategy includes: switching the navigation parameters to the normal navigation parameter configuration, resuming the original navigation task, i.e., navigating from the current pose to the original target navigation point, and continuing to navigate at the specified frequency. judg Implement a method for determining entry into a confined environment.
[0056] The process of the method for determining entry into a narrow environment is as follows: Figure 4 As shown, it determines whether the robot has entered a narrow environment based on the distance between the current pose and the navigation failure pose, as well as the degree of matching between the current global path and the appropriate passable pose corresponding to the navigation failure pose. The specific steps are as follows:
[0057] 2.1.1) Calculate the navigation strategy information set sequentially. strategy The distance between all failed navigation poses (NFPs) and the current pose is calculated, and this distance is set to be less than a preset threshold γ.dist This condition is used to add NFP to the candidate point set. candidate ={NFP k} k＝1,2,3,...,q NFP k Let q represent the k-th NFP that satisfies the above conditions, and let q represent the total number of NFPs that satisfy the above conditions.
[0058] 2.1.2) Calculate set sequentially candidate Each NFP k The corresponding appropriate passage pose pairs for STPs k The current global path containing m′ path points matching degree d k ,in Indicates path cur The first in The path points are calculated as follows: i) sequentially in path... cur Finding the first satisfaction and Distance less And the angle difference is less than Path points Let the distance and angle difference be denoted as . and in STPs k The corresponding appropriate starting point pose; if the path cur If none of the path points in the equation satisfy the conditions described in i), then the matching degree d k =0; ii) If it exists Then from Continue searching for the first satisfaction and Distance less And the angle difference is less than Path points in STPs k The corresponding appropriate endpoint pose is denoted as follows: the distance and angle difference are recorded as follows. and If path cur If none of the path points in the equation satisfy the conditions described in ii), then the matching degree d k =0;iii) If and If both exist, then
[0059] 2.1.3) Find the maximum matching degree d e =max{d k} k＝1,2,3,...,q If d e If the result is greater than 0, the result is true, and (NFP) is returned.e STPs e ), where STPs e It is used to calculate the maximum matching degree d e Appropriate passage pose pair, NFP e It is STPs e The corresponding navigation failure pose; all other cases will result in a false judgment.
[0060] At this point, the narrow environment entry judgment method determines that the result is true. The navigation strategy switching process pauses the narrow environment entry judgment method and switches to the narrow environment navigation strategy: the robot's navigation parameters are changed to the narrow environment navigation parameter configuration. The original navigation task from point A to point B is changed to sequentially navigating to the navigation failure pose (NFP) corresponding to the judgment result. e Appropriate starting point posture Appropriate passage end position Multi-point navigation task; the robot first navigates to That is, adjust your posture to enter the narrow environment, and then move along the trajectory in the middle of the road in the narrow environment. Next, the navigation strategy is switched to the normal navigation strategy: the navigation parameters are switched to the normal navigation parameter configuration, and the original navigation task is resumed, that is, the robot navigates to the original target point B and continues to navigate at the rate. judg Implement a method for determining entry into a confined environment.
[0061] The specific details of the navigation parameter configuration in the narrow environment are as follows:
[0062] Navigation parameter configuration in confined environments includes local trajectory planner parameter configuration and global cost map parameter configuration. Compared to normal navigation parameter configuration, confined environment navigation parameter configuration reduces the minimum obstacle distance to the robot chassis radius r. robot Reducing the sampling point density of the trajectory feasibility check makes the collision detection of the local trajectory planner more lenient, and setting the expansion radius of the global cost map to γ narr To make the trajectory generation closer to the center of the road, the maximum length of the global path subset is reduced to less than 1 meter. The linear velocity and linear acceleration are reduced, while the angular velocity and angular acceleration are increased to make the robot more sensitive to adjusting its angle in narrow environments.
[0063] When the robot needs to pass through the narrow passage C described in this embodiment again while performing other navigation tasks, the narrow environment entry judgment method will determine it as true and return (NFP). e STPs e During the navigation strategy switching phase, a narrow navigation strategy will be implemented to help the robot pass through the narrow passage C smoothly.
[0064] In summary, the method of this invention enables robots to adapt to narrow environments by automatically switching navigation strategies without human intervention. This improves the success rate and smoothness of the robot's autonomous passage through narrow environments where the width is slightly larger than the robot's body width, and reduces the minimum passable width of narrow environments. It is applicable to any narrow environment in the scenario, has better versatility, and has broad research and practical application value, making it worthy of promotion.
[0065] The above-described embodiments are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Therefore, any changes made in accordance with the shape and principle of the present invention should be covered within the protection scope of the present invention.
Claims
1. A method for an indoor robot navigation system to adapt to confined environments, characterized in that, include: Navigation strategy information generation stage: During the robot's indoor navigation task, it collects navigation failure poses caused by confined spaces. , generation and Corresponding appropriate passage pose pair This generates navigation strategy information for use during navigation strategy switching. Navigation strategy switching phase: During the robot's indoor navigation task, the timing for switching to a narrow environment navigation strategy is determined based on the navigation strategy information provided in the navigation strategy information generation phase, and the strategy is executed. This includes switching the narrow environment navigation parameter configuration and executing multi-point navigation tasks to assist the robot in safely navigating narrow environments. After passing through the narrow environment, the robot switches to the normal navigation strategy, which includes restoring the normal navigation parameter configuration and resuming the original navigation task. The specific steps are as follows: 2.1) When the robot performs indoor navigation tasks, it operates at a preset frequency. A method for determining whether a robot has entered a narrow environment is implemented. This method determines whether the robot has entered a narrow environment based on the distance between the current pose and the navigation failure pose, as well as the degree of matching between the current global path and the suitable passable pose corresponding to the navigation failure pose. The specific steps are as follows: 2.1.1) Calculate navigation strategy information sequentially. All navigation failure poses The distance from the current pose is set, and this distance is less than a preset threshold. This condition Add to candidate point set ,in This indicates that the first one that satisfies the above conditions indivual , Indicates that there are a total of One that meets the above conditions ; 2.1.2) Calculate sequentially Each Corresponding appropriate passage pose pair With The current global path of each path point Matching degree ,in express The first in There are path points, calculated as follows: i) sequentially in... Finding the first satisfaction and Distance less And the angle difference is less than Path points Let the distance and angle difference be denoted as . and ,in express The corresponding appropriate starting point pose; if If none of the path points in the equation satisfy the conditions described in i), then the matching degree is... ;ii) If it exists Then from Continue searching for the first satisfaction and Distance less And the angle difference is less than Path points ,in express The corresponding appropriate endpoint pose is denoted as follows: the distance and angle difference are recorded as follows. and ,like If none of the path points in the equation satisfy the conditions described in section ii), then the matching degree is... ;iii) If and If both exist, then ; 2.1.3) Find the maximum matching degree ,like If the result is true, then return true. ,in It is used to calculate the maximum matching degree. The appropriate passage posture yes The corresponding navigation failure pose; all other cases will result in a false judgment. 2.2) When the judgment result of the narrow environment entry judgment method is true, the narrow environment entry judgment method is paused and the navigation strategy is switched to the narrow environment navigation strategy. This strategy includes: switching the navigation parameters to the narrow environment navigation parameter configuration, and changing the original navigation task to a multi-point navigation task that navigates sequentially to the appropriate starting point pose and the appropriate ending point pose corresponding to the navigation failure pose of the judgment result. 2.3) Upon reaching the endpoint of the appropriate travel pose pair, the navigation strategy switches to the normal navigation strategy. This strategy includes: switching the navigation parameters to the normal navigation parameter configuration, resuming the original navigation task, i.e., navigating from the current pose to the original target navigation point, and continuing according to... Frequency-based narrow environment entry judgment method.
2. The method for an indoor robot navigation system to adapt to confined environments according to claim 1, characterized in that, The indoor navigation system assisted by this method is the Navigation Stack navigation system under the ROS robot operating system, which consists of five modules: a map service module, an environment perception module, an autonomous localization module, a path planning module, and a motion control module. The workflow of the indoor navigation system is as follows: the map service module loads the global cost map; the environment perception module acquires data of the surrounding environment through the robot's sensors; the autonomous localization module calculates the robot's current pose at a set frequency based on the data acquired by the environment perception module, and when a target navigation point is received, it begins to execute the navigation task; the global path planner in the path planning module calculates the optimal global path from the current pose to the target navigation point at a set frequency based on the global cost map; the motion control module updates the local cost map based on the data acquired by the environment perception module, and the local trajectory planner calculates a trajectory that conforms as closely as possible to the global path based on the local cost map at a set frequency and issues corresponding speed control commands to control the robot to move along the trajectory; the above five modules work together until the robot reaches the target navigation point.
3. The method for an indoor robot navigation system to adapt to confined environments according to claim 2, characterized in that, The navigation strategy information is from indivual and based on its generation The set formed is represented as ,in, Indicates the found number Navigation failure pose Indicates according to The generated appropriate passage pose pair From the appropriate starting point position and appropriate passage end position Composition, represented as The position of The robot's x-coordinate on the map y-axis and direction constitute.
4. The method for an indoor robot navigation system to adapt to narrow environments according to claim 3, characterized in that, The navigation strategy information generation process is implemented through navigation recovery behavior, as detailed below: 1.1) Pre-set the triggering time for navigation recovery behavior, which includes the following three situations: continuous failure of global path planning, continuous failure of local trajectory planning, and oscillation phenomenon in which the robot moves back and forth between two positions; 1.2) When the robot encounters one of the three situations described in step 1.1) during indoor navigation, the navigation task is paused, and navigation recovery behavior is executed: a suitable passage pose pair generation method is performed, and when successfully generated... Then, the current pose is considered a navigation failure pose due to a confined environment, and... and Write navigation strategy information and load the updated navigation strategy information into the navigation strategy switching phase, then continue to attempt to execute the original navigation task; when generated If the navigation task fails, it is assumed that the three situations described in step 1.1) are not caused by the narrow environment.
5. A method for an indoor robot navigation system to adapt to confined environments according to claim 4, characterized in that, In step 1.2), the suitable passage pose generation method refers to the current global cost map and pose, and attempts to generate a suitable passage pose at the entrance of a potentially narrow environment ahead. Generated at the exit ,in and To get closer to the middle of the road, follow these steps: 1.2.1) Modify the expansion radius of the global cost map in the navigation parameters to the preset narrowing threshold. It then invokes the global path planner to replan a path from the current pose to the target navigation point, containing... Global path of each path point ,in Indicates the first in the global path The poses of each path point, and the replanned global path. Approach the area closest to the obstacles on either side of the narrow environment; 1.2.2) Optimization using b-spline curve fitting method This results in a smoother global path. ,in Indicates the first position in the optimized global path The pose of each path point; 1.2.3) Calculation ,in for The set of widths of the path points in the middle. yes In The width of the surrounding environment is calculated by finding... First definition point With the second definition point And calculate the distance between the two points: in the global cost map, at map resolution Given the radius step size and initial radius, To maximize the search radius, The search for obstacle points is conducted by gradually expanding the circular area outward from the center. The search stops after the first obstacle point is found, and that point is designated as the obstacle point. First definition point If no obstacle point is found within the circular area of the maximum search radius, then... Set to maximum value The first definition point was found. Then, along the vector direction The first obstacle encountered is searched for by step size, and that point is used as... Second definition point ;calculate ,in and They represent of coordinates and coordinate, and Representing points respectively of coordinates and coordinate; 1.2.4) After the calculation is complete, search in sequence. When the search results are found width First satisfaction At that time, First definition point Second definition point The midpoint of the position as The position, perpendicular to the vector and the direction vector direction as Direction; find Later from The The search begins with item 1, and when the search reaches item 2... width First satisfaction At that time, Position as The position, perpendicular to the vector and the direction vector direction as The direction, among which yes The first definition point; if The search was completed but no results were found that met the criteria. or If so, then the appropriate passage posture is considered to be correct. Generation failed; 1.2.5) If If generation is successful, its location will be locally optimized based on the global cost map: Centered on the circle, vector The line in question serves as the dividing line. The opposite direction is the orientation, and the radius of the robot chassis. A semi-circular search range is formed with a radius, and the point with the minimum search cost is found within this range. After the search is completed, the point is found. Points that meet the conditions Forming a point set Calculate the vectors sequentially direction and angular difference, of which yes The direction will be used to determine the position of the point that minimizes the angle difference. Optimized location; with Centered on the circle, vector The line in question serves as the dividing line. The direction is the orientation. A semi-circular search range is formed with a radius, and the point with the minimum search cost is found within this range. After the search is completed, the point is found. Points that meet the conditions Forming a point set Calculate the vectors sequentially direction and angular difference, of which yes The direction will be used to determine the position of the point that minimizes the angle difference. The optimized location.
6. A method for an indoor robot navigation system to adapt to confined environments according to claim 5, characterized in that, In step 2.2), the specific configuration of the navigation parameters in the narrow environment is as follows: Navigation parameter configuration in confined environments includes local trajectory planner parameter configuration and global cost map parameter configuration; the confined environment navigation parameter configuration reduces the minimum obstacle distance to the robot chassis radius. Reducing the sampling point density of the trajectory feasibility check makes the collision detection of the local trajectory planner more lenient, and setting the expansion radius of the global cost map to [missing information]. To reduce the maximum length of the global path subset to less than 1 meter, making the trajectory generation closer to the center of the road, the linear velocity and linear acceleration are reduced, while the angular velocity and angular acceleration are increased, making the robot more sensitive to adjusting angles in narrow environments.