A robot hierarchical motion planning and control method based on visual topology guidance

Abstract

The application belongs to the technical field of robot control, and discloses a robot hierarchical motion planning and control method based on visual topology guidance, which comprises the following steps: constructing a visual topology graph and setting a target image node; collecting current observation information and establishing an observation set; generating a sub-target sequence; establishing a robot motion model and generating a candidate control set; expanding a candidate trajectory; performing contour collision detection and feasibility screening; comprehensively evaluating a feasible trajectory and obtaining an optimal control amount; executing the optimal control and performing closed-loop updating, and executing a recovery behavior when there is no feasible trajectory. The application realizes safe, interpretable and closed-loop navigation and obstacle avoidance control of the robot under the guidance of the visual topology graph.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, specifically relating to a hierarchical motion planning and control method for robots based on visual topology guidance. Background Technology

[0002] With the development of robotics technology, robots have been widely applied in service operations, logistics and distribution, inspection and testing, warehousing and handling, and indoor and outdoor autonomous mobile platforms. In these applications, robots need to generate executable motion trajectories based on task objectives and environmental perception information, and output control commands that meet safety, continuity, and dynamic feasibility requirements. Therefore, robot motion planning and control capabilities have become a crucial foundational capability affecting their autonomous operation performance.

[0003] Existing robot motion planning methods typically include global geometry map-based planning methods, reactive planning methods based on local sensor information, and visual perception-based learning planning methods. Global geometry map-based planning methods usually require pre-constructing an environment map and determining the robot's current pose based on localization results during operation, then combining path planning and local control to achieve motion execution. This type of method performs well in structured environments, but in scenarios with frequent environmental changes, significant occlusion, high map maintenance costs, or difficulty in maintaining long-term localization accuracy, it is susceptible to factors such as map mismatch, localization drift, and high deployment complexity.

[0004] Planning methods based on local sensing information typically utilize sensors such as LiDAR and depth cameras to acquire the distribution of obstacles around the robot and generate short-term motion commands based on local cost functions or safe distance constraints. These methods can handle local obstacle avoidance problems well, but their target guidance usually relies on externally given global paths, target points, or localization information. When stable high-level guidance is lacking, the robot is prone to problems such as local optima, unstable direction selection, and unclear detour intentions.

[0005] In recent years, using visual information to guide robot motion has gradually become a research hotspot. Visual topology mapping can express the robot's motion trend from the starting region to the target region using pre-acquired reference image sequences, thus reducing the reliance on accurate global maps and continuous localization. However, existing vision-guided methods often focus on outputting directional commands or sparse sub-targets, lacking explicit consideration of the robot's velocity state, acceleration constraints, control space boundaries, and the executability of local trajectories. This results in generated results that, while showing target approach, may pose risks of collisions, stalling, or oscillations in complex local environments.

[0006] Furthermore, visual guidance results and underlying motion control processes are often independent of each other, making it difficult to simultaneously consider target tracking, trajectory safety, operational efficiency, control smoothness, and orientation consistency within the same planning cycle. When a robot is in a state with dense obstacles, narrow passages, or short-term infeasible trajectories, the lack of a recovery mechanism that incorporates current perception information can easily lead to continuous stagnation or repeated generation of infeasible control commands. Therefore, how to provide a hierarchical motion planning and control method that can utilize visual topological information for high-level motion guidance, while combining robot state, motion constraints, and local perception information for trajectory generation, safety screening, control optimization, and recovery processing, has become a pressing technical problem to be solved in this field. Summary of the Invention

[0007] The purpose of this invention is to provide a robot hierarchical motion planning and control method based on visual topology guidance, in order to solve the problems of existing visual navigation methods such as the disconnect between high-level semantic guidance and low-level physical execution, lack of local geometric constraints and dynamic feasibility in the output results, and easy occurrence of trajectory infeasibility, collision or local obstruction in obstacle-dense or narrow scenes. In this way, the invention can realize safe, interpretable and closed-loop navigation and obstacle avoidance control of the robot under the guidance of visual topology map.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0009] A visual topology-guided hierarchical motion planning and control method for robots includes the following steps:

[0010] Multiple images are acquired along the reference path to form a visual topology map, and one image in the visual topology map is marked as the target image node;

[0011] Acquire observation images, LiDAR point clouds, and robot status of the robot during the current planning cycle;

[0012] Based on the current observed image, historical visual observation window and target image node, the sub-target sequence of the current planning period is obtained, and the sub-target at the end of the sub-target sequence is extracted as the local planning evaluation sub-target;

[0013] The control space for the current planning cycle is generated by combining acceleration constraints, global velocity boundaries, and velocities in the robot's state. A set of candidate controls is obtained by sampling within the control space.

[0014] For each candidate control variable in the candidate control set, forward rolling prediction is performed based on the robot kinematics model to obtain candidate trajectories. Candidate trajectories with collision risk are eliminated, and feasible trajectories are output.

[0015] The comprehensive cost of each feasible trajectory is calculated by combining the evaluation sub-objectives of local planning, and the candidate control quantity corresponding to the feasible trajectory with the minimum comprehensive cost is taken as the optimal control quantity.

[0016] The robot is controlled according to the optimal control quantity to complete the current planning cycle and start the next planning cycle until the robot reaches the target image node.

[0017] Several alternative methods are provided below, but they are not intended as additional limitations on the overall solution above. They are merely further additions or optimizations. Provided there are no technical or logical contradictions, each alternative method can be combined individually with respect to the overall solution above, or multiple alternative methods can be combined with each other.

[0018] Preferably, the reference path is a path without interference from obstacles, and the images in the visual topology map are arranged in chronological order according to the extension direction of the reference path.

[0019] Preferably, each planning cycle uses the robot's current body coordinate system as the local planning coordinate system, and the sub-target sequence, lidar point cloud, and candidate trajectory are all represented in the local planning coordinate system.

[0020] Preferably, the step of obtaining the sub-target sequence for the current planning period based on the current observed image, historical visual observation window, and target image nodes includes:

[0021] Input the current observed image, historical visual observation window, and target image node into the neural network model;

[0022] The neural network model outputs sub-objectives for multiple future time points;

[0023] Arrange the sub-targets in order of time from most recent to furthest to obtain the sub-target sequence.

[0024] Preferably, the step of generating the control space for the current planning cycle by combining acceleration constraints, global velocity boundaries, and velocities in the robot's state includes:

[0025] Obtain the linear velocity and angular velocity of the robot in the current planning cycle;

[0026] Based on linear acceleration constraints and angular acceleration constraints, determine the dynamic feasible sampling intervals for linear velocity and angular velocity under the current planning period, respectively;

[0027] Based on the upper and lower boundaries of linear velocity and angular velocity in the global velocity boundary, the dynamic feasible sampling interval is clipped to obtain the control space of linear velocity and angular velocity in the current planning cycle.

[0028] Preferably, the step of predicting the candidate trajectory by forward rolling based on the robot's kinematics model for each candidate control variable in the candidate control set includes:

[0029] Based on candidate control variables and a fixed integration step size, the robot's pose in the local time domain is forward propagated to obtain the relative pose increments at multiple future moments relative to the robot's current body coordinate system.

[0030] The relative pose increments at all future moments corresponding to each candidate control variable constitute the candidate trajectory in the local time domain.

[0031] Preferably, the process of eliminating candidate trajectories with collision risks and outputting feasible trajectories includes:

[0032] Obtain the initial contour of the robot in the body coordinate system;

[0033] Based on the relative pose increments at each future time in the candidate trajectory, the initial contour is translated and rotated to obtain the contour projection in the predicted state at each future time.

[0034] Intersection detection is performed between the contour projection of each future time-predicted state and the LiDAR point cloud in the current robot body coordinate system;

[0035] If the contour projection of a candidate trajectory in any future time prediction state intersects with the current lidar point cloud, a collision risk is considered to exist, and the corresponding candidate trajectory is determined to be an infeasible trajectory; otherwise, it is determined to be a feasible trajectory.

[0036] Preferably, the comprehensive cost of each feasible trajectory is calculated by combining the evaluation sub-objectives of local planning, wherein the calculation function of the comprehensive cost includes a target tracking term, a time efficiency term, an obstacle clearance term, a smoothing term, and an orientation consistency term, wherein:

[0037] The target tracking term is used to characterize the positional deviation between the endpoint of the candidate trajectory and the local planning evaluation sub-objective;

[0038] The time efficiency term is used to suppress excessively low linear velocities;

[0039] The obstacle gap term is used to characterize the minimum gap between the candidate trajectory and the obstacle;

[0040] The smoothing term is used to characterize the linear velocity and angular velocity changes between the current candidate control quantity and the optimal control quantity at the previous moment.

[0041] The orientation consistency term is used to characterize the deviation between the endpoint orientation of the candidate trajectory and the target orientation provided by the local planning evaluation sub-objective;

[0042] The target tracking term, time efficiency term, obstacle clearance term, smoothing term, and orientation consistency term are weighted and summed to obtain the comprehensive cost.

[0043] Preferably, the time efficiency term is expressed as follows:

[0044] Take the absolute value of the linear velocity component in the candidate control quantity;

[0045] The ratio of the absolute value of the linear velocity component to the maximum linear velocity in the global velocity boundary is used as the velocity ratio.

[0046] After limiting the speed ratio to its maximum value, the reverse complementary amount is obtained;

[0047] By weighting the reverse complementary quantities using time efficiency weights, the time efficiency term is obtained.

[0048] As a preferred option, it also includes:

[0049] When no feasible trajectory is obtained, the recovery behavior is triggered, which means setting the linear velocity to zero and determining the direction with the fewest obstacles based on the current LiDAR point cloud. The robot is then controlled to perform a stationary rotation in the direction with the fewest obstacles. During the stationary rotation, the robot continuously acquires the observation images, LiDAR point cloud, and robot state of the current planning cycle for trajectory replanning. When a feasible trajectory is obtained again, the recovery behavior stops and processing continues based on the feasible trajectory.

[0050] The visual topology-guided robot hierarchical motion planning and control method provided by this invention has the following advantages compared with the prior art:

[0051] First, this invention pre-constructs a visual topology map along a reference path and utilizes the current observation image, historical visual observation windows, and target image nodes to generate a sequence of sub-targets arranged from near to far along the navigation direction. This enables the robot to obtain continuous and stable navigation guidance information without relying on a global geometric map and explicit global localization. Compared to methods that only output a single direction command or a single target point, this method can more fully represent the long-term temporal motion trend during navigation, improving the continuity, interpretability, and target consistency of navigation guidance.

[0052] Second, this invention combines high-level path guidance provided by the visual topology map with motion feasibility analysis within the robot's local time domain. This allows the robot to generate candidate control variables that satisfy dynamic constraints based on its current velocity state, acceleration constraints, and velocity boundaries while moving towards the target direction. Furthermore, it incorporates local environmental information to select a more suitable motion trajectory for actual execution. This avoids the local motion instability problem caused by existing visual navigation methods that only possess directional approach capabilities but lack trajectory executability constraints.

[0053] Third: This invention achieves full-time domain safety screening of candidate trajectories by performing forward rolling prediction of candidate control variables in the robot's current body coordinate system and using the spatial relationship between the robot's body contour and the LiDAR point cloud for collision detection. Compared to methods that only output motion based on the target direction, this invention can eliminate trajectories with collision risks in advance during the trajectory generation stage, thereby effectively improving the robot's obstacle avoidance safety in narrow passages, areas with dense obstacles, and locally complex environments.

[0054] Fourth: This invention constructs a comprehensive cost function that includes target tracking, time efficiency, obstacle clearance, smoothness, and orientation consistency. This function provides a unified evaluation of feasible trajectories that have passed safety screening, ensuring that the selected optimal control variables not only guide the robot towards the target but also consider operational efficiency, obstacle safety distance, control continuity, and end-effector orientation rationality. This achieves a balance between target reachability and local obstacle avoidance, reducing the probability of the robot stalling, oscillating, and making sharp turns during navigation, and improving the smoothness and stability of the navigation trajectory.

[0055] Fifth: This invention employs a rolling time-domain closed-loop update method. After the robot executes the optimal control parameters for the current planning cycle, it continuously utilizes new observation images, LiDAR point clouds, and robot states to regenerate sub-targets, unfold trajectories, and evaluate trajectories. This allows the navigation results to adapt to environmental changes, sensor errors, and execution deviations in real time. This enhances the robot's online adjustment capabilities and robustness in dynamic scenes and uncertain environments.

[0056] Sixth: This invention triggers a recovery behavior when a feasible trajectory cannot be obtained through local planning, causing the robot to rotate in place at zero linear velocity and find a relatively unobstructed direction based on the current LiDAR point cloud. After changing the observation direction, it continuously replans the trajectory. This method can proactively adjust the robot's relative relationship with the environment in situations of local deadlock, limited field of view, or short-term unsolvability, improving the robot's ability to escape from occluded scenes, narrow spaces, and local obstructions, as well as its continuous navigation capabilities. Attached Figure Description

[0057] Figure 1 This is a schematic diagram of the hierarchical architecture of the robot hierarchical motion planning and control method based on visual topology guidance of the present invention;

[0058] Figure 2 This is a flowchart of the robot hierarchical motion planning and control method based on visual topology guidance according to the present invention. Detailed Implementation

[0059] 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.

[0060] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to limit the invention.

[0061] like Figure 1 As shown, this embodiment provides a hierarchical motion planning and control method for robots based on visual topology guidance, which includes a visual topology guidance layer, a local planning layer, and a control execution and recovery layer. The visual topology guidance layer is used to generate a sequence of navigation sub-targets based on the current visual observation and target image; the local planning layer is used to generate candidate trajectories and select the optimal control quantity under robot dynamics constraints and obstacle constraints; the control execution and recovery layer is used to execute the optimal control quantity and perform recovery behavior when no feasible trajectory is available.

[0062] This embodiment employs a layered approach. The upper layer is responsible for generating consistent guidance information based on the visual topology map, current observations, and historical observations to address the path-taking issue. The middle layer is responsible for generating a dynamically executable and safe trajectory under current velocity, acceleration constraints, and local obstacle distribution to address the safe walking issue. The lower layer is responsible for execution, feedback, and recovery to address the recovery strategy issue. This layered approach avoids mixing visual semantic guidance, local obstacle avoidance constraints, and closed-loop execution feedback in the same process, thereby reducing the instability caused by directly outputting lower-level control variables from visual results. At the same time, it avoids losing global goal consistency by relying solely on local obstacle avoidance.

[0063] Specifically, such as Figure 2 As shown, the method in this embodiment includes the following steps:

[0064] S101. Construct a visual topology graph and set the target image nodes.

[0065] A series of images are acquired along the robot's predetermined navigation path (reference path), and a visual topology map is constructed according to the acquisition order. In this embodiment, RGB (Red, Green, Blue) images are acquired using an image acquisition device. The entire visual topology map is composed of the acquired images. For example, if one image is acquired per second, and a total of 100 images are acquired, then the set of 100 images serves as the visual topology map.

[0066] Each image node in the visual topology map corresponds to a location or viewpoint on the path, and is a sequence of reference navigation images that does not contain obstacle interference. The visual topology map is mainly used to provide high-level visual guidance information, rather than to build a global geometric map that includes obstacles.

[0067] The target image node represents the target location or scene that the robot needs to reach. The target image node can select an image of the final location or an image of another location along the path. When the observed image acquired during navigation matches the target image node, the target location or scene is considered to have been reached, and navigation ends.

[0068] S102. Collect current observation information and establish an observation set.

[0069] The system acquires the robot's current observation image, LiDAR point cloud, robot state, historical visual observation window, and target image nodes in the visual topology graph. The robot state includes at least robot pose, linear velocity, and angular velocity. The historical visual observation window consists of observation images from multiple consecutive time points prior to the current time. Each planning cycle uses the robot's current body coordinate system as the local planning coordinate system, ensuring that the sub-target sequence, LiDAR point cloud, and candidate trajectories are all represented within this local planning coordinate system.

[0070] Robot at all times Acquire the current RGB observation image LiDAR point cloud and robot status The robot's state can be represented as:

[0071]

[0072] in, and express The robot's current position coordinates at any time. express The robot is facing an angle at all times. and They represent The robot's current linear velocity and angular velocity at any given moment.

[0073] To provide temporal context, this embodiment also saves several consecutive frames of RGB observation images prior to the current moment, forming a historical visual observation window. :

[0074]

[0075] in, For history RGB observation image at time [time] For history RGB observation image at time [time] For history The RGB observation image at a given time, as set in this embodiment. .

[0076] Therefore, the system The set of observations at time Represented as:

[0077]

[0078] in, This represents a node in the target image.

[0079] S103. Generate sub-target sequence: Based on the current RGB observation image, historical visual observation window, and target image nodes, generate the sub-target sequence for the current planning period.

[0080] The current RGB observation image Historical visual observation window and target image nodes The input is a visual sub-target generation module, which in this embodiment is a neural network model, specifically the ViNT (Visual Navigation Transformer) model. The model outputs multiple sub-targets defined in the local planning coordinate system at future time points. The visual sub-target generation module is responsible for providing high-level directional guidance and does not directly output low-level control commands.

[0081] Multiple sub-targets are arranged from near to far according to the predicted navigation direction. The closer sub-targets are used to represent immediate guidance, and the farther sub-targets are used to represent long-term guidance. This embodiment effectively utilizes the farthest sub-target for time-domain guidance.

[0082] In this embodiment, the sub-target sequence consists of five sub-targets, resulting in the sub-target sequence. as follows:

[0083]

[0084] Each sub-target point is represented as follows:

[0085]

[0086] in, express The first time generated Sub-goals , and For sub-targets Location, express The transpose of the target is used. The fifth sub-objective is selected as the local planning evaluation sub-objective to balance local obstacle avoidance and long-term directional guidance.

[0087] S104. Establish robot motion model and generate candidate control set: Combine the robot's current velocity state, acceleration constraints and velocity boundaries to sample the control space and obtain candidate control set.

[0088] Local planning is performed in the robot's current body coordinate system. For the robot, its kinematic model can be represented as:

[0089]

[0090]

[0091]

[0092] in, and for The robot's position coordinates at any given time. for The robot's orientation angle at any given moment. This represents the discrete integration step size.

[0093] To ensure that the control commands meet the requirements of dynamic executability, the robot's linear acceleration and angular acceleration are defined separately:

[0094]

[0095]

[0096] And satisfy:

[0097]

[0098]

[0099] This allows us to determine the dynamically feasible sampling range within the current planning period:

[0100]

[0101]

[0102] in, express linear acceleration at time t, for The linear velocity of the robot at any given moment. express Angular acceleration at time t, for The robot's angular velocity at any given moment. For linear acceleration constraints, i.e., maximum linear acceleration, For linear velocity, For angular acceleration constraints, i.e., maximum angular acceleration, ω is the angular velocity.

[0103] Combined with the upper and lower boundaries of linear velocity in the global velocity boundary and and the upper and lower boundaries of the corners and The sampled area is then clipped to obtain the final sampled region, which serves as the control space.

[0104] Then, discrete sampling is performed within the control space according to a preset sampling step size or sampling number to generate multiple candidate control quantities, thereby forming a candidate control set. Each candidate control variable includes at least one linear velocity component and one angular velocity component.

[0105] S105. Expand candidate trajectory: For each candidate control variable in the candidate control set, perform forward rolling prediction based on the robot kinematics model to obtain the candidate trajectory in the local time domain.

[0106] Based on each candidate control variable and a fixed integral step size, the robot's pose within the finite planning time domain is propagated forward according to the robot's kinematic model; the pose relative to the robot's current body coordinate system origin is obtained. The relative pose increment at a future moment; by The candidate trajectories are formed by the relative pose increments at each future moment. Each predicted pose is denoted as follows: (The citation is missing from the original text.)

[0107]

[0108] in, Indicates the first The relative pose increment at a future moment and For the first The relative position increment at a future moment. Indicates the first The relative orientation increment at a future moment, , by all The candidate trajectory that constitutes the candidate control quantity.

[0109] S106. Perform contour collision detection and feasibility screening: Based on the spatial relationship between the robot body contour and the LiDAR point cloud, perform collision detection on each candidate trajectory, eliminate infeasible trajectories, and obtain a set of feasible trajectories.

[0110] Obtain the initial contour of the robot in the body coordinate system. For any future time interval, the relative pose increment... By performing translation and rotation transformations on the initial contour, the robot contour in the predicted state is represented as follows:

[0111]

[0112] in, For the first Contour projection of a future time-predicted state It is a two-dimensional planar rotation matrix.

[0113] The contour projection in the predicted state is intersected with the LiDAR point cloud in the current robot body coordinate system; if the candidate trajectory satisfies the following at any future prediction time:

[0114]

[0115] If the contour projection of a candidate trajectory at a future time within the planning time domain intersects with the LiDAR point cloud, then the candidate trajectory is determined to have collided with an obstacle and is not a feasible trajectory. Only candidate trajectories that do not intersect are retained as feasible trajectories, resulting in the set of feasible trajectories. No intersection is defined as:

[0116]

[0117] S107. Conduct a comprehensive evaluation of feasible trajectories and obtain the optimal control quantity (instruction): Construct a comprehensive cost function for each feasible trajectory, and determine the optimal control quantity from the set of feasible trajectories based on the comprehensive cost function.

[0118] Establish a comprehensive cost function for feasible trajectories that pass collision detection. :

[0119]

[0120] in, Indicates candidate control quantity The overall cost, For target tracking items, For time efficiency, For obstacle clearance term, For smoothing terms, For consistency terms.

[0121] The target tracking term is used to characterize the positional deviation between the candidate trajectory endpoint and the local planning evaluation sub-objective, and is expressed by the formula:

[0122]

[0123] in, This represents the endpoint of the candidate trajectory. To evaluate the location of sub-objectives in local planning, Weights are used to track the target.

[0124] The time efficiency term is used to suppress excessively small linear velocities. The calculation process is as follows: Take the absolute value of the linear velocity component in the candidate control quantity; calculate the ratio of the absolute value of the linear velocity component to the maximum linear velocity in the global velocity boundary, using this as the velocity ratio; after limiting the velocity ratio to its maximum value, obtain the inverse complementary quantity; weight the inverse complementary quantity using time efficiency weights to obtain the time efficiency term. The formula is expressed as:

[0125]

[0126] in, Weighted by time efficiency, This represents the linear velocity component in the candidate control variable currently being evaluated.

[0127] The obstacle clearance term characterizes the minimum clearance between the candidate trajectory and obstacles, and can be based on a preset minimum safe clearance for the trajectory. It is constructed in exponential form as follows:

[0128]

[0129] in, For obstacle weights, This is the preset minimum safety threshold.

[0130] The smoothing term characterizes the linear and angular velocity changes between the current candidate control variable and the control variable executed at the previous time step. It penalizes abrupt changes in control commands between adjacent planning cycles. The formula is as follows:

[0131]

[0132] in, This represents the angular velocity component in the candidate control variable currently being evaluated. Indicates time Linear velocity in the optimal control quantity Indicates time Angular velocity in the optimal control quantity For smoothing weights.

[0133] The orientation consistency term characterizes the deviation between the orientation of the candidate trajectory endpoint and the target orientation provided by the sub-objective of the local planning evaluation. When the sub-objective sequence provides the target orientation... When the orientation is consistent, the term can be expressed as:

[0134]

[0135] in, The endpoint orientation of the candidate trajectory is calculated from the position of the visual sub-target. To achieve consistency weights, This indicates that the angle is constrained to Interval.

[0136] This indicates the actual orientation of the current candidate trajectory at the predicted endpoint, obtained by forward rolling prediction from the candidate control quantity; This represents the reference orientation determined by the local planning evaluation sub-objective output by the sub-objective generation step, used to characterize the expected direction of movement in the current planning cycle. The orientation consistency term is used to constrain the deviation between the candidate trajectory endpoint orientation and this reference orientation, thereby linking the sub-objective generation results with the local trajectory evaluation process. This is used in determining the target orientation. When determining feasibility, the obstacle space is used for verification. If there are no obstacles or sufficient passage space, the target orientation is deemed feasible; otherwise, the target orientation is deemed infeasible.

[0137] Ultimately, the candidate control variable with the lowest overall cost is selected as the optimal control variable. :

[0138]

[0139] S108. Execute optimal control and perform closed-loop update.

[0140] Optimal control quantity The data is sent to the robot for execution. After a short period of execution, the robot re-acquires new RGB observation images, LiDAR point clouds, and robot status, and repeats steps S102 to S107, thus constructing a rolling time-domain closed-loop control process. Within each planning cycle, only a portion of the trajectory corresponding to the optimal control command is executed. This process does not require executing the complete trajectory at once, but rather replans while executing, to enhance the system's adaptability to dynamic obstacles, perceived noise, and execution errors.

[0141] S109. Perform recovery behavior when there is no feasible trajectory.

[0142] If the set of feasible trajectories is empty after collision detection and feasibility screening in step S106, the robot is determined to be in a partially obstructed state. At this time, a recovery behavior is triggered, executing zero linear velocity and in-place rotation control. :

[0143]

[0144] in, The direction with the fewest obstacles is determined based on the LiDAR point cloud. By analyzing the distribution of the surrounding point cloud, the orientation of the open area is determined, and the robot rotates towards the relatively unobstructed direction. For example, the annular point cloud is divided into regions according to angles, and the number and distance distribution of obstacle points in each region are counted to quantitatively determine which direction has the least obstruction and the largest passage space. Finally, the robot is controlled to rotate in place to align with this unobstructed direction. During the recovery behavior execution, the process returns to step S102 and continues to repeat the local planning. When a feasible trajectory is obtained again, the in-place rotation stops and normal navigation resumes, proceeding to step S107.

[0145] To visually demonstrate the advantages of the method of this invention, the following experiments were conducted.

[0146] This embodiment first uses a Scout Mini differential motion vehicle as the experimental platform. An Intel RealSense D455i RGB camera and a Livox Mid360 LiDAR are mounted on the vehicle to acquire the current observation image and local obstacle point cloud information, respectively. The onboard computing unit is a laptop computer used to run the visual topology-guided robot hierarchical motion planning and control method of this invention. In the experiment, the method of this invention is compared with DWA (Dynamic Window Approach) and TEB (Timed Elastic Band) to verify the navigation success rate, path efficiency, and operational stability of the method in real-world scenarios. This embodiment sets up three typical real-world scenarios:

[0147] (1) Indoor scenario: A relatively dense array of obstacles is set up in an indoor environment to verify the robot's obstacle avoidance and target arrival capabilities under near-distance obstacle interference;

[0148] (2) Corridor scenario: Construct a narrow corridor environment with corners to verify the robot's trajectory smoothness and reaction capability under narrow passage and sharp turn conditions;

[0149] (3) Outdoor scenario: Scattered obstacles are placed in an open outdoor area to verify the navigation stability and robustness of the robot in a long path and open environment.

[0150] To further verify the applicability and transferability of the method of the present invention on different robot platforms, this embodiment also conducted cross-platform experiments on quadruped robot and humanoid robot platforms. The quadruped robot experiment used the Unitree Go2 as the test platform, which is equipped with a wide-angle camera, a Livox Mid360 LiDAR, and a small computing unit to acquire visual observation images, local point cloud information, and robot motion status. The humanoid robot experiment used the Unitree G1 as the test platform, which is equipped with an Intel RealSense D455 camera, a Livox Mid360 LiDAR, and an NVIDIA Jetson Orin computing unit to verify the deployment effect of the method of the present invention under different robot forms, different motion modes, and different sensor arrangement conditions.

[0151] In experiments with quadruped and humanoid robots, RGB images were first acquired along a reference path under obstacle-free conditions to form a reference image sequence for visual guidance. Then, during the testing phase, obstacles were placed near the reference path, enabling the robot to reach the target image node under visual topology guidance. Simultaneously, local obstacle avoidance and speed control were performed based on the current LiDAR point cloud. These experiments do not rely on a global geometric map or global positioning information; they only utilize the currently observed image, historical visual observation window, target image node, local point cloud, and robot speed state to complete closed-loop motion planning and control. For both quadruped and humanoid robots, the underlying motion controller is responsible for converting the speed control output by the method of this invention into gait execution actions for the corresponding platform. The method of this invention does not directly control the robot's joint angles, joint velocities, or joint torques.

[0152] To quantitatively evaluate the navigation performance of the method of this invention, the following three indicators are used:

[0153] (1) SR (Success Rate): represents the percentage of experiments in which the robot successfully reaches the target image node without collision.

[0154] (2) PL (Path Length): represents the actual distance the robot travels from the starting point to the target position;

[0155] (3) NT (Navigation Time): Represents the total time consumed by the robot to complete the entire navigation task.

[0156] Among them, success rate is used to measure the reliability and robustness of the method, path length is used to measure the path efficiency in the path planning and obstacle avoidance process, and navigation time is used to comprehensively reflect the robot's motion efficiency and the effectiveness of control decisions.

[0157] The experimental results of the three real-world scenarios in this embodiment are shown in Table 1:

[0158] Table 1 Comparison of navigation results of different methods in real-world environments

[0159]

[0160] As shown in Table 1, the method of this invention achieved the highest success rate in all three real-world scenarios. Specifically, in indoor, corridor, and outdoor scenarios, the success rate of the method reached 85%, 80%, and 80%, respectively, which is higher than DWA and TEB, indicating that this method has better local obstacle avoidance and stable control capabilities.

[0161] Regarding path length, the method of this invention is generally on par with TEB and DWA, but maintains shorter or comparable path lengths in all three scenarios. This indicates that the performance improvement of the method does not rely on detours or aggressive shortcuts, but rather on improving navigation reliability while ensuring path rationality. In terms of navigation time, the method of this invention achieves the shortest navigation time in corridor and outdoor scenarios, and its navigation time is close to that of TEB in indoor scenarios. These results demonstrate that the method of this invention can more effectively select executable control variables and reduce time losses caused by robot oscillations, pauses, or local obstructions in complex environments through joint constraints of sub-target guidance, candidate trajectory sampling, collision detection, and comprehensive cost evaluation.

[0162] This embodiment further conducts cross-platform experiments on quadruped robot and humanoid robot platforms. The experimental results are shown in Table 2.

[0163] Table 2. Navigation experimental results of the method of the present invention on a legged robot platform.

[0164]

[0165] As shown in Table 2, the method of this invention successfully completed 20 indoor obstacle avoidance and navigation experiments on the Unitree Go2 quadruped robot, reaching the target image node in 17 of them, with a success rate of 85%. On the Unitree G1 humanoid robot, the method also successfully reached the target image node in 15 of the 20 indoor obstacle avoidance and navigation experiments, with a success rate of 75%. These results demonstrate that the method of this invention can be applied not only to wheeled mobile platforms but also to legged robot platforms such as quadruped robots and humanoid robots.

[0166] In the quadruped robot experiment, the robot was able to move along a reference direction based on visual topology guidance results. When local obstacles appeared, it combined LiDAR point cloud data for trajectory selection and speed adjustment, thereby avoiding obstacles and continuing to move towards the target image node. This result demonstrates that the speed control output of the method of this invention can be used in conjunction with the underlying gait controller of the quadruped robot to form a closed loop of visual guidance, local obstacle avoidance, and speed execution.

[0167] In humanoid robot experiments, the robot's shape, movement, and sensor mounting positions differed from those of wheeled and quadruped robots. However, the method of this invention, by adjusting the robot's body contour, velocity boundaries, and acceleration constraints, was still able to complete the navigation task using the same visual topology guidance and local trajectory evaluation framework. This result demonstrates that the method of this invention has a certain degree of platform adaptability, enabling the conversion of visual sub-targets into velocity control commands that satisfy local safety constraints under different robot configurations.

[0168] In summary, the results show that the method of this invention can achieve a high navigation success rate, a short navigation time, and a superior path efficiency in real-world environments, indicating that the proposed navigation mechanism based on a combination of visual topology map guidance and local planning has good practical deployment capabilities and engineering application value.

[0169] In another embodiment, the present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of a vision-topology-guided robot hierarchical motion planning and control method.

[0170] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. Any references to memory, storage, databases, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory.

[0171] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this invention.

[0172] The above embodiments merely illustrate several implementation methods of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of the present invention should be determined by the appended claims.

Claims

1. A hierarchical motion planning and control method for robots based on visual topology guidance, characterized in that, Includes the following steps: Multiple images are acquired along the reference path to form a visual topology map, and one image in the visual topology map is marked as the target image node; Acquire observation images, LiDAR point clouds, and robot status of the robot during the current planning cycle; Based on the current observed image, historical visual observation window and target image node, the sub-target sequence of the current planning period is obtained, and the sub-target at the end of the sub-target sequence is extracted as the local planning evaluation sub-target; The control space for the current planning cycle is generated by combining acceleration constraints, global velocity boundaries, and velocities in the robot's state. A set of candidate controls is obtained by sampling within the control space. For each candidate control variable in the candidate control set, forward rolling prediction is performed based on the robot kinematics model to obtain candidate trajectories. Candidate trajectories with collision risk are eliminated, and feasible trajectories are output. The comprehensive cost of each feasible trajectory is calculated by combining the evaluation sub-objectives of local planning, and the candidate control quantity corresponding to the feasible trajectory with the minimum comprehensive cost is taken as the optimal control quantity. The robot is controlled according to the optimal control quantity to complete the current planning cycle and start the next planning cycle until the robot reaches the target image node.

2. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, The reference path is a path that does not contain obstacles, and the images in the visual topology map are arranged in chronological order according to the extension direction of the reference path.

3. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, Each planning cycle uses the robot's current body coordinate system as the local planning coordinate system, and the sub-target sequence, lidar point cloud, and candidate trajectory are all represented in the local planning coordinate system.

4. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, The process of obtaining the sub-target sequence for the current planning period based on the current observed image, historical visual observation window, and target image nodes includes: Input the current observed image, historical visual observation window, and target image node into the neural network model; The neural network model outputs sub-objectives for multiple future time points; Arrange the sub-targets in order of time from most recent to furthest to obtain the sub-target sequence.

5. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, The process of generating the control space for the current planning cycle by combining acceleration constraints, global velocity boundaries, and velocities in the robot's state includes: Obtain the linear velocity and angular velocity of the robot in the current planning cycle; Based on linear acceleration constraints and angular acceleration constraints, determine the dynamic feasible sampling intervals for linear velocity and angular velocity under the current planning period, respectively; Based on the upper and lower boundaries of linear velocity and angular velocity in the global velocity boundary, the dynamic feasible sampling interval is clipped to obtain the control space of linear velocity and angular velocity in the current planning cycle.

6. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, For each candidate control variable in the candidate control set, forward rolling prediction is performed based on the robot's kinematics model to obtain the candidate trajectory, including: Based on candidate control variables and a fixed integration step size, the robot's pose in the local time domain is forward propagated to obtain the relative pose increments at multiple future moments relative to the robot's current body coordinate system. The relative pose increments at all future moments corresponding to each candidate control variable constitute the candidate trajectory in the local time domain.

7. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 6, characterized in that, The process of eliminating candidate trajectories with collision risks and outputting feasible trajectories includes: Obtain the initial contour of the robot in the body coordinate system; Based on the relative pose increments at each future time in the candidate trajectory, the initial contour is translated and rotated to obtain the contour projection in the predicted state at each future time. Intersection detection is performed between the contour projection of each future time-predicted state and the LiDAR point cloud in the current robot body coordinate system; If the contour projection of a candidate trajectory in any future time prediction state intersects with the current lidar point cloud, a collision risk is considered to exist, and the corresponding candidate trajectory is determined to be an infeasible trajectory; otherwise, it is determined to be a feasible trajectory.

8. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, The comprehensive cost of each feasible trajectory is calculated by combining local planning evaluation sub-objectives. The calculation function for this comprehensive cost includes a target tracking term, a time efficiency term, an obstacle clearance term, a smoothing term, and an orientation consistency term, wherein: The target tracking term is used to characterize the positional deviation between the endpoint of the candidate trajectory and the local planning evaluation sub-objective; The time efficiency term is used to suppress excessively low linear velocities; The obstacle gap term is used to characterize the minimum gap between the candidate trajectory and the obstacle; The smoothing term is used to characterize the linear velocity and angular velocity changes between the current candidate control quantity and the optimal control quantity at the previous moment. The orientation consistency term is used to characterize the deviation between the endpoint orientation of the candidate trajectory and the target orientation provided by the local planning evaluation sub-objective; The target tracking term, time efficiency term, obstacle clearance term, smoothing term, and orientation consistency term are weighted and summed to obtain the comprehensive cost.

9. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 8, characterized in that, The time efficiency term is represented as follows: Take the absolute value of the linear velocity component in the candidate control quantity; The ratio of the absolute value of the linear velocity component to the maximum linear velocity in the global velocity boundary is used as the velocity ratio. After limiting the speed ratio to its maximum value, the reverse complementary amount is obtained; By weighting the reverse complementary quantities using time efficiency weights, the time efficiency term is obtained.

10. The robot hierarchical motion planning and control method based on visual topology guidance according to claim 1, characterized in that, Also includes: When no feasible trajectory is obtained, the recovery behavior is triggered, which means setting the linear velocity to zero and determining the direction with the fewest obstacles based on the current LiDAR point cloud. The robot is then controlled to perform a stationary rotation in the direction with the fewest obstacles. During the stationary rotation, the robot continuously acquires the observation images, LiDAR point cloud, and robot state of the current planning cycle for trajectory replanning. When a feasible trajectory is obtained again, the recovery behavior stops and processing continues based on the feasible trajectory.

Images