A robot autonomous navigation control method, system, device and medium
By performing topology matching and obstacle trajectory prediction on the robot navigation system, the robot navigation strategy was optimized, solving the navigation dilemma when there is no way to detour in space, and realizing an efficient solution for coordinated passage between robots and humans.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- NANJING XINYING SMART INFORMATION TECHNOLOGY CO LTD
- Filing Date
- 2026-04-30
- Publication Date
- 2026-07-21
Smart Images
Figure CN122431386A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot autonomous navigation technology, specifically to a robot autonomous navigation control method, system, device, and medium. Background Technology
[0002] Current methods for robot navigation typically employ spatial dimension-based obstacle avoidance strategies. This means that when sensors detect an obstacle, a path replanning module is triggered to search for an alternative path in the map space to avoid the conflict. However, this approach has inherent limitations in structured passageway scenarios.
[0003] For example, in a warehouse corridor scenario, a delivery robot is traveling along a 1.2-meter-wide aisle when a worker enters the aisle from a side door and walks towards the robot. The existing obstacle avoidance system detects the obstacle and immediately triggers replanning, attempting to search for a detour path on the map. However, due to the narrow aisle space and the robot's own width of 0.8 meters, there is insufficient space on either side for it to maneuver, causing the replanning module to fail repeatedly. It then switches to a wait-in strategy, stopping the robot's movement. The worker, noticing the robot's stop, also slows down or even stops, resulting in a stalemate where both sides are waiting. Alternatively, if the worker slightly changes position, the robot may determine that a new planable window has appeared, restarting and quickly approaching the obstacle, causing a new cycle of stopping and waiting until manual intervention is required to resolve the issue.
[0004] When there is no way to bypass it in space, it is trapped in an unsolvable dilemma, unable to use the time information of the movement of obstacles to coordinate the passage sequence. Summary of the Invention
[0005] In view of the above-mentioned problems, the present invention provides a robot autonomous navigation control method, system, device and medium.
[0006] Therefore, the technical problem solved by the present invention is that when there is no way to bypass the obstacle in space, the existing method will fall into an unsolvable dilemma and cannot use the time information of the obstacle movement to coordinate the passage sequence.
[0007] To solve the above-mentioned technical problems, the present invention provides the following technical solution: a robot autonomous navigation control method, comprising the following steps: matching the robot's current pose with the topology of the channel region in a static grid map, extracting the spatial constraint relationships at each path node in the channel region, and obtaining a node constraint map; extrapolating the obstacle trajectory of the robot's perception data within a continuous control cycle, using the spatial constraints of each node in the path node constraint map as boundaries, calculating the occupancy time window of the obstacle at each path node, and obtaining a node timing occupancy table; performing a first timing evaluation using the idle time window of each path node in the node timing occupancy table and the expected time of the robot's arrival at each path node, selecting the departure time offset at each path node that the robot can pass through without conflict, and obtaining a timing adjustment sequence; and performing feedforward adjustment on the robot according to the timing adjustment sequence.
[0008] As a preferred embodiment of the robot autonomous navigation control method described in this invention, the step of obtaining the node timing occupancy table includes: performing differential processing on the perception data within a continuous control cycle according to the cycle number to obtain the periodic displacement sequence of the obstacle; performing nonlinear extrapolation on the trajectory of the obstacle based on the changing trend of the displacement in the periodic displacement sequence to obtain the extrapolated trajectory; taking the intersection time period of the extrapolated trajectory and the spatial constraint boundary of each path node as the occupancy time window; and summarizing all the occupancy time windows to obtain the node timing occupancy table.
[0009] As a preferred embodiment of the robot autonomous navigation control method described in this invention, the step of nonlinearly extrapolating the trajectory of an obstacle includes: using the change in the angle between the periodic displacement vectors of adjacent control cycles as a curvature estimate; using the curvature estimate and the magnitude of the periodic displacement vector of the current control cycle as driving parameters, and superimposing the directional deflection corresponding to the curvature estimate according to the current motion direction to generate trajectory segments within the current extrapolation step size; using the endpoint position and the direction vector at the endpoint of the trajectory segment as the extrapolation starting state of the next control cycle, and gradually splicing the trajectory segments to obtain the extrapolated trajectory; The step of calculating the curvature estimate includes: taking the angle change of multiple consecutive control cycles as input, assigning weights to each angle change according to the cycle number to obtain the weighted value corresponding to each angle change; and using the sum of all weighted values as the curvature estimate of the current control cycle.
[0010] As a preferred embodiment of the robot autonomous navigation control method described in this invention, the step of selecting the departure time offsets at each path node that the robot can pass through without conflict includes: using the time difference between the start time of the idle time window of each path node and the expected time of the robot arriving at each path node as the candidate offset of each path node; accumulating the candidate offsets of all path nodes in the order of the path nodes to obtain the accumulated travel time of each path node; replacing the candidate offsets corresponding to the path nodes whose accumulated travel time is less than the shortest travel time with the smallest candidate offset that satisfies the shortest travel time constraint to obtain the verified candidate offsets; summing up the verified candidate offsets of all path nodes to obtain the timing control sequence.
[0011] As a preferred embodiment of the robot autonomous navigation control method of the present invention, the step of determining the shortest travel time between adjacent path nodes includes: taking the path segment length between adjacent path nodes in the node constraint diagram as input, and combining it with the robot's maximum allowable speed under the spatial constraint width corresponding to the path segment, to obtain the lower bound of the motion time of the path segment; taking the time resolution of the occupancy time window at the adjacent path node as a reference, rounding the lower bound of the motion time upward to an integer multiple of the time resolution to obtain the shortest travel time between adjacent path nodes.
[0012] In a preferred embodiment of the robot autonomous navigation control method described in this invention, the step of feedforward adjustment of the robot according to the timing control sequence includes: taking the departure time offset of each path node in the timing control sequence and the path segment length between adjacent path nodes as input, and using the quotient of the travel time after adjusting the path segment length and departure time offset as the target speed of the path node; when the robot arrives at each path node, the difference between the actual arrival time of the robot and the target arrival time of the corresponding path node in the timing control sequence is calculated to obtain the time deviation of the current path node; the time deviation is evenly distributed to the target speed correction amount of each path node according to the number of remaining path nodes, and the target speed of each path node is corrected to obtain a compensated speed command sequence; the robot is adjusted using the compensated speed command sequence.
[0013] As a preferred embodiment of the robot autonomous navigation control method of the present invention, the step of obtaining the node constraint graph includes: extracting the morphological skeleton of the channel region in the static grid map to obtain the channel skeleton; performing key point detection on the channel skeleton and using the detection results as path nodes; performing a lateral scan of the channel region with each path node as the center and using the passage width at each path node as the spatial constraint of that node; using the channel skeleton between adjacent path nodes as path segments, and combining the spatial constraints of each path node to construct the node constraint graph.
[0014] This invention provides an autonomous navigation and control system for robots.
[0015] To solve the above technical problems, the present invention provides the following technical solution: a robot autonomous navigation control system, comprising: a graph construction module, used to match the robot's current pose with the topology of the channel region in a static grid map, extract the spatial constraint relationship at each path node in the channel region, and obtain a node constraint graph; The trajectory calculation module is used to extrapolate the obstacle trajectory of the robot's perception data in a continuous control cycle. Taking the spatial constraints of each node in the path node constraint diagram as the boundary, it calculates the time window of the obstacle's occupancy at each path node to obtain the node timing occupancy table. The evaluation module is used to perform a first time-series evaluation based on the idle time window of each path node in the node time-series placeholder table and the expected time when the robot arrives at each path node, and to filter out the departure time offset at each path node that the robot can pass through without conflict, thereby obtaining the time-series adjustment sequence. The execution module performs feedforward adjustments to the robot based on the timing control sequence.
[0016] The present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the robot autonomous navigation control method.
[0017] The present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the robot autonomous navigation control method described above.
[0018] The beneficial effects of this invention are as follows: By extrapolating the nonlinear trajectory of continuous worker movement data, this invention predicts the worker's positioning time window at each channel node and establishes a node timing positioning table. Based on this, the conflict-free departure time offset of the robot at each path node is calculated. It is found that by delaying the robot's departure from its current position by approximately N seconds, the worker will complete the channel crossing before the robot arrives, at which point the channel will be completely empty. In other words, the robot does not need to stop and wait for the worker's reaction, nor does it need to search for any detour paths. Instead, it actively reduces its speed through feedforward adjustment and accelerates to the target speed after the worker has passed to complete the crossing. Throughout the entire process, the robot maintains constant motion, eliminating the repeated stops and oscillations caused by failed spatial replanning. Attached Figure Description
[0019] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying 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.
[0020] Figure 1 This is a general flowchart of a robot autonomous navigation control method provided in one embodiment of the present invention.
[0021] Figure 2 The diagram shows a comparison of the effects of a robot autonomous navigation control method provided in one embodiment of the present invention. Detailed Implementation
[0022] To make the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the protection scope of the present invention.
[0023] Example 1, referring to Figure 1 and Figure 2 This is one embodiment of the present invention, which provides a robot autonomous navigation control method, including the following steps: The robot's current pose is matched with the topology of the channel region in the static grid map, and the spatial constraint relationship at each path node in the channel region is extracted to obtain the node constraint map.
[0024] The robot's perception data during continuous control cycles is extrapolated to the obstacle trajectory. Using the spatial constraints of each node in the path node constraint diagram as the boundary, the time window for the obstacle to occupy each path node is calculated to obtain the node timing occupancy table.
[0025] The first timing evaluation is performed using the idle time window of each path node in the node timing placeholder table and the expected time when the robot arrives at each path node. The departure time offset at each path node that the robot can pass through without conflict is selected to obtain the timing control sequence.
[0026] The robot is fed forward adjusted according to the timing control sequence.
[0027] In a warehouse corridor scenario, a delivery robot is traveling along a 1.2-meter-wide aisle when a worker enters from a side door and walks towards the robot. The existing obstacle avoidance system detects the obstacle and immediately triggers replanning, attempting to search for a detour path on the map. However, due to the narrow aisle space and the robot's 0.8-meter width, there is insufficient space on either side for it to maneuver, causing the replanning module to fail repeatedly. It then switches to a wait-in strategy, stopping the robot. The worker, noticing the robot's stop, also slows down or even stops, resulting in a stalemate where both sides are waiting. Alternatively, if the worker slightly changes position, the robot may determine a new plannable window has appeared, restarting and quickly approaching, causing a new cycle of stopping and waiting until manual intervention is required to resolve the issue.
[0028] By extrapolating the nonlinear trajectory of the worker's continuous movement data, the time window for the worker's position at each channel node is predicted, and a node timing position table is established. Based on this, the conflict-free departure time offset of the robot at each path node is calculated. It is found that by delaying the robot's departure from its current position by approximately N seconds, the worker will complete the passage before the robot arrives, at which point the channel will be completely empty. In other words, the robot does not need to stop and wait for the worker's reaction, nor does it need to search for any detour paths. Instead, it actively reduces its speed through feedforward adjustment and accelerates to the target speed after the worker has passed to complete the passage. Throughout the entire process, the robot remains in motion, eliminating the repeated stops and oscillations caused by failed spatial replanning.
[0029] Reference Figure 2 As shown, the existing method (red) will trigger a stop when a problem occurs while the robot is moving, while the present invention (blue) will adjust the robot's moving speed to prevent problems from occurring.
[0030] Example 2, refer to Figure 1 As shown, this is an embodiment of the present invention. Based on the previous embodiment, a robot autonomous navigation control method is provided, including the following steps: S1. Match the robot's current pose with the topology of the channel region in the static grid map, extract the spatial constraint relationships at each path node in the channel region, and obtain the node constraint map.
[0031] The steps for obtaining the node constraint graph include S1.1 to S1.4: S1.1. Extract the morphological skeleton of the channel region in the static raster map to obtain the channel skeleton.
[0032] In this embodiment, a morphological thinning operation is performed on the set of grids marked as passable areas in the static grid map. The edge grids of the channel region are peeled off layer by layer through iterative erosion until the region width converges to a connected line segment with a single pixel width, while preserving the connected topology of the original channel region to obtain the channel skeleton.
[0033] For example, in a grid map with a resolution of 0.05m / grid, a warehouse scene includes a straight passageway that is 12 meters long and 1.2 meters wide and a horizontal passageway that is 8 meters long and 1.4 meters wide. The two passageways intersect in the middle of the map to form a crossroads area.
[0034] After performing morphological refinement on this channel region, the straight channel is extracted as a single-pixel skeleton line segment with a length of 240 grids distributed along its central axis, and the horizontal channel is extracted as a single-pixel skeleton line segment with a length of 160 grids. The two skeleton line segments are connected at the intersection to jointly form the channel skeleton of the scene.
[0035] S1.2 Perform key point detection on the channel skeleton and use the detection results as path nodes.
[0036] Traverse all grids on the channel skeleton, count the number of connected skeleton grids in each grid's eight neighborhoods, determine grids with a connection count of 1 as endpoints, determine grids with a connection count of 3 or more as intersections, unify endpoints and intersections as path nodes, and record the position coordinates of each path node in the map coordinate system.
[0037] For example, key point detection is performed on the obtained channel skeleton. Each end of the straight channel skeleton has a grid with an eight-neighbor connectivity of 1, which is determined as an endpoint node and denoted as the first node (0.5m, 6.0m) and the second node (11.5m, 6.0m). Similarly, the two ends of the transverse channel frame are identified as the third node (6.0m, 0.5m) and the fourth node (6.0m, 7.5m); At the intersection, there is a four-way connected skeleton grid with an eight-neighbor connectivity of 4. It is determined to be an intersection node and is denoted as the fifth node (6.0m, 6.0m).
[0038] S1.3. Using each path node as the center, perform a horizontal scan of the channel area and use the passage width at each path node as the spatial constraint of that node.
[0039] For each path node, extend scan lines to both sides along the direction perpendicular to the channel skeleton tangent at that path node, count the number of consecutive passable grids before the first non-passable grid on both sides, multiply the sum of the scan lengths on both sides by the grid resolution to obtain the passable width at that node, and use this passable width as the spatial constraint of that path node.
[0040] Specifically, a horizontal scan is performed on the first node, which is located at the end of the straight passage skeleton. The tangent direction of the skeleton is horizontal, and the scan line extends vertically. It scans upward to the passage boundary for a total of 12 grids and downward to the passage boundary for a total of 12 grids. The passage width is 24 × 0.05m = 1.2m. Therefore, the spatial constraint of the first node is 1.2m.
[0041] When performing a lateral scan at the fifth node, with the straight passage direction as a reference, scan 12 grids on each side vertically, with a passage width of 1.2m. The spatial constraint of the fifth node is tentatively set at 1.2m. With the lateral passage direction as a reference, scan 14 grids on each side vertically, with a passage width of 28 × 0.05m = 1.4m. The spatial constraint of the fifth node is tentatively set at 1.4m. The smaller value between the two directions is taken as the conservative spatial constraint for this node. Therefore, the spatial constraint of the fifth node is 1.2m.
[0042] S1.4. Take the channel skeleton between adjacent path nodes as path segments, and construct a node constraint graph by combining the spatial constraints of each path node.
[0043] Using each path node as the vertex of the node constraint graph and the skeleton connecting line segments between adjacent path nodes as the edges of the graph, the number of skeleton grids traversed by each skeleton connecting line segment is counted and multiplied by the grid resolution to obtain the length of the corresponding path segment. The spatial constraints of each vertex and the path segment lengths of each edge are used together as attributes of the node constraint graph to complete the construction of the node constraint graph.
[0044] S2. Extrapolate the obstacle trajectory of the robot's perception data in the continuous control cycle. Using the spatial constraints of each node in the path node constraint diagram as the boundary, calculate the time window of the obstacle's occupancy at each path node to obtain the node timing occupancy table.
[0045] The steps for obtaining the node timing placeholder table include S2.1 to S2.4: S2.1. Differential processing is performed on the sensing data within the continuous control cycle according to the cycle number to obtain the cycle displacement sequence of the obstacle.
[0046] The sensor data is sampled at fixed control cycles, and the coordinates of the center position of the obstacle detected in the k-th control cycle are recorded as follows: The position coordinates of adjacent cycles are subtracted to obtain the periodic displacement vector of the kth control cycle. The periodic displacement vectors of n consecutive control cycles are arranged according to the cycle number to form the periodic displacement sequence of the obstacle.
[0047] For example, if the system control cycle is 0.1s, the lidar outputs the obstacle position at this cycle frequency. Within 5 consecutive control cycles, the center position coordinates of a worker are detected as follows: (8.0m, 6.0m), (7.7m, 6.0m), (7.4m, 6.0m), (7.1m, 6.0m), (6.8m, 6.0m).
[0048] By subtracting the coordinates of adjacent periods, the periodic displacement sequences are obtained as (−0.3m,0), (−0.3m,0), (−0.3m,0), and (−0.3m,0). The obstacle is currently moving at a constant speed along the −x direction. The displacement modulus of a single period is 0.3m, and the corresponding speed is 0.3m / 0.1s = 3.0m / s.
[0049] S2.2. Based on the changing trend of displacement in the periodic displacement sequence, the trajectory of the obstacle is nonlinearly extrapolated to obtain the extrapolated trajectory.
[0050] The steps for nonlinear extrapolating the trajectory of the obstacle include A1 to A3: A1. The change in the angle between the periodic displacement vectors of adjacent control cycles is used as the curvature estimate.
[0051] It should be noted that the steps for calculating the curvature estimate include A1.1 to A1.2: A1.1. Using the angle change over multiple consecutive control cycles as input, assign weights to each angle change according to the cycle number to obtain the weighted value corresponding to each angle change.
[0052] The angle change over m consecutive control cycles is taken as input, and linearly increasing weights are assigned according to the cycle number from far to near, so that the more recent angle change contributes more to the curvature estimation, thus obtaining the weighted value corresponding to each angle change.
[0053] In this embodiment, if the angle change over m=3 control cycles is taken as input, the weight denominator is 1+2+3=6, and the weights for the three cycles are 1 / 6, 2 / 6, and 3 / 6 respectively. If the angle change in the current k-th control cycle is 4°, and the angle changes in the previous two cycles are 2° for k−2 and 3° for k−1, then the corresponding weights are 2×1 / 6≈0.33, 3×2 / 6=1.00, and 4×3 / 6=2.00 respectively.
[0054] A1.2. The sum of all weighted values is used as the curvature estimate for the current control cycle.
[0055] Sum the m weighted values obtained in step A1.1 to obtain the curvature estimate for the current control cycle. The specific calculation formula is expressed as follows: ; In the formula, m is the number of weighted values. Let be the angle change over m consecutive control cycles. As weight.
[0056] For example, the sum of the three weighted values is 0.33 + 1.00 + 2.00 = 3.33, which means that the curvature estimate of the current control cycle is 3.33°, indicating that the predicted direction deflection of the obstacle in each extrapolation step is 3.33°.
[0057] A2. Using the curvature estimate and the magnitude of the periodic displacement vector of the current control cycle as driving parameters, the trajectory segments within the current extrapolation step size are generated by superimposing the curvature estimate with the directional deflection amount corresponding to the current motion direction.
[0058] Using the magnitude of the periodic displacement vector of the current control cycle as the step distance for each extrapolation step, and taking the current motion direction angle as a reference, the extrapolation direction angle is obtained by superimposing the curvature estimation value. Therefore, the formula for calculating the extrapolation direction angle is expressed as follows: ; In the formula, The extrapolation direction angle, This represents the current direction angle of motion.
[0059] Starting from the current extrapolation starting position, along the direction The distance from the magnitude of the forward displacement vector is used to generate a directed line segment as the trajectory segment within the current extrapolation step size, with the direction vector at the endpoint being... .
[0060] For example, if the current extrapolation step size is 0.3m and the current direction angle is 180°, then after superimposing the curvature estimate of 3.33° along the −x direction, the extrapolation direction angle becomes 183.33°. Starting from the current starting position (6.8m, 6.0m), move 0.3m along the 183.33° direction, and the ending position is (6.8 + 0.3 × cos183.33°, 6.0 + 0.3 × sin183.33°) ≈ (6.500m, 5.983m), generating the first extrapolation trajectory segment.
[0061] A3. Using the endpoint position and direction vector of the trajectory segment as the extrapolation start state for the next control cycle, the trajectory segments are gradually spliced together to obtain the extrapolated trajectory.
[0062] The endpoint position and direction angle of the obtained trajectory segment are used as the starting position and starting direction of the next extrapolation. Step A2 is repeated with the updated curvature estimate to generate the next trajectory segment. This process is repeated until the total extrapolation length covers all path nodes in the scene or the number of extrapolation steps reaches the preset limit. All trajectory segments are then spliced together end to end to obtain the complete extrapolated trajectory.
[0063] For example, taking the endpoint position of the obtained trajectory segment (6.500m, 5.983m) and the extrapolation direction angle of 183.33° as the starting state of the second extrapolation step, the curvature estimate remains at 3.33°, the extrapolation direction angle is updated to 186.66°, and the step distance remains at 0.3m. The endpoint of the second trajectory segment is then (6.500 + 0.3 × cos186.66°, 5.983 + 0.3 × sin186.66°) ≈ (6.202m, 5.949m). This iteration generates a total of 15 trajectory segments. After splicing the beginning and end of each segment, an extrapolated trajectory is obtained that starts from the initial position and extends to the left in an arc-shaped curve.
[0064] S2.3. The time interval at which the extrapolated trajectory intersects with the spatial constraint boundaries of each path node is used as the occupancy time window.
[0065] For each path node in the node constraint graph, construct a constraint interval in the transverse channel direction with its spatial constraint as the diameter and the node coordinates as the center. Determine whether each trajectory segment on the extrapolated trajectory enters the constraint interval. Record the moment when the extrapolated trajectory first enters the constraint interval as the occupation start moment and record the moment when the extrapolated trajectory leaves the constraint interval as the occupation end moment. Use the time interval between the occupation start moment and the occupation end moment as the occupation time window of the obstacle at the path node.
[0066] Specifically, if the coordinates of the fifth node are (6.0m, 6.0m) and the spatial constraint is 1.2m, when constructing the constraint interval, the boundary for determining the occupancy is a range of ±0.6m along the lateral direction of the channel, centered on the fifth node. By detecting each segment of the extrapolated trajectory, it is found that the endpoint coordinates of the sixth trajectory segment (6.302m, 5.872m) first enter this constraint interval, corresponding to 6 × 0.1s = 0.6s; the endpoint coordinates of the tenth trajectory segment (5.703m, 5.715m) first leave this constraint interval, corresponding to 10 × 0.1s = 1.0s. Therefore, the time window for the obstacle to occupy the fifth node is [0.6s, 1.0s].
[0067] S2.4 Summarize all the time windows for the node time sequence to obtain the node time sequence placeholder table.
[0068] Traverse all path nodes in the node constraint graph, calculate the time window for each path node, and construct a node time sequence placeholder table with the path node number as the row index and the start and end times of the placeholder as column attributes.
[0069] If the extrapolated trajectory of an obstacle does not pass through the constraint interval of a certain path node, then the corresponding row record of that node is empty, indicating that there is no occupation conflict at that node.
[0070] S3. Perform a first timing evaluation using the idle time window of each path node in the node timing placeholder table and the expected time when the robot arrives at each path node. Filter out the departure time offset at each path node that the robot can pass through without conflict, and obtain the timing control sequence.
[0071] The steps for selecting the departure time offsets at each path node that allow the robot to pass without conflict include S3.1 to S3.4: S3.1. The time difference between the start time of the idle time window of each path node and the expected time when the robot arrives at each path node is used as the candidate offset of each path node.
[0072] Traverse all path nodes on the robot's planned path and determine whether the robot's expected arrival time falls within the time window of that node.
[0073] When the robot's expected arrival time falls within a certain time window, the difference between the start time of the most recent idle time window after the end of the time window and the expected arrival time is used as the candidate offset for that node. If the expected arrival time does not fall within any placeholder time window (i.e., it is directly in an idle period), the candidate offset of the node is 0.
[0074] Taking a warehousing scenario as an example, the robot's current planned path is to move from the first node (0.5m, 6.0m) to the fifth node (6.0m, 6.0m), and then to the second node (11.5m, 6.0m). The robot is currently located at the first node, and the navigation speed is set to 1.5m / s.
[0075] The estimated arrival time is calculated based on the length of each path segment and the navigation speed: The path segment length from the first node to the fifth node is 5.5m, and the estimated arrival time is 5.5 / 1.5≈3.67s; The path segment from the fifth node to the second node is 5.5m long, and the estimated arrival time is 3.67 + 5.5 / 1.5 ≈ 7.33s.
[0076] Querying the node timing placeholder table obtained in step S2, the obstacle placeholder time window at node five is [3.2s, 4.0s]. The robot's estimated arrival time is 3.67s, so it falls within this placeholder time window, resulting in a timing conflict. After this placeholder time window ends, the nearest idle time window at node five starts at 4.0s. Therefore, the candidate offset for node five is 4.0 - 3.67 = 0.33s. There is no placeholder record at node two. The robot's estimated arrival time is 7.33s, directly falling within the idle period. The candidate offset for node two is 0.
[0077] S3.2. Accumulate the candidate offsets of all path nodes in the order of the path nodes to obtain the accumulated travel time of each path node.
[0078] According to the planned path node sequence, the nominal travel time of the path segment is the difference between the estimated time when the robot arrives at two adjacent nodes. The cumulative travel time of the path segment is obtained by adding the candidate offset of the next node.
[0079] In this embodiment, the nominal travel time from the first node to the fifth node is 3.67s. Adding the candidate offset of 0.33s for node five, the cumulative travel time is 3.67 + 0.33 = 4.0s. When traveling from the fifth node to the second node, the nominal travel time is 7.33 - 3.67 = 3.67 seconds. Adding the candidate offset of 0 for node two, the cumulative travel time is 3.67 + 0 = 3.67 seconds.
[0080] S3.3 Replace the candidate offsets corresponding to the path nodes whose cumulative travel time is less than the shortest travel time with the smallest candidate offset that satisfies the shortest travel time constraint to obtain the verified candidate offsets.
[0081] The steps for determining the shortest travel time between adjacent path nodes include B1 to B2: B1. Using the path segment length between adjacent path nodes in the node constraint graph as input, and combining it with the robot's maximum allowable speed under the spatial constraint width corresponding to the path segment, the lower bound of the motion time of the path segment is obtained.
[0082] For each pair of adjacent path nodes in the node constraint graph, the lower bound of the robot's motion time through that path segment is obtained by dividing the path segment length by the maximum allowable robot speed corresponding to the channel space constraint width of that segment.
[0083] In this embodiment, the path segment length from the first node to the fifth node is 5.5m, corresponding to a channel space constraint width of 1.2m. The maximum allowable speed of the robot under a channel width of 1.2m is 1.5m / s, and the lower bound of the motion time is 5.5÷1.5≈3.67s. The path length and channel width of the segment from the fifth node to the second node are the same, and the lower bound of the motion time is 3.67s.
[0084] B2. Based on the time resolution of the time window occupied at adjacent path nodes, the lower bound of the movement time is rounded up to an integer multiple of the time resolution to obtain the shortest travel time between adjacent path nodes.
[0085] Using a control cycle of 0.1s as the time resolution, the lower bound of the movement time of each path segment is rounded up to an integer multiple of 0.1s to obtain the shortest travel time.
[0086] In this embodiment, the lower bound of the movement time from the first node to the fifth node is 3.67s. Rounding up to the nearest integer multiple of 0.1s, the shortest passage time is (3.67 / 0.1)×0.1, which is 3.7s after rounding. The lower bound of the movement time from the fifth node to the second node is 3.67s, and the shortest passage time is 3.7s.
[0087] After calculating the shortest travel time, the cumulative travel time of each path segment is constrained and verified in sequence. For example, when verifying the travel time from the first node to the fifth node, the cumulative travel time of 4.0s ≥ 3.7s satisfies the shortest travel time constraint. The candidate offset of node five, 0.33s, passes the verification and remains unchanged.
[0088] When traveling from the fifth node to the second node, the cumulative travel time is 3.67s < 3.7s, which does not meet the shortest travel time constraint. Therefore, the candidate offset of node two needs to be replaced. The smallest candidate offset that meets the shortest travel time constraint is 3.7 - 3.67 = 0.03s. Since 0.03s is less than one time resolution unit, it is rounded up to one time resolution unit, resulting in a verified candidate offset of 0.1s. After replacing 0 with 0.1s, the cumulative travel time from the fifth node to the second node is updated to 3.67 + 0.1 = 3.77s ≥ 3.7s, which meets the constraint.
[0089] S3.4 Summarize the verified candidate offsets of all path nodes to obtain the timing control sequence.
[0090] The candidate offsets of all path nodes on the planned path after verification are sorted and summarized in the order of the path nodes. The timing control sequence is constructed using the path node number as the index and the candidate offsets of each node after verification as the control parameters.
[0091] S4. Perform feedforward adjustment on the robot according to the timing control sequence.
[0092] Specifically, this includes the following steps S4.1 to S4.4: S4.1. Using the departure time offset of each path node in the time-series control sequence and the path segment length between adjacent path nodes as input, the quotient of the travel time after adjusting the path segment length and the departure time offset is used as the target speed of the path node.
[0093] For each path segment on the planned path, the target speed of the robot on that path segment is obtained by dividing the length of that path segment by the adjusted travel time (which is the nominal travel time plus the departure time offset). The formula for calculating the target speed is: ; In the formula, Let be the target speed of the i-th path segment. Let be the length of the i-th path segment. Nominal driving time This represents the departure time offset of the endpoint path node for this segment.
[0094] Meanwhile, the target speed and corresponding path segment length of each path node are used to estimate the time when the robot arrives at the target at each node, which serves as a reference benchmark for calculating the time deviation.
[0095] In this embodiment, the robot is currently located at the first node, and the timing control sequence is {the fifth node: the departure time offset is 0.33s, the second node: the departure time offset is 0.1s}. The nominal travel time of each path segment is 3.67s, and the length of each path segment is 5.5m.
[0096] The adjusted passage time for the section from the first node to the fifth node is 3.67 + 0.33 = 4.0 s, the target speed is 5.5 ÷ 4.0 = 1.375 m / s, and the target arrival time at the fifth node is 4.0 s. For example, the adjusted passage time from the fifth node to the second node is 3.67 + 0.1 = 3.77 s, the target speed is 5.5 ÷ 3.77 ≈ 1.459 m / s, and the target arrival time at the second node is 4.0 + 3.77 = 7.77 s.
[0097] This yields the robot's initial velocity command sequence.
[0098] S4.2 When the robot arrives at each path node, the difference between the robot's actual arrival time and the target arrival time of the corresponding path node in the timing control sequence is calculated to obtain the time deviation of the current path node.
[0099] During the robot's movement, the actual position of the robot is detected in real time using sensor data. When the distance between the robot's position and the coordinates of the path node is less than the judgment threshold, the current time is recorded as the actual arrival time of the robot at the path node. The difference between the current time and the target arrival time of the node in step S4.1 is calculated to obtain the time deviation at the current path node.
[0100] In this embodiment, the robot travels towards the fifth node at a speed of 1.375 m / s. Due to the locally high coefficient of ground friction, the actual arrival time at node five is 4.15 s. The time deviation of the current path node is 4.15 − 4.0 = +0.15 s. The positive time deviation indicates that the robot actually arrives at node five 0.15 s later than the target time, and the speed command needs to be compensated and corrected in subsequent path segments.
[0101] S4.3. The time deviation is evenly distributed to the target speed correction amount of each path node according to the number of remaining path nodes, and the target speed of each path node is corrected to obtain the compensated speed command sequence.
[0102] Divide the time deviation at the current path node by the number of remaining path nodes to obtain the time compensation amount evenly distributed across each remaining path segment. This compensation is then used to correct the target speed for each remaining path segment. The formula for calculating the corrected target speed is: ; In the formula, Let Δt be the target velocity after compensation for the j-th path segment, and Δt be the time-based compensation amount allocated to each path segment. Let be the length of the i-th path segment. Nominal driving time This represents the departure time offset of the endpoint path node for this segment.
[0103] What needs to be known is that This represents the number of remaining path nodes after the current node. Let be the time deviation of the i-th path node.
[0104] When the travel time corresponding to the corrected target speed is less than the shortest travel time for the path segment, the path is truncated with the speed corresponding to the shortest travel time as the upper limit. The remaining uncompensated deviation after truncation is then passed to the path nodes to avoid exceeding the channel speed constraint.
[0105] In this embodiment, after the robot reaches the fifth node, the remaining path nodes are the second node. The time deviation of +0.15s is fully distributed between the fifth node and the second node. The original adjusted travel time was 3.77s, with a compensation of 0.15s. The corrected target travel time is 3.77 - 0.15 = 3.62s.
[0106] Through step S3.3, the shortest travel time from the fifth node to the second node is 3.7s. Since 3.62s < 3.7s, the corrected travel time is lower than the lower bound of the constraint. Therefore, the truncation process is performed, and the corrected target travel time is adjusted to 3.7s. The compensated target speed is 5.5 / 3.7 ≈ 1.486m / s.
[0107] The remaining uncompensated deviation is 0.15−(3.77−3.7)=0.08s. Since the second node is the end point of the planned path and there is no remaining path segment, this residual deviation is recorded in the system log and will not be passed on. It will be absorbed and digested by the position controller at the end point of the path.
[0108] The resulting compensated speed command sequence is that the robot starts from the fifth node and travels to the second node at 1.486 m / s, with the expected arrival time of the second node updated to 4.15 + 3.7 = 7.85 s.
[0109] S4.4 Adjust the robot using the compensated speed command sequence.
[0110] The compensated speed command sequence obtained in step S4.3 is sent to the robot's underlying motion controller. The underlying controller then performs closed-loop speed regulation on the robot's drive motors according to the speed commands, driving the robot to pass through each path node in sequence.
[0111] When the robot reaches each path node, steps S4.2 to S4.3 are repeated to recalculate the current time deviation with the latest actual arrival time and update the speed commands for subsequent segments, thereby achieving rolling compensation for execution errors until the robot reaches the end of the planned path.
[0112] In this embodiment, the robot starts from node five at 1.486 m / s and drives itself along the straight-line skeleton towards the second node according to the speed command.
[0113] When the robot's actual arrival time at the second node is 7.88s, which differs from the updated expected arrival time of 7.85s by +0.03s, the deviation is absorbed by the endpoint position controller since the second node is the end point of the planned path, and the overall navigation task is completed.
[0114] Example 3 is an embodiment of the present invention, which provides a robot autonomous navigation control system, including: The graph construction module is used to match the robot's current pose with the topology of the channel region in the static grid map, extract the spatial constraint relationship at each path node in the channel region, and obtain the node constraint graph. The trajectory calculation module is used to extrapolate the obstacle trajectory of the robot's perception data in a continuous control cycle. Taking the spatial constraints of each node in the path node constraint diagram as the boundary, it calculates the time window of the obstacle's occupancy at each path node to obtain the node timing occupancy table. The evaluation module is used to perform a first time-series evaluation based on the idle time window of each path node in the node time-series placeholder table and the expected time when the robot arrives at each path node, and to filter out the departure time offset at each path node that the robot can pass through without conflict, thereby obtaining the time-series adjustment sequence. The execution module performs feedforward adjustments to the robot based on the timing control sequence.
[0115] This embodiment also provides an electronic device applicable to a robot autonomous navigation control method, comprising: a memory and a processor; the memory is used to store computer-executable instructions, and the processor is used to execute the computer-executable instructions to implement the robot autonomous navigation control method proposed in the above embodiment.
[0116] This embodiment also provides a storage medium on which a computer program is stored. When the program is executed by a processor, it implements a robot autonomous navigation control method as proposed in the above embodiments.
[0117] The storage medium proposed in this embodiment and the method for implementing autonomous navigation control of a robot proposed in the above embodiments belong to the same inventive concept. Technical details not described in detail in this embodiment can be found in the above embodiments, and this embodiment has the same beneficial effects as the above embodiments.
[0118] Based on the above description of the implementation methods, those skilled in the art can clearly understand that the present invention can be implemented using software and necessary general-purpose hardware, and of course, it can also be implemented using hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as a computer floppy disk, read-only memory (ROM), random access memory (RAM), flash memory, hard disk, or optical disk, etc., including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods of the various embodiments of the present invention.
[0119] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.
Claims
1. A robot autonomous navigation control method, characterized in that, Includes the following steps: The robot's current pose is matched with the topology of the channel region in the static grid map, and the spatial constraint relationship at each path node in the channel region is extracted to obtain the node constraint map; The robot's perception data during continuous control cycles is extrapolated to the obstacle trajectory. Using the spatial constraints of each node in the path node constraint diagram as the boundary, the time window for the obstacle to occupy each path node is calculated to obtain the node timing occupancy table. The first timing evaluation is performed using the idle time window of each path node in the node timing placeholder table and the expected time when the robot arrives at each path node. The departure time offset at each path node that the robot can pass through without conflict is selected to obtain the timing control sequence. The robot is fed forward adjusted according to the timing control sequence.
2. The robot autonomous navigation control method as described in claim 1, characterized in that, The steps for obtaining the node timing placeholder table include: The sensing data within a continuous control cycle is differentially processed according to the cycle number to obtain the cycle displacement sequence of the obstacle. The trajectory of the obstacle is nonlinearly extrapolated based on the changing trend of the displacement in the periodic displacement sequence to obtain the extrapolated trajectory. The time interval at which the extrapolated trajectory intersects with the spatial constraint boundaries of each path node is used as the occupancy time window. By summarizing all the time windows for the node's time slots, we obtain the node time sequence placeholder table.
3. The robot autonomous navigation control method as described in claim 2, characterized in that, The steps for nonlinear extrapolating the trajectory of an obstacle include: The change in the angle between the periodic displacement vectors of adjacent control cycles is used as the curvature estimate. Using the curvature estimate and the magnitude of the periodic displacement vector of the current control cycle as driving parameters, the trajectory segments within the current extrapolation step size are generated by superimposing the curvature estimate with the directional deflection amount corresponding to the current motion direction. Using the endpoint position and direction vector at the endpoint of the trajectory segment as the extrapolation start state for the next control cycle, the trajectory segments are gradually spliced together to obtain the extrapolated trajectory. The step of calculating the curvature estimate includes: Using the angle change over multiple consecutive control cycles as input, weights are assigned to each angle change according to the cycle number to obtain the weighted value corresponding to each angle change. The sum of all weighted values is used as the curvature estimate for the current control cycle.
4. The robot autonomous navigation control method as described in claim 3, characterized in that, The steps for selecting the departure time offsets at each path node that allow the robot to pass without conflict include: The time difference between the start time of the idle time window of each path node and the expected time when the robot arrives at each path node is used as the candidate offset of each path node. The candidate offsets of all path nodes are accumulated in the order of the path nodes to obtain the accumulated travel time of each path node. The candidate offsets corresponding to the path nodes whose cumulative travel time is less than the shortest travel time are replaced with the smallest candidate offsets that satisfy the shortest travel time constraint, and the verified candidate offsets are obtained. The verified candidate offsets of all path nodes are aggregated to obtain the timing control sequence.
5. The robot autonomous navigation control method as described in claim 4, characterized in that, The steps to determine the shortest travel time between adjacent path nodes include: Using the path segment length between adjacent path nodes in the node constraint graph as input, and combining it with the robot's maximum allowable speed within the spatial constraint width corresponding to the path segment, the lower bound of the motion time of the path segment is obtained. Based on the time resolution of the time window occupied at adjacent path nodes, the lower bound of the movement time is rounded up to an integer multiple of the time resolution to obtain the shortest travel time between adjacent path nodes.
6. The robot autonomous navigation control method as described in claim 5, characterized in that, The steps for feedforward adjustment of the robot based on the time-series control sequence include: Using the departure time offset of each path node in the time-series control sequence and the path segment length between adjacent path nodes as input, the quotient of the path segment length and the travel time after adjustment of the departure time offset is used as the target speed of the path node. When the robot arrives at each path node, the difference between the robot's actual arrival time and the target arrival time of the corresponding path node in the timing control sequence is calculated to obtain the time deviation of the current path node. The time deviation is evenly distributed to the target speed correction amount of each path node according to the number of remaining path nodes, and the target speed of each path node is corrected to obtain the compensated speed command sequence. The robot is adjusted using a compensated speed command sequence.
7. The robot autonomous navigation control method as described in claim 6, characterized in that, The steps to obtain the node constraint graph include: Morphological skeleton extraction is performed on the channel regions in the static raster map to obtain the channel skeleton; Key point detection is performed on the channel skeleton, and the detection results are used as path nodes; With each path node as the center, the passage area is scanned laterally, and the passage width at each path node is used as the spatial constraint of that node. The channel skeleton between adjacent path nodes is used as a path segment, and a node constraint graph is constructed by combining the spatial constraints of each path node.
8. A robot autonomous navigation control system, employing the robot autonomous navigation control method as described in any one of claims 1 to 7, characterized in that, include: The graph construction module is used to match the robot's current pose with the topology of the channel region in the static grid map, extract the spatial constraint relationship at each path node in the channel region, and obtain the node constraint graph. The trajectory calculation module is used to extrapolate the obstacle trajectory of the robot's perception data in a continuous control cycle. Taking the spatial constraints of each node in the path node constraint diagram as the boundary, it calculates the time window of the obstacle's occupancy at each path node to obtain the node timing occupancy table. The evaluation module is used to perform a first time-series evaluation based on the idle time window of each path node in the node time-series placeholder table and the expected time when the robot arrives at each path node, and to filter out the departure time offset at each path node that the robot can pass through without conflict, thereby obtaining the time-series adjustment sequence. The execution module performs feedforward adjustments to the robot based on the timing control sequence.
9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the robot autonomous navigation control method according to any one of claims 1 to 7.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the robot autonomous navigation control method according to any one of claims 1 to 7.