Robot motion planning method and system based on local sub-target guided learning

Abstract

The application provides a robot motion planning method and system based on local sub-target guidance learning, comprising: acquiring a current system state of a robot; inputting the current system state into a pre-constructed policy network to output an action vector; determining an execution mode based on a received rescaling factor; if it is determined that an action execution mode of reinforcement learning is executed, obtaining a scaled action based on the action vector and the rescaling factor, and sending the scaled action to a robot execution mechanism; and if it is determined that a motion planner mode is called, generating a local sub-target state based on the current system state and the action vector, taking the current system state as a starting node and the local sub-target state as a target node, performing local collision-free path planning through a motion planner, and sending a successfully planned path to the robot execution mechanism, so that an optimal balance between the efficiency of global exploration and the safety and reliability of local actions in a complex environment is achieved.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, and in particular to a robot motion planning method and system based on local sub-goal guided learning. Background Technology

[0002] With the development of industrial automation and intelligent service robot technology, robots need to complete tasks such as loading and unloading cargo, sorting goods in warehouses, and providing home services in complex, unstructured environments with dynamic or dense obstacles. This places stringent requirements on the speed, reliability, and safety of their path planning. Current mainstream solutions have significant shortcomings:

[0003] Traditional motion planning (MP) methods (such as Rapidly Exploring Random Tree (RRT) and its variants) can guarantee collision-free paths, but sampling in high-dimensional or complex obstacle spaces is blind, resulting in low planning efficiency, slow convergence, and a lack of context awareness, making it impossible to optimize paths using dynamic environmental information or task experience. Deep reinforcement learning (RL) methods (such as Deep Deterministic Policy Gradient (DDPG) and Soft Actor-Critic (SAC)) make decisions quickly and explore intelligently after training, but they suffer from high training difficulty, low sample efficiency, and are prone to collision risks in new environments or in the early stages of training, resulting in insufficient obstacle avoidance robustness.

[0004] Existing fusion solutions that combine the advantages of both methods use fixed thresholds or rules to achieve a "hard switch" between MP and RL. This switching mechanism is rigid and cannot be dynamically adjusted according to contexts such as the density of environmental obstacles and the stage of the task. At the same time, the functional decoupling is insufficient, either leading to an efficiency bottleneck due to over-reliance on MP or causing security risks due to over-trust in RL. It is difficult to balance the efficiency of global exploration with the safety of local actions.

[0005] Therefore, how to achieve the optimal balance between the efficiency of global exploration in complex environments and the safety and reliability of local actions is a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] This invention provides a robot motion planning method and system based on local sub-goal guided learning, which aims to achieve an optimal balance between the efficiency of global exploration in complex environments and the safety and reliability of local actions.

[0007] On one hand, the present invention provides a robot motion planning method based on local sub-objective guided learning, which includes:

[0008] Obtain the robot's current system state; the current system state includes the robot's own pose information, environmental obstacle information, and task objective;

[0009] The current system state is input into a pre-built policy network, which outputs an action vector, which is configured as a local sub-target to guide the robot's movement.

[0010] The execution mode is determined based on the received rescaling factor; the execution mode includes reinforcement learning action execution mode and motion planner invocation mode.

[0011] If it is determined that the execution mode is a reinforcement learning action, a scaling action is obtained based on the action vector and the rescaling factor and sent to the robot actuator;

[0012] If the motion planner mode is invoked, a local sub-target state is generated based on the current system state and the motion vector. The local sub-target state is used as the starting node and the local sub-target state is used as the target node. The motion planner performs local collision-free path planning and sends the successfully planned path to the robot actuator.

[0013] On the other hand, the present invention also provides a robot motion planning system based on local sub-objective guided learning, which includes:

[0014] The acquisition module is used to acquire the current system state of the robot; the current system state includes the robot's own pose information, environmental obstacle information, and task objective.

[0015] The input module is used to input the current system state into a pre-built policy network and output an action vector, which is configured as a local sub-target to guide the robot's movement.

[0016] The determination module is used to determine the execution mode based on the received rescaling factor; the execution mode includes reinforcement learning action execution mode and motion planner invocation mode;

[0017] The reinforcement learning module is used to obtain a scaling action based on the action vector and the rescaling factor and send it to the robot actuator if it is determined that the execution mode is to perform reinforcement learning action.

[0018] The motion planning module is used to generate a local sub-target state based on the current system state and the motion vector if the motion planner mode is invoked. It then uses the current system state as the starting node and the local sub-target state as the target node to perform local collision-free path planning through the motion planner, and sends the successfully planned path to the robot actuator.

[0019] The robot motion planning method and system based on local sub-target guided learning provided by this invention obtains the robot's current system state; inputs the current system state into a pre-constructed policy network, outputting action vectors; determines the execution mode based on the received rescaling factor; if determined to be the reinforcement learning action execution mode, a scaled action is obtained based on the action vector and the rescaling factor and sent to the robot actuator; if determined to be the motion planner invocation mode, a local sub-target state is generated based on the current system state and action vectors, and a local collision-free path is planned through the motion planner with the current system state as the starting node and the local sub-target state as the target node, and the successfully planned path is sent to the robot actuator. In this way, the role of RL is elevated from low-level action generation to high-level goal setting, allowing MP to focus on the realization of safe paths. At the same time, the adaptive decision-making of the rescaling factor realizes the dynamic and smooth switching between MP and RL, thereby achieving the optimal balance between the efficiency of global exploration in complex environments and the safety and reliability of local actions. Attached Figure Description

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

[0021] Figure 1 This is a flowchart illustrating the robot motion planning method based on local sub-target guided learning provided in an embodiment of the present invention.

[0022] Figure 2 This is a schematic diagram of the structure of a robot motion planning system based on local sub-target guided learning provided in an embodiment of the present invention. Detailed Implementation

[0023] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0024] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, in the description of this invention, terms such as "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0025] Figure 1 This is a flowchart illustrating the robot motion planning method based on local sub-target guided learning provided in an embodiment of the present invention.

[0026] like Figure 1 As shown, the robot motion planning method based on local sub-objective guided learning provided in this embodiment of the invention mainly includes the following steps:

[0027] 101. Obtain the current system status of the robot;

[0028] In a specific implementation process, the robot first collects its own pose information (including the angles of each joint, the position and attitude of the end effector), environmental obstacle information (the position and shape of static obstacles, and the speed and direction of movement of dynamic obstacles) and task objectives (such as the specified work position and movement trajectory requirements) through sensors (such as radar and cameras), and integrates them to form the current system state, ensuring that the state information fully covers the core parameters required for motion planning.

[0029] 102. Input the current system state into the pre-built policy network and output the action vector. The action vector is configured as a local sub-objective to guide the robot's movement.

[0030] In a specific implementation, the policy network employs the soft actor-critic (SAC) algorithm for end-to-end training. Its core design innovation lies in upgrading the output role from "direct action commands" to "local sub-goal guidance." The network output layer constrains the action vector within the range [-1, 1] using a tanh activation function, avoiding extreme commands that exceed the robot's motion capabilities. For example, in joint space control scenarios, the action vector corresponds to the incremental targets of each joint angle; in Cartesian space control scenarios, the action vector corresponds to the incremental targets of the end effector's position and orientation, ensuring the rationality and feasibility of local sub-goals.

[0031] 103. Determine the execution mode based on the received rescaling factor;

[0032] In a specific implementation, the execution modes include RL execution mode and MP call mode; the rescaling factor is the core parameter for dynamically switching between MP call mode and RL execution mode, and its value is related to the environmental safety and familiarity with the task area.

[0033] Specifically, the process of obtaining the rescaling factor may include: obtaining the robot's environmental state information, robot pose information, and task execution history data; assessing environmental safety based on the environmental state information and robot pose information to obtain environmental safety; assessing regional familiarity based on the task execution history database to obtain regional familiarity; generating a dynamic adjustment signal based on environmental safety and regional familiarity; and updating the rescaling factor according to the dynamic adjustment signal.

[0034] Specifically, sensors can collect environmental status information in real time, such as the density, distribution, and shape complexity of static obstacles, and the number, speed, and direction of movement of dynamic obstacles. Robot pose information, such as current position, posture, and relative position to obstacles, can also be collected. Simultaneously, historical task data can be extracted from the task execution history database, such as past execution records for the current area, path planning results, and collision information, ensuring the comprehensiveness and real-time nature of the evaluation data.

[0035] In a specific implementation process, environmental safety assessment is carried out based on environmental state information and robot pose information. The core is to quantify the safety risks of the current environment to the robot's movement. The assessment result is a value between 0 and 1 (the closer the value is to 1, the higher the environmental safety; the closer it is to 0, the higher the safety risk), which directly reflects the collision risk of the robot performing RL actions in the current environment.

[0036] Specifically, based on the relative relationship between the movement speed and direction of dynamic obstacles and the robot's current position in the environmental state information, a threat level score for each dynamic obstacle is calculated. Static obstacle distribution features are extracted, and a static environment complexity score is generated based on static obstacle density, the minimum distance between static obstacles and the robot, and the shape complexity of static obstacles. The threat level score and the static environment complexity score are then fused, weighted, summed, and normalized to output the environmental safety score. A lower environmental safety score indicates a greater tendency to use the MP (Multi-Path Implementation) call mode for safe path planning; a higher environmental safety score indicates a greater tendency to use the RL (Restricted Path Execution) mode for safe path planning.

[0037] In this embodiment, a threat level score is first calculated for dynamic obstacles. The speed (the faster the speed, the higher the collision risk and the greater the score weight) and direction (if the direction points to the robot's current position, the threat level is high; if it is away from or perpendicular to the robot, the threat level is low) of each dynamic obstacle are extracted. Combined with the relative position of the obstacle and the robot (the closer the distance, the higher the threat level), the threat score of a single dynamic obstacle is calculated using the method of "speed weight × distance coefficient × direction coefficient". For example, the speed weight is set to 0.3 (1 when the speed is at its maximum and 0 when the speed is 0), the distance coefficient is set to 0.4 (1 when the distance is closest and 0 when the distance exceeds the safety threshold), and the direction coefficient is set to 0.3 (1 when the direction points to the robot and 0 when it is away from the robot). The threat level score of a single dynamic obstacle is the sum of the three factors, ranging from 0 to 1. Then, the average of the threat scores of all dynamic obstacles is taken to obtain the overall dynamic threat score.

[0038] Secondly, a static environment complexity score is performed for static obstacles. Three core features can be extracted: static obstacle density (the number of obstacles per unit volume; higher density results in a higher score), minimum distance between static obstacles and the robot (closer distance results in a higher score), and static obstacle shape complexity (more irregular shapes, such as those with sharp protrusions or grooves, result in a higher score). The weight of each feature can be set to 1 / 3, with a score range of 0-1. The scores of the three features are added together to obtain the static environment complexity score, which ranges from 0 to 3. This score is then normalized to the 0-1 interval (i.e., divided by 3) to obtain a standardized static environment complexity score.

[0039] Then, the environmental safety score is calculated using a weighted summation method. For example, the dynamic threat score is weighted at 0.6, and the static environment complexity score is weighted at 0.4 (because dynamic obstacles have higher uncertainty and a greater impact on safety). The summation yields an initial score (range 0-1), which is then normalized (ensuring the result strictly falls within the 0-1 range) to finally output the environmental safety score. For example, if the dynamic threat score is 0.3 and the static environment complexity score is 0.2, the initial score is 0.3 × 0.6 + 0.2 × 0.4 = 0.26. After normalization, the environmental safety score is 0.26, indicating a high current environmental safety risk and a tendency to invoke the MP invocation mode. If the dynamic threat score is 0.1 and the static environment complexity score is 0.1, the environmental safety score is 0.1 × 0.6 + 0.1 × 0.4 = 0.1. After normalization, it is 0.1, indicating a low safety risk and a tendency to execute the RL execution mode.

[0040] In a specific implementation process, the area familiarity assessment can be carried out based on the task execution history database. The core is to quantify the robot's "experience accumulation" in the current work area. The assessment result is also a value between 0 and 1 (the closer the value is to 1, the higher the area familiarity; the closer it is to 0, the higher the unfamiliarity). It directly reflects the reliability of the RL strategy in the current area (high familiarity means that the RL strategy has been fully verified and has high reliability; low familiarity means that the RL strategy lacks experience and has higher risk).

[0041] Specifically, the robot's current position can be matched with the position records in historical task data to obtain the historical execution area; the success rate, average planning time, average path length and number of collisions of historical tasks in the matched area can be extracted to construct the area familiarity feature vector; the feature vector is input into a pre-trained lightweight neural network model to output the area familiarity; the higher the area familiarity, the more inclined to use the RL execution mode directly, and the lower the area familiarity, the more inclined to use the MP call mode for safe path planning.

[0042] In detail, a location matching algorithm (such as GPS-based or environmental feature-based positioning matching) can be used to compare the robot's current location with historical location records in the task execution history database. Historical execution areas with an overlap of ≥80% with the current area can be selected as matching areas. The range of the matching area can be adjusted according to the task scenario (e.g., for indoor work areas with a small range, the overlap threshold is set to 90%; for outdoor work areas with a large range, the overlap threshold is set to 70%) to ensure the consistency between the matching area and the current work environment.

[0043] Secondly, four core features can be extracted from the historical task data of the matching region to construct a region familiarity feature vector: historical task success rate (where the number of successfully completed tasks / total number of tasks; the higher the success rate, the higher the familiarity), average planning time (where the average planning time of all historical tasks; the shorter the time, the more familiar the environment and the higher the planning efficiency), average path length (where the average path length of all historical tasks; the more reasonable the length, the more thorough the exploration of the environment), and collision count (where the total number of collisions in historical tasks; the fewer the collisions, the higher the reliability of the RL execution mode in the region). Each feature is normalized to the 0-1 range (e.g., success rate is directly 0-1, average planning time is normalized by "(maximum planning time - current average time) / maximum planning time", and collision count is normalized by "(maximum collision count - current collision count) / maximum collision count") to ensure the consistency of the feature vector.

[0044] Then, a lightweight neural network model can be selected using a small multilayer perceptron (MLP). This model is pre-trained on a large amount of historical data (the training data consists of feature vectors of different regions and manually labeled familiarity tags), and has fast inference capabilities, making it suitable for real-time evaluation scenarios. The constructed region familiarity feature vectors are input into this model, and the model calculates the output region familiarity through forward propagation. The value ranges from 0 to 1, where feature vectors with high success rates, short average planning times, and few collisions have an output familiarity close to 1; conversely, feature vectors with low success rates and few collisions have an output familiarity close to 0.

[0045] For example, if the historical task success rate of the matching region is 95%, the average planning time is 0.8 after normalization, the average path length is 0.7 after normalization, and the number of collisions is 0.9 after normalization, the constructed feature vector is (0.95, 0.8, 0.7, 0.9). After inputting it into the pre-trained model, the output region familiarity is 0.85, indicating that the region is highly familiar, the RL execution mode is highly reliable, and the RL execution mode is preferred. If the feature vector is (0.6, 0.3, 0.4, 0.2), the output familiarity is 0.3, indicating that the region is unfamiliar, the RL execution mode is risky, and the MP call mode is preferred.

[0046] In a specific implementation, after receiving the rescaling factor obtained from the above process, the rescaling factor can be input into a nonlinear normalized mapping function, converting it into a Bernoulli distribution sampling parameter between 0 and 1. This parameter directly corresponds to the probability of the two execution modes: the closer the parameter is to 1, the more inclined it is towards the RL execution mode; the closer it is to 0, the more inclined it is towards the MP call mode. This probabilistic sampling achieves a "soft switch" between the two modes, rather than a "hard switch" based on fixed rules, ensuring adaptability to environmental changes.

[0047] Specifically, the sigmoid function can be selected as the nonlinear normalization mapping function. It can map any range of rescaling factors to between 0 and 1, forming the probability sampling parameters required for the Bernoulli distribution. Moreover, the mapping process is monotonic, ensuring a positive correlation between the rescaling factor and the sampling parameters (i.e., the larger the rescaling factor, the closer the sampling parameters are to 1; conversely, the smaller the rescaling factor, the closer they are to 0), thus achieving a precise correlation between the execution mode probability and the environment and task state.

[0048] After completing the nonlinear transformation, Bernoulli distribution sampling can be performed based on the obtained sampling parameters (probability values ​​between 0 and 1). The Bernoulli distribution contains only two output results (such as "1" and "0"). The system pre-determines the correspondence between the sampling results and the execution mode. For example, when the sampling result is "1", it corresponds to the RL execution mode; when the sampling result is "0", it corresponds to the MP call mode. The randomness of the sampling process ensures the flexibility of mode switching and avoids the rigidity caused by fixed rules, while the determinism of the sampling parameters ensures that the mode selection tendency matches the environment and task state, realizing adaptive decision-making with "regularity within randomness".

[0049] For example, when the rescaling factor is large (reflecting high environmental safety and area familiarity), the sampling parameters after mapping by the sigmoid function are close to 1, and the probability of the sampling result being "1" is extremely high, so the RL execution mode is likely to be executed, giving full play to its rapid decision-making advantage; when the rescaling factor is small (reflecting low environmental safety and area familiarity), the sampling parameters are close to 0, and the probability of the sampling result being "0" is extremely high, so the MP calling mode is likely to be invoked, relying on its safe path planning capability; when the rescaling factor is in the middle value, the sampling probabilities of the two modes are balanced, and the system can dynamically adjust according to real-time environmental feedback, taking into account both exploration and safety.

[0050] 104. If it is determined that the RL execution mode is being executed, the scaling action is obtained based on the motion vector and the rescaling factor and sent to the robot actuator.

[0051] In a specific implementation, when the sampling result is determined to be in RL execution mode, the action vector output by the policy network can be directly multiplied by the rescaling factor to obtain the scaled action command. The dynamic adjustment of the rescaling factor can ensure that the action amplitude matches the environmental safety and area familiarity (e.g., when the safety and familiarity are high, the factor value is large and the action amplitude is moderate, meeting the needs of efficient adjustment; when the safety is low, the factor value is small and the action amplitude is conservative, reducing the risk of collision). Then, the scaled action is directly sent to the robot actuator to achieve rapid local adjustment.

[0052] 105. If the system is determined to be in MP call mode, a local sub-target state is generated based on the current system state and motion vector. The local sub-target state is used as the starting node and the local sub-target state is used as the target node. The motion planner performs local collision-free path planning and sends the successfully planned path to the robot actuator.

[0053] In a specific implementation, when the sampling result indicates an MP call mode, the system generates a clear local sub-target state based on the current system state and the local sub-target action vector output by the policy network. The motion planner, starting from the current system state and ending at the local sub-target state, performs a collision-free path search in the robot configuration space to ensure the safety and feasibility of the path. After successful planning, the complete path or segmented path points are sent to the actuator to achieve precise and safe motion control.

[0054] The process of generating a clear local sub-target state may include: multiplying the action vector by a preset scaling factor to obtain the adjusted action increment; and adding the current system state to the action increment to obtain the local sub-target state.

[0055] Specifically, a preset scaling factor can be set. This preset scaling factor can be determined based on the robot's physical motion limits (such as the maximum rotation angle of a joint and the maximum travel distance of the end effector), task accuracy requirements, and environmental space range. Its function is to convert standardized motion vectors into specific incremental values ​​that conform to the robot's actual motion capabilities. For example, if the maximum rotation angle increment of a certain joint of the robot is ±10°, then the preset scaling factor is set to 10, converting motion vectors in the range of [-1,1] into actual joint angle increments of [-10°,10°], ensuring the physical feasibility of the motion increments.

[0056] After obtaining the adjusted motion increment, this motion increment can be vector-added with the aforementioned current system state. The current system state includes core parameters such as the robot's own pose (e.g., joint angles, end effector position), environmental obstacle information, and task objective. Among these, the pose parameters (joint space or Cartesian space pose) are directly related to the motion increment. During the addition process, only the pose parameters are incrementally added, while the environmental obstacle information and task objective remain unchanged, ensuring that the local sub-target state focuses on the robot's motion target while preserving the constraint information of the environment and task. For example, if the Cartesian coordinates of the end effector in the current system state are (x0, y0, z0), and the increment of the motion vector after scaling is (Δx, Δy, Δz), then the end effector coordinates of the local sub-target state are (x0+Δx, y0+Δy, z0+Δz), clearly defining the local position that the robot needs to reach.

[0057] In this embodiment, the preset scaling factor can be dynamically adjusted according to the task scenario. For example, in high-precision operation scenarios (such as equipment maintenance and parts assembly), the scaling factor is set to be smaller, the action increment is more refined, and the accuracy of the sub-target state is ensured; in large-scale movement scenarios (such as warehouse sorting and area patrol), the scaling factor is set to be larger, the action increment is more significant, and the planning efficiency is improved.

[0058] In a specific implementation, the process of performing collision-free path search may include:

[0059] In the robot's configuration space, multiple random state points are generated through random sampling;

[0060] Based on environmental obstacle information, collision detection is performed on each random state point to obtain collision detection results; the collision detection results include no collision and collision.

[0061] Starting with the current system state as the starting node and the local sub-target state as the target node, the path network is gradually expanded, connecting collision-free random state points to the existing path network until a collision-free path from the starting state to the target state is generated.

[0062] If a collision-free path is successfully generated within the preset time, the planning is successful, and the collision-free path is recognized as the successfully planned path.

[0063] If no collision-free path is generated within the preset time, the planning fails.

[0064] Specifically, the motion planner is based on the robot's configuration space (i.e., the space formed by the combination of all the robot's joint degrees of freedom). It can use algorithms such as Rapid Random Tree Exploration (RRT) for random sampling. This algorithm generates state points randomly and gradually expands the path tree. It is suitable for path search in high-dimensional space, and the sampling density can be dynamically adjusted according to the complexity of the environment (e.g., increasing the sampling density when there are dense obstacles to improve the probability of path discovery; and reducing the sampling density when the environment is open to improve planning efficiency). It generates multiple random state points covering the configuration space, providing a foundation for the construction of the path network.

[0065] After generating random state points, collision detection can be performed on each state point based on the previously acquired environmental obstacle information to filter out collision-free safe state points. The core of collision detection is to verify whether the robot pose corresponding to the random state point will interfere with environmental obstacles, ensuring the safety of the subsequent path network. After the detection is completed, all collision-free random state points are marked as safe points and used as potential nodes in the path network.

[0066] The path network expansion process takes the current system state as the starting node and the local sub-target state as the target node, and adopts a "step-by-step connection + proximity search" strategy: First, the starting node is added to the path network. Then, the state point closest to the current network node (based on Euclidean distance in configuration space) is searched from the safe state points. The connection between the two points (i.e., the local path) is verified to be collision-free (collision detection logic is the same as before). If there is no collision, the safe state point is connected to the path network. This process is repeated to continuously expand the coverage of the path network until the target node (local sub-target state) is included in the path network or a continuous path from the starting node to the target node is formed.

[0067] In a specific implementation process, a planning time threshold can be preset. This planning time threshold can be determined according to the real-time requirements of the task (e.g., the threshold is set to 0.5 seconds for emergency operation scenarios and 2 seconds for high-precision operation scenarios). If a continuous collision-free path is successfully generated within the threshold time, the planning is considered successful. The path is then split into discrete path points according to the robot's execution requirements (e.g., split by time step or distance step) and sent to the execution mechanism. If no effective path is generated after the threshold time has expired, the planning is considered to have failed. The system will then send a negative reward signal to the aforementioned strategy network, triggering the strategy network to adjust the local sub-objectives of the next time step or to activate the rollback mechanism (e.g., return to the previous safe state point for replanning).

[0068] This embodiment balances the comprehensiveness and efficiency of path search by using random sampling and dynamic sampling density adjustment in the RRT algorithm; it ensures the safety of the path network by combining collision detection; and it balances planning accuracy and real-time performance by using a preset time threshold, avoiding the impact of indefinite planning on task execution.

[0069] In some embodiments, the process of performing collision detection for each random state point may include:

[0070] Map each random state point to the robot's own pose information;

[0071] For each random state point, multi-resolution collision detection is performed on the robot's own pose information to obtain the collision detection results; wherein, multi-resolution collision detection includes:

[0072] First-level pre-screening: Use the robot's external simplified geometric model to perform an intersection test with the simplified models of all obstacles in the environmental obstacle information; if they intersect, it is determined that there is a collision; if they do not intersect, it is determined that there is no collision, and the currently detected random state point is marked as a safe point;

[0073] Level 2 Precise Collision Detection: For random state points that fail the Level 1 pre-screening, the robot's precise geometric model is used to perform precise intersection calculation tests with the precise models of obstacles in the environmental obstacle information; if they intersect, it is determined that there is a collision; if they do not intersect, it is determined that there is no collision, and the currently detected random state point is marked as a safe point.

[0074] Specifically, for random state points generated in the robot's configuration space, they can be mapped to the robot's own pose information through forward kinematics calculations. This pose information encompasses core motion parameters such as joint angles, the position and orientation of the end effector, and the overall robot posture, providing crucial information reflecting the robot's actual motion state. The mapping process is based on the robot's kinematic model. From the combination of joint degrees of freedom corresponding to the random state points in the configuration space, the actual pose parameters of each moving part of the robot are derived in reverse. For example, by combining joint angles from the random state points, the coordinates and orientation of the end effector in the workspace are calculated, ensuring that the mapping results accurately reflect the robot's own motion posture in that state, providing accurate foundational data for subsequent collision detection.

[0075] After mapping, a "coarse-to-fine" multi-resolution collision detection strategy can be executed for the robot's own pose information corresponding to each random state point, maximizing detection efficiency while ensuring detection accuracy.

[0076] The first-level pre-screening focuses on "quickly eliminating safe points without collision risks" and employs an external simplified geometric model for intersection testing. The external simplified geometric model of the robot is a simple, wraparound geometry constructed based on its mapped pose information. For example, a minimum cuboid is constructed based on the end effector pose, and a cylinder is constructed based on the length and angle of the joint links, ensuring that the simplified model roughly covers the robot's physical location in that pose. Similarly, simplified models of environmental obstacles are created, simplifying complex shapes (such as shelves, equipment, and dynamic obstacles) into easily computable geometric shapes like cuboids and spheres. The intersection test quickly filters by determining whether the simplified robot model and the simplified obstacle model overlap: if there is no intersection, the robot pose corresponding to that random state point is directly determined to have no collision risk and marked as a safe point, without needing to proceed to the next testing stage; if there is an intersection, it indicates a potential collision risk at that state point, requiring further verification in the second-level precise testing stage.

[0077] The second level of precise collision detection performs accurate verification of "high-risk state points." Its core is to perform detailed intersection calculations between a precise geometric model constructed based on the robot's own pose information and a precise model of the obstacle. The robot's precise geometric model fully reproduces the actual structural features under its pose, including the body outline, the true shape of joints and links, and the detailed structure of the end effector (such as the teeth of the gripper and the shape of the tool). Furthermore, the model parameters strictly match the mapped pose information (e.g., joint angles determine the relative positions of links, and the end effector pose determines its precise coordinates in space). The obstacle's precise model reproduces its true physical contours, including details such as surface protrusions, grooves, and irregular corners. Precise intersection calculations accurately determine whether there is actual interference between the robot and the obstacle through point-by-point comparison, surface distance measurement, and spatial overlap volume analysis: if the precise models do not intersect, it is determined to be a collision-free point and marked as safe; if there is any intersection, it is determined to be a collision, and this random state point is excluded and does not participate in subsequent path network construction.

[0078] In this embodiment, the first-level pre-screening quickly filters out the vast majority of state points without collision risk, and concentrates computing resources on the accurate detection of high-risk points. This avoids the problems of excessive computation and insufficient real-time performance caused by single accurate detection, and eliminates the risk of missed collision detection caused by the simplified model through the second-level accurate detection.

[0079] In some embodiments, to further improve the adaptability and efficiency of collision detection, the present invention also provides the following technical solution: For random state points that fail the first-level pre-screening, before performing precise intersection calculation tests using the robot's precise geometric model and the precise models of obstacles in the environmental obstacle information, the following operations can also be performed:

[0080] Calculate the Euclidean distance from each random state point that failed the first-level pre-screening to the nearest obstacle;

[0081] Based on the preset mapping relationship between Euclidean distance and detection accuracy, the detection accuracy of the second-level precise collision detection is determined, and the precise geometric model of the robot and the precise model of the obstacle are selected according to the detection accuracy.

[0082] The closer the Euclidean distance, the higher the accuracy of the robot's precise geometric model and the precise model of the obstacle.

[0083] Specifically, for random state points that fail the first-level pre-screening (i.e. high-risk points with collision risk), the Euclidean distance between the robot pose corresponding to the state point and the nearest obstacle can be calculated. The Euclidean distance is calculated based on coordinates in the three-dimensional workspace. The two points with the smallest distance from the obstacle surface on the robot body and link surface are selected, and their straight-line distance is calculated to ensure that the distance value can truly reflect the proximity between the robot and the obstacle.

[0084] In this embodiment, a mapping table between Euclidean distance and detection accuracy can be preset. This mapping relationship is determined based on a large amount of experimental data. The core logic is that "the closer the distance, the higher the collision risk, and the higher the detection accuracy is required; the farther the distance, the lower the collision risk, and the detection accuracy can be appropriately reduced to improve efficiency." For example, when the Euclidean distance is ≤5cm, the detection accuracy is set to "ultra-high accuracy"; when 5cm < distance ≤15cm, the detection accuracy is set to "high accuracy"; and when 15cm < distance ≤30cm, the detection accuracy is set to "standard accuracy".

[0085] In a specific implementation process, different detection accuracies correspond to geometric models with different levels of detail: the "ultra-high precision" model fully reproduces all surface details of the robot and obstacles (such as screws, grooves, protrusions, and other minute structures); the "high precision" model reproduces the main surface details, ignoring millimeter-level minute structures; and the "standard precision" model reproduces the overall outline and key structures, ignoring minor details. The system determines the detection accuracy by querying a mapping table based on the calculated Euclidean distance, and then automatically selects the corresponding precise geometric model of the robot and the precise model of the obstacle to perform targeted and precise intersection calculations.

[0086] For example, if the Euclidean distance from a random state point to the nearest obstacle is 3cm (which falls within the "ultra-high precision" range), then a robot and obstacle model containing all minute structures is selected, and intersection calculations are performed with millimeter-level precision to ensure no collisions are missed. If the distance is 20cm (which falls within the "standard precision" range), then a model with simplified minor details is selected to reduce computational complexity while ensuring the effectiveness of collision-free verification.

[0087] In this embodiment, the collision risk level is determined by Euclidean distance, and the detection accuracy and geometric model details are dynamically matched. In scenarios with high collision risk, the detection accuracy is guaranteed and collision accidents are avoided; in scenarios with low collision risk, the computational load is reduced and the detection efficiency is improved.

[0088] Based on the same general inventive concept, this invention also protects a robot motion planning system based on local sub-objective guided learning. The robot motion planning system based on local sub-objective guided learning provided by this invention will be described below. The robot motion planning system based on local sub-objective guided learning described below can be referred to in correspondence with the robot motion planning method based on local sub-objective guided learning described above.

[0089] Figure 2 This is a schematic diagram of the structure of a robot motion planning system based on local sub-objective guided learning provided in an embodiment of the present invention, as shown below. Figure 2 As shown, the robot motion planning system based on local sub-target guided learning in this embodiment includes an acquisition module 21, an input module 22, a decision module 23, a reinforcement learning module 24, and a motion planning module 25.

[0090] The acquisition module 21 is used to acquire the current system state of the robot; the current system state includes the robot's own pose information, environmental obstacle information, and task objective.

[0091] Input module 22 is used to input the current system state into a pre-built policy network and output action vectors, which are configured as local sub-goals to guide the robot's movement.

[0092] The determination module 23 is used to determine the execution mode based on the received rescaling factor; the execution mode includes RL execution mode and MP call mode.

[0093] The reinforcement learning module 24 is used to obtain the scaling action based on the action vector and the rescaling factor and send it to the robot actuator if the RL execution mode is determined to be executed.

[0094] The motion planning module 25 is used to generate a local sub-target state based on the current system state and motion vector if the MP call mode is determined. It then uses the current system state as the starting node and the local sub-target state as the target node to perform local collision-free path planning through the motion planner, and sends the successfully planned path to the robot actuator.

[0095] It should be noted that all relevant information that may be involved in the various embodiments of the present invention is processed in strict accordance with the requirements of laws and regulations, following the principles of legality, legitimacy, and necessity, based on the reasonable purpose of the business scenario, and is information that users actively provide or generate during the use of the product / service, as well as information obtained with user authorization.

[0096] The information processed by this invention may vary depending on the specific product / service scenario and should be based on the specific scenario in which the user uses the product / service. This may involve user account information, device information, or other related information. This invention will treat the relevant information and its processing with the utmost diligence.

[0097] This invention places great emphasis on the security of relevant information and has adopted reasonable and feasible security protection measures that comply with industry standards to protect user information and prevent unauthorized access, public disclosure, use, modification, damage or loss of relevant information.

[0098] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods of various embodiments or some parts of embodiments.

[0099] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot motion planning method based on local sub-objective guided learning, characterized in that, include: Obtain the robot's current system state; the current system state includes the robot's own pose information, environmental obstacle information, and task objective; The current system state is input into a pre-built policy network, which outputs an action vector, which is configured as a local sub-target to guide the robot's movement. The execution mode is determined based on the received rescaling factor; the execution mode includes reinforcement learning action execution mode and motion planner invocation mode. If it is determined that the execution mode is a reinforcement learning action, a scaling action is obtained based on the action vector and the rescaling factor and sent to the robot actuator; If the motion planner mode is invoked, a local sub-target state is generated based on the current system state and the motion vector. The local sub-target state is used as the starting node and the local sub-target state is used as the target node. The motion planner performs local collision-free path planning and sends the successfully planned path to the robot actuator.

2. The robot motion planning method based on local sub-objective guided learning according to claim 1, characterized in that, Based on the current system state and the action vector, a local sub-target state is generated, including: Multiply the motion vector by a preset scaling factor to obtain the adjusted motion increment; The current system state is added to the action increment to obtain the local sub-target state.

3. The robot motion planning method based on local sub-objective guided learning according to claim 1, characterized in that, Based on the received rescaling factor, the execution mode is determined, including: The rescaling factor is input into a nonlinear normalized mapping function for nonlinear transformation, and the rescaling factor is mapped to Bernoulli distribution sampling parameters. Bernoulli distribution sampling is performed based on the sampling parameters, and the current execution mode is determined based on the sampling results.

4. The robot motion planning method based on local sub-objective guided learning according to claim 1, characterized in that, Using the current system state as the starting node and the local sub-target state as the target node, a local collision-free path is planned using a motion planner, including: In the robot's configuration space, multiple random state points are generated through random sampling; Based on environmental obstacle information, collision detection is performed on each random state point to obtain collision detection results; the collision detection results include no collision and collision. Starting with the current system state as the starting node and the local sub-target state as the target node, the path network is gradually expanded, connecting collision-free random state points to the existing path network until a collision-free path from the starting state to the target state is generated. If a collision-free path is successfully generated within the preset time, the planning is successful, and the collision-free path is regarded as the successfully planned path. If no collision-free path is generated within the preset time, the planning fails.

5. The robot motion planning method based on local sub-objective guided learning according to claim 4, characterized in that, Based on environmental obstacle information, collision detection is performed on each random state point to obtain collision detection results, including: Map each random state point to the robot's own pose information; For each random state point, multi-resolution collision detection is performed on the robot's own pose information to obtain the collision detection results; wherein, multi-resolution collision detection includes: First-level pre-screening: Using the robot's external simplified geometric model, perform an intersection test with the simplified models of all obstacles in the environmental obstacle information; if they intersect, it is determined that there is a collision; if they do not intersect, it is determined that there is no collision, and the currently detected random state point is marked as a safe point; Second-level precise collision detection: For random state points that fail the first-level pre-screening, the precise geometric model of the robot is used to perform precise intersection calculation tests with the precise models of obstacles in the environmental obstacle information; if they intersect, it is determined that there is a collision; if they do not intersect, it is determined that there is no collision, and the currently detected random state point is marked as a safe point.

6. The robot motion planning method based on local sub-objective guided learning according to claim 5, characterized in that, For random state points that fail the first-level pre-screening, before performing a precise intersection calculation test using the robot's precise geometric model and the precise obstacle model in the environmental obstacle information, the following steps are also included: Calculate the Euclidean distance from each random state point that failed the first-level pre-screening to the nearest obstacle; Based on the preset mapping relationship between Euclidean distance and detection accuracy, the detection accuracy of the second-level precise collision detection is determined, and the precise geometric model of the robot and the precise model of the obstacle are selected according to the detection accuracy. The closer the Euclidean distance, the higher the accuracy of the robot's precise geometric model and the obstacle's precise model.

7. The robot motion planning method based on local sub-objective guided learning according to claim 1, characterized in that, The process of obtaining the rescaling factor includes: Acquire the robot's environmental state information, robot pose information, and task execution history data; An environmental safety assessment is performed based on the environmental state information and the robot pose information to obtain the environmental safety level. Based on the task execution history database, regional familiarity is assessed to obtain regional familiarity. Based on the environmental safety level and the area familiarity, a dynamic adjustment signal is generated; The rescaling factor is updated based on the dynamic adjustment signal.

8. The robot motion planning method based on local sub-objective guided learning according to claim 7, characterized in that, An environmental safety assessment is performed based on the environmental state information and the robot pose information to obtain the environmental safety level, including: Based on the relative relationship between the movement speed and direction of dynamic obstacles and the robot's current position in the environmental state information, a threat level score for each dynamic obstacle to the robot is calculated. Extract the distribution features of static obstacles, and generate a static environment complexity score based on the static obstacle density, the minimum distance between the static obstacle and the robot, and the shape complexity of the static obstacle; The threat level score and the static environment complexity score are combined, and the environmental security level is output by weighted summation and normalization. Among them, the lower the environmental safety level, the more likely it is to call the motion planner mode for safe path planning; the higher the environmental safety level, the more likely it is to execute the reinforcement learning action execution mode for safe path planning.

9. The robot motion planning method based on local sub-objective guided learning according to claim 7, characterized in that, Based on the aforementioned task execution history database, a regional familiarity assessment is performed to obtain regional familiarity, including: Based on the robot's current position and the position records in historical task data, the historical execution area is matched to obtain the matching area; Extract the success rate, average planning time, average path length, and number of collisions of historical tasks within the matching area to construct a region familiarity feature vector; The feature vector is input into a pre-trained lightweight neural network model, which outputs the region familiarity. The higher the region familiarity, the more likely it is to directly use the reinforcement learning action execution mode; the lower the region familiarity, the more likely it is to call the motion planner mode for safe path planning.

10. A robot motion planning system based on local sub-objective guided learning, characterized in that, include: The acquisition module is used to acquire the current system state of the robot; the current system state includes the robot's own pose information, environmental obstacle information, and task objective. The input module is used to input the current system state into a pre-built policy network and output an action vector, which is configured as a local sub-target to guide the robot's movement. The determination module is used to determine the execution mode based on the received rescaling factor; the execution mode includes reinforcement learning action execution mode and motion planner invocation mode; The reinforcement learning module is used to obtain a scaling action based on the action vector and the rescaling factor and send it to the robot actuator if it is determined that the execution mode is to perform reinforcement learning action. The motion planning module is used to generate a local sub-target state based on the current system state and the motion vector if the motion planner mode is invoked. It then uses the current system state as the starting node and the local sub-target state as the target node to perform local collision-free path planning through the motion planner, and sends the successfully planned path to the robot actuator.

Images