Path planning method, device and system for robot and corresponding robot

Abstract

A path planning method, apparatus, system, and corresponding robot are provided. The method includes executing at least one planning loop: acquiring the current states of the agent and robot, and the states of static obstacles; calculating the synthetic motion excitation force of the agent using an excitation force model based on parameters related to these states to predict the agent's next position, and calculating the synthetic motion excitation force of the robot using the excitation force model based on the agent's next position to predict the robot's next position; optimizing and updating the robot's trajectory based on the agent's and robot's next positions; and checking whether the optimized trajectory has converged: if not, returning to recalculate and replanning the trajectory; or if yes, continuing with the following steps: determining whether the optimized trajectory extends to a preset time range; if not, returning to repeat the above planning loop; if yes, outputting a motion command to the robot based on the robot's optimized trajectory.

Description

Technical Field

[0001] This application relates to a novel game theory-based path planning method, apparatus, system, and corresponding robot for robots. Background Technology

[0002] Robotic navigation using mobile agents in constrained static environments requires individual collaboration and the robot's ability to capture crowd interactions for safe and collaborative movement. Despite extensive research on socially conscious navigation, challenges remain regarding trajectory prediction and planning, behavioral modeling, and the selection of evaluation methods.

[0003] a) Traditional model-based methods

[0004] Traditional model-based crowd navigation methods can be roughly classified into reaction-based methods and optimization-based methods.

[0005] Reaction-based crowd navigation methods typically understand a crowd as a group of individuals, such as Van den Berg et al.'s Optimal Reciprocal Collision Avoidance (ORCA) algorithm or Helbing et al.'s Social Force Model (SFM). Although these methods and their variants are widely used in autonomous navigation, these reaction-based algorithms lack the ability to predict the future behavior of neighboring agents and are therefore susceptible to the "freezing robot" problem.

[0006] Optimization-based methods have also been applied to crowd navigation, including the timedelastic band (TEB) local path planning device proposed by Rosmann et al., which uses a g2o optimization framework and model predictive control (MPC) methods, as well as iMPC proposed by Chen et al. and MPC local path planning devices by Rosmann et al. However, these methods rarely consider the impact of robot-human interaction. Khambhaita and Alami used an online-interactive approach, which plans a timedelastic band for both robot and human to achieve cooperative navigation, while Nishimura et al. used Trajectron++ to sample human trajectories to obtain robot condition predictions for planning. However, existing online-interactive methods typically require long computation times and exhibit poor convergence when dealing with dense or highly dynamic human crowds.

[0007] b) Learning-based methods

[0008] Learning-based methods have also been widely applied in robot crowd navigation. Reinforcement learning is used to obtain a value network that implicitly models and estimates the crowd state. Chen et al. trained a CADRL policy under the reciprocity assumption of interactive behavior and achieved social awareness by rewarding the policy with social norms. Additionally, Chen et al. introduced attention mechanisms and graph structures in SARL and RGL to better encode relationships between agents. Learning-based methods were applied in predicting human trajectories using data-driven regression in RobustTP by Chandra et al. and generative approaches in Katyal et al.'s research. The model proposed in RobustTP was further integrated with reinforcement learning in a decoupled manner to generate motion commands in the research of Sathyamoorthy et al. Eiffert et al. used generative RNNs to predict human responses to robot actions, and this method was later applied to assist Monte Carlo Tree Search (MCTS) path planning devices in recursively searching for rational actions. However, these policy networks are typically trained in open areas and neglect potential interactions with static environments. Jin et al. proposed an end-to-end reinforcement learning-based approach for mapless navigation with dynamic obstacles. It defines a "social safety zone" by linearly propagating current human movement, but it may fail to capture high-level human interactions. Liu et al. extended SARL by additionally observing an occupancy grid map, while Dugas et al. further improved SARL by introducing a transformer architecture to understand static environments. However, it is still not guaranteed that all these learning-based policies will perform well outside the training scenario.

[0009] c) Game theory method

[0010] Game theory is increasingly being used in robotics applications. Wang et al. modeled individual behavior in multi-agent systems and applied it to multi-agent competitive scenarios. Williams et al. used the Iterated Best Response (IBR) method to capture agile interactions between autonomous vehicles. Reily et al. proposed a game theory approach for performing communication-free multi-robot navigation. Fridovich-Keil et al. employed the Iterative Linear-Quadratic (ILQ) method to approximate local Nash equilibrium for traffic simulation and motion planning. However, existing game theory methods are typically tested on simple interaction scenarios in autonomous driving and may not achieve stable convergence when faced with highly dynamic and dense human groups with complex interactions. Furthermore, these existing methods treat robots as circular without optimizing their orientation to reduce computational burden.

[0011] Therefore, an improved method for robot path planning is needed. Summary of the Invention

[0012] The purpose of this invention is to provide a solution that at least partially solves or alleviates the problems of the prior art described above.

[0013] According to a first aspect of the invention, a path planning method for a robot is provided, wherein the robot is surrounded by at least one movable agent, the method comprising executing at least one planning loop for planning a path for the robot, the planning loop comprising:

[0014] Obtain the current state of the at least one movable agent, the current state of the robot, and the state of static obstacles in the robot's surrounding environment at the current time;

[0015] Based on the acquired state-related parameters of the at least one mobile agent, the robot, and the static obstacle, the excitation force model is used to calculate the synthetic motion excitation force of the at least one mobile agent to predict the next position of the at least one mobile agent at the next time. Based on the predicted next position of the at least one mobile agent at the next time, the excitation force model is used to calculate the synthetic motion excitation force of the robot to predict the next position of the robot at the next time.

[0016] Based on the predicted next position of the at least one mobile agent at the next time step and the predicted next position of the robot at the next time step, the robot's trajectory is optimized and updated; and

[0017] Check whether the robot's optimized trajectory has converged:

[0018] If convergence fails, the process returns to recalculating the synthesized motion excitation forces of the at least one mobile agent and the robot, thereby replanning the robot's trajectory; or

[0019] If convergence is achieved, the following steps are performed: determine whether the optimized trajectory of the robot extends to a preset time range. If not, return to repeat the above planning loop. If it does, output the corresponding motion command to the robot based on the optimized trajectory of the robot.

[0020] According to a second aspect of the invention, a path planning device for a robot is provided, wherein the robot is surrounded by at least one movable intelligent agent, characterized in that the path planning device is configured to perform at least one planning loop for planning a path for the robot, the path planning device comprising:

[0021] The state acquisition module is configured to acquire the current state of the at least one movable agent, the current state of the robot, and the state of static obstacles in the robot's surrounding environment at the current time.

[0022] The excitation force model unit is configured to calculate the synthetic motion excitation force of the at least one mobile agent based on the acquired state-related parameters of the at least one mobile agent, the robot, and the static obstacle using the excitation force model to predict the next position of the at least one mobile agent at the next time, and to calculate the synthetic motion excitation force of the robot based on the predicted next position of the at least one mobile agent at the next time using the excitation force model to predict the next position of the robot at the next time.

[0023] A robot trajectory update module is configured to optimize and update the robot's trajectory based on the predicted next position of the at least one mobile agent at the next time step and the predicted next position of the robot at the next time step; and

[0024] The judgment module is configured to check whether the robot's optimized trajectory has converged;

[0025] If convergence fails, the process returns to recalculating the synthesized motion excitation forces of the at least one mobile agent and the robot, thereby replanning the robot's trajectory; or

[0026] If convergence is achieved, the following steps are performed: determine whether the optimized trajectory of the robot extends to a preset time range. If not, return to repeat the above planning loop. If it does, output the corresponding motion command to the robot based on the optimized trajectory of the robot.

[0027] According to a third aspect of the present invention, a path planning system for a robot is provided, comprising a memory and a processor, the memory storing a computer program configured, when executed by the processor, to perform the path planning method for a robot according to the first aspect of the present invention.

[0028] According to a fourth aspect of the invention, a robot is provided, which includes a path planning system for the robot according to a second aspect of the invention.

[0029] According to a fifth aspect of the invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, causes the above-described path planning method for a robot to be executed.

[0030] This application presents a game theory-based path planning method for robots capable of performing interactive crowd navigation in complex, constrained static environments. The proposed method utilizes game theory and motivational models to model the interactions between the robot, humans, and the static environment. It provides human-friendly navigation commands through online interactive trajectory and posture optimization. The method employs Nash equilibrium for crowd state estimation and trajectory optimization, allowing the path planning device to collaboratively navigate the robot with humans in complex static environments. Compared to existing crowd navigation methods, the algorithm in this application can consider robot orientation to optimize the robot's posture without introducing a heavy computational burden. Compared to existing conventional model-based methods, the method in this application can perform online interaction prediction for complex crowd interactions, addressing the challenges of frozen robots. Compared to learning-based methods, the method in this application does not involve a data-driven training process. Therefore, it exhibits more robust and stable performance even outside of experimental scenarios and better understands complex static structures in constrained environments. Compared to existing game theory methods, the method in this application novelly uses an iterative trajectory expansion and optimization process to achieve better convergence in highly dynamic scenarios with dense human crowds. It also maintains greater human comfort and human compatibility than existing methods. Attached Figure Description

[0031] The invention will now be described only by way of non-limiting exemplary embodiments with reference to the accompanying drawings, in which:

[0032] Figure 1 An example of the excitation force model proposed according to the present invention is illustrated;

[0033] Figure 2 An example of visualization of robot obstacle avoidance according to the present invention is shown;

[0034] Figures 3(a) and 3(b) illustrate the first and second scenarios of the test scheme according to the present invention, respectively;

[0035] Figure 4 A schematic diagram illustrating the posture and orientation of a robot in an environment containing static obstacles and mobile intelligent agents;

[0036] Figure 5 A flowchart illustrating a path planning method for a robot according to one embodiment of the present invention is shown; and

[0037] Figure 6 An example of a path planning device for a robot according to one embodiment of the present invention is shown. Detailed Implementation

[0038] To make the above and other features and advantages of the present invention clearer, the invention will be further described below with reference to the accompanying drawings. It should be understood that the specific embodiments given herein are for the purpose of explanation to those skilled in the art and are exemplary only, not restrictive.

[0039] The features described herein may be embodied in different forms and should not be construed as limited to the examples described herein. Rather, the embodiments described herein are provided merely to illustrate some of the many possible ways in which the apparatus, methods, and / or devices described herein may be apparent upon understanding the disclosure of this application.

[0040] As used herein, the term “and / or” includes any one of the associated listed items and any combination of any two or more of the associated listed items.

[0041] The terminology used herein is for the purpose of describing various embodiments only and is not intended to limit the scope of this disclosure. Unless the context clearly indicates otherwise, "a," "an," and "the" are intended to also include plural forms. The terms "comprising," "including," and "having" specify the presence of the stated features, operations, components, elements, and / or combinations thereof, but do not exclude the presence or addition of one or more other features, operations, components, elements, and / or combinations thereof.

[0042] 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 is for the purpose of describing particular embodiments only and is not intended to limit the embodiments of the invention.

[0043] Detailed example

[0044] a) Algorithm Design

[0045] In the case of at least one movable agent surrounding the robot, the path planning method and apparatus for the robot proposed in this application include coupled extension and optimization phases. In each planning loop, the path planning method and apparatus alternately extend and optimize the trajectory of the robot and the associated agent using an excitation force model defined below, until all trajectories are extended to a specific time horizon and reach a stable equilibrium. Figure 2 During deployment, the path planning method and apparatus can return to the first step on its trajectory as an action command and repeat the entire process in the next loop.

[0046] Figure 5 A flowchart illustrating a path planning method for a robot according to one embodiment of the present invention is shown.

[0047] like Figure 5 As shown, a path planning method for a robot includes executing at least one planning loop to plan the robot's path, the planning loop comprising:

[0048] Obtain the current state of the at least one movable agent, the current state of the robot, and the state of static obstacles in the robot's surrounding environment at the current time;

[0049] Based on the acquired state-related parameters of the at least one mobile agent, the robot, and the static obstacle, the excitation force model is used to calculate the synthetic motion excitation force of the at least one mobile agent to predict the next position of the at least one mobile agent at the next time. Based on the predicted next position of the at least one mobile agent at the next time, the excitation force model is used to calculate the synthetic motion excitation force of the robot to predict the next position of the robot at the next time.

[0050] Based on the predicted next position of the at least one mobile agent at the next time step and the predicted next position of the robot at the next time step, the robot's trajectory is optimized and updated; and

[0051] Check whether the robot's optimized trajectory has converged:

[0052] If convergence fails, the process returns to recalculating the synthesized motion excitation forces of the at least one mobile agent and the robot, thereby replanning the robot's trajectory; or

[0053] If convergence is achieved, the following steps are performed: determine whether the optimized trajectory of the robot extends to a preset time range. If not, return to repeat the above planning loop. If it does, output the corresponding motion command to the robot based on the optimized trajectory of the robot.

[0054] In one implementation, the algorithm of the present invention can be deployed in a constrained 2D or 3D space with movable agents and static obstacles. In the excitation force model, such as... Figure 1 As shown, a mobile agent (e.g., a pedestrian) is modeled as a circular collider with a specific radius of motion, position, and velocity (e.g., walking speed). The algorithm first obtains observations of, for example, the states of neighboring humans from an external perception system, which include their radius of motion, current position, and linear velocity in the world coordinate system. The algorithm also obtains state observations of static obstacles from the external perception system. In the excitation force model, as... Figure 1 As shown, the static obstacle is modeled as a square collider, the size of which is represented by the side length of the square collider, and the state of the static obstacle includes its size and position in the world coordinate system. It should be understood that this external sensing system can be installed on the robot or on other sensing devices, as long as the observations of the states of adjacent humans and static obstacles sensed by the external sensing system can be applied to the algorithm.

[0055] In the excitation force model, such as Figure 1 As shown, the robot is modeled as having a length r l and width r w The shape is a rectangle (e.g., a square or rectangular shape). To optimize the robot's orientation, the distance r is considered to be the intersection point of the line connecting the geometric center of the robot (the geometric center) with the center of a circle of an adjacent agent and one side of the rectangle. i And the r i Considered as a variable, where θ ij It is the angle between the vector from the geometric center of the rectangular robot to the center of the adjacent agent's circle and the robot's direction of travel (or orientation) (see...). Figure 4 ):

[0056]

[0057] in θ refers to θ at time step t. ij .

[0058] The aforementioned "optimization of the robot's orientation" is performed during the optimization phase, but the robot's orientation can also be considered during the extension phase. In the extension phase, the robot's orientation from its previous state is used as an initial solution for its next orientation, and optimization is achieved by synthesizing the motion excitation force of the orientation from the target orientation and the rotational gravitational and repulsive forces generated by surrounding agents.

[0059] For each planning loop, the path planning device progressively expands and optimizes the trajectory of the robot and its neighbors from the current time step to a predetermined time range. An agent's trajectory includes continuous states of the agent with constant time intervals between them. The linear motion between two adjacent agent states is approximated.

[0060] This algorithm models the robot's crowd navigation problem as an N-player, multi-level, infinite dynamic game. Nash equilibrium is considered the optimal form of a multi-agent system, where the navigation cost J... i The following inequalities must be satisfied:

[0061]

[0062] Navigation cost refers to the behavioral cost incurred by the robot to reach the target location, which can be calculated by integrating the motion incentive force. Let P represent the optimal strategy that the i-th agent can achieve in the current system state. i Let N represent the current policy of the i-th agent (i.e., the path planning of the i-th agent for the current and future time steps), and let N represent the total number of agents.

[0063] It should be understood that the robot can also be regarded as an intelligent agent in this method. Therefore, i and j can both be used to represent intelligent agents. In particular, i represents the robot itself as an intelligent agent, and the set of j (represented as {j} in the pseudocode below) represents other intelligent agents.

[0064] If no agent has an incentive to deviate from his or her policy—unless its neighbors do so—a set of policies reaches Nash equilibrium, where the resultant translational and rotational excitation forces acting on all agents are zero. Therefore, the excitation force model defined above is used to capture the motion excitations of both the robot and the human to achieve several motion dynamics and collision avoidance constraints, as well as attempts to approach a target. The proposed algorithm references the force model in the Social Forces Model (SFM) and the Timed Elastic Band (TEB) algorithm to model forces from the static environment, other agents at the same time step, and the agent's own state at adjacent steps relative to a particular agent's state. For each type of motion excitation, the path planning device calculates translational and rotational forces in a 2D plane, with their directions indicated by symbols. For example, for repulsive motion excitations, the path planning device calculates translational and rotational repulsive forces in a 2D plane.

[0065] Figure 1 The proposed incentive model is illustrated. q i t and q j t Let q represent the states of robot i and agent j at time step t, respectively. i t-1 and q i t+1 Let f represent the states of robot i at time steps t-1 and t+1, respectively. For the translational force, f tv It is the kinematic constraint that limits the robot's speed, f ta It is the kinematic constraint force that limits the robot's acceleration, f ts It is the attraction from neighboring states that makes the path smooth, f tg It is the attraction from the goal, f tb and f tβ These are the repulsive forces from adjacent human agents and static obstacles, respectively. For the rotational force, h... rb and h rβ These are the rotational repulsive forces from adjacent human agents and the static obstacle, respectively. The translational force f... T and rotational force h R The resultant force can be interpreted as the agent's motion excitation. Therefore, the social navigation cost J i Nash equilibrium is the potential energy of the state within the field, described by the resultant force of translational and rotational forces. The forces on each agent's state at any time step are independent and calculated only based on its current orientation. Therefore, the path planning device can roughly estimate Nash equilibrium by optimizing the position and orientation of the trajectory states; specifically, it approximates Nash equilibrium by optimizing the position and orientation of the trajectory states so that the resultant force on each agent at any given time is zero.

[0066] Figure 6 An example is a path planning device 600 for a robot according to the present invention, wherein the robot is surrounded by at least one movable intelligent agent, the path planning device 600 being configured to perform at least one planning loop for planning a path for the robot, the path planning device 600 comprising:

[0067] The state acquisition module 601 is configured to acquire the current state of the at least one movable intelligent agent, the current state of the robot, and the state of static obstacles in the robot's surrounding environment at the current time.

[0068] The excitation force model unit 602 is configured to calculate the synthetic motion excitation force of the at least one mobile agent based on the acquired state-related parameters of the at least one mobile agent, the robot, and the static obstacle to predict the next position of the at least one mobile agent at the next time, and to calculate the synthetic motion excitation force of the robot based on the predicted next position of the at least one mobile agent at the next time to predict the next position of the robot at the next time.

[0069] A robot trajectory update module 603 is configured to optimize and update the robot's trajectory based on the predicted next position of the at least one mobile agent and the predicted next position of the robot at the next time step; and

[0070] The judgment module 604 is configured to check whether the optimized trajectory of the robot has converged;

[0071] If convergence fails, the process returns to recalculating the synthesized motion excitation forces of the at least one mobile agent and the robot, thereby replanning the robot's trajectory; or

[0072] If convergence is achieved, the following steps are performed: determine whether the optimized trajectory of the robot extends to a preset time range. If not, return to repeat the above planning loop. If it does, output the corresponding motion command to the robot based on the optimized trajectory of the robot.

[0073] The proposed path planning device employs a coupled scheme for trajectory extension and optimization, where two stages operate alternately, gradually extending the trajectory into the receding horizon. In the extension stage, for a mobile agent such as a human, its normal walking speed can be used as the optimization input, and its temporary goal is estimated through linear propagation. From the current ending state on the trajectory, the extension stage makes an initial guess for the next step based on attractive and repulsive forces, calculating the synthesized translational extension force f based on the vector superposition of various translational forces. E Furthermore, the position of the next state is dynamically planned based on the agent. The growth length is clamped to satisfy the robot's velocity and acceleration constraints. Specifically, the growth length refers to the length of the trajectory extended by the robot at each time step. The robot's end-effector trajectory growth point extends towards the target location due to the gravitational influence of the target location, and its maximum extension length is clamped. The orientation of the new state is obtained using the same method; that is, the calculation of the new pose and the new position of the next state are done in the same way.

[0074] During the optimization phase, although it is assumed that all agents behave reasonably and interactively, the expansion may be suboptimal or even lead to collisions when agents move in non-communication scenarios (i.e., agents do not communicate directly with each other, such as pedestrians walking in a crowd without directly informing each other of their trajectories and movement plans). Therefore, after each expansion, the trajectory is further optimized through iterative gradient descent (i.e., gradient descent method), using the same temporary objective and preferred agent velocities as in the expansion phase. In each iteration, the attitude is updated by taking small steps along the resultant force of the forces they receive with a step rate γ∈(0,1], where the step rate is a parameter of gradient descent and represents the magnitude of optimization in each iteration. In this way, a single state can influence its neighboring agents in the same step through repulsive forces, while also smoothing out its own previous states. This is to achieve online interaction estimation with neighboring agents. For each state, it receives attractive forces from its previous and next states along the line connecting the adjacent states, minimizing the angle between these three states for a smooth trajectory. Note that the path planning device aims to provide an interactive and human-friendly navigation solution, rather than focusing on accurately predicting human actions. The actual actions of neighboring agents may differ from the estimated states. However, the "feedback property" in the rolling time domain helps the path planning device react to possible changes in human intentions and avoid collisions.

[0075] After trajectory convergence, the expansion phase takes another set of initial guesses and expands the trajectory. Then, the optimization phase iteratively adjusts them again until they reach a new equilibrium. This process is run continuously in a single planning loop until the trajectory reaches a certain level, at which the path planning device can use the first step output of the robot trajectory as the robot's motion command. The pseudocode for the single planning loop is presented in Algorithm 1.

[0076]

[0077] b) Test Plan

[0078] The proposed path planning device was tested in various simulated and real-world environments. The device used a 10×10m occupancy grid map with a resolution of 0.08m and observed the position, velocity, and radius of neighboring agents within a 5.0m radius. The quadruped robot had a shape of 1.0×0.5m and a preferred speed of 1.5m / s. The planning performance was evaluated using a success rate R0. S and the average velocity change ΔV, which indicates the smoothness of motion. R To evaluate this, when focusing on providing a human-friendly navigation solution, the average congestion time T for all agents to reach their goal was also analyzed. C The average human speed change ΔV reflects the impact of robots on human actions. C and the average minimum separation rate S M In addition, the average directional cost C is introduced. D As a measure of human-robot compatibility, C is calculated if the robot and the human move toward each other at a short distance and at a high speed. D It is high. S0 is calculated using boundary distance and variable robot radius for agents with different radii. M and C D :

[0079]

[0080]

[0081] in, and Let i and j represent the position vectors of agents i (the robot) and j (another agent) at time step 0 (the current time), respectively. and Let S represent the velocity vectors of agents i (the robot) and j (another agent) at time step 0 (the current moment), respectively. Mij Let r represent the average minimum separation rate between agent i (i.e., the robot) and agent j (some other agent), and r j This represents the radius of the agent j, which is modeled as a circle.

[0082] First, the crowd navigation performance was evaluated in a simulated open-area scenario. Each neighboring human agent was randomly assigned a radius r within [0.3, 0.5] m and a preferred velocity within [1.0, 1.5] m / s. 2000 scenarios were prepared, each containing 8 agents, with initial positions and targets randomly assigned around a circular area. The simulated human agents used the ORCA algorithm. In the first experiment, the method of this invention stably performed human-friendly movement by maintaining high social distance, smooth velocity transitions, and low orientation costs. It also achieved high travel efficiency and a high success rate. Table 1 shows the evaluation results. Figure 2 A visual example is shown. The path planning method of this application maintains a low level of impact on the crowd while producing a smooth robot path. This helps the robot present a clear movement intention to humans without causing them disturbance or inconvenience.

[0083] Table 1: Average performance in the open area

[0084]

[0085] Among them, R S This indicates the success rate of the planning performance. The average velocity change ΔV represents the smoothness of motion. R The average value, The average human velocity change ΔV represents the effect of the robot on human actions. C The average value, S represents the average minimum separation rate. M The average value, and The average directional cost C represents the measure of human-robot compatibility. D The average value.

[0086] The next simulation experiment involved dynamic agents and static obstacles, with the agent's radius and preferred speed set within the same range as in the open area. Humans were simulated using the same algorithm as the one proposed in this paper, without directional optimization. Two scenarios were used to establish the static environment: in the first scenario, the agent performed "obstacle avoidance" on a map containing static objects, while the robot and five other mobile agents attempted to move from one side to the other around a 4.0m circle (Figure 3(a)). The second scenario was set as a "crossroads" with a road width of 3.0m, where the robot and five other agents were randomly positioned to move from one entrance to another (Figure 3(b)). Both scenarios were tested on 150 groups of agents with different starting and target positions. Table 2 shows the evaluation results, with the top row from the "obstacle avoidance" scenario and the bottom row from the "crossroads" scenario. The method of this invention has a high success rate, high navigation efficiency, and short travel time. Furthermore, it provides a low ΔV R A smooth transition is achieved, while maintaining better human compatibility. Figure 3 shows a visual example.

[0087] Table 2: Average performance with static structure

[0088]

[0089] The meanings of the symbols in Table 2 are the same as those described in Table 1 above, and will not be repeated here.

[0090] In a real-world experiment, the proposed algorithm was deployed on the Jueying miniature quadruped robot. The path planning device operated at 5Hz on an Intel i7-7600U. The robot received its own pose information and that of its neighboring agents via an external optical tracking motion capture system. The planning area was limited to a 5×7m rectangle. During multiple repetitions, the robot exhibited human-friendly interactions and movements. When a human passed or encountered the robot, the path planning device displayed cooperative behavior. It can also handle different crowd movement patterns in highly dynamic scenes, allowing humans to safely focus on their own actions. Even when humans exhibited uncooperative movements, the robot was still able to quickly adjust itself and avoid potential collisions, demonstrating the feedback capability of the method of this invention.

[0091] It should be understood that the various modules of the path planning method and apparatus for robots of the present invention can be implemented entirely or partially through software, hardware, firmware, or a combination thereof. Each module can be embedded in a processor in hardware or firmware form, or independent of the processor, or stored in memory in software form for the processor to call and execute the operation of the respective module. Each module can be implemented as an independent component or unit, or two or more modules can be implemented as a single component or unit.

[0092] Those skilled in the art should understand that Figure 6 The schematic diagrams of the path planning device for robots illustrated herein are merely exemplary block diagrams illustrating partial structures related to the solutions of the present invention and do not constitute a limitation on the processor or computer program embodying the solutions of the present invention. Specific processors or computer programs may include more or fewer modules, components, or units than shown in the figures, or may combine or split certain modules, components, or units, or may have different arrangements of modules, components, or units.

[0093] Another aspect of the present invention provides a path planning system for a robot, comprising a memory and a processor, wherein the memory stores a computer program that, when executed by the processor, causes the path planning method for a robot to be executed. When executed by the processor, the computer program is capable of implementing the steps of any of the above-described path planning methods for a robot of the present invention.

[0094] Another aspect of the present invention provides a robot, as described above, which includes the path planning device or system described in any of the above embodiments.

[0095] The path planning device or system in any of the above-mentioned schemes can be installed or applied in the robot mentioned herein and can perform the methods discussed in any of the above embodiments, without further details.

[0096] Another aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the path planning method for a robot described in any of the above embodiments.

[0097] Those skilled in the art will understand that all or part of the steps of the above-described path planning method for robots can be performed by a computer program instructing related hardware, such as computer devices or processors. The computer program may be stored in a non-transitory computer-readable storage medium, and when executed, it causes the steps of the path planning method for robots of the present invention to be performed. Depending on the context, any references herein to memory, storage, databases, or other media may include non-volatile and / or volatile memory. Examples of non-volatile memory include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, magnetic tape, floppy disk, magneto-optical data storage device, optical data storage device, hard disk, solid-state drive, etc. Examples of volatile memory include random access memory (RAM), external cache memory, etc.

[0098] The technical features of the above implementation schemes can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above implementation schemes 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 specification.

[0099] Although the invention has been described in conjunction with embodiments, those skilled in the art will understand that the above description and drawings are exemplary and not restrictive, and the invention is not limited to the disclosed embodiments. Various modifications and variations are possible without departing from the spirit of the invention.

Claims

1. A path planning method for a robot, wherein the robot is surrounded by at least one movable intelligent agent, characterized in that, The method includes executing at least one planning loop for planning a robot's path, the planning loop comprising: Obtain the current state of the at least one movable agent, the current state of the robot, and the state of static obstacles in the robot's surrounding environment at the current time; Based on the acquired state-related parameters of the at least one mobile agent, the robot, and the static obstacle, an excitation force model is used to calculate the synthetic motion excitation force of the at least one mobile agent to predict the next position of the at least one mobile agent at the next time step. Furthermore, based on the predicted next position of the at least one mobile agent at the next time step, the excitation force model is used to calculate the synthetic motion excitation force of the robot to predict the next position of the robot at the next time step. The excitation force model uses translational and rotational forces in a two-dimensional plane to calculate the synthetic motion excitation force. The translational force includes a kinematic constraint force limiting the robot's velocity, a kinematic constraint force limiting the robot's acceleration, an attractive force from adjacent states to smooth the trajectory, an attractive force from the robot's target position, a repulsive force from an adjacent agent, and a translational repulsive force from the static obstacle. The rotational force includes a rotational repulsive force from an adjacent agent and a rotational repulsive force from the static obstacle. Based on the predicted next position of the at least one mobile agent at the next time step and the predicted next position of the robot at the next time step, the robot's trajectory is optimized and updated; and Check whether the robot's optimized trajectory has converged: If convergence fails, the process returns to recalculating the synthesized motion excitation forces of the at least one mobile agent and the robot, thereby replanning the robot's trajectory; or If convergence is achieved, the following steps are performed: determine whether the optimized trajectory of the robot extends to a preset time range. If not, return to repeat the above planning loop. If it does, output the corresponding motion command to the robot based on the optimized trajectory of the robot.

2. The path planning method for a robot according to claim 1, wherein in the excitation force model, the static obstacle is modeled as a square, the size of the static obstacle is represented by the side length of the square, and the state of the static obstacle includes: The size and position of the static obstacle in the world coordinate system.

3. The path planning method for a robot according to claim 1, wherein in the excitation force model, each of the at least one movable agent is modeled as a circle, and each of the at least one movable agent has an activity radius, position, and linear velocity relative to the robot in a world coordinate system.

4. The path planning method for a robot according to any one of claims 1-3, wherein in the excitation force model, the robot is modeled as a rectangular shape having a length r. l and width r w .

5. The path planning method for a robot according to claim 4, wherein the optimization step further comprises optimizing the robot's orientation based on the following formula. ， Where r i θ represents the distance θ from the geometric center of the rectangular robot to the intersection of the line connecting the geometric center and the center of a circle of an adjacent agent with one side of the rectangle. ij Let represent the angle between the vector pointing from the geometric center of the rectangular robot to the center of the circle of the adjacent agent and the robot's direction of travel, where θ refers to θ at time step t. ij .

6. A path planning device for a robot, wherein the robot is surrounded by at least one movable intelligent agent, characterized in that, The path planning device is configured to execute at least one planning loop for planning a path for the robot, the path planning device comprising: The state acquisition module is configured to acquire the current state of the at least one movable agent, the current state of the robot, and the state of static obstacles in the robot's surrounding environment at the current time. An excitation force model unit is configured to calculate, based on acquired state-related parameters of the at least one movable agent, the robot, and the static obstacle, a synthetic motion excitation force for the at least one movable agent to predict the next position of the at least one movable agent at a next time step, and to calculate, based on the predicted next position of the at least one movable agent at a next time step, a synthetic motion excitation force for the robot to predict the next position of the robot at a next time step. The synthetic motion excitation force is calculated using translational and rotational forces in a two-dimensional plane in the excitation force model. The translational forces include kinematic constraints limiting the robot's velocity, kinematic constraints limiting the robot's acceleration, attractive forces from adjacent states to smooth the trajectory, attractive forces from the robot's target position, repulsive forces from an adjacent agent, and translational repulsive forces from the static obstacle. The rotational forces include rotational repulsive forces from adjacent agents and rotational repulsive forces from the static obstacle. A robot trajectory update module is configured to optimize and update the robot's trajectory based on the predicted next position of the at least one mobile agent at the next time step and the predicted next position of the robot at the next time step; and The judgment module is configured to check whether the robot's optimized trajectory has converged; If convergence fails, the process returns to recalculating the synthesized motion excitation forces of the at least one mobile agent and the robot, thereby replanning the robot's trajectory; or If convergence is achieved, the following steps are performed: determine whether the optimized trajectory of the robot extends to a preset time range. If not, return to repeat the above planning loop. If it does, output the corresponding motion command to the robot based on the optimized trajectory of the robot.

7. The path planning device for a robot according to claim 6, wherein in the excitation force model, the static obstacle is modeled as a square, the size of the static obstacle is represented by the side length of the square, and the state of the static obstacle includes: The size and position of the static obstacle in the world coordinate system.

8. The path planning device for a robot according to claim 6, wherein in the excitation force model, each of the at least one movable agent is modeled as a circle, and each of the at least one movable agent has an activity radius, position, and linear velocity relative to the robot in a world coordinate system.

9. The path planning device for a robot according to any one of claims 6-8, wherein in the excitation force model, the robot is modeled as a rectangular shape having a length r. l and width r w .

10. The path planning apparatus for a robot according to claim 9, wherein the robot trajectory update module is further configured to optimize the robot's orientation based on the following formula. ， Where r i θ represents the distance θ from the geometric center of the rectangular robot to the intersection of the line connecting the geometric center and the center of a circle of an adjacent agent with one side of the rectangle. ij Let represent the angle between the vector pointing from the geometric center of the rectangular robot to the center of the circle of the adjacent agent and the robot's direction of travel, where θ refers to θ at time step t. ij .

11. A path planning system for a robot, comprising a memory and a processor, characterized in that, The memory stores a computer program that, when run by a processor, is configured to perform the path planning method for a robot according to any one of claims 1-5.

12. A robot, characterized in that, It includes the path planning system for robots as described in claim 11.

13. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the computer program causes the path planning method for a robot according to any one of claims 1 to 5 to be executed.

Images