Internal constraint mobile robot navigation method integrating visual perception and speed optimization

Abstract

The invention provides an internal constraint mobile robot navigation method fusing visual perception and speed optimization, and aims to realize autonomous navigation of a mobile robot in a scene with a U-shaped obstacle through multi-modal perception. Firstly, a laser radar is used to construct a two-dimensional map; secondly, based on a global path and two-stage speed optimization window filtering algorithm, a speed gradient thought and a dynamic self-adaptive internal constraint TEB navigation algorithm, on the premise that the system is kept stable, the navigation speed of the mobile robot is increased, and the mobile robot can smoothly pass through the interior of the U-shaped obstacle; and finally, combining a post-processing rule in a visual perception part, and proposing an orientation sensitive feature enhancement mechanism to obtain a passable signal, so as to improve the perception capability in a special environment. Experimental verification shows that the method has remarkable advantages in the aspects of feasibility and stability, the task success rate is increased by 50% compared with a traditional method, the average navigation time consumption is reduced by 20%, and the all-weather operation requirement of the mobile robot in a dynamic environment can be effectively met.

Description

Technical Field

[0001] This invention belongs to the field of intelligent perception and motion planning applications for mobile robots, particularly sensor detection technology, machine learning and path planning technology in the process of mobile robot navigation, and especially relates to an internal constraint mobile robot navigation method that integrates visual perception and speed optimization. Background Technology

[0002] With the rapid development of intelligent manufacturing and intelligent power distribution system equipment manufacturing, Automated Guided Vehicles (AGVs), Laser Guided Vehicles (LGVs), industrial robots, special-purpose robots, and intelligent unmanned aerial vehicles have been widely used. This has also stimulated the development of artificial intelligence fields such as intelligent sensing and control equipment and computer vision technology. Laser measuring instruments are diverse, especially in radar and related equipment manufacturing. Equipping mobile robots with suitable models can significantly improve mapping and positioning efficiency, particularly in special operational scenarios such as intelligent power distribution systems. To avoid collisions between multiple mobile robots caused by the instability of a single sensor, resulting in low efficiency in quality, safety, and environmental inspection and testing services, this invention introduces a visual sensor combined with an orientation-sensitive feature enhancement mechanism to further improve the system's robustness.

[0003] Chinese patent CN 202210761400.9, "Improved TEB Navigation Method for Mobile Robots Applicable to Narrow Spaces," proposes a 7-segment S-shaped acceleration / deceleration TEB algorithm based on pose auxiliary points and velocity interpolation for navigating narrow spaces. Although it constructs constraints for proximity to obstacles and distance from the global path, both constraints are calculated using the straight-line distance between the global path point and the obstacle, neglecting the impact of the mobile robot's distance from the global path itself. Furthermore, the fixed-point path planning algorithm uses pose auxiliary points as a state machine for switching between points; if the deviation is large, it will continuously adjust in place, resulting in a high computational load. This approach neglects the real-time requirements of practical applications.

[0004] Chinese patent CN 202410625900.9, "An AGV Path Planning Method Integrating Improved JPS and TEB Algorithms," proposes a JPS algorithm that uses a dynamic tangent point adjustment method to process turning points on the global static path to generate a global path. The local path planning incorporates a TEB algorithm that integrates shortest distance constraints, adds critical corner speed constraints, uses the AGSRDOA strategy, expands path smoothness constraints, and employs intelligent path tracking and control optimization strategies to complete the navigation task. However, it fails to consider the impact of the algorithm's effect on the robot's mechanical structure due to the robot's actual speed, and it also neglects timeliness.

[0005] The paper "Research on 3D Mapping and Navigation of Mobile Robots Fusion IMU / LiDAR" proposes a D* algorithm based on the obstacle distance influence coefficient ρ and the four-point gradient descent method for dynamic obstacle avoidance, combined with the TEB path planning algorithm to complete the navigation of the mobile robot. During the robot's navigation, cylinders and blocks are randomly added at irregular intervals to test the dynamic obstacle avoidance effect. However, this method only considers navigation under the presence of conventional obstacles and does not take into account the actual working scenario where the robot needs to perform inspection operations at the power cabinet in a U-shaped scene. Therefore, it still suffers from situations where the robot gets trapped in local minima and cannot escape the obstacle.

[0006] The papers "Research on Autonomous Navigation Technology of Mobile Robot Based on Laser SLAM" and "Research on Navigation Algorithm of Unmanned Delivery Vehicle Based on ROS" both start by discussing the advantages and disadvantages of the A* algorithm and use the DWA algorithm for local path planning. The paper "Research on Localization and Navigation of Omnidirectional Mobile Robot Based on Laser and Vision Sensors" uses the same localization algorithm, but does not describe the path planning method. Furthermore, none of the above three papers consider the extreme operation situation of U-shaped obstacles, which may lead to danger in navigation tasks.

[0007] Conventional studies often construct large obstacle constraints for real-world indoor or outdoor work scenarios, neglecting situations where robots need to navigate U-shaped obstacles to complete tasks. In such situations, navigation instability can lead to dangerous situations for mobile robots. Furthermore, in multi-robot collaborative operations, priority passage must be considered to avoid collisions. This invention combines deep learning methods to identify passable signals and incorporates a post-processing rule-based location-sensitive feature enhancement mechanism to improve the accuracy and intelligence of the identification. By constructing dynamically adaptive local path planning constraints, and utilizing velocity window filtering algorithms and velocity gradient concepts, the impact of velocity jumps on the mechanical structure is reduced, smoothing the mobile robot's trajectory and speed, thus improving the robot's robustness in work scenarios. Summary of the Invention

[0008] To address the shortcomings of existing methods, this invention proposes an internal constraint-based mobile robot navigation method that integrates visual perception and speed optimization. This method focuses on resolving navigation failures in complex environments during autonomous navigation. First, it combines global path planning information with a two-level speed-optimized window filtering algorithm, a speed gradient approach, and a TEB local path planning and navigation algorithm with dynamic adaptive internal constraints. This avoids getting trapped in local minima when navigating U-shaped obstacles. The speed window filtering algorithm and speed gradient approach are combined to improve the mobile robot's operating speed. A location-sensitive feature enhancement mechanism combined with post-processing rules is proposed. A depth camera is used to identify traffic lights and provide feedback on passable signals, improving the mobile robot's accuracy in identifying passable signals and its high-speed and robust autonomous navigation in complex scenarios.

[0009] To achieve the above objectives, the present invention employs the following specific technical solution:

[0010] S1: Establish a practical application scenario and use LiDAR to acquire environmental information for 2D mapping. This includes the following sub-steps:

[0011] S1.1: Considering the high computational requirements of dynamic vision and the applicability of practical applications, a main control board with an AI computing power of 0.5 TFLOPS was selected as the core controller, which can simultaneously meet the requirements of high load operation and real-time performance in complex scenarios.

[0012] S1.2: The point cloud data collected by the LiDAR is published to the SLAM system. Environmental information is distinguished based on the grid status: occupied, unknown, and idle. By combining the LiDAR observation data and prior information, the grid status can be dynamically updated to determine the actual state of the grid. When an obstacle exists within the detection range of the LiDAR, the corresponding grid is set to occupied, commonly represented by black.

[0013] like Figure 3 The image shows how to acquire three-dimensional spatial information and convert it into a two-dimensional planar map for display.

[0014] S2: To improve the accuracy of passability signal recognition in S4, the environmental map obtained in S1 is used to match pre-initialized orientation factors with sensor data, including the initial position of the mobile robot and the target point. This includes the following sub-steps:

[0015] S2.1: Obtain the prior environment map information from S1.2 and input it into the positioning method;

[0016] S2.2: First, initialize particles within the feasible region of the known map, with each particle representing a possible pose of the robot.

[0017] Based on the robot's motion commands and the state at the previous moment, the state of each particle at the current moment is predicted. The weight of each particle is calculated using a sensor model, and the calculation formula is shown in equation (1):

[0018]

[0019] In the formula, Representing particle state Given a map m, the lidar observed z t The probability, where N represents the number of lidar observation points, i.e., the number of laser beams. This represents the observation probability of the k-th measurement point.

[0020] The particle weights are calculated and updated. To ensure that the sum of the particle weights is 1, normalization is performed. The particles are resampled according to their weights to reduce the influence of particles with large weight differences on the estimation results. The robot pose is estimated by calculating the weighted average of all particles. The calculation formula is shown in Equation (2).

[0021]

[0022] In the formula, This represents the weight of the i-th particle at time t. This represents the state of the i-th particle at time t. This indicates that the robot estimates its pose.

[0023] S2.3: Data transmission is achieved through node publishing and subscription based on the Robot Operating System (ROS). The localization algorithm in step S2.2 is encapsulated into a localization node. _pos The topic of publishing robot pose information in real time was published on the Topic. _pos The robot moves within a known map, subscribes to pose information topics in real time, and initializes the task target point.

[0024] S2.4: Based on step S2.3, obtain the task target point information of the mobile robot, decompose the single target point planning into a multi-target point planning problem, that is, divide the entire task into two states. State 1 requires the combination of traffic light recognition feedback signals to control the robot to complete autonomous navigation, while State 2 only considers the pure autonomous navigation problem. Real-time scheduling through task division is used to balance the robot's computing power and avoid excessive computing power causing message blocking and ROS node crashes. Its calculation formula is shown in equation (3):

[0025]

[0026] In the formula, q traffic This indicates that the navigation target point attribute will be initialized to the traffic light orientation factor attribute. Indicates the status of the mobile robot And autonomous navigation constraints p, S(t) red ,t green ) indicates that the mobile robot is in the red light time window t red Internal parking, green light time window t green internal traffic;

[0027] S3: The two states are distinguished only by whether traffic lights need to be recognized. Both use the A* algorithm, a window filtering algorithm based on two-level velocity optimization, and the TEB local path planning and navigation algorithm with dynamic adaptive internal constraints to complete the autonomous navigation task. The A* algorithm is used to generate the globally optimal path. The TEB navigation algorithm based on two-level velocity optimization and dynamic adaptive internal constraints enables the mobile robot to successfully pass through the U-shaped obstacle area. The velocity gradient concept is used to improve the speed of the mobile robot's autonomous navigation. Specifically, it includes the following sub-steps:

[0028] S3.1: Based on the initial point position and the target point position, the global planner generates the shortest path between the two points that does not pass through obstacles;

[0029] S3.2: Load the initial stage parameters of the TEB navigation algorithm based on two-stage velocity optimization window filtering algorithm, velocity gradient idea and dynamic adaptive internal constraints. The calculation formula is shown in equation (4):

[0030]

[0031] In the formula, l i ΔT represents the robot's pose, and ΔT represents the time interval between adjacent poses.

[0032] S3.3: Construct constraints f near obstacles obs and constraints f that are far from the global path path The calculation formulas are shown in equations (5) and (6):

[0033]

[0034] Where, d 1_min,j This represents the minimum distance between the j-th path point of the mobile robot and the obstacle. d represents the threshold range between the mobile robot and obstacles. 2_min,j This represents the minimum distance from the reference point to the j-th path point of the mobile robot. The threshold value for the mobile robot to stray from the path is represented by S, where S represents the scaling factor, n represents the exponential factor, and ε represents the buffer factor.

[0035] S3.4: If a U-shaped obstacle exists in the task scenario, normally obstacle constraints are constructed to keep the mobile robot away from the U-shaped area and prevent it from getting stuck. However, this method fails if navigation tasks need to be completed through U-shaped obstacles.

[0036] Therefore, based on the known map, the two longest sides of the U-shaped obstacle are selected as the base sides, and the calculation formula is shown in equation (7):

[0037]

[0038] Where x i Represents the maximum and minimum values ​​of the U-shaped obstacle in the x-axis of the map coordinate system, and y-axis... i This represents the maximum and minimum values ​​of the U-shaped obstacle along the y-axis of the map coordinate system.

[0039] The inscribed circle of the U-shaped obstacle is calculated using the formulas shown in equations (8) and (9):

[0040]

[0041] In the formula, r represents the radius of the inscribed circle, and (h,k) represents the coordinates of the center of the circle.

[0042] Construct internal constraints, starting from the center of the inscribed circle, first draw feasible paths extending along both sides of the x-axis. When the range of feasible paths coincides with... The outward extension stops when the threshold ranges intersect. Continue extending along the y-axis outwards from the U-shaped obstacle, entering the U-shaped obstacle in a counter-clockwise direction as the positive direction of expansion, and leaving the U-shaped obstacle in a clockwise direction as the positive direction of expansion. The expansion path reaches the y-axis. max Within the threshold range, a path constraint f serves as the entry point into the U-shaped obstacle. U_ipath A path constraint f as a way to leave the U-shaped obstacle U_opath ;

[0043] S3.5: Based on the original constraints, the TEB algorithm constraints are adjusted according to the adaptive dynamic factor. The calculation formulas are shown in equations (10), (11), and (12):

[0044] F = f obs +σ(α)·f path +[1-σ(α)]·{σ(β)·f u_ipath +[1-σ(β)]·f u_opath} (10)

[0045]

[0046] In the formula, α is the proximity of the U-shaped region, α0 is the activation threshold of the U-shaped obstacle, i.e., the point of entry into the U-shaped obstacle, β is the degree of movement inside the U-shaped obstacle, β0 is the switching point of entering or leaving the U-shaped obstacle, i.e., the geometric center position of the U-shaped obstacle, k1 represents the region switching sensitivity, and k2 represents the stage transition smoothness.

[0047] S3.6: The speed command generated by the navigation system adopts a two-level optimization. First, the Savitzky-Golay filter is applied to suppress noise and smooth the output of a continuous speed function. Then, a smooth speed profile conforming to an S-shaped curve is generated by the speed gradient increment algorithm based on jerk constraints to ensure the continuity of acceleration during motion. The calculation formulas are shown in Equations (13), (14) and (15):

[0048]

[0049] In the formula, h k (t) represents the time-varying filter coefficients, M is the filter half-window width, Δt is the sampling time interval, j(t) is the jerk, and t a To speed up the end time, t d For the deceleration start time, t f Let t be the time when the motion ends, a(t) be the acceleration, v(t) be the velocity, and s(t) be the path length.

[0050] S4: When navigating to the traffic light according to step S3, determine whether to continue navigation based on the feedback result. If the traffic light changes instantly, the decision is made based on the orientation-sensitive feature enhancement mechanism. If the distance threshold is exceeded, navigation continues. This includes the following sub-steps:

[0051] S4.1: To ensure that the mobile robot can still correctly respond to traffic light signals in different scenarios and save system computation, the visual detection module only provides feedback on two situations: red light (stop), green light (pass), and no signal detected (pass).

[0052] S4.2: Training is performed based on the steps in S4.1;

[0053] S4.3: Use the trained optimal weight model to identify and predict passable signals;

[0054] S4.4: Input the results identified in step S4.3 into the post-processing rule module for union determination;

[0055] S4.5: Based on the initial target point parameters in step S2.4, navigation stops when the situation is normal and a red light is detected. Considering the scenario where the light changes instantaneously from green to red after normal operation and passage, a location-sensitive feature enhancement mechanism is proposed. If the distance d exceeds a distance threshold d... maxIf the light changes, the navigation task continues, shortening the mobile robot's operation time. Distance is determined using the sum of squared Euclidean distances, calculated as shown in equation (16):

[0056] d=(N x -P x ) 2 +(N y -P y ) 2 (16)

[0057] In the formula, N x N represents the x-axis coordinate of the mobile robot's current position on the map in step S1. y P represents the y-coordinate of the mobile robot's current location on the map. x P represents the x-axis coordinate of the previous target point on the map. y This represents the y-axis coordinate of the previous target point on the map.

[0058] S5: To successfully navigate inside the U-shaped obstacle and correctly provide visual signals, the following sub-steps are included:

[0059] S5.1: Initialize the navigation points obtained in S2.4 into the initialization list, i.e. It includes two types of attributes: traffic light orientation factor attribute and ordinary navigation point factor attribute;

[0060] S5.2: Obtain the initial position of the mobile robot and update the starting point list, i.e., Start_List = {q0};

[0061] S5.3: Read the initialization list sequentially according to the preset order, and update the navigation point i (i = 1, 2, ..., l) to the target point list, i.e., Goal_List = {q1};

[0062] S5.4: Navigate to the target point according to step S3. If the navigation to the target point is successful, update the navigation point i (i = 1, 2, ..., l) to the completion point list, i.e. Finish_List = {q1}, and update the target point list to the starting point list, i.e. Start_List = {q1}.

[0063] S5.5: Clear the target point list; at this point, Goal_List = {}.

[0064] S5.6: If the target point attribute is a traffic light orientation factor attribute at this time, then complete the traffic light recognition according to step S4, and determine whether to continue the task based on the feedback signal until the green light condition is met or the red light condition is ignored.

[0065] S5.7: Continue traversing the initialization list and update the target point of the next order to the target point list, i.e., Goal_List = {q2};

[0066] S5.8: Repeat steps S5.4 to S5.6. Each successful navigation will sequentially add the current target point to the completion point list, i.e., Finish_List = {q1, q2, ..., q...} l};

[0067] S5.9: Navigation ends when the completed point list is the same as the initialization list, i.e., Finish_List = Init_List. At this point, the mobile robot has completed all navigation tasks.

[0068] The present invention has the following beneficial effects:

[0069] 1. This invention uses LiDAR and depth cameras as sensors to address extreme operational situations during mobile robot navigation, specifically the scenario where the robot becomes trapped and loses its localization when entering a U-shaped obstacle. By constructing an obstacle internal constraint strategy, the invention improves the engineering practicality of this method. A U-shaped obstacle region navigation method based on inscribed circle path expansion is proposed, combined with adaptive dynamic factor adjustment of constraints, successfully solving the failure problem of traditional obstacle avoidance algorithms in extreme scenarios. Experiments show that the mobile robot's path tracking deviation within the U-shaped region is ≤5cm, and the task success rate is improved by 50% compared to traditional methods, providing reliable navigation assurance for complex operational scenarios.

[0070] 2. A two-stage speed optimization strategy is adopted, with Savitzky-Golay filters suppressing noise and S-shaped speed gradient algorithm with jerk constraints generating a smooth speed profile. The peak value of linear velocity jump is reduced by 0.05. Under the premise of ensuring system stability, the shortest task time is shortened by 14 seconds and the longest task time is shortened by 35 seconds, significantly improving work efficiency.

[0071] 3. A location-sensitive feature enhancement mechanism is proposed. Combined with the YOLOv5 traffic light recognition post-processing rule that intelligently ignores invalid light-changing signals by determining the Euclidean distance threshold, the automatic passage strategy of exceeding the Euclidean distance threshold is used to shorten the task response time. The traffic light recognition confidence reaches 0.89, realizing robust passage decision-making in dynamic scenarios.

[0072] 4. By integrating grid map construction, Monte Carlo localization, global and local path planning, and visual perception modules, computing power is dynamically scheduled through a task state machine. Tests show that the number of successful attempts has increased from 4 to 10, and the worst-case time is still better than the optimal value without using the algorithm proposed in this invention, achieving all-weather operation capability in dynamic environments. Attached Figure Description

[0073] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0074] Figure 1 Flowchart of a navigation method for internally constrained mobile robots that integrates visual perception and speed optimization;

[0075] Figure 2 This is a diagram illustrating an actual navigation scenario for a mobile robot.

[0076] Figure 3 A diagram illustrating how a mobile robot can correctly recognize traffic light signals.

[0077] Figure 4 A diagram of a feasible path planned for a mobile robot in Rviz.

[0078] Figure 5 This image illustrates the actual navigation process of a mobile robot correctly recognizing traffic lights.

[0079] Figure 6 The image shows the effect of a mobile robot successfully passing through a navigation system when a green light turns red in an instant, thanks to a location-sensitive feature enhancement mechanism.

[0080] Figure 7 A comparison chart of navigation speed curves for a mobile robot navigating a U-shaped obstacle. Detailed Implementation

[0081] To make the above-mentioned objectives, features, and advantages of the present invention more apparent and understandable, the overall structure flowchart of the internal constraint mobile robot navigation method integrating visual perception and speed optimization is as follows: Figure 1 As shown, the external environment is first rasterized using the Gmapping algorithm, and the orientation factors, navigation target point, and initial position of the mobile robot are initialized using a localization algorithm. Global path planning information is input into a window filtering algorithm based on two-level velocity optimization, a velocity gradient approach, and a TEB local path planning and navigation algorithm with dynamic adaptive internal constraints. When the robot moves within the orientation factor range, traffic lights are identified using a visual recognition method that uses a Euclidean distance threshold. The system determines whether to continue navigation based on feedback communication information. If a passable signal is received, the TEB local path planning algorithm is invoked for autonomous navigation until the target point is reached, completing the autonomous navigation task. This includes the following steps:

[0082] S1: Establish a practical application scenario, utilize LiDAR to acquire environmental information, and process the 3D information into a 2D map. This includes the following sub-steps:

[0083] S1.1: Considering the high computational requirements of dynamic vision and the applicability of practical applications, after research and comparison, a main control board with an AI computing power of 0.5 TFLOPS was selected as the core controller, which can simultaneously meet the requirements of high load operation and real-time performance in complex scenarios.

[0084] S1.2: The point cloud data collected by the lidar is published to the SLAM system. The environmental information is distinguished according to the grid status, namely: occupied, unknown, and idle. By combining the observation data of the lidar and prior information, the grid status can be dynamically updated to determine the grid status. When there is an obstacle within the detection range of the lidar, the grid will be set to the occupied state, that is, black is commonly used to indicate that there is an obstacle in this grid. There are only two states in the known map: occupied and idle. The calculation formula is shown in Equation (1):

[0085]

[0086] In the formula, logOdd(s|z) represents the logarithm of the posterior probability of the raster state s=1 relative to s=0 after observing data z. LogOdd(s) represents the logarithm of the probability ratio of observed data z under raster states s=1 and s=0, and logOdd(s) represents the prior logarithm of the raster state s=1 relative to s=0 before the data z is observed.

[0087] like Figure 3 The image shows how to acquire three-dimensional spatial information and convert it into a two-dimensional planar map for display.

[0088] S2: To improve the accuracy of passability signal recognition in S4, the environmental map obtained in S1 is used to pre-initialize the orientation factor based on the Monte Carlo localization framework through sensor data matching. This includes the initial position of the mobile robot and the target point. Specifically, it includes the following sub-steps:

[0089] S2.1: Obtain the prior environment map information from S1.2 and input it into the positioning method;

[0090] S2.2: First, initialize particles within the feasible region of the known map. Each particle represents a possible pose of the robot, and its calculation formula is shown in equation (2):

[0091]

[0092] In the formula, P0 is the initial particle set. This represents the three-dimensional pose of the i-th particle. This indicates the initial weights, where M is the total number of particles.

[0093] Based on the robot's motion commands and the state at the previous moment, the state of each particle at the current moment is predicted, and the calculation formula is shown in equation (3):

[0094]

[0095] In the formula, Let x represent the state of the i-th particle at time t. t-1 [i] Let u represent the state of the i-th particle at time t-1. t ε represents the control input, and ε represents the random variable of motion noise.

[0096] The sensor model is used below to calculate the weight of each particle, and the calculation formula is shown in equation (4):

[0097]

[0098] In the formula, Representing particle state Given a map m, the lidar observed z t The probability, where N represents the number of lidar observation points, i.e., the number of laser beams. This represents the observation probability of the k-th measurement point.

[0099] The particle weights are updated by calculating the likelihood probability under the current observation, and the calculation formula is shown in equation (5):

[0100]

[0101] In the formula, This represents the weight of the i-th particle at time t.

[0102] To ensure that the sum of particle weights is 1, normalization is performed, and the calculation formula is shown in equation (6):

[0103]

[0104] The particle weights are resampled to reduce the influence of particles with large weight differences on the estimation results. The robot pose is estimated by calculating the weighted average of all particles, as shown in Equation (7).

[0105]

[0106] In the formula, This indicates that the robot estimates its pose.

[0107] S2.3: Based on ROS, each function is encapsulated as a node, and data transmission is achieved through node publishing and subscription. The positioning algorithm in step S2.2 is encapsulated as a positioning node. _posThe topic of publishing robot pose information in real time was published on the Topic. _pos The robot moves within a known map, subscribes to pose information topics in real time, and initializes the task target point.

[0108] S2.4: Obtain the task target point information of the mobile robot according to step S2.3, decompose the single target point planning into a multi-target point planning problem, that is, divide the entire task into two states, and the calculation formula is as shown in equation (8).

[0109] In the formula, q traffic This indicates that the navigation target point attribute will be initialized to the traffic light orientation factor attribute. Indicates the status of the mobile robot And autonomous navigation constraints p, S(t) red ,t green ) indicates that the mobile robot is in the red light time window t red Internal parking, green light time window t green Passage within.

[0110] State 1 requires combining traffic light recognition feedback signals to control the robot to complete autonomous navigation. State 2 only considers the problem of pure autonomous navigation. It balances the robot's computing power through task division and real-time scheduling to avoid excessive computing power causing message blocking and ROS node crashes.

[0111] S3: The two states only distinguish whether traffic lights need to be recognized. Both autonomous navigation tasks combine the same global and local path planning. Each autonomous navigation task uses the A* algorithm, a window filtering algorithm based on two-level velocity optimization, the velocity gradient concept, and the TEB local path planning and navigation algorithm with dynamic adaptive internal constraints. The A* algorithm is used to generate the globally optimal path, and the TEB algorithm, combined with the obstacle internal constraint strategy, enables the mobile robot to successfully pass through U-shaped obstacle areas. The velocity gradient concept is used to improve the speed of the mobile robot's autonomous navigation. Specifically, it includes the following sub-steps:

[0112] S3.1: Based on the initial point position and the target point position, the global planner generates the shortest path between the two points that does not pass through obstacles;

[0113] S3.2: Load the initial stage parameters of the TEB navigation algorithm based on two-stage velocity optimization window filtering algorithm, velocity gradient idea and dynamic adaptive internal constraints. The calculation formula is shown in equation (9):

[0114]

[0115] In the formula, l i ΔT represents the robot's pose, and ΔT represents the time interval between adjacent poses.

[0116] S3.3: Construct constraints f near the obstacle obs and constraints f that are far from the global path path The calculation formulas are shown in equations (10) and (11):

[0117]

[0118] Where, d 1_min,j This represents the minimum distance between the j-th path point of the mobile robot and the obstacle. d represents the threshold range between the mobile robot and obstacles. 2_min,j This represents the minimum distance from the reference point to the j-th path point of the mobile robot. The threshold value for the mobile robot to stray from the path is represented by S, where S represents the scaling factor, n represents the exponential factor, and ε represents the buffer factor.

[0119] S3.4: If a U-shaped obstacle exists in the task scenario, obstacle constraints are usually constructed to keep the mobile robot away from the U-shaped area and prevent it from getting stuck. However, extreme operating conditions are usually not considered, and this method will fail if navigation tasks need to be completed through U-shaped obstacles.

[0120] Therefore, based on the known map, the specific coordinates of the U-shaped obstacle can be determined. The two longest sides of the U-shaped obstacle are selected as the base sides, and the calculation formula is shown in equation (12):

[0121]

[0122] Where x i Represents the maximum and minimum values ​​of the U-shaped obstacle in the x-axis of the map coordinate system, and y-axis... i This represents the maximum and minimum values ​​of the U-shaped obstacle along the y-axis of the map coordinate system.

[0123] Calculate the inscribed circle of the U-shaped obstacle, here taking L as an example. x >L y Based on this, the calculation formulas are shown in equations (13) and (14):

[0124]

[0125] In the formula, r represents the radius of the inscribed circle, and (h,k) represents the coordinates of the center of the circle.

[0126] Construct internal constraints, starting from the center of the inscribed circle, first draw feasible paths extending along both sides of the x-axis. When the range of feasible paths coincides with... The outward extension stops when the threshold ranges intersect. Continue extending along the y-axis outwards from the U-shaped obstacle, entering the U-shaped obstacle in a counter-clockwise direction as the positive direction of expansion, and leaving the U-shaped obstacle in a clockwise direction as the positive direction of expansion. The expansion path reaches the y-axis. maxWithin the threshold range, a path constraint f serves as the entry point into the U-shaped obstacle. U_ipath A path constraint f as a way to leave the U-shaped obstacle U_opath ;

[0127] S3.5: Based on the original constraints, the TEB algorithm constraints are adjusted according to the adaptive dynamic factor. The calculation formulas are shown in equations (15), (16) and (17):

[0128] F = f obs +σ(α)·f path +[1-σ(α)]·{σ(β)·f u_ipath +[1-σ(β)]·f u_opath} (15)

[0129]

[0130] In the formula, α is the proximity of the U-shaped region, α0 is the activation threshold of the U-shaped obstacle, i.e., the point of entry into the U-shaped obstacle, β is the degree of movement inside the U-shaped obstacle, β0 is the switching point of entering or leaving the U-shaped obstacle, i.e., the geometric center position of the U-shaped obstacle, k1 represents the region switching sensitivity, and k2 represents the stage transition smoothness.

[0131] S3.6: The speed command generated by the navigation system adopts a two-level optimization. First, the Savitzky-Golay filter is applied to suppress noise and smooth the output of a continuous speed function. Then, a smooth speed profile conforming to an S-shaped curve is generated by the speed gradient increment algorithm based on jerk constraints to ensure the continuity of acceleration during motion. The calculation formulas are shown in Equations (18), (19) and (20):

[0132]

[0133] In the formula, h k (t) represents the time-varying filter coefficients, M is the filter half-window width, Δt is the sampling time interval, j(t) is the jerk, and t a To speed up the end time, t d For the deceleration start time, t f Let t be the time when the motion ends, a(t) be the acceleration, v(t) be the velocity, and s(t) be the path length.

[0134] S4: When navigating to the traffic light based on the global and local path planning in step S3, it is necessary to determine whether to continue navigation based on the feedback results. If the traffic light changes instantly, the determination is made based on the orientation-sensitive feature enhancement mechanism. If the threshold is exceeded, navigation continues. This includes the following sub-steps:

[0135] S4.1: To ensure the robustness of the mobile robot's operation and enable it to correctly respond to traffic light signals in different scenarios, a dataset is created by collecting various forms of traffic light images. To save on hardware system computation, only red and green light signals are recognized. That is, the visual detection module only provides feedback on two situations: red light means stop, green light means proceed, and no signal is recognized and both are allowed.

[0136] S4.2: Based on the dataset from step S4.1, the yolov5s.pt model of version yolov5-5.0 is selected as the initial weights for training;

[0137] S4.3: Use the trained optimal weight model to identify and predict passable signals;

[0138] S4.4: Input the results identified in step S4.3 into the post-processing rule module for union determination;

[0139] S4.5: Based on the initial target point parameters in step S2.4, navigation stops when the situation is normal and a red light is detected. Considering the scenario where a light suddenly changes from green to red after normal operation and passage, a location-sensitive feature enhancement mechanism is proposed. If the distance d exceeds a distance threshold d... max If the light changes, the navigation task continues, shortening the mobile robot's operation time. Distance is determined using the sum of squared Euclidean distances, calculated as shown in equation (21):

[0140] d=(N x -P x ) 2 +(N y -P y ) 2 (twenty one)

[0141] In the formula, N x N represents the x-axis coordinate of the mobile robot's current position on the map in step S1. y P represents the y-coordinate of the mobile robot's current location on the map. x P represents the x-axis coordinate of the previous target point on the map. y This represents the y-axis coordinate of the previous target point on the map.

[0142] S5: To successfully navigate inside the U-shaped obstacle and correctly relay visual signals, the following sub-steps are included:

[0143] S5.1: Initialize the navigation points obtained in S2.4 into the initialization list, i.e. It includes two types of attributes: traffic light orientation factor attribute and ordinary navigation point factor attribute;

[0144] S5.2: Obtain the initial position of the mobile robot and update the starting point list, i.e., Start_List = {q0};

[0145] S5.3: Read the initialization list sequentially according to the preset order, and update the navigation point i (i = 1, 2, ..., l) to the target point list, i.e., Goal_List = {q1};

[0146] S5.4: Navigate to the target point according to step S3. If the navigation to the target point is successful, update the navigation point i (i = 1, 2, ..., l) to the completion point list, i.e. Finish_List = {q1}, and update the target point list to the starting point list, i.e. Start_List = {q1}.

[0147] S5.5: Clear the target point list; at this point, Goal_List = {}.

[0148] S5.6: If the target point attribute is a traffic light orientation factor attribute at this time, then complete the traffic light recognition according to step S4, and determine whether to continue the task based on the feedback signal until the green light condition is met or the red light condition is ignored.

[0149] S5.7: Continue traversing the initialization list and update the target point of the next order to the target point list, i.e., Goal_List = {q2};

[0150] S5.8: Repeat steps S5.4, S5.5, and so on up to S5.7. Each successful navigation adds the current target point sequentially to the completion point list, i.e., Finish_List = {q1, q2, ..., q...}. l};

[0151] S5.9: Navigation ends when the completed point list is the same as the initialization list, i.e., Finish_List = Init_List. At this point, the mobile robot has completed all navigation tasks.

[0152] Existing research largely relies on obstacle constraint mechanisms to ensure obstacle avoidance for mobile robots, but it does not fully consider the navigation requirements in extreme operating scenarios. To address this issue, this study proposes a location-sensitive feature enhancement mechanism that integrates dynamic visual perception to achieve robust navigation in U-shaped obstacle environments. The testing and recording process is as follows: Figure 3 , Figure 4 , Figure 5 , Figure 6 and Figure 7 As shown in Table 1, the test record data is as follows. Figure 3 It can be clearly seen that the mobile robot stably identifies traffic light devices at turns using a depth camera (confidence level 0.89), and combines this with a location-sensitive feature enhancement mechanism to provide real-time feedback on passage decisions. The navigation process proceeds normally, without instantaneous changes in the traffic lights. Figure 4 and Figure 5 As shown, the mobile robot quickly and accurately completes the navigation task. When the traffic light instantly changes from green to red, the orientation-sensitive feature enhancement mechanism proposed in this patent is utilized, such as... Figure 7 As shown, it can also pass smoothly under the condition of satisfying objective physical facts, without waiting in place or being blocked by not recognizing traffic light messages, thus saving navigation time. Figure 4 Specifically, this means that: 1) the system maintains smooth navigation in the area outside the U-shaped obstacle; 2) after entering the inner area, the system dynamically optimizes path tracking and triggers significant turning actions only at the entrance and exit to adapt to trajectory constraints. Figure 5 The mobile robot was documented in detail. Figure 4 The actual navigation process along the planned path includes: 1) starting straight navigation and terminating navigation at a red light, then successfully passing through at a green light; 2) successfully entering and leaving a U-shaped obstacle; and 3) completing the final straight navigation segment. Compared to the algorithm before using this invention, the deviation between the local path (green) and the global path (red) generated by this method is always ≤5cm, and the smoothness of the speed curve is significantly improved, effectively suppressing step jumps in control commands.

[0153] Table 1 Test Comparison

[0154]

[0155] Under the same test conditions (duration / location), the data in Table 1 shows that the algorithm's success rate is significantly improved by 40% compared to the baseline model. In terms of navigation timeliness, the shortest and longest navigation times are reduced by 14 seconds and 35 seconds respectively, and the worst-case time of this algorithm is still better than the optimal value of the original algorithm. Task stability is doubled. Figure 4 It can be clearly seen that the peak value of the linear velocity jump is reduced by 0.05, which comprehensively confirms the high speed and reliability of the algorithm of the present invention.

[0156] The specific implementation schemes described above further illustrate the inventive purpose and technical solution of the present invention. The above embodiments are only used to illustrate the technical solution of the present invention, and are not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that any modifications or equivalent substitutions made to the technical solution of the present invention are included within the scope of protection of the present invention.

Claims

1. A navigation method for internally constrained mobile robots that integrates visual perception and speed optimization, characterized in that, The following steps are involved: S1: Building a practical application scenario using LiDAR for 2D mapping; S2: The environment map obtained in step S1 is pre-initialized, which includes the following sub-steps: S2.1: Obtain prior environment map information and input it into the positioning method; S2.2: Initialize within the feasible area of ​​the map and estimate the robot pose; S2.3: Based on the Robot Operating System (ROS), subscribe to pose information topics in real time and initialize the task target point; S2.4: Obtain the task target point information of the mobile robot. The calculation formula is shown in equation (1): In the formula, q traffic This indicates that the navigation target point attribute will be initialized to the traffic light orientation factor attribute. Indicates the status of the mobile robot And autonomous navigation constraints p, S(t) red ,t green ) indicates that the mobile robot is in the red light time window t red Internal parking, green light time window t green internal traffic; S3: Both state navigation methods use the A* algorithm, two-stage velocity optimization window filtering, velocity gradient concepts, and the TEB algorithm with dynamic adaptive internal constraints for path planning. Specifically, it includes the following sub-steps: S3.1: The global planner generates the shortest path between two points that does not pass through obstacles; S3.2: Load initial phase parameters; S3.3: Construct constraints f near the obstacle obs and constraints f that are far from the global path path The calculation formulas are shown in equations (2) and (3): In the formula, d 1_min,j This represents the minimum distance between the j-th path point of the mobile robot and the obstacle. d represents the threshold range between the mobile robot and obstacles. 2_min,j This represents the minimum distance from the reference point to the j-th path point of the mobile robot. The threshold value for the mobile robot to stray from the path is represented by S, where S represents the scaling factor, n represents the exponential factor, and ε represents the buffer factor. S3.4: Constructing the entry path constraint f of the internal constraints U_ipath Leaving the path constraint f U_opath ; S3.5: Constraints of the TEB algorithm based on adaptive dynamic factor adjustment, the calculation formulas of which are shown in equations (4), (5) and (6): F=f obs +σ(a)·f path +[1-σ(α)]·{σ(β)·f u_ipath +[1-σ(β)]·f u_opath } (4) In the formula, α is the proximity of the U-shaped region, α0 is the activation threshold of the U-shaped obstacle, i.e., the point of entry into the U-shaped obstacle, β is the degree of movement inside the U-shaped obstacle, β0 is the switching point of entering or leaving the U-shaped obstacle, i.e., the geometric center position of the U-shaped obstacle, k1 represents the region switching sensitivity, and k2 represents the stage transition smoothness. S3.6: The speed command generated by the navigation system adopts a two-level optimization. First, the Savitzky-Golay filter is applied to suppress noise and smooth the output of a continuous speed function. Then, a smooth speed profile conforming to an S-shaped curve is generated by the speed gradient increment algorithm based on jerk constraints to ensure the continuity of acceleration during motion. The calculation formulas are shown in Equations (7), (8) and (9): In the formula, h k (t) represents the time-varying filter coefficients, M is the filter half-window width, Δt is the sampling time interval, j(t) is the jerk, and t a To speed up the end time, t d For the deceleration start time, t f Let t be the time when the motion ends, a(t) be the acceleration, v(t) be the velocity, and s(t) be the path length. S4: When navigating to a traffic light, determine whether to continue navigation based on the feedback result. If the traffic light changes instantly, the determination is based on the orientation-sensitive feature enhancement mechanism. If the distance threshold is exceeded, navigation continues. S5: Navigate inside the U-shaped obstacle and correctly report visual signals, which includes the following sub-steps: S5.1: Initialize the navigation point list S5.2: Get the initial position and update the starting point list Start_List = {q0}; S5.3: Update the navigation point i (i = 1, 2, ..., l) to the goal point list Goal_List = {q1}; S5.4: If the target point is successfully navigated to according to step S3, update the navigation point i (i = 1, 2, ..., l) to the finish point list Finish_List = {q1}, and update the target point list to the starting point list Start_List = {q1}; S5.5: Clear the target point list; at this point, Goal_List = {}. S5.6: If the target point attribute is a traffic light orientation factor attribute at this time, then complete the traffic light recognition according to step S4, and determine whether to continue the task based on the feedback signal until the green light condition is met or the red light condition is ignored. S5.7: Continue traversing the initialization list and update the target point of the next order to the target point list, i.e., Goal_List = {q2}; S5.8: Repeat steps S5.4 to S5.

7. Each successful navigation adds the current target point sequentially to the completion point list, i.e., Finish_List = {q1, q2, ..., q...}. l }; S5.9: Navigation ends when the completed point list is the same as the initialization list, i.e., Finish_List = Init_List. At this point, the mobile robot has completed all navigation tasks.

Images