Obstacle avoidance navigation method and system for service robot based on spatial topology cognition

Abstract

The application discloses a service robot obstacle avoidance navigation method and system based on space topology cognition, relates to the technical field of robot navigation, and comprises the following steps: S1, collecting door domain navigation data, and performing preprocessing on the door domain navigation data; S2, constructing a door domain time width matrix, a door body opening matrix, a robot prediction occupation matrix and a door domain remaining opening matrix, screening a passing section, and generating an automatic door passing topology node result; S3, constructing a phase navigation graph, generating a phase candidate path sequence, and screening to obtain a phase folding path result; S4, constructing a topology projection forbidden sector, and combining a door node phase window to perform waiting passing, door passing or backtracking to regenerate a phase folding path, and outputting a service robot obstacle avoidance navigation result. The problems that in the prior art, in a door domain scene such as an automatic door, a passing window is difficult to accurately determine, dynamic obstacles are difficult to effectively constrain, and as a result, door passing decision is unstable and navigation efficiency is insufficient are solved.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, specifically to a service robot obstacle avoidance navigation method and system based on spatial topology cognition. Background Technology

[0002] With the continuous expansion of applications in intelligent manufacturing, commercial services, medical delivery, and building inspection, service robots have evolved from simple indoor fixed-point movement to autonomous navigation equipment oriented towards complex open environments. As navigation tasks shift from path following in static environments to continuous autonomous navigation in dynamic environments, robot navigation and control technology has also evolved from traditional fixed path planning to comprehensive navigation technology that integrates environmental perception, path decision-making, and dynamic obstacle avoidance. Visual intelligence-related technologies are also gradually being applied to service robots' state perception and navigation behavior control in complex scenarios. Especially in passage areas with obvious spatial topological boundaries and high-frequency dynamic interference characteristics, such as shopping mall entrances and exits, building lobbies, elevator vestibules, and automatic door passages, how to ensure the robot's continuous passage efficiency while also ensuring obstacle avoidance safety has become an important research direction in the field of service robot navigation and control.

[0003] For example, application CN121764104A discloses a robot navigation obstacle avoidance method and a robot. This relates to the field of robot control technology. The method is applied to a robot and includes: responding to a navigation command, determining the remaining global path distance between the robot's current pose and the global endpoint pose; if the remaining global path distance is less than a second preset distance threshold, performing obstacle detection on the target area where the global endpoint is located and obtaining the obstacle detection result for the target area; if there are no obstacles in the target area, controlling the robot to continue walking on the current navigation path until the robot reaches the global endpoint pose; if there are obstacles in the target area, obtaining the obstacle's attribute information; generating a corresponding obstacle avoidance strategy based on the attribute information, and controlling the robot to execute the obstacle avoidance strategy to reach the global endpoint pose.

[0004] For example, application CN119781484B discloses a robot navigation obstacle avoidance method and apparatus. The method includes accessing a camera network of a navigation scene, periodically acquiring images from all cameras, recognizing the images, locating obstacle outlines, and constructing a scene map based on the obstacle outlines; arranging the scene map in chronological order, and extending the obstacle outlines in the latest scene map; determining the robot's motion path based on the extended scene map, sending it to the robot as navigation information; and determining the application frequency of the robot's obstacle avoidance module based on the extended scene map, and sending it to the robot.

[0005] However, although existing technologies can achieve robot navigation and obstacle avoidance control from the perspectives of obstacle detection in the endpoint area, obstacle attribute recognition, scene map construction, obstacle contour extension, and dynamic path adjustment, there are still key shortcomings in scenarios where service robots pass through automatic doors, lobby passages, and other areas with clear doorway boundaries. On the one hand, some solutions focus on detecting, recognizing, or extending obstacles and generating obstacle avoidance strategies or adjusting movement paths accordingly, but lack a time-width integrated spatial expression method for the doorway passage process. It is difficult to integrate the door opening status, effective doorway width, robot passage occupancy, and obstacle occupancy range into the same topological cognition framework for passage determination. On the other hand, existing path planning and obstacle avoidance control are mostly based on general scene maps or endpoint area detection results for decision-making. They lack phase constraint mechanisms for the passability of automatic door nodes and topological projection prohibition determination mechanisms for lateral crossing obstacles in front of doors. This easily leads to problems such as inaccurate judgment of passage windows, frequent backtracking after local doorway failure, or low efficiency of replanning in doorway scenarios, making it difficult to achieve stable obstacle avoidance navigation for service robots based on spatial topological cognition.

[0006] Therefore, in order to address the above problems, there is an urgent need for service robot obstacle avoidance and navigation methods and systems based on spatial topology cognition. Summary of the Invention

[0007] Technical problems to be solved To address the shortcomings of existing technologies, this invention provides a service robot obstacle avoidance and navigation method and system based on spatial topology cognition. This solves the problems of existing service robots having difficulty in accurately determining the passage window and effectively constraining dynamic obstacle interference in doorway scenarios such as automatic doors, resulting in unstable doorway decision-making and insufficient navigation efficiency.

[0008] Technical solution To achieve the above objectives, the present invention provides the following technical solution: a service robot obstacle avoidance and navigation method based on spatial topology cognition, comprising: S1, collecting doorway navigation data, and performing anomaly removal, missing data completion, and temporal reconstruction processing on the doorway navigation data, outputting preprocessed doorway navigation data; S2, constructing a doorway temporal width matrix, a doorway openness matrix, and a robot predicted occupancy matrix based on the preprocessed doorway navigation data, and generating a doorway remaining openness matrix by combining obstacle occupancy, filtering passage segments by embedding and comparing the robot predicted occupancy matrix and the doorway remaining openness matrix, and generating automatic doorway navigation data. S3: Based on the automatic door passage topology node results and ordinary topology nodes, construct a phase navigation map, extract the door node phase window and door node passage width constraint, calculate the phase passage duration value and the remaining passage width value of the connecting edge, generate a phase candidate path sequence, and filter to obtain the phase folding path result; S4: Based on the phase folding path result and the preprocessed door domain navigation data, perform connecting edge navigation, construct the topology projection forbidden sector, and combine the door node phase window to perform waiting passage, passage through the door, or backtracking to regenerate the phase folding path, and output the service robot obstacle avoidance navigation result.

[0009] Further, the specific steps for collecting doorway navigation data and performing anomaly removal, missing data completion, and temporal reconstruction on the doorway navigation data are as follows: Collect doorway navigation data during service robot passage. This data includes data collection timestamps, automatic door number, automatic door node number, door opening start timestamp, door closing start timestamp, door opening duration, door leaf displacement, left boundary position of the doorway, right boundary position of the doorway, doorway width, number of obstacles in front of the door, distance to dynamic obstacles in front of the door, lateral movement speed of dynamic obstacles in front of the door, robot's current horizontal coordinate, and robot... The system collects the following data: current position ordinate, robot heading angle, robot linear velocity, distance from the center line of the automatic door, robot body width, robot body length, minimum turning radius, maximum allowable linear velocity, control command issuance delay, braking response time, target speed through the doorway center, and safe stopping distance in front of the door. For the collected doorway navigation data, the system employs a Hamper filter algorithm to remove abnormal jump values ​​and performs robust smoothing through continuous sampling. A piecewise linear interpolation algorithm is then used to complete short-term missing records and reconstruct the temporal sequence of the doorway navigation data, outputting the preprocessed doorway navigation data.

[0010] Furthermore, the specific steps for constructing the door domain time-width matrix, door opening matrix, and robot prediction occupancy matrix based on the preprocessed door domain navigation data are as follows: Read the preprocessed door domain navigation data, collect it according to the automatic door number and automatic door node number, and arrange it in ascending order according to the data acquisition timestamp to form a door node time sequence record; for each record, subtract the left boundary position value from the right boundary position value of the door opening, retaining records where the absolute value of the difference is not greater than the error threshold; then, divide the net width value of the door opening laterally into multiple [various categories] according to a fixed width n. The door width unit divides the data acquisition timestamp into multiple door domain time units at fixed time intervals t, and establishes a door domain time-width matrix using the door domain time units and door domain width units as indices. The door domain time-width matrix and the door node time sequence are read. For each record, the door leaf opening displacement value is first divided by the net width of the door opening to obtain the door opening ratio value. Then, the door opening ratio value is multiplied by the total number of door domain width units and rounded down to obtain the number of opening width units. Finally, starting from the door opening start timetamp, and using the door closing start timetamp and... The earlier of the door opening start timestamp and the door opening duration is used as the endpoint to determine the effective opening time interval corresponding to the current record. Within the effective opening time interval, matrix units with the doorway centerline as the center of symmetry and a number equal to the number of opening width units are marked as door open units, and the remaining matrix units are marked as door closed units, generating a door open matrix. The door node timing record sequence is read, and for each record, the predicted arrival time of the robot at the automatic door centerline is calculated based on the robot's distance from the automatic door centerline, robot linear velocity, maximum allowable linear velocity, control command issuance delay, braking response duration, and the target speed value passing through the doorway centerline. Based on the robot's body length, robot body width, minimum turning radius, and heading angle, the predicted door passage time interval and the number of lateral occupancy width units are calculated. Then, the door open matrix is ​​read, and a robot predicted occupancy matrix is ​​generated, starting from the door domain time unit corresponding to the predicted arrival time and extending to the continuous door domain time units corresponding to the predicted door passage time interval.

[0011] Further, the specific steps for generating the remaining open matrix of the door region in conjunction with obstacle occlusion are as follows: Read the robot's predicted occupancy matrix, the door open matrix, and the door node time sequence. For each record, based on the lateral movement speed value of the dynamic obstacle in front of the door and the predicted door passage time interval, calculate the lateral sweep width value of the dynamic obstacle in front of the door within the predicted door passage time interval, and convert it into the number of obstacle occupancy width units by combining the number of obstacles in the door area. Mark the door region width units that are equal to the number of obstacle occupancy width units and are continuously distributed along the lateral movement direction of the dynamic obstacle in front of the door, with the door region width unit corresponding to the center line of the door opening as the starting reference. Subtract the obstacle occupancy units from the door open matrix to obtain the remaining open matrix of the door region.

[0012] Furthermore, the specific steps for generating automatic door access topology node results by embedding and comparing the robot's predicted occupancy matrix with the remaining open matrix of the door domain to filter access segments are as follows: The robot's predicted occupancy matrix is ​​compared cell by cell with the remaining open matrix of the door domain, retaining records where the robot's predicted occupancy matrix falls entirely within the remaining open matrix of the door domain, generating access records that can be embedded into the door node; all access records that can be embedded into the door node corresponding to the same automatic door node are read, and records where the difference between two adjacent predicted door arrival times is equal to a fixed time interval t and the corresponding horizontal occupancy width cell count remains unchanged are merged into candidate access segments; for each candidate access segment, the segment start data acquisition timestamp and segment end timestamp are calculated. The data acquisition timestamp, the minimum remaining width unit value of the segment, the corresponding horizontal occupied width unit value, and the maximum deviation of the robot's heading angle relative to the centerline of the automatic door within the segment are collected. First, the candidate passage segment with the largest minimum remaining width unit value is selected. When there are multiple candidate passage segments with the same minimum remaining width unit value, the candidate passage segment with the smallest maximum deviation of the robot's heading angle is selected as the target passage segment. The start data acquisition timestamp, end data acquisition timestamp, minimum remaining width unit value, and corresponding horizontal occupied width unit value of the target passage segment are written into the corresponding automatic door node to generate the automatic door passage topology node result.

[0013] Furthermore, based on the automatic door access topology node results and ordinary topology nodes, the specific steps for constructing a phase navigation map and extracting the door node phase window and door node passage width constraints are as follows: Read the automatic door access topology node results and the ordinary topology nodes of the service robot's scene to construct an initial navigation topology map containing ordinary topology nodes, automatic door nodes, and node connection relationships; for each automatic door node, determine the start and end data acquisition timestamps as the door node phase window, and use the minimum remaining width unit value of the segment as the base value of the door node passage width; for each connecting edge, read the node coordinate values ​​corresponding to the starting and ending nodes of the connecting edge, calculate the connecting edge length value, and generate the phase navigation map; read the phase navigation map and the preprocessed door domain navigation data, and for each connecting edge, based on the robot's current position... The predicted edge arrival time of the robot to the endpoint node is calculated using the x-coordinate value, the y-coordinate value of the robot's current position, the robot's linear velocity value, the robot's maximum allowable linear velocity value, the control command issuance delay value, the braking response time value, and the connection edge length value. When the endpoint node is a normal topology node, the phase wait value is recorded as zero. When the endpoint node is an automatic gate node, the predicted edge arrival time is compared with the gate node's phase window. If the predicted edge arrival time is earlier than the gate node's phase window start data acquisition timestamp, the time difference between the predicted edge arrival time and the gate node's phase window start data acquisition timestamp is used as the phase wait value. If the predicted edge arrival time is within the gate node's phase window, the phase wait value is recorded as zero. If the predicted edge arrival time is later than the gate node's phase window end data acquisition timestamp, the corresponding connection edge is marked as a frozen edge.

[0014] Further, the specific steps for calculating the phase passage duration and remaining passage width of the connecting edge, generating a phase candidate path sequence, and filtering to obtain the phase folding path results are as follows: Read the phase navigation map; for connecting edges not marked as frozen edges, divide the connecting edge length by the robot's maximum allowable linear velocity, then add it to the phase waiting value, control command issuance delay, and braking response duration to obtain the connecting edge phase passage duration; when the endpoint node is an automatic gate node, calculate the difference between the gate node's passage width base value and the corresponding lateral occupied width unit value to obtain the connecting edge's remaining passage width value; expand from the starting node... For each candidate path, the phase passage time of each connecting edge is accumulated, and the minimum value of the remaining passage width of the connecting edge in each automatic gate node within the path is determined as the continuous passage width value of the path, generating a phase candidate path sequence; the phase candidate path sequence is read, and the candidate path with the largest continuous passage width value is selected first; when there are candidate paths with the same continuous passage width value, the candidate path with the smallest accumulated phase passage time of the connecting edge is selected, and the node sequence, connecting edge sequence, gate node phase window and continuous passage width value corresponding to the target path are output, generating the phase folding path result.

[0015] Further, the specific steps for constructing the topology projection restricted sector by performing connection edge navigation based on the phase folding path results and preprocessed gate navigation data are as follows: Read the phase folding path results and preprocessed gate navigation data, and perform navigation according to the current connection edge in the target path; when the endpoint node of the current connection edge is an automatic gate node, determine the vertex of the topology projection restricted sector based on the robot's current position x-coordinate value and the robot's current position y-coordinate value, determine the center line of the topology projection restricted sector based on the current connection edge direction, determine the radius of the topology projection restricted sector based on the distance value of the dynamic obstacle in front of the gate, calculate the lateral projection width value based on the product of the lateral movement speed value of the dynamic obstacle in front of the gate and the time difference between the current data acquisition timestamp and the data acquisition timestamp at the end of the gate node phase window, and then determine the two boundary lines of the topology projection restricted sector by connecting the projection boundary corresponding to the lateral projection width value and the sector vertex, thus generating the topology projection restricted sector.

[0016] Furthermore, the specific steps for outputting the service robot obstacle avoidance navigation results, combined with the gate node phase window execution of waiting to pass, passing through the gate, or backtracking to regenerate the phase folding path, are as follows: Read the gate node phase window. When the current data acquisition timestamp is earlier than the gate node phase window's initial data acquisition timestamp, control the robot to decelerate and stop at a position equal to the safe stopping distance from the center line of the automatic door. When the current data acquisition timestamp is within the gate node phase window and the robot's current heading angle's forward ray does not intersect with the topological projection forbidden sector, control the robot to pass through the automatic door node along the current connecting edge. When the current data acquisition timestamp is later than the gate node phase window's final data acquisition timestamp, or the robot's current heading angle's forward ray intersects with the topological projection forbidden sector, mark the current connecting edge as a phase mismatch edge, backtrack to the previous ordinary topological node, and regenerate the phase folding path using the previous ordinary topological node as the current starting node, outputting the service robot obstacle avoidance navigation results.

[0017] The second aspect of this invention provides a service robot obstacle avoidance and navigation system based on spatial topology cognition, comprising: a gate domain acquisition and processing module, a gate domain access modeling module, a phase path generation module, and a projection obstacle avoidance execution module, wherein: the gate domain acquisition and processing module is used to acquire gate domain navigation data, and perform anomaly removal, missing data completion, and temporal reconstruction processing on the gate domain navigation data, outputting preprocessed gate domain navigation data; the gate domain access modeling module is used to construct a gate domain temporal width matrix, a gate opening matrix, and a robot predicted occupancy matrix based on the preprocessed gate domain navigation data, and generate a gate domain remaining open matrix by combining obstacle occupancy, and filter the system by embedding the robot predicted occupancy matrix and the gate domain remaining open matrix. The system selects passage segments and generates automatic door passage topology node results. The phase path generation module is used to construct a phase navigation map based on the automatic door passage topology node results and ordinary topology nodes, extract the door node phase window and door node passage width constraint, calculate the phase passage duration value and the remaining passage width value of the connecting edge, generate a phase candidate path sequence, and filter to obtain the phase folding path results. The projection obstacle avoidance execution module is used to execute connecting edge navigation based on the phase folding path results and preprocessed door domain navigation data, construct topology projection forbidden sectors, and combine the door node phase window to execute waiting passage, passage through the door, or backtracking to regenerate the phase folding path, and output the service robot obstacle avoidance navigation results.

[0018] Beneficial effects The present invention has the following beneficial effects: (1) A service robot obstacle avoidance navigation method and system based on spatial topology cognition, by constructing a door domain time-width matrix and further forming a door opening matrix, a robot prediction occupancy matrix and a door domain remaining open matrix, the automatic door passage determination no longer stays at the single comparison level of door opening and closing status or remaining opening time, but can simultaneously represent the door domain passability status in the time dimension and the lateral passage width dimension, thereby improving the precision and stability of automatic door node passage determination.

[0019] (2) Service robot obstacle avoidance navigation method and system based on spatial topology cognition, by performing embedding comparison between robot predicted occupancy matrix and gate domain remaining open matrix, the robot body occupancy requirement during the door passage process is uniformly matched with the remaining passable space of the gate domain. It can identify the actual embeddable passage segment before path generation, thereby reducing the risk of passage failure caused by accidental entry in front of the door, interruption of passage through the door and repeated attempts at the door.

[0020] (3) Service robot obstacle avoidance navigation method and system based on spatial topology cognition. By introducing the gate node phase window and the gate node passage width constraint to generate the phase folding path, the path selection is not only affected by the spatial connectivity relationship, but also by the automatic gate node passage timing and continuous passage width. Thus, the obtained target path takes into account both temporal matching and spatial traversability, and improves the rationality of path planning in complex gate domain scenarios.

[0021] (4) Service robot obstacle avoidance navigation method and system based on spatial topology cognition: By constructing topological projection restricted sectors and combining door node phase window execution waiting passage, passage through the door or back to regenerate phase folding path, the interference range of lateral moving obstacles in front of the door can be transformed into restricted constraints in the connection edge execution stage, thereby enhancing the obstacle avoidance execution capability of service robots under dynamic interference conditions and improving safety and passage efficiency in continuous navigation process. Attached Figure Description

[0022] Figure 1 Flowchart of a service robot obstacle avoidance navigation method based on spatial topology cognition; Figure 2 This is a structural diagram of a service robot obstacle avoidance and navigation system based on spatial topology cognition. Figure 3 This is a phase navigation graph based on gate node phase windows; Figure 4 This is a schematic diagram of the time-width embedding comparison of the gate domain. Detailed Implementation

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

[0024] Please see Figures 1-4This invention provides a technical solution: a service robot obstacle avoidance and navigation method based on spatial topology cognition, comprising: S1, collecting door domain navigation data, and performing anomaly removal, missing data completion, and temporal reconstruction processing on the door domain navigation data, outputting preprocessed door domain navigation data; S2, constructing a door domain temporal width matrix, a door body opening matrix, and a robot predicted occupancy matrix based on the preprocessed door domain navigation data, and generating a door domain remaining open matrix by combining obstacle occupancy, filtering passage segments by embedding and comparing the robot predicted occupancy matrix and the door domain remaining open matrix, and generating an automatic door passage topology segment. S3: Based on the automatic door access topology node results and ordinary topology nodes, construct a phase navigation map, extract the door node phase window and door node passage width constraint, calculate the phase passage duration value and the remaining passage width value of the connecting edge, generate a phase candidate path sequence, and filter to obtain the phase folding path result; S4: Based on the phase folding path result and the preprocessed door domain navigation data, perform connecting edge navigation, construct the topology projection forbidden sector, and combine the door node phase window to perform waiting passage, passage through the door, or backtracking to regenerate the phase folding path, and output the service robot obstacle avoidance navigation result.

[0025] Specifically, the steps for collecting door navigation data and performing anomaly removal, missing data completion, and time-series reconstruction on the door navigation data are as follows: Collect door navigation data when the service robot passes through. This data includes the data collection timestamp, automatic door number, automatic door node number, door opening start timestamp, door closing start timestamp, door opening duration, door leaf displacement, left boundary position of the doorway, right boundary position of the doorway, net width of the doorway, number of obstacles in front of the door, distance to dynamic obstacles in front of the door, lateral movement speed of dynamic obstacles in front of the door, horizontal coordinate of the robot's current position, vertical coordinate of the robot's current position, robot heading angle, robot linear velocity, and so on. The system includes the following parameters: distance from the person to the centerline of the automatic door; robot width; robot length; minimum turning radius; maximum permissible linear velocity; control command delay; braking response time; target speed at the center of the doorway; and safe stopping distance in front of the door. A local two-dimensional coordinate system is first constructed within the door area, with the intersection of the automatic door's centerline and the ground as the origin. The positive X-axis is parallel to the horizontal width of the doorway, and the positive Y-axis is perpendicular to the doorway plane and points towards the robot's approach direction. Door opening displacement is collected by the automatic door displacement sensor. The door opening start time stamp, door closing start time stamp, and door opening duration are determined by the automatic door controller. The system extracts the door closing signal, door opening signal, and hold-open timing signal; the left and right boundary position values ​​of the doorway are extracted by scanning the door frame boundary with a LiDAR and then uniformly converted to the X-axis of the local two-dimensional coordinate system of the door area; the net width of the doorway is obtained by subtracting the left boundary position value from the right boundary position value; the number of obstacles in front of the door is obtained by counting the number of independent obstacle targets in the detection area in front of the door; the distance to dynamic obstacles in front of the door is obtained by projecting the obstacle target's distance along the Y-axis of the local two-dimensional coordinate system of the door area; the lateral movement speed of dynamic obstacles in front of the door is obtained by dividing the change in displacement of the obstacle target along the X-axis by the corresponding time difference over two consecutive sampling periods; the robot's current position x-coordinate and the robot's position x-coordinate are also extracted. The current vertical coordinate of the robot is obtained by converting the robot's positioning results to the local two-dimensional coordinate system of the door area. The robot's heading angle is defined as the angle between the robot's forward direction and the positive Y-axis. The distance between the robot and the center line of the automatic door is taken as the absolute value of the robot's current vertical coordinate. The robot's body width, length, minimum turning radius, maximum allowable linear speed, control command delay, braking response time, target speed through the doorway center, and safe stopping distance in front of the door are determined by the robot chassis calibration results, control parameter table, and door area access debugging parameters. The data acquisition timestamp is written by a unified clock source at a fixed sampling period, which is between 0.05s and 0.The 20s range is determined jointly based on the duration of the automatic door opening process, the robot's approach speed to the door, and the lateral crossing speed of the dynamic obstacle in front of the door, ensuring continuous recording of changes in door state, robot pose, and obstacle lateral movement. For the collected door navigation data, a Hamper filter algorithm is used to remove abnormal jump values ​​and perform robust smoothing through continuous sampling. This includes recording the door opening displacement, left and right boundary positions of the doorway, the doorway's net width, the distance to the dynamic obstacle in front of the door, the lateral movement speed of the dynamic obstacle, the robot's current x-coordinate, the robot's current y-coordinate, the robot's heading angle, and the robot's linear velocity. Sliding windows were constructed using the value of the robot's distance from the centerline of the automatic door and the value of the robot's distance from the centerline. The length of the sliding window ranged from 3 to 11 sampling points, and the window length was determined by the separation effect of normal fluctuation amplitude and abnormal jump amplitude in historical door navigation samples. Within each sliding window, the local median and the median absolute deviation were calculated. When the absolute value of the deviation between the current sampled value and the local median was greater than a multiple threshold multiplied by the median absolute deviation, the current sampled value was identified as an abnormal jump value and replaced with the local median. The multiple threshold ranged from 2.5 to 4.5, and was determined after performing outlier identification experiments on historical door navigation samples to minimize door boundary fluctuations and robot... To prevent the accidental deletion of pose fluctuations and obstacle velocity fluctuations, and to filter out instantaneous jump points, a piecewise linear interpolation algorithm is used to perform short-term missing record completion and temporal reconstruction processing on the gate navigation data. First, the time series records are grouped according to the automatic gate number and automatic gate node number. Then, the time interval between adjacent data acquisition timestamps is checked. When the time interval is greater than the fixed sampling period but not greater than the short-term missing upper limit, the interval is determined as a short-term missing record. The short-term missing upper limit is set between 0.10s and 0.60s. This range is determined through on-site acquisition link continuity testing and gate state change testing to ensure that state changes can be approximately recovered by interpolation when the missing time is short, and that the missing time is long when the missing time is long. For long-term missing records, no distortion is introduced during completion. For short-term missing records, the starting and ending records of the missing interval are used as interpolation endpoints. Piecewise linear interpolation is performed on the door opening displacement value, the left boundary position value of the doorway, the right boundary position value of the doorway, the net width of the doorway, the distance value of the dynamic obstacle in front of the door, the lateral movement speed value of the dynamic obstacle in front of the door, the horizontal coordinate value of the robot's current position, the vertical coordinate value of the robot's current position, the robot's heading angle value, the robot's linear velocity value, and the distance value of the robot from the center line of the automatic door, to obtain a continuous time-series record consistent with the fixed sampling period. After anomaly removal and missing record completion, all records are sorted in ascending order according to the data acquisition timestamp, and the preprocessed door area navigation data is output.

[0026] In this implementation plan, the acquisition sources of gate navigation data, the construction method of the local two-dimensional coordinate system of the gate area, the range of key parameter values ​​and their determination basis, the judgment rules for abnormal jump values, and the boundary for filling short-term missing records are uniformly defined. Combined with the relevant perception results of visual intelligence to participate in gate boundary recognition, obstacle motion representation, and pose data correction, the preprocessed gate navigation data is kept consistent in terms of time reference, spatial reference, numerical reliability, and continuity. This provides a stable and reliable data foundation for the subsequent construction of the gate time-width matrix, generation of the gate opening matrix, generation of the robot prediction occupancy matrix, and generation of the gate remaining open matrix. In this way, the accuracy and repeatability of the automatic gate passage judgment results are improved, and the risk of gate status misjudgment caused by sampling jitter, coordinate chaos, abnormal jump points, and short-term missing records is reduced.

[0027] Specifically, the steps for constructing the gate domain time-width matrix, gate opening matrix, and robot predicted occupancy matrix based on the preprocessed gate domain navigation data are as follows: Read the preprocessed gate domain navigation data, collect it according to the automatic door number and automatic door node number, and arrange it in ascending order according to the data acquisition timestamp to form a gate node time-series record sequence; in specific implementation, the automatic door number defines the physical gate object, and the automatic door node number defines the gate domain passage position in the navigation topology, so that the same gate forms an independent time-series data stream within a continuous sampling period; for each record, subtract the left boundary position value of the door opening from the right boundary position value, and then compare it with the net width of the door opening. The values ​​are verified, and records with absolute differences not exceeding the error threshold are retained. The error threshold ranges from 0.02m to 0.08m and is determined based on the door frame calibration results, the statistical results of LiDAR boundary extraction errors, and the statistical results of visual intelligent boundary recognition errors. Then, the net width of the doorway is divided into multiple doorway width units along the horizontal direction according to a fixed width n, and the data acquisition timestamp is divided into multiple doorway time units according to a fixed time interval t. A doorway time-width matrix is ​​established using the doorway time unit and the doorway width unit as indices. The fixed width n ranges from 0.05m to 0.15m, and the fixed time interval t ranges from 0.05s to 0.The technical principle behind establishing the door domain time-width matrix in 20s lies in mapping the continuously changing door domain passage space into a set of two-dimensional units in discrete time and discrete width dimensions, thereby providing a unified representation framework for the representation of the door's open area, obstacle occlusion area, and robot-occupied area. The door domain time-width matrix and the door node time-series record sequence are read. For each record, the door opening displacement value is first divided by the net width of the doorway to obtain the door opening ratio. Then, the door opening ratio is multiplied by the total number of door domain width units and rounded down to obtain the number of opening width units. Finally, the door opening start time stamp is used as the... Starting from the earlier of the door closing start timestamp and the door opening start timestamp plus the door opening duration, the effective opening time interval corresponding to the current record is determined. Within the effective opening time interval, matrix units symmetrical about the doorway centerline and equal in number to the number of opening width units are marked as door opening units, and the remaining matrix units are marked as door closing units, generating a door opening matrix. The door node timing record sequence is read, and for each record, based on the robot's distance from the automatic door centerline, robot linear velocity, maximum allowable robot linear velocity, and the time of control command issuance... The predicted arrival time of the robot at the center line of the automatic door is calculated by combining the delay value, braking response time value, and the target speed value passing through the doorway center. In specific implementation, the distance between the robot and the center line of the automatic door is first determined as the approach distance, and the robot's linear velocity value is determined as the current approach speed. When the current approach speed is greater than the robot's maximum allowable linear velocity value, the maximum allowable linear velocity value is used instead of the current approach speed for calculation. When the current approach speed is less than or equal to zero, the target speed value passing through the doorway center is used as the approach speed for calculation. Finally, the approach distance is divided by the approach speed to obtain the basic approach time. The predicted door arrival time is obtained by adding the control command issuance delay value and the braking response time value, so that the predicted door arrival time simultaneously reflects spatial distance constraints, speed constraints, and control execution lag. Based on the robot body length value, robot body width value, robot minimum turning radius value, and robot heading angle value, the predicted door passage time interval and the number of lateral width units occupied are calculated. In specific implementation, the robot body length value divided by the target speed value passing through the doorway center is used as the basic door passage time, and then a safety margin time is added to obtain the predicted door passage time interval, where the safety margin time is taken from 0.10s to 0.50s; Using the robot's body width as the base lateral occupancy width, the steering envelope increment is determined based on the robot's minimum turning radius and heading angle. The base lateral occupancy width is then added to the steering envelope increment to obtain the robot's actual lateral occupancy width. The steering envelope increment increases with the absolute value of the robot's heading angle and decreases with the minimum turning radius, representing the robot's outward expansion relative to its static width during doorway correction. The actual lateral occupancy width is then divided by a fixed width *n* and rounded up to obtain the number of lateral occupancy width units, thus defining the robot's lateral occupancy range. A direct mapping relationship can be established with the gate width unit. Then, the gate opening matrix is ​​read, and a robot prediction occupancy matrix is ​​generated, starting from the gate time unit corresponding to the predicted gate entry time and extending to the continuous gate time units corresponding to the predicted gate passage time interval. This robot prediction occupancy matrix covers the continuous gate time units from the robot's entry into the gate to its complete exit from the gate in the time dimension, and covers the continuous gate width units corresponding to the number of lateral occupancy width units in the width dimension. This expresses the robot's gate passage behavior as a two-dimensional discrete occupancy region in the gate time-width matrix, providing a direct basis for subsequent embedding comparison with the remaining gate opening matrix.

[0028] In this implementation plan, by unifying and refining the construction basis, discretization rules, time boundary determination methods, and width occupancy mapping methods of the gate domain time-width matrix, gate opening matrix, and robot prediction occupancy matrix, the gate domain passage space can form a comparable, embeddable, and determinate discrete expression in both the time and width dimensions. This allows the automatic door opening state, robot approach process, and door passage occupancy requirements to be incorporated into the same judgment framework, improving the accuracy and stability of the automatic door passage topology node results and reducing passage judgment errors caused by door opening boundary recognition deviations, inaccurate time window divisions, and posture occupancy estimation distortions.

[0029] Specifically, the steps for generating the remaining open matrix of the gate region in conjunction with obstacle occlusion are as follows: Read the robot's predicted occupancy matrix, the gate's open matrix, and the time sequence of gate nodes. For each record, based on the lateral movement speed value of the dynamic obstacle in front of the gate and the predicted passage time interval, calculate the lateral sweep width value of the dynamic obstacle in front of the gate within the predicted passage time interval, and convert it into the number of obstacle occupancy width units by combining the number of obstacles in the area in front of the gate. In practice, first extract the lateral movement speed value of the dynamic obstacle in front of the gate corresponding to the current record in the local two-dimensional coordinate system of the gate region. The predicted passage time interval is represented by the duration between the robot entering the gate and the robot's tail leaving the gate. Multiply the absolute value of the lateral movement speed value of the dynamic obstacle in front of the gate by the length of the predicted passage time interval, and then superimpose the obstacle's base width value and the safety margin width value to obtain the lateral sweep width value of the dynamic obstacle in front of the gate within the predicted passage time interval. The obstacle's base width value is between 0.20m and 0.80m, and the safety margin width value is between 0.05m and 0.The obstacle base width is determined based on the statistical results of common target dimensions for dynamic obstacles in front of the door, while the safety margin width is determined based on the statistical results of ranging errors and the safety requirements for obstacle avoidance in front of the door. When the number of obstacles in the area in front of the door is equal to 1, the lateral sweep width is determined as the total lateral occupancy width. When the number of obstacles in the area in front of the door is greater than 1, the union of the lateral sweep widths of each dynamic obstacle on the horizontal side of the doorway is used as the total lateral occupancy width. When the lateral distribution relationship of each dynamic obstacle cannot be distinguished, the lateral sweep width value is multiplied by the number of obstacles in the area in front of the door as the upper limit of the total lateral occupancy width. Subsequently, the total lateral occupancy width is divided by a fixed width n and rounded up to obtain the number of obstacle occupancy width units, where the fixed width is n. The fixed width *n* is consistent with the gate width cell division rule in the gate domain time-width matrix, enabling the conversion of continuous width occupancy into discrete width occupancy. The technical principle behind this processing method is that the impact of dynamic obstacles in front of the gate on the lateral passage space of the doorway within the predicted passage time interval is manifested as a sweeping occupancy range formed by the obstacle's own lateral dimensions, lateral movement displacement, and ranging uncertainty. Therefore, mapping the obstacle's base width value, the lateral movement speed value of the dynamic obstacle in front of the gate, the predicted passage time interval, and the safety margin width value together into the number of obstacle occupancy width cells allows the conversion of continuous motion influence into directly comparable discrete occupancy influence within the gate domain time-width matrix. The number of cells equal to the number of obstacle occupancy width cells and... The door width unit corresponding to the center line of the doorway is used as the starting reference. Door width units continuously distributed along the lateral movement direction of the dynamic obstacle in front of the door are marked as obstacle occupancy units. In specific implementation, the door width unit corresponding to the center line of the doorway is used as the starting reference. The lateral movement direction of the dynamic obstacle in front of the door is determined according to the sign of the lateral movement speed value. Door width units are selected sequentially along the lateral movement direction of the dynamic obstacle in front of the door until the total number of selected door width units reaches the number of width units occupied by the obstacle. The selected door width units are marked as obstacle occupancy units within all door time units covered by the corresponding predicted door passage time interval. The obstacle occupancy units are subtracted from the door opening matrix to obtain the door... The remaining open space matrix is ​​used to characterize the actual remaining passable space of the automatic door in any door region time unit and any door region width unit under the combined effects of door opening constraints and obstacle occupancy constraints. In practice, occupancy checks are performed on each matrix cell marked as an open door unit. If a matrix cell also belongs to an obstacle occupancy unit, its open state is changed to a restricted state. If a matrix cell does not belong to an obstacle occupancy unit, its open state remains unchanged. After updating all matrix cells, the remaining open space matrix is ​​formed. This matrix represents the actual remaining passable space of the automatic door in any door region time unit and any door region width unit under the combined effects of door opening constraints and obstacle occupancy constraints. This provides a direct basis for the subsequent embedding comparison between the robot's predicted occupancy matrix and the remaining open space matrix.

[0030] In this implementation scheme, the lateral movement speed of dynamic obstacles in front of the door, the predicted passage time interval, and the number of obstacles in the area in front of the door are uniformly mapped to the number of obstacle occupancy width units. Under the door domain time-width matrix, an obstacle occupancy unit distribution corresponding to the door opening matrix is ​​formed. This allows the remaining open matrix of the door domain to simultaneously represent the joint constraints of the door opening boundary and the dynamic obstacle compression boundary on the actual passage space. This improves the realism and consistency of the expression of the remaining passable space of the automatic door node and reduces the risk of misjudgment in the robot's predicted occupancy matrix embedding caused by the insufficient discretization of the influence of continuous obstacle movement.

[0031] Specifically, the steps for generating automatic gate access topology nodes by embedding and comparing the robot's predicted occupancy matrix with the gate's remaining open matrix are as follows: The robot's predicted occupancy matrix is ​​compared cell by cell with the gate's remaining open matrix, and records where the robot's predicted occupancy matrix falls entirely within the gate's remaining open matrix are retained, generating gate node embeddable access records. In practice, the gate's time unit and width unit are used as unified comparison indices to perform state checks on each matrix unit covered by the robot's predicted occupancy matrix. When any matrix unit corresponds to a restricted state in the gate's remaining open matrix, the current record is determined to be an unembeddable record, and the current record will not proceed to the subsequent cell splicing process. The current record is only considered an embeddable passage record for a door node if all matrix units covered by the robot's predicted occupancy matrix are in an open state in the remaining open matrix of the door region. The principle behind the unit-by-unit embedding comparison technique is to map the robot's door passage occupancy requirement to the actual remaining open space of the door region within the same discrete time-width framework, thereby unifying door opening constraints, obstacle occupancy constraints, and robot occupancy constraints into a single spatial accommodability determination at the matrix unit level. All embeddable passage records for the same automatic door node are read, and records where the difference between two adjacent predicted door times equals a fixed time interval t and the corresponding number of lateral occupancy width units remains unchanged are merged into candidate passage segments. The fixed time interval t ranges from 0.05s to 0.20s. This fixed time interval t is consistent with the time discrete granularity during the establishment of the gate domain time-width matrix, ensuring that candidate passage segments maintain a continuous splicing relationship in the time dimension. In specific implementation, all gate nodes are arranged in ascending order of predicted gate time, and the embeddable passage records are checked one by one to examine the changes in the time difference and the number of horizontal width units occupied between adjacent records. When the time difference equals the fixed time interval t and the number of horizontal width units occupied remains unchanged, the current record is merged into the candidate passage segment corresponding to the previous record. When the time difference is greater than the fixed time interval t or the number of horizontal width units occupied changes, the current candidate passage segment is terminated and the next one is restarted. Candidate passage segments; the technical principle of this splicing method is that only embeddable records that are continuous in time and have a stable width occupancy are considered as the same passage segment, which can avoid mistakenly splicing discrete passable records that are discontinuous in time and abrupt in width into a single continuous passage interval; for each candidate passage segment, the following are statistically analyzed: the start data acquisition timestamp of the segment, the end data acquisition timestamp of the segment, the minimum remaining width unit value of the segment, the corresponding horizontal occupancy width unit value, and the maximum deviation value of the robot's heading angle relative to the center line direction of the automatic door within the segment; in specific implementation, the minimum remaining width unit value of the segment is taken from the minimum remaining open width of the door domain among all the door node embeddable passage records covered by the current candidate passage segment;The centerline direction of the automatic door is defined as the direction passing through the centerline of the automatic door and pointing from the starting node of the current connecting edge to the ending node of the current connecting edge. This direction is consistent with the target forward direction of the robot during the door passage phase. The maximum deviation of the robot's heading angle relative to the centerline direction of the automatic door within a segment is taken from the maximum absolute value of the angle between the robot's heading angle and the centerline direction of the automatic door in all records covered by the current candidate passage segment. This represents the most unfavorable passage boundary in the width dimension and the maximum correction requirement in the attitude dimension of the current candidate passage segment, respectively. First, the candidate passage segment with the largest number of minimum remaining width units is selected. When there are multiple candidate passage segments with the same number of minimum remaining width units, the candidate passage segment with the smallest maximum deviation of the robot's heading angle is selected as the target passage segment. The technical principle of this selection rule is to prioritize retaining the candidate passage segment with the most sufficient remaining width, so that the target passage segment has higher stability in terms of spatial capacity, and then, under the same width conditions... The algorithm prioritizes candidate passage segments with smaller attitude deviations, reducing the robot's correction actions and improving passage continuity when entering automatic gate nodes. The start and end timestamps of the target passage segment, the minimum remaining width unit value, and the corresponding lateral occupancy width unit value are written to the corresponding automatic gate node to generate the automatic gate passage topology node result. In practice, the start and end timestamps jointly determine the effective gate node phase window, the minimum remaining width unit value represents the minimum continuous passable width of the automatic gate node within the target passage segment, and the corresponding lateral occupancy width unit value represents the stable occupancy requirement when the robot traverses the current automatic gate node. This ensures that the automatic gate passage topology node result includes both passable boundaries in the time and width dimensions, providing a direct basis for subsequent phase navigation map construction, calculation of connection edge phase passage duration, and calculation of connection edge remaining passage width.

[0032] In this implementation scheme, the embedding comparison results of the robot's predicted occupancy matrix and the remaining open matrix of the gate region are further organized into gate node embeddable passage records, candidate passage segments, and automatic gate passage topology node results. This allows the passage determination of automatic gate nodes to no longer be limited to local matching at a single moment, but to comprehensively reflect the stability of the remaining width of the gate region, the consistency of the robot's lateral occupancy, and the deviation constraints of the robot's heading angle relative to the centerline direction of the automatic gate within a continuous time range. This improves the ability of the automatic gate passage topology node results to represent the actual passage process and reduces the risk of misjudgment of the passage window caused by the discrete appearance of instantaneous passable records, width boundary fluctuations, and sudden changes in posture deviations.

[0033] Specifically, the steps for constructing a phase navigation map based on the automatic door access topology node results and ordinary topology nodes, and extracting the door node phase window and door node passage width constraints, are as follows: Read the automatic door access topology node results and the ordinary topology nodes of the service robot's scene to construct an initial navigation topology map containing ordinary topology nodes, automatic door nodes, and node connection relationships. In practice, ordinary topology nodes are taken from the non-automatic door nodes in the topology map of the service robot's scene, used to represent basic passage positions such as corridor turning points, passage bifurcation points, task start points, and task end points, which are not directly limited by the automatic door opening sequence. Automatic door nodes are taken from the topology... The automatic door access nodes in the topology map represent the door domain access positions with door node phase window constraints. Node connections are established based on the actual reachability relationships in the topology map. Any connection edge is only retained when there is a continuous drivable passage between the two endpoints, thus ensuring that the initial navigation topology map simultaneously contains static topological connectivity information and automatic door node access constraint information. For each automatic door node, the start and end data acquisition timestamps are determined as the door node phase window, and the minimum remaining width unit value of the segment is used as the base value for the door node access width. The start data acquisition timestamp corresponds to the point at which the target access segment begins to have stable embeddability. The entry condition is defined by the time when the data acquisition ends and the data collection ends, corresponding to the time when the target passage segment remains embeddable. The gate node phase window is used to characterize the effective passage range of the automatic gate node in the time dimension. The minimum remaining width unit value of the segment is used to characterize the continuous passage width that the automatic gate node can maintain even in the most unfavorable state within the target passage segment. For each connecting edge, the coordinate values ​​of the starting and ending nodes of the connecting edge are read, the length of the connecting edge is calculated, and a phase navigation map is generated. In specific implementation, the length of the connecting edge is obtained from the Euclidean distance between the coordinate values ​​of the starting and ending nodes. If the connecting edge crosses an automatic gate node, then the connection... The edge inherits the gate node phase window and gate node passage width base value corresponding to the automatic gate node. If the connecting edge only connects to ordinary topology nodes, the connecting edge only retains the geometric connectivity attribute. This allows the phase navigation graph to further superimpose the gate node temporal passage constraints on the basis of the geometric connectivity relationship of the traditional topology graph. Read the phase navigation graph and the preprocessed gate domain navigation data. For each connecting edge, calculate the predicted edge arrival time of the robot to the destination node based on the robot's current position x-coordinate value, robot's current position y-coordinate value, robot linear velocity value, robot's maximum allowable linear velocity value, control command issuance delay value, braking response time value, and connecting edge length value.In practice, the remaining path length from the robot to the endpoint node is first calculated based on the robot's current horizontal coordinate, vertical coordinate, and the endpoint node coordinates. Then, the robot's linear velocity is used as the current motion speed reference. When the robot's linear velocity is greater than its maximum permissible linear velocity, the maximum permissible linear velocity is used instead of the calculated velocity. When the robot's linear velocity is less than or equal to zero, the velocity at which the center of the doorway corresponding to the current connecting edge passes through the target is used as the calculated velocity. The remaining path length is then divided by the calculated velocity to obtain the basic time to reach the edge. Finally, the control command delay and braking response time are compared. The long value is superimposed on the base edge-to-edge time and added to the current data acquisition timestamp to obtain the predicted edge-to-edge time when the robot reaches the destination node along the current connected edge. The technical principle of this processing method is that the robot's current position, motion capability, and control delay are all converted to the same time scale, so that the predicted edge-to-edge time reflects not only spatial distance but also speed constraints and execution lag. When the destination node is a normal topology node, the phase wait value is recorded as zero. Since normal topology nodes are not subject to the gate node phase window constraint, the robot does not need to perform additional time alignment processing after reaching the normal topology node, so the phase wait value is directly set to zero. When the destination node is an automatic gate node... The predicted edge arrival time is compared with the gate node phase window. If the predicted edge arrival time is earlier than the start data acquisition timestamp of the gate node phase window, the time difference between the predicted edge arrival time and the start data acquisition timestamp of the gate node phase window is used as the phase waiting value. This indicates that although the robot has the motion conditions to reach the automatic gate node, the automatic gate node has not yet entered the effective passage range, so it needs to wait in front of the current automatic gate node until the gate node phase window opens. If the predicted edge arrival time is within the gate node phase window, the phase waiting value is recorded as zero. This indicates that the robot's arrival time at the automatic gate node is synchronized with the effective passage range of the automatic gate node. Pre-connecting edges have direct passage conditions in the temporal dimension. If the predicted edge time is later than the gate node phase window termination data acquisition timestamp, the corresponding connecting edge is marked as a frozen edge. A frozen edge indicates a connecting edge that cannot continue to participate in subsequent candidate path expansion under the gate node phase window constraint corresponding to the current data acquisition timestamp. This indicates that the robot cannot complete the journey within the effective passage range of the automatic gate node in the current motion state. Continuing to retain this connecting edge will cause the path expansion to enter a failed branch. Therefore, by marking it as a frozen edge, this connecting edge is removed from the subsequent candidate path expansion process, enabling the phase navigation graph to suppress temporally mismatched paths in advance during the path generation stage.

[0034] In this implementation scheme, the automatic gate access topology node results are organized together with ordinary topology nodes into a phase navigation map. At the connection edge level, a set of unified constraints are introduced, including gate node phase window, gate node access width base value, predicted edge arrival time, phase waiting value, and frozen edge. This makes the path connectivity not only reflect static geometric reachability, but also simultaneously reflect the accessible boundary of the automatic gate node in the time dimension and the traversable boundary in the width dimension. At the same time, combined with the continuous updating of the position of ordinary topology nodes, automatic gate node status, and connection edge endpoint coordinates by visual intelligence related perception results, the ability of the phase navigation map to adapt to the real gate domain passage process can be improved. This reduces the risk of retaining invalid connection edges caused by path expansion not considering temporal mismatch and width limitation, thereby improving the effectiveness and stability of the phase folding path generation results.

[0035] Table 1 Automatic Door Access Topology Node Data Table

[0036] Table 2 Phase Navigation Connection Side Data Table

[0037] Table 3 Phase Candidate Path Results

[0038] In this embodiment, the service robot starts from the starting node S to perform a delivery task. There are two candidate paths in the scenario: one path goes through the ordinary topology node N1 to the automatic door node D1 and then continues to the target node G; the other path goes through the ordinary topology node N2 to the automatic door node D2. The method first collects and preprocesses the door domain navigation data, then generates the automatic door passage topology node results based on the door domain time-width matrix, door opening matrix, robot predicted occupancy matrix, and door domain remaining open matrix. Then, it combines the ordinary topology nodes to construct a phase navigation map, calculates the phase passage status of each connecting edge, and filters to form phase folding path results. Finally, during the automatic door execution phase, it combines the topology projection forbidden sector to complete obstacle avoidance navigation control. Figure 3 This is used to illustrate the phase navigation results corresponding to this embodiment.

[0039] Table 1 shows the access topology node data for automatic gates, illustrating the access results of automatic gate nodes D1 and D2 under the constraints of the gate node phase window. Specifically, the start timestamp of the gate node phase window for automatic gate node D1 is 3.2s, the end timestamp is 5.0s, the minimum remaining width unit of the segment is 4 units, the corresponding lateral occupied width unit is 3 units, and the status is passable. For automatic gate node D2, the start timestamp is 1.8s, the end timestamp is 2.4s, the minimum remaining width unit of the segment is 3 units, the corresponding lateral occupied width unit is 3 units, and the status is restricted. As shown in Table 1, automatic gate node D1 not only has a longer passable time interval, but also retains a large continuous passable width after deducting the robot's lateral occupancy width. Therefore, it can be used as an effective automatic gate node in the subsequent target path. Automatic gate node D2 still retains a basic passable width in the width dimension, but its phase window duration is short. Once the subsequent arrival time is later than the end time of the phase window, it cannot continue to participate in path expansion as an effective passable node.

[0040] Table 2 is the data table for phase navigation connection edges, listing the connection edge length, predicted arrival time, phase waiting value, phase travel time, remaining travel width, and status for connection edges E1 to E5. Specifically, connection edge E1 is the connection edge from the starting node S to the ordinary topology node N1, with a connection edge length of 2.2m, a predicted arrival time of 1.4s, a phase waiting value of 0.0s, a phase travel time of 1.4s, and a status of selected connection edge; connection edge E2 is the connection edge from the ordinary topology node N1 to the automatic door node D1, with a connection edge length of 2.0m, a predicted arrival time of 3.6s, a phase waiting value of 0.0s, a phase travel time of 2.2s, a remaining travel width of 4 units, and a status of selected connection edge; connection edge E3 is the connection edge from the automatic door node D1 to the target node G, with a connection edge length of 2.8m, a predicted arrival time of 1.4s, a phase waiting value of 0.0s, a phase travel time of 2.2s, a remaining travel width of 4 units, and a status of selected connection edge; connection edge E3 is the connection edge from the automatic door node D1 to the target node G, with a connection edge length of 2.8m, a predicted arrival time of 1.4s, a phase waiting value of 0.0s, a phase travel time of 2.2s, a remaining travel width of 4 units, and a status of selected connection edge; and connection edge E3 is the connection edge from the automatic door node D1 to the target node G, with a connection edge length of 2.8m, a predicted arrival time of 1.4s, a phase waiting value of 0.0s, a phase travel time of 2.4s, and a status of selected connection edge. Edge E4 is a connection from the starting node S to the ordinary topology node N2. The edge length is 2.0m, the predicted edge arrival time is 1.2s, the phase wait time is 0.0s, the connection phase passage time is 1.2s, and the status is candidate edge. Edge E5 is a connection from the ordinary topology node N2 to the automatic door node D2. The edge length is 1.8m, the predicted edge arrival time is 3.1s, the phase wait time is 0.0s, the connection phase passage time is 1.9s, the remaining passage width of the edge is 3 units, and the status is frozen edge. As shown in Table 2, the predicted arrival time of connecting edge E2 to automatic gate node D1 is 3.6s, which is within the phase window range of the gate node corresponding to D1, so it can remain passable. However, the predicted arrival time of connecting edge E5 to automatic gate node D2 is 3.1s, which is 2.4s later than the data acquisition time stamp of the gate node phase window termination of D2. Therefore, it is marked as a frozen connecting edge and will no longer participate in the subsequent candidate path expansion.

[0041] Table 3 shows the candidate path results, listing the selection results of paths P1 and P2 under the constraints of continuous passage width and cumulative phase passage duration. Specifically, the node sequence corresponding to path P1 is S→N1→D1→G, the continuous passage width is 4 units, the cumulative phase passage duration is 5.8s, and the path status is target path; the node sequence corresponding to path P2 is S→N2→D2, the continuous passage width is 3 units, the cumulative phase passage duration is 3.1s, and the path status is frozen and eliminated. As can be seen from Table 3, although path P2 has a shorter cumulative phase passage duration, it cannot form a complete executable passage path because the corresponding connecting edge E5 is frozen when it passes through automatic gate node D2; although path P1 has a slightly longer cumulative phase passage duration, it simultaneously satisfies the gate node phase window constraint and the continuous passage width constraint when passing through automatic gate node D1, and is therefore determined as the target path.

[0042] like Figure 3 As shown, after the service robot starts from the starting node S, it forms two candidate paths. The path from the ordinary topology node N1 to the automatic door node D1 and then to the target node G remains a valid expansion path. The path from the ordinary topology node N2 to the automatic door node D2 is terminated because the corresponding connecting edge is frozen. Figure 3 The invention visually demonstrates the constraint relationship between the phase window of the automatic gate node and the expansion result of the connecting edge, as well as the process in which the continuous passage width value and the cumulative phase passage duration value jointly participate in the path selection. This shows that the present invention not only considers the topological connectivity relationship in the path decision-making process, but also simultaneously considers the temporal passability and spatial traversability of the automatic gate node, thereby improving the effectiveness and stability of the target path selection result in the automatic gate scenario.

[0043] Depend on Figure 4 As can be seen, the left sub-figure shows the superposition result of the door opening matrix and the obstacle occlusion matrix, where light green cells represent open cells and light red cells represent obstacle occlusion cells. The middle sub-figure shows the remaining open matrix of the door region after subtracting the obstacle occlusion cells from the door opening matrix. The right sub-figure shows the embedding comparison result of the robot predicted occupancy matrix and the remaining open matrix of the door region, where blue-bordered cells represent robot predicted occupancy cells. It can be seen from the right sub-figure that the robot predicted occupancy cells fall completely within the remaining open cells of the door region within the corresponding door region time and width cell ranges. Therefore, this predicted door passage time interval can be determined as an embeddable passage interval. Figure 4 This indicates that the present invention does not rely solely on the opening and closing status of the door or the remaining opening time to determine the passage of the automatic door node. Instead, it uses an embedding comparison between the door opening matrix, the remaining opening matrix of the door domain under the door domain time-width matrix, and the robot's predicted occupancy matrix to make a unified determination of the temporal passability and spatial traversability of the automatic door node.

[0044] Specifically, the steps for calculating the phase travel time and remaining travel width of the connecting edge, generating a phase candidate path sequence, and filtering to obtain the phase folding path result are as follows: Read the phase navigation map; for connecting edges not marked as frozen edges, divide the connecting edge length by the robot's maximum permissible linear velocity to obtain the shortest expected travel time of the current connecting edge under the speed upper bound constraint; then add this to the phase waiting value, control command issuance delay value, and braking response time value to obtain the connecting edge phase travel time value. In practical implementation, the connecting edge length value represents the geometric distance the robot needs to travel from the starting node to the ending node along the current connecting edge; the robot's maximum permissible linear velocity value is used to limit the speed upper bound of the connecting edge travel phase, making... The phase passage time value of the connecting edge represents the shortest expected time cost of the current connecting edge under the uniform speed upper bound constraint. The phase waiting value is used to compensate for the pre-waiting time formed when the robot has met the arrival conditions but the end node has not yet entered the gate node phase window. The control command issuance delay value is used to represent the transmission delay between the navigation control command issued by the path decision side and the chassis execution side. The braking response time value is used to represent the physical lag between the chassis execution action from triggering to the actual speed change. This allows different connecting edges to be compared under a uniform time cost scale. When the end node is an automatic gate node, the difference between the gate node passage width base value and the corresponding lateral occupied width unit value is calculated to obtain the remaining passage width value of the connecting edge. The gate node passage width base value represents the continuous passable width that the automatic gate node can maintain even in the most unfavorable state within the target passage segment. The corresponding lateral occupancy width unit value represents the minimum lateral width required by the robot when traversing the current automatic gate node. The difference between the two represents the width redundancy retained by the robot after passing through the automatic gate node. Starting from the initial node, each candidate path is expanded. For each candidate path, the phase passage time value of each connecting edge is accumulated, and the minimum value of the remaining passage width value of the connecting edges in each automatic gate node within the path is determined as the continuous passage width value of the path, generating a phase candidate path sequence. In specific implementation, path expansion proceeds edge by edge according to the node connection relationship in the initial navigation topology diagram. During the expansion process, any path only allows one... The system incorporates connected edges not marked as frozen edges, and updates the cumulative path time by adding each edge along the path direction. This ensures that the total path time cost reflects the complete travel cost of the robot from the starting node to the current ending node. The underlying principle is that if any automatic gate node has insufficient remaining passage width to allow the robot to pass stably, the entire path may fail to pass through the gate. Therefore, the actual safety margin of the path is determined by the minimum remaining passage width within the path. The minimum value of the remaining passage width of the connected edges corresponding to each automatic gate node within the path is used as the continuous passage width value of the path. This directly characterizes the bottleneck width boundary of the entire path, preventing the path from being misjudged as the preferred path when the local width is large but the local bottleneck is obvious.The candidate path sequence is read, and the candidate path with the largest continuous passage width value is selected first. When there are candidate paths with the same continuous passage width value, the candidate path with the smallest cumulative connected edge phase passage time value is selected. The node sequence, connected edge sequence, gate node phase window value, and continuous passage width value of each automatic gate node corresponding to the target path are output to generate the phase folding path result. Here, the phase folding path represents the executable path formed after converting the gate node phase window constraint corresponding to the automatic gate node into the connected edge time cost. The phase folding path result represents the target path result obtained after incorporating the phase waiting value, connected edge phase passage time value, and connected edge remaining passage width value into the path selection in the initial navigation topology map. In specific implementation, the candidate path with the largest continuous passage width value is selected first to ensure that the target path is within the automatic gate node range. Points exhibit higher continuous passage stability. Furthermore, by selecting the candidate path with the minimum cumulative connected edge phase passage time value under consistent width conditions, the overall passage time cost of the target path during execution can be further reduced. Specifically, the node sequence represents the order in which the robot traverses ordinary topology nodes and automatic gate nodes along the target path; the connected edge sequence represents the edge-level execution order of the target path in the topology graph; the gate node phase window corresponding to each automatic gate node represents the effective time interval allowed for entry at each automatic gate node; and the continuous passage width value represents the minimum continuous passable width that the target path can maintain under all automatic gate node constraints. This allows the phase-folded path results to be directly used for subsequent connected edge navigation, topology projection forbidden sector construction, waiting passage determination, gate passage determination, and backtracking and regenerating phase-folded path determination.

[0045] In this implementation, by introducing the phase passage duration value and the remaining passage width value of the connecting edge into the path expansion and path selection process, the formation of the phase candidate path sequence is no longer only affected by the topological connectivity relationship, but can simultaneously reflect the passage cost constraint in the time dimension and the passage margin constraint in the width dimension of the automatic gate node. Thus, the phase folding path result has both time sequence matching, continuous traversability and execution stability, reducing the risk of target path misselection caused by the failure to identify local width bottlenecks and the imbalance of total path duration.

[0046] Specifically, the steps for constructing a topology projection forbidden sector by performing connected edge navigation based on phase folding path results and preprocessed gate navigation data are as follows: Read the phase folding path results and preprocessed gate navigation data, and perform navigation according to the current connected edge in the target path; in practice, first extract the start node, end node, gate node phase window, and continuous passage width value corresponding to the current connected edge from the phase folding path results, then map the data record corresponding to the current data acquisition timestamp in the preprocessed gate navigation data to the current connected edge, ensuring that the robot's current position x-coordinate value used in the current connected edge navigation phase is consistent with the robot's current position x-coordinate value. The robot's forward ordinate, heading angle, distance to the dynamic obstacle in front of the door, and lateral movement speed of the dynamic obstacle in front of the door are all within the same local two-dimensional coordinate system of the door region. When the endpoint of the current connecting edge is an automatic door node, the vertex of the topological projection restricted sector is determined based on the robot's current abscissa and ordinate. Specifically, the coordinate point corresponding to the robot's current position is used as the geometric starting point of the topological projection restricted sector, ensuring that the restricted entry decision is always based on the robot's current actual position. The centerline of the topological projection restricted sector is determined based on the direction of the current connecting edge, specifically by taking the line from the starting node to the ending node of the current connecting edge. The direction vector serves as the centerline direction, causing the topological projection restricted sector to expand along the robot's current expected forward direction. The radius of the topological projection restricted sector is determined based on the distance value of the dynamic obstacle in front of the door and the longitudinal safety margin distance value. Specifically, the projected distance of the dynamic obstacle in front of the door along the Y-axis of the local two-dimensional coordinate system of the door area is added to the longitudinal safety margin distance value to obtain the sector radius. This ensures that the restricted sector covers the forward motion range from the robot's current position to the predicted intrusion front of the dynamic obstacle in front of the door. The longitudinal safety margin distance value is taken from 0.10m to 0.50m, and the longitudinal safety margin distance value is based on the statistical results of the door braking convergence error, ranging error, and door area safety. The avoidance requirements are determined by calculating the lateral projection width based on the time difference between the lateral movement speed of the dynamic obstacle in front of the gate and the time difference between the current data acquisition timestamp and the timetamp at which the gate node phase window ends data acquisition. Specifically, the remaining prediction duration is obtained by subtracting the current data acquisition timestamp from the timetamp at which the gate node phase window ends data acquisition. Then, the absolute value of the lateral movement speed of the dynamic obstacle in front of the gate is multiplied by the remaining prediction duration, and the lateral safety margin width is added to obtain the lateral projection width that the dynamic obstacle in front of the gate may form during the remaining execution phase of the current connection edge. The lateral safety margin width is taken as 0.05m to 0.The 20m lateral safety margin width value is determined based on the statistical results of the lateral distance measurement error in front of the gate and the obstacle lateral cutting-in avoidance requirements. The technical principle of this processing method is that the risk of a dynamic obstacle in front of the gate intruding into the forward passage space of the connecting edge during the remaining time period of the gate node phase window is manifested both as width compression caused by lateral displacement expansion and forward safety boundary contraction caused by longitudinal approach error. Therefore, the lateral movement speed value, remaining prediction time, and lateral safety margin width value of the dynamic obstacle in front of the gate are jointly mapped to the lateral projection width value, and the distance value of the dynamic obstacle in front of the gate and the longitudinal safety margin distance value are jointly mapped to the sector radius. This can unify the lateral cutting-in effect and forward approach effect of the dynamic obstacle in the future period into the geometric restricted area in the space in front of the current connecting edge. Then, the sector angle of the topological projection restricted sector is determined based on the lateral projection width value and the sector radius. Specifically, with the center line as the axis of symmetry, the lateral projection width value is evenly distributed to both sides of the center line at the end of the sector radius. The arctangent of the ratio of half of the lateral projection width value to the sector radius is taken as the sector half angle, and then... The left and right boundary lines are formed by rotating the sector half an angle clockwise and counterclockwise from the centerline, thereby determining the sector angle range of the topological projection forbidden sector; the topological projection forbidden sector is then generated; in specific implementation, the sector-shaped area enclosed by the vertex, centerline, sector radius, left boundary line, and right boundary line is marked as the forbidden area in the current connection edge execution phase. Any forward band-shaped area formed by expanding the robot's body width with the forward ray corresponding to the robot's current heading angle as the center and intersecting with this forbidden area is considered as a forbidden area. The current connecting edge has a risk of obstacle intrusion and does not meet the conditions for safe passage through the door. If the forward strip region is located outside the restricted area and still points to the endpoint node of the current connecting edge, then the current connecting edge is determined to remain executable. This allows the topological projection restricted sector to not only characterize the static positional impact of dynamic obstacles in front of the door at the current moment, but also the predicted intrusion impact of dynamic obstacles on the robot's forward passage space of the connecting edge during the remaining time period of the door node's phase window. This provides a unified geometric basis for subsequent waiting passage determination, door passage determination, and phase mismatch edge determination.

[0047] In this implementation scheme, the robot's current horizontal coordinate, current vertical coordinate, current connecting edge direction, distance to the dynamic obstacle in front of the door, lateral movement speed of the dynamic obstacle in front of the door, and door node phase window are uniformly projected onto the same geometric judgment framework. Combined with visual intelligence-related perception results, the future lateral intrusion range of the dynamic obstacle in front of the door is expressed forward. This allows the topological projection forbidden sector to simultaneously reflect the spatial conflict boundary of the current connecting edge execution phase and the dynamic risk boundary within the remaining passage period. This improves the ability of the connecting edge navigation process to identify lateral cutting interference in front of the door and reduces the phenomenon of missed door passage risks caused by judging based solely on the instantaneous obstacle position.

[0048] Specifically, the steps for outputting the service robot's obstacle avoidance navigation results, combining the gate node phase window execution with waiting for passage, passage through the gate, or backtracking to regenerate the phase folding path, are as follows: Read the gate node phase window. In practice, first extract the start and end data acquisition timestamps of the gate node phase window corresponding to the current connected edge's endpoint node from the phase folding path results. Then, compare the current data acquisition timestamp with the gate node phase window time-by-time to ensure that the current connected edge's execution state is always constrained by the effective passage time interval of the automatic gate node. When the current data acquisition timestamp is earlier than the start data acquisition timestamp of the gate node phase window, control the robot to decelerate and stop along the centerline direction of the current connected edge. The target stopping position is located in front of the centerline of the automatic door, at a distance equal to the safe stopping distance in front of the door. In practice, the remaining approach distance from the robot's current position to the target stopping position is calculated based on the robot's current vertical coordinate, the automatic door's centerline position, and the direction of the current connecting edge's centerline. The target stopping position is defined as the position located at the safe stopping distance in front of the automatic door's centerline along the direction of the current connecting edge's centerline. The safe stopping distance is determined by combining the braking distance corresponding to the robot's current speed and the safety margin distance, with the safety margin distance ranging from 0.05m to 0.30m. Then, the deceleration trajectory is determined based on the robot's linear velocity, maximum permissible linear velocity, and braking response time, ensuring the robot... The robot completes velocity convergence and remains stationary before the gate node phase window opens. The technical principle behind this approach is that if the robot arrives at the automatic gate node earlier than the initial data acquisition timestamp of the gate node phase window, continuing the approach motion could cause the robot to enter the central area of ​​the gate region before the automatic gate is in a stable, passable state, thus causing the passage action to deviate from the effective time boundary. Therefore, by pre-decelerating and stopping at the safe stopping distance value in front of the gate, the robot's forward motion state can be re-aligned with the initial time of the gate node phase window. When the current data acquisition timestamp is within the gate node phase window and forms a forward band centered on the forward ray corresponding to the robot's current heading angle and extending along the robot's width... When the area does not intersect with the topological projection restricted sector, the robot is controlled to pass through the automatic gate node along the current connecting edge. In specific implementation, the forward ray corresponding to the robot's current heading angle is used as the forward determination center line for passing through the gate. Then, half the width of the robot body is extended to both sides of the center line to form a forward strip area. When this forward strip area is always located outside the topological projection restricted sector, it means that the robot's current forward direction and its outer envelope will not enter the predicted intrusion area of ​​the dynamic obstacle in front of the gate within the remaining time period of the gate node phase window. At this time, forward motion control commands are continuously output according to the current connecting edge direction, the target speed value of the gate center, and the continuous passage width value of the path, so that the robot enters the gate center area along the current connecting edge and completes the automatic gate node crossing.The technical principle behind this processing method is that the robot's door-passing behavior can simultaneously possess temporal matching and spatial safety only when the gate node phase window constraint in the time dimension is satisfied, the topological projection forbidden sector constraint in the spatial dimension is satisfied, and the forward strip region corresponding to the robot's outer envelope does not enter the forbidden boundary. Therefore, using the simultaneous fulfillment of all three conditions as the door-passing release condition can avoid the risk of lateral cutting in front of the door caused by releasing the robot solely based on time conditions. When the current data acquisition timestamp is later than the gate node phase window's data acquisition termination timestamp, or when the forward strip region formed by expanding along the robot's body width with the forward ray corresponding to the robot's current heading angle as the center and the topological projection forbidden sector constraint is satisfied, the robot's door-passing behavior can simultaneously possess temporal matching and spatial safety. When projected restricted sectors intersect, the current connecting edge is marked as a phase mismatch edge. The robot then backtracks to the previous normal topology node and regenerates the phase folding path using the previous normal topology node as the current starting node, outputting the service robot's obstacle avoidance navigation result. In practice, if the current data acquisition timestamp exceeds the gate node's phase window termination timestamp, it indicates that the robot failed to complete the entry action within the effective passage time interval of the automatic gate node, and the current connecting edge is invalid in the temporal dimension. If the advancing strip region intersects with the topological projected restricted sector, it indicates that the robot's outer envelope has entered the predicted intrusion boundary of the dynamic obstacle in front of the gate in the current forward direction, and the current connecting edge is invalid in the spatial dimension. The connection is considered invalid at the time of connection execution. Phase-mismatched edges are those that fail during the connection execution phase due to real-time temporal boundary mismatch or real-time spatial forbidden boundary conflict. Frozen edges are those that are pre-determined as non-scalable during the path generation phase based on the comparison between the predicted edge time and the gate node phase window. Once any failure condition is met, the execution of the current connection is stopped, the current connection is written into the phase-mismatched edge set, and then the connection order in the target path is used to backtrack to the previous normal topology node, allowing the robot to return to a stable topology position unrestricted by the automatic gate node timing. Then, using the previous normal topology node as the new starting position, the robot re-enters the gate navigation data updated at the current time. The new calculation calculates the phase window of the gate node, the phase passage duration of the connecting edge, and the remaining passage width of the connecting edge to generate a new phase-folding path. The technical principle behind this approach is that the phase mismatch judgment result is directly fed back to the path regeneration process. This allows the service robot to avoid continuing to attempt to pass through the gate along a failed path when real-time changing conditions such as automatic gate timing mismatch or lateral intrusion of dynamic obstacles occur. Instead, it uses ordinary topology nodes as stable replanning base points to find a new executable path that meets the current time and space boundaries. This forms a closed-loop phase-folding path control process of waiting for passage, passing through the gate, and backtracking to regenerate the path, ultimately outputting the service robot's obstacle avoidance navigation result.

[0049] In this embodiment, by incorporating the gate node phase window, the safe stopping distance in front of the gate, the forward strip area formed by extending the robot's body width with the forward ray corresponding to the robot's current heading angle as the center, the topological projection restricted sector, and the phase mismatch edge into the current connection edge execution judgment process, the motion control of the service robot in front of the automatic gate node no longer relies solely on the single arrival time judgment, but can simultaneously reflect time boundary constraints, spatial restricted boundary constraints, and path failure backoff boundary constraints. This improves the service robot's ability to adapt to dynamic changes in the gate area and reduces the risk of gate failure caused by passage timing deviation, forward direction mismatch, and continued execution of failed paths.

[0050] like Figure 2 As shown, the second aspect of the present invention provides a service robot obstacle avoidance and navigation system based on spatial topology cognition, comprising: a gate domain acquisition and processing module, a gate domain access modeling module, a phase path generation module, and a projection obstacle avoidance execution module, wherein: the gate domain acquisition and processing module is used to acquire gate domain navigation data, and perform anomaly removal, missing data completion, and temporal reconstruction processing on the gate domain navigation data, outputting preprocessed gate domain navigation data; the gate domain access modeling module is used to construct a gate domain temporal width matrix, a gate opening matrix, and a robot predicted occupancy matrix based on the preprocessed gate domain navigation data, and generate a gate domain remaining open matrix in combination with obstacle occupancy, and calculate the obstacle avoidance by comparing the embedding ratio of the robot predicted occupancy matrix with the gate domain remaining open matrix. The system generates automatic door passage topology node results by filtering passage segments; the phase path generation module is used to construct a phase navigation map based on the automatic door passage topology node results and ordinary topology nodes, extract the door node phase window and door node passage width constraint, calculate the phase passage duration value and the remaining passage width value of the connecting edge, generate a phase candidate path sequence, and filter to obtain the phase folding path results; the projection obstacle avoidance execution module is used to execute connecting edge navigation based on the phase folding path results and preprocessed door domain navigation data, construct topology projection forbidden sectors, and combine the door node phase window to execute waiting passage, passage through the door, or backtracking to regenerate the phase folding path, and output the service robot obstacle avoidance navigation results.

[0051] In this implementation scheme, by incorporating door domain navigation data preprocessing, automatic door passage topology node result generation, phase folding path result generation, and topology projection prohibited sector constraint execution into a unified technical chain, and combining visual intelligence-related perception results to continuously support door boundaries, obstacle movement, and node status, the service robot's obstacle avoidance navigation results simultaneously possess the ability to determine door domain temporal passability, path continuity and traversability, and dynamic risk suppression in front of the door. This improves the consistency of navigation decisions, passage stability, and execution safety in automatic door scenarios, and reduces the risk of navigation instability caused by fragmented data processing chains, disconnected passage constraints, and inconsistencies between path decisions and execution judgments.

[0052] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0053] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A service robot obstacle avoidance and navigation method based on spatial topology cognition, characterized in that, Includes the following steps: S1: Collect gate navigation data, and perform anomaly removal, missing data completion and temporal reconstruction on the gate navigation data, and output the preprocessed gate navigation data; S2. Based on the preprocessed gate navigation data, construct the gate temporal width matrix, gate opening matrix, and robot predicted occupancy matrix. Combine obstacle occupancy to generate the gate remaining open matrix. Filter passage segments by embedding and comparing the robot predicted occupancy matrix and the gate remaining open matrix to generate automatic gate passage topology node results. S3. Construct a phase navigation map based on the automatic gate access topology node results and ordinary topology nodes, extract the gate node phase window and gate node access width constraint, calculate the phase access duration value and the remaining access width value of the connecting edge, generate a phase candidate path sequence, and filter to obtain the phase folding path results; S4 performs connection edge navigation based on the phase folding path results and preprocessed gate domain navigation data, constructs topological projection forbidden sectors, and combines the gate node phase window to perform waiting to pass, passing through the gate, or backtracking to regenerate the phase folding path, outputting the service robot obstacle avoidance navigation results.

2. The service robot obstacle avoidance and navigation method based on spatial topology cognition according to claim 1, characterized in that: The specific steps for collecting gate navigation data and performing anomaly removal, missing data completion, and temporal reconstruction on the gate navigation data are as follows: Collect door navigation data when the service robot passes through. The door navigation data includes data collection timestamp, automatic door number, automatic door node number, door opening start timestamp, door closing start timestamp, door opening holding duration, door leaf opening displacement, left boundary position of the doorway, right boundary position of the doorway, doorway net width, number of obstacles in front of the door, distance to dynamic obstacles in front of the door, lateral movement speed of dynamic obstacles in front of the door, robot's current position x-coordinate, robot's current position y-coordinate, robot heading angle, robot linear velocity, distance of the robot from the center line of the automatic door, robot body width, robot body length, robot minimum turning radius, robot maximum allowable linear velocity, control command issuance delay, braking response duration, target speed through the doorway center, and safe stopping distance in front of the door. For the collected gate navigation data, the Hamper filter algorithm is used to remove abnormal jump values ​​and perform robust smoothing processing of continuous sampling; the piecewise linear interpolation algorithm is used to perform short-term missing record completion and temporal reconstruction processing on the gate navigation data, and the preprocessed gate navigation data is output.

3. The service robot obstacle avoidance and navigation method based on spatial topology cognition according to claim 2, characterized in that: The specific steps for constructing the gate domain time-width matrix, gate opening matrix, and robot prediction occupancy matrix based on the preprocessed gate domain navigation data are as follows: Read the preprocessed door navigation data, collect it according to the automatic door number and automatic door node number, and arrange it in ascending order according to the data acquisition timestamp to form a door node time sequence record. For each record, subtract the left boundary position value of the door opening from the right boundary position value, and retain the records where the absolute value of the difference is not greater than the error threshold. Then, divide the net width value of the door opening into multiple door width units along the horizontal direction according to a fixed width n, divide the data acquisition timestamp into multiple door time units according to a fixed time interval t, and establish a door time-width matrix using the door time unit and door width unit as indices. Read the door domain time-width matrix and the door node time sequence. For each record, first divide the door opening displacement value by the net width of the door opening to obtain the door opening ratio value. Then multiply the door opening ratio value by the total number of door domain width units and round down to obtain the number of opening width units. Then, take the door opening start time stamp as the starting point and the earlier of the door closing start time stamp and the door opening start time stamp plus the door opening duration value as the ending point to determine the effective opening time interval corresponding to the current record. Mark the matrix units within the effective opening time interval that are symmetrical about the center line of the door opening and have a number equal to the number of opening width units as door open units, and mark the remaining matrix units as door closed units to generate the door open matrix. Read the timing record sequence of the door nodes. For each record, calculate the predicted arrival time of the robot at the center line of the automatic door based on the robot's distance from the center line of the automatic door, the robot's linear velocity, the robot's maximum allowable linear velocity, the delay of the control command issuance, the braking response time, and the target speed value for passing through the center of the doorway. Calculate the predicted door passage time interval and the number of lateral width units based on the robot's body length, width, minimum turning radius, and heading angle. Then, read the door opening matrix and generate a robot prediction occupancy matrix, starting from the door domain time unit corresponding to the predicted arrival time and extending to the continuous door domain time units corresponding to the predicted door passage time interval.

4. The service robot obstacle avoidance navigation method based on spatial topology cognition according to claim 3, characterized in that: The specific steps for generating the gate domain remaining open matrix by combining obstacle occlusion are as follows: Read the robot's predicted occupancy matrix, the door opening matrix, and the door node time sequence. For each record, calculate the lateral sweep width of the dynamic obstacle in front of the door within the predicted door passage time interval based on the lateral movement speed value of the obstacle and the predicted passage time interval. Combine this with the number of obstacles in the door area to convert it into the number of obstacle occupancy width units. Mark the door area width units that are equal to the number of obstacle occupancy width units and are continuously distributed along the lateral movement direction of the dynamic obstacle in front of the door, starting from the door area width unit corresponding to the center line of the door opening. Subtract the obstacle occupancy units from the door opening matrix to obtain the remaining door area opening matrix.

5. The obstacle avoidance and navigation method for service robots based on spatial topology cognition according to claim 4, characterized in that: The specific steps for filtering passage segments and generating automatic gate passage topology node results by embedding and comparing the robot-predicted occupancy matrix with the gate region remaining open matrix are as follows: The robot's predicted occupancy matrix is ​​compared cell by cell with the gate domain's remaining open matrix. Records in which the robot's predicted occupancy matrix falls entirely within the gate domain's remaining open matrix are retained, and gate nodes are generated that can be embedded into access records. The system reads all embedded passage records corresponding to the same automatic door node, merges two adjacent records where the difference between the predicted door arrival times is equal to a fixed time interval t and the corresponding horizontal occupancy width unit value remains unchanged into candidate passage segments; for each candidate passage segment, the system calculates the segment start data acquisition timestamp, segment end data acquisition timestamp, minimum remaining width unit value, corresponding horizontal occupancy width unit value, and the maximum deviation of the robot's heading angle relative to the automatic door centerline; first, it selects the candidate passage segment with the largest minimum remaining width unit value; when there are multiple candidate passage segments with the same minimum remaining width unit value, it selects the candidate passage segment with the smallest maximum deviation of the robot's heading angle as the target passage segment; Write the start data acquisition timestamp, end data acquisition timestamp, minimum remaining width unit value, and corresponding horizontal width unit value of the target passage segment into the corresponding automatic door node to generate the automatic door passage topology node result.

6. The service robot obstacle avoidance and navigation method based on spatial topology cognition according to claim 5, characterized in that: The specific steps for constructing a phase navigation map based on the automatic gate access topology node results and ordinary topology nodes, and extracting the gate node phase window and gate node passage width constraints are as follows: Read the results of the automatic door access topology nodes and the ordinary topology nodes of the service robot scene, and construct an initial navigation topology map containing ordinary topology nodes, automatic door nodes and node connection relationships; for each automatic door node, determine the start data acquisition timestamp and the end data acquisition timestamp as the door node phase window, and use the minimum remaining width unit value of the segment as the base value of the door node access width. For each connecting edge, read the node coordinates corresponding to the starting and ending nodes of the connecting edge, calculate the length of the connecting edge, and generate a phase navigation graph; Read the phase navigation map and preprocessed gate navigation data. For each connecting edge, calculate the predicted edge arrival time of the robot to the endpoint node based on the robot's current position x-coordinate, robot current position y-coordinate, robot linear velocity, robot maximum allowable linear velocity, control command issuance delay, braking response time, and connecting edge length. When the endpoint node is a normal topology node, the phase wait value is recorded as zero. When the endpoint node is an automatic gate node, compare the predicted edge arrival time with the gate node's phase window. If the predicted edge arrival time is earlier than the gate node's phase window start data acquisition timestamp, the time difference between the predicted edge arrival time and the gate node's phase window start data acquisition timestamp is used as the phase wait value. If the predicted edge arrival time is within the gate node's phase window, the phase wait value is recorded as zero. If the predicted edge arrival time is later than the gate node's phase window end data acquisition timestamp, the corresponding connecting edge is marked as a frozen edge.

7. The service robot obstacle avoidance and navigation method based on spatial topology cognition according to claim 6, characterized in that: The specific steps for calculating the phase passage time value and the remaining passage width value of the connecting edge, generating a phase candidate path sequence, and filtering to obtain the phase folding path result are as follows: Read the phase navigation map. For connected edges not marked as frozen edges, divide the length of the connected edge by the robot's maximum allowable linear velocity, and then add it to the phase waiting value, control command issuance delay value, and braking response duration value to obtain the phase passage duration value of the connected edge. When the endpoint node is an automatic gate node, calculate the difference between the gate node's passage width base value and the corresponding number of lateral occupied width units to obtain the remaining passage width value of the connected edge. Expand each candidate path starting from the starting node, accumulate the phase passage duration value of each connected edge for each candidate path, and determine the minimum value of the remaining passage width value of the connected edge in each automatic gate node within the path as the continuous passage width value of the path, generating a phase candidate path sequence. Read the candidate path sequence of phases, first select the candidate path with the largest continuous passage width value; when there are candidate paths with the same continuous passage width value, select the candidate path with the smallest cumulative connected edge phase passage time value, and output the node sequence, connected edge sequence, gate node phase window and continuous passage width value of each automatic gate node corresponding to the target path, and generate the phase folding path result.

8. The service robot obstacle avoidance navigation method based on spatial topology cognition according to claim 7, characterized in that: The specific steps for performing connection edge navigation and constructing topological projection forbidden sectors based on phase folding path results and preprocessed gate domain navigation data are as follows: Read the phase folding path results and preprocessed gate navigation data, and perform navigation according to the current connecting edge in the target path. When the endpoint node of the current connecting edge is an automatic gate node, determine the vertex of the topological projection restricted sector based on the robot's current position x-coordinate and y-coordinate, determine the center line of the topological projection restricted sector based on the current connecting edge direction, determine the radius of the topological projection restricted sector based on the distance value of the dynamic obstacle in front of the gate, calculate the lateral projection width value based on the product of the lateral movement speed value of the dynamic obstacle in front of the gate and the time difference between the current data acquisition timestamp and the end data acquisition timestamp of the gate node phase window, and then determine the two boundary lines of the topological projection restricted sector by connecting the projection boundary corresponding to the lateral projection width value and the sector vertex, thus generating the topological projection restricted sector.

9. The obstacle avoidance and navigation method for service robots based on spatial topology cognition according to claim 8, characterized in that: The specific steps for combining the gate node phase window to perform waiting for passage, passage through the gate, or backtracking to regenerate the phase folding path and output the obstacle avoidance navigation results of the service robot are as follows: The system reads the gate node phase window. When the current data acquisition timestamp is earlier than the start data acquisition timestamp of the gate node phase window, it controls the robot to decelerate and stop at a position that is a safe stopping distance from the center line of the automatic door. When the current data acquisition timestamp is within the gate node phase window and the forward ray corresponding to the robot's current heading angle does not intersect with the topological projection restricted sector, it controls the robot to pass through the automatic door node along the current connecting edge. When the current data acquisition timestamp is later than the end data acquisition timestamp of the gate node phase window or the forward ray corresponding to the robot's current heading angle intersects with the topological projection restricted sector, it marks the current connecting edge as a phase mismatch edge, backtracks to the previous ordinary topological node, and regenerates the phase folding path using the previous ordinary topological node as the current starting node, outputting the service robot obstacle avoidance navigation result.

10. A service robot obstacle avoidance and navigation system based on spatial topology cognition, characterized in that, include: The system comprises a gate domain acquisition and processing module, a gate domain access modeling module, a phase path generation module, and a projection obstacle avoidance execution module, among which: The gate domain acquisition and processing module is used to acquire gate domain navigation data, and perform anomaly removal, missing data completion and temporal reconstruction processing on the gate domain navigation data, and output preprocessed gate domain navigation data. The gate access modeling module is used to construct a gate temporal width matrix, a gate opening matrix, and a robot predicted occupancy matrix based on the preprocessed gate navigation data, and to generate a gate remaining open matrix by combining obstacle occupancy. The module filters access segments by embedding and comparing the robot predicted occupancy matrix and the gate remaining open matrix, and generates automatic gate access topology node results. The phase path generation module is used to construct a phase navigation map based on the automatic gate access topology node results and ordinary topology nodes, extract the gate node phase window and gate node access width constraint, calculate the phase access duration value and the remaining access width value of the connecting edge, generate a phase candidate path sequence, and filter to obtain the phase folding path result. The projection obstacle avoidance execution module is used to perform connection edge navigation based on the phase folding path results and preprocessed gate domain navigation data, construct topological projection forbidden sectors, and combine the gate node phase window to perform waiting to pass, passing through the gate, or backtracking to regenerate the phase folding path, and output the service robot obstacle avoidance navigation results.

Images