A method and system for human-robot collaborative trajectory planning based on mixed Nash equilibrium

Abstract

The application discloses a kind of human-machine collaborative trajectory planning methods based on mixed Nash equilibrium, comprising: step 1, construct pedestrian interaction model, for predicting the movement intention of pedestrian before interacting with robot;Step 2, the nominal mixed strategy of pedestrian is generated based on the predicted trajectory output by pedestrian interaction model, and the nominal mixed strategy of robot is generated based on the planning trajectory output by robot motion planner;Step 3, establish objective function, and process the strategy uncertainty in human-robot interaction process based on Bayes updating rule, obtain the mixed Nash equilibrium strategy of robot by solving objective function;Step 4, it is converted into the control instruction of robot, to drive robot movement.The application simulates the non-complete rationality of pedestrian decision-making by introducing rational factor, effectively avoids the problems of "frozen robot" and "reciprocal dance", and can improve the navigation efficiency and behavior predictability of robot in dense crowd while ensuring safety.

Description

Technical Field

[0001] This invention relates to the fields of machine learning and autonomous robot navigation technology, specifically to a human-machine collaborative trajectory planning method and system based on hybrid Nash equilibrium. It focuses on the problem of autonomous unmanned intelligent robot navigation in crowds, and explores interactive human-machine collaborative navigation algorithms based on non-cooperative game theory to solve the safety and efficiency problems of robot navigation in dense crowd environments. Background Technology

[0002] Since the 1990s, roboticists have been studying navigation in human environments. Two landmark studies were the RHINO and MINERVA experiments, in which robotic systems were deployed in museums to guide thousands of visitors. Additional work on guide robots, such as Robox, Mobot, Rackham, and CiceRobot, has followed. These studies used traditional indoor navigation stacks, such as the dynamic window approach, where humans were modeled as non-reactive obstacles. While these methods are sufficient when robots interact with sparse crowds, their navigation efficiency is limited due to safety concerns. For example, the robot's speed is deliberately limited to avoid continuous emergency stops.

[0003] Research on navigation in crowds can be broadly categorized into three methods:

[0004] 1) Prediction-then-planning approach: This method, simply put, predicts human movement and plans robot actions to avoid hindering human movement. Decoupling prediction and planning is limited by its core assumption—the robot's behavior will not affect human behavior. Ignoring human reactions may lead to unexpected robot actions, while human reactions may exceed the robot's expectations, resulting in brief, oscillating interactions, often referred to as the "reciprocal dance." Failing to consider human reactions in uncertainty quantification can also lead to overly conservative navigation robot behavior—by not considering human responses to belief updates during planning, the uncertainty of the prediction may cause the robot to perceive all feasible paths as unsafe, with the only safe option being to remain stationary. This phenomenon is often called the "frozen robot problem."

[0005] 2) Coupled Prediction and Planning Approach: Considering the limitations of decoupling prediction and planning, it is necessary to remove the assumption that the robot does not interfere with human intentions. Therefore, an alternative framework called coupled prediction and planning attempts to simultaneously plan robot actions and predict human actions. Limitations of Coupled Prediction and Planning Approaches: The main limitation of existing coupled prediction and planning methods is the lack of a rigorous cooperation model, as humans and robots cooperate only through mutual observation rather than explicit communication. Cooperation is often granted in coupled prediction and planning methods. For example, methods that formulate crowd navigation as a joint decision-making problem assume that humans and robots share the same joint decision-making objective, making these methods more similar to centralized multi-agent planning frameworks.

[0006] 3) Game Theory Programming: Strictly speaking, game theory programming also falls under the category of coupled prediction and planning. The key difference between game theory methods and other coupled prediction and planning methods is that game theory assumes that each agent makes an individual optimal decision and provides a strict optimality criterion for the decision—the concept of equilibrium. Current research on game planning mainly suffers from two drawbacks: 1. High computational resource consumption; 2. Lack of flexibility in representing uncertainty. Summary of the Invention

[0007] This invention addresses the shortcomings of existing planning algorithms in current research by proposing a human-machine collaborative trajectory planning model based on hybrid Nash equilibrium. This model aims to overcome the uncertainty and efficiency problems of traditional methods in handling crowd navigation by integrating the advantages of prediction, planning, and game theory.

[0008] This invention provides a human-machine collaborative trajectory planning method based on hybrid Nash equilibrium, characterized by the following steps: Step 1, establishing a pedestrian interaction model: constructing a pedestrian interaction model to predict the pedestrian's motion intention before interacting with the robot; Step 2, constructing a hybrid policy set for pedestrians and robots: generating a nominal hybrid policy for the pedestrian based on the predicted trajectory output by the pedestrian interaction model, and generating a nominal hybrid policy for the robot based on the planned trajectory output by the robot motion planner, where the nominal hybrid policy is a probability distribution in the policy space; Step 3, establishing an objective function and solving the game: establishing an objective function that integrates the expected collision risk and the penalty for nominal policy deviation, and handling the policy uncertainty in the human-machine interaction process based on Bayesian update rules, obtaining the robot's hybrid Nash equilibrium policy by solving the objective function; Step 4, control execution: converting the robot's hybrid Nash equilibrium policy into control commands for the robot, thereby driving the robot's movement.

[0009] The human-machine collaborative trajectory planning method based on hybrid Nash equilibrium provided by this invention may also have the following features: In step S1, the pedestrian interaction model is an improved FO-ORCA model based on the optimal reciprocal collision avoidance ORCA algorithm. The improvement includes escape velocity responsibility optimization and model parameters learned through neural networks. Finally, an intention filter is added to obtain the pedestrian interaction model. Then, extended Kalman filtering and maximum likelihood estimation are added to estimate the model error to obtain the pedestrian interaction model.

[0010] The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium provided in this invention may also have the following feature: wherein, for escape velocity responsibility optimization, specifically for the intelligent agent... Introducing optimization of collision avoidance parameters:

[0011]

[0012] In the formula, The preferred speed is the speed that agent A uses when no other agents are present. Let be the set of optimal collision avoidance velocities for agent A relative to agent B. To maintain their current velocities, the interacting agents construct the following cost function:

[0013]

[0014] In the formula, the cost function is defined as follows: , Let A be the current velocity of agent A. Similarly, use By calculating and optimizing this cost function, the intelligent agent was obtained. and Responsibility parameters between and The optimization process of model parameters learned through neural networks is as follows: each agent simulates using the optimized ORCA, with the agent operating at a time step defined in the dataset. Continuous movement, total duration is During the selection phase, the following cost functions are used for comparison and optimization:

[0015]

[0016] in, (k) is the trajectory generated by simulation. Represents time in publicly available datasets The actual location coordinates of the pedestrian. At each simulation time... The distances between a target pedestrian and its three nearest neighbors are recorded from real data. These distances are used as input features to the parameter dataset, and four optimizations are performed, each producing a different set of optimal parameters. These four results are then used as output labels. The neural network learns the mapping from these input features to the corresponding parameter sets, mathematically represented as follows:

[0017]

[0018] In the formula, the function Indicates weight A parameterized neural network takes the input feature vector Mapping to the output parameter set Specifically, These represent the four calibration parameters of the pedestrian dynamics model set.

[0019] The human-machine collaborative trajectory planning method based on hybrid Nash equilibrium provided by this invention may also have the following feature: In step S2, the nominal hybrid policy is the hybrid policy before interaction. It is assumed that each agent has a nominal hybrid policy before interacting with other agents, denoted as... ,in Indexing the agent. The hybrid strategy for pedestrian interactions is used as the nominal hybrid strategy for the pedestrians. A Gaussian process-based trajectory generation method is chosen. Specifically, the trajectory predicted by the pedestrian interaction model is set as the mean function. To capture the uncertainty of the pedestrian strategy, the covariance function of the trajectory is set as the RBF kernel function, with the specific expression as follows:

[0020]

[0021] Consideration time Time The above mixing strategy, and through parameters and To control the fluctuation range of the trajectory and the smoothness or correlation decay rate of the function, the trajectory is discretized and sampled to obtain the pedestrian's nominal mixing strategy. Similarly, for robots using meta-planners to generate time... Time The trajectory from the starting point to the target point is used as the mean function, and the nominal hybrid strategy of the robot is obtained using the kernel function described above. .

[0022] The human-machine collaborative trajectory planning method based on hybrid Nash equilibrium provided by this invention may also have the following features: In step S2, for pedestrians, the trajectory generated by FO-ORCA prediction is its mean function. Multiple trajectories are sampled using a kernel function, and then smoothed using a smoothing function. For robots, the A* algorithm is used as the original planner, and a heuristic function is set.

[0023]

[0024] In the formula, Indicates the distance from the starting point to the current node. The actual path cost, Indicates the current node Heuristic cost estimation for the target node Represents a node Based on the overall priority, the algorithm is expanded first. The smallest node is chosen to be in Euclidean space. ,Right now:

[0025]

[0026] We obtain the target point that is closest to the current position of the robot, select a straight line as the mean function of the robot, and use a kernel function to form the robot's hybrid strategy.

[0027] The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium provided by this invention may also have the following feature: wherein, in step S3, the joint expected collision risk between the two agents is defined as the joint expectation of the collision risk of the hybrid strategy by the two agents:

[0028]

[0029] In the formula, , For two trajectories , The physical collision risk function aims to assess the degree of geometric interference between trajectories. In the iterative Bayesian approach, in addition to prior beliefs (i.e., the nominal mixed policy), a conditional likelihood function is also needed. Assuming each agent follows the expected utility assumption, the following conditional function can be used to model a given agent. If a hybrid strategy is adopted, then the other agent adopts a pure strategy. The probability magnitude:

[0030]

[0031] According to the Bayesian algorithm, after the interaction, the agent... The Bayesian posterior mixture strategy is as follows:

[0032]

[0033] in This is the normalization term. Building upon this, we further extend it to Bayesian policy updates in multi-agent systems:

[0034]

[0035]

[0036] In the formula Indicates a given division A hybrid strategy involving all agents except those mentioned above. Follow pure strategy The likelihood.

[0037] The human-machine collaborative trajectory planning method based on hybrid Nash equilibrium provided in this invention may also have the following features: In step S3, the objective function is established based on risk collision and intention preservation. To maintain the convergence of the algorithm and considering the non-perfect rationality of pedestrians during interaction, the intelligent agent... Establish the following objective function:

[0038]

[0039] In the formula This represents the degree of rationality of an intelligent agent. For a pedestrian, who is in a state of bounded rationality, the value is greater than 0 and less than 1, while for a robot, which is in a state of perfect rationality, the value is equal to 1. , Indicates except for intelligent agents For all other agents, this term represents the expected collision risk, and the last term is the penalty for deviating from the nominal policy. Its mathematical expression is:

[0040]

[0041] Considering the imperfect rationality of pedestrians, the Bayesian update for pedestrian mixed strategies needs to be modified, as shown in the following mathematical expression:

[0042]

[0043] The system-level joint objective function can then be defined as follows:

[0044]

[0045] The algorithm converges when the following condition is met:

[0046]

[0047] The human-machine collaborative trajectory planning method based on hybrid Nash equilibrium provided in this invention may also have the following feature: wherein, rational factors Dynamically adjusts over time:

[0048]

[0049] in They represent time. The rate of change, density, and population size, therefore the function Fitting is performed using regression models or neural networks.

[0050] This invention provides a human-machine collaborative trajectory planning system based on hybrid Nash equilibrium, characterized by the following features: a pedestrian interaction model building module: constructing a pedestrian interaction model to predict the pedestrian's motion intention before interaction with the robot; a hybrid strategy set construction module: generating the pedestrian's nominal hybrid strategy based on the predicted trajectory output by the pedestrian interaction model, and generating the robot's nominal hybrid strategy based on the planned trajectory output by the robot motion planner, wherein the nominal hybrid strategy is a probability distribution in the strategy space; an objective function establishment and game solving module: establishing an objective function that integrates the expected collision risk and the penalty for nominal strategy deviation, and handling the strategy uncertainty in the human-machine interaction process based on Bayesian update rules, and obtaining the robot's hybrid Nash equilibrium strategy by solving the objective function; and a control execution module: converting the robot's hybrid Nash equilibrium strategy into control commands for the robot, thereby driving the robot's movement. Attached Figure Description

[0051] Figure 1 This is a schematic diagram of robot crowd navigation in the background technology;

[0052] Figure 2 This is a flowchart of the human-machine collaborative trajectory planning method based on hybrid Nash equilibrium in an embodiment of the present invention;

[0053] Figure 3 This is a flowchart illustrating the convergence of the hybrid Nash equilibrium solution algorithm in an embodiment of the present invention.

[0054] Figure 4 This is a graph showing the convergence verification results of the hybrid Nash equilibrium algorithm in an embodiment of the present invention. Figure 4 'a' represents an interaction scenario between two intelligent agents; Figure 4 b represents an interaction scenario involving three intelligent agents;

[0055] Figure 5 This is a diagram showing the experimental results of a simulated scenario in an embodiment of the present invention. Figure 5 A is a diagram of the flow of people, the left is a snapshot of the simulated environment, and the right is a diagram of the agent's trajectory; Figure 5b is the centralized interaction diagram, the left is a snapshot of the simulated environment, and the right is the trajectory diagram of the agent. Detailed Implementation

[0056] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the scope of protection of the present invention.

[0057] This embodiment provides a human-machine collaborative trajectory planning method based on hybrid Nash equilibrium.

[0058] Figure 2 This is a flowchart of a human-machine collaborative trajectory planning method based on hybrid Nash equilibrium in an embodiment of the present invention.

[0059] In this embodiment, it is assumed that there is a two-dimensional navigation environment. There are several intelligent agents, including robots and pedestrians. First, a unique set of indices is defined for all agents. The first index, 1, is reserved for the robot. The state space of each agent is represented as follows: This embodiment is primarily interested in the planar position of the intelligent agent. Navigating the robot to its destination is the problem addressed in this embodiment.

[0060] like Figure 2 As shown, the human-machine cooperative trajectory planning method based on hybrid Nash equilibrium provided in this embodiment includes the following steps:

[0061] Step S1, Establish pedestrian interaction model: Construct a pedestrian interaction model to predict the pedestrian's movement intentions before interacting with the robot.

[0062] In this embodiment, the classic crowd simulation model based on the Optimal Reciprocal Collision Avoidance (ORCA) algorithm is selected, and the following improvements are made to the model:

[0063] 1) Optimization of escape speed responsibility

[0064] This improvement addresses limitations associated with inherent liability parameter settings. In motion planning, pedestrians typically strive to maintain their current speed, including direction and amplitude, to reach their destination as efficiently as possible. To meet this practical requirement, this embodiment introduces optimization of collision avoidance parameters (in the form of an agent). (For example)

[0065] (1)

[0066] In the formula, The preferred speed is the speed that agent A uses when no other agents are present. ) is the optimal collision avoidance speed set of agent A relative to agent B.

[0067] To maintain their current velocity, the interacting agents can construct the following cost function:

[0068] (2)

[0069] Here, the cost function is defined as , Let this be the current velocity of agent A. Similarly, It can be used To calculate. By optimizing this cost function, the intelligent agent is obtained. and Responsibility parameters between and .

[0070] 2) Model parameter learning

[0071] Each agent is simulated using an optimized ORCA, starting from the actual location of the corresponding pedestrian. The agents operate at time steps centrally defined in the dataset. Continuous movement, total duration is In this way, the optimized ORCA-driven agent's position can be compared with the pedestrian's actual position. The following cost function is used for comparison and optimization during the selection phase:

[0072] (3)

[0073] in, (k) is the trajectory generated by simulation. Represents time in publicly available datasets The actual coordinates of the pedestrian's location.

[0074] In each simulation time In this embodiment, the distances between a target pedestrian and its three nearest neighbors are recorded from real data; these distances are used as input features for the parameter dataset. Due to the inherent randomness in the JADE optimization process, this embodiment performs four optimizations, each producing a different set of optimal parameters. These four results are then used as output labels. The neural network is used to learn the mapping from these input features to the corresponding parameter set, mathematically represented as:

[0075] (4)

[0076] Where the function Indicates weight A parameterized neural network takes the input feature vector (A three-dimensional distance vector containing the target in time) Mapping the distances to its three nearest neighbors to the output parameter set Specifically, These represent the four calibration parameters of the pedestrian dynamics model set.

[0077] Finally, by adding an intent filter, the pedestrian interaction model can be obtained. To make the model more robust, extended Kalman filtering and maximum likelihood estimation are added to estimate the model error, thus forming the pedestrian interaction model (FO-ORCA).

[0078] Step S2: Construct a hybrid policy set for pedestrians and robots: Generate a nominal hybrid policy for pedestrians based on the predicted trajectory output by the pedestrian interaction model, and generate a nominal hybrid policy for robots based on the planned trajectory output by the robot motion planner. The nominal hybrid policy is a probability distribution in the policy space.

[0079] In classic game theory, each agent (robot or human) makes decisions based on its own interests and the current environment, typically assuming that the decisions of other agents are known. However, in human-robot cooperative trajectory planning, the interaction between the robot and the human is dynamic and constantly changing. Considering this, this embodiment uses a "hybrid Nash equilibrium" to describe the cooperative and game-theoretic relationship between humans and robots. Hybrid Nash equilibrium not only captures the behavioral strategies of each agent but also handles uncertainties, avoiding the "frozen robot problem" caused by overly conservative behavior or the "reciprocal dance" problem caused by a lack of cooperation.

[0080] In this embodiment, the pure strategy Defined as a trajectory, it is defined as a continuous mapping from time to state in the navigation space:

[0081] (5)

[0082] in This represents the state space of each agent. Indicates the time horizon.

[0083] The policy space is defined as the space of all feasible pure policies that an agent may consider, denoted as . .

[0084] Hybrid strategies are in the policy space The probability distribution, expressed by the probability density function. express:

[0085] (6)

[0086] (7)

[0087] The hybrid strategy space is denoted as It is the strategy space. The space of all probability density functions.

[0088] Suppose that each agent has a nominal mixture policy before interacting with other agents, denoted as . ,in Indexing for the agent. (The so-called nominal blending strategy is the blending strategy before interaction.)

[0089] In this embodiment, the hybrid strategy of pedestrian interaction is taken as the nominal hybrid strategy of the human, that is, the hybrid strategy between the human and the robot when there is no interaction. A trajectory generation method based on Gaussian Process (GP) is selected. Specifically, in this embodiment, the trajectory predicted by the pedestrian interaction model (FO-ORCA) is set as the mean function. To capture the uncertainty of pedestrian strategy, the covariance function of the trajectory is set as the RBF kernel function, with the following specific expression:

[0090] (8)

[0091] Consideration time Time The above mixing strategy, and through parameters and This is used to control the range of trajectory fluctuations and the smoothness or correlation decay rate of the function. Since the Gaussian process is "infinite-dimensional," it needs to be discretized and sampled to obtain a hybrid strategy between pedestrians and robots in the absence of interaction. Similarly, for robots, a meta-planner can be used to generate time. Time The trajectory from the starting point to the target point is used as the mean function, and the nominal hybrid strategy of the robot is obtained using the kernel function described above. .

[0092] In this embodiment, for pedestrians, the trajectory predicted by FO-ORCA is its mean function. Multiple trajectories are sampled using the kernel function mentioned earlier, and then smoothed using a smoothing function. For robots, this embodiment uses the A* algorithm as the original planner, and sets the heuristic function as follows:

[0093] (9)

[0094] in Indicates the distance from the starting point to the current node. The actual path cost (known value). Indicates the current node Heuristic cost estimation (predicted value) for the target node. This represents a node. Based on the overall priority, the algorithm is expanded first. The smallest node. To accommodate different robot types, this embodiment chooses Euclidean space as... ,Right now:

[0095] (10)

[0096] In summary, we obtain the target point that is closest to the current position of the robot, select a straight line as the mean function of the robot, and use a kernel function to form the robot's hybrid strategy.

[0097] Figure 3 This is a flowchart illustrating the convergence of the hybrid Nash equilibrium solution algorithm in an embodiment of the present invention.

[0098] Step S3, establishing the objective function and solving the game: (e.g.) Figure 3 As shown, an objective function that integrates the expected collision risk and the penalty for nominal policy deviation is established, and the policy uncertainty in the human-computer interaction process is handled based on the Bayesian update rule. The hybrid Nash equilibrium policy of the robot is obtained by solving the objective function.

[0099] After obtaining the nominal hybrid policies of pedestrians and robots, the next step is to consider the interaction model between the two in order to update the hybrid policies of both parties and plan a collision-free trajectory.

[0100] In this embodiment, collision risk is defined as a function. It is used to assess the collision risk between two pure strategies (trajectories).

[0101] In this embodiment, a given intelligent agent Hybrid strategy Another pure strategy With intelligent agents The expected risks of a collision are:

[0102] (11)

[0103] In this embodiment, the joint expected collision risk between two agents is defined as the joint expectation of the two agents regarding the collision risk of the hybrid strategy:

[0104] (12)

[0105] In the formula, , For two trajectories , The physical collision risk function aims to assess the degree of geometric interference between trajectories. In the iterative Bayesian approach, in addition to prior beliefs, i.e., the nominal mixed policy, a conditional likelihood function is also needed, which can usually be interpreted as a measurement model. In this embodiment, it is assumed that each agent follows the expected utility assumption, and the following conditional function can be used to model a given agent. If a hybrid strategy is adopted, then the other agent adopts a pure strategy. The probability magnitude:

[0106] (13)

[0107] According to the Bayesian algorithm, after the interaction, the agent... The Bayesian posterior mixture strategy is as follows:

[0108] (14)

[0109] in This is the normalization term.

[0110] Building upon this, we can further extend it to Bayesian policy updates in multi-agent systems:

[0111] (15)

[0112] (16)

[0113] in Indicates a given division A hybrid strategy involving all agents except those mentioned above. Follow pure strategy The likelihood of the agent can also be obtained through equation (14). Post-interaction hybrid strategy .

[0114] This embodiment establishes the objective function based on risk collision and intent preservation. To maintain the convergence of the algorithm and considering that pedestrians may not be entirely rational during interaction, the agent... Establish the following objective function:

[0115] (17)

[0116] in This represents the degree of rationality of an intelligent agent. For a pedestrian, who is in a state of bounded rationality, the value is greater than 0 and less than 1, while for a robot, which is in a state of perfect rationality, the value is equal to 1. , Indicates except for intelligent agents For all other agents, this term represents the expected collision risk, and the last term is the penalty for deviating from the nominal policy. Its mathematical expression is:

[0117] (18)

[0118] Considering the imperfect rationality of pedestrians, the Bayesian update for pedestrian mixed strategies needs to be modified, as shown in the following mathematical expression:

[0119] (19)

[0120] The system-level joint objective function can then be defined as follows:

[0121] (20)

[0122] The algorithm converges when the following condition is met:

[0123] (twenty one)

[0124] A robot's decisions are determined by algorithms, making it a perfectly rational agent. However, a pedestrian's decisions are the result of multiple factors, making them a boundedly rational agent. Therefore, a rationality factor is introduced based on maximum entropy theory. This is to accommodate the bounded rationality of pedestrians' decision-making. It assumes that pedestrians cannot strictly adhere to the optimal strategy, and their behavior will contain some randomness or noise. To represent this, pedestrian behavior can be controlled by parameters that adjust for the degree of rationality. To achieve this. In this embodiment, pedestrian behavior is assumed to vary with factors such as scene, density, and speed, and is implemented in the following way. Dynamically adjusts over time:

[0125] (twenty two)

[0126] in They represent time. The rate of change, density, and population size, therefore the function Fitting can be achieved using regression models or neural networks.

[0127] Figure 4 This is a graph showing the convergence verification results of the hybrid Nash equilibrium algorithm in an embodiment of the present invention. Figure 4 'a' represents an interaction scenario between two intelligent agents; Figure 4 b represents an interaction scenario between three intelligent agents.

[0128] Step S4, Control Execution: The robot's hybrid Nash equilibrium strategy is converted into control commands for the robot, thereby driving the robot's movement.

[0129] In this embodiment, to simplify the objective function, the pedestrian's irrationality factor is temporarily set to 0.5, and the robot's rationality factor is set to 1. The convergence of the algorithm is determined by the number of iterations and the convergence condition of equation (21). After convergence, the robot's hybrid Nash equilibrium strategy is averaged and used as the robot's control input to reach the next target point.

[0130] To verify the effectiveness of the method in this embodiment, experiments were conducted on two aspects: algorithm convergence and simulated environment navigation. In the algorithm convergence experiment, the pedestrian interaction model was simplified to a straight-line running model, and the following collision risk function was used for the experiment:

[0131] (twenty three)

[0132] Choose one or two pedestrians to interact with a robot and solve for its Nash mixed policy solution.

[0133] Figure 5 This is a diagram showing the experimental results of a simulated scenario in an embodiment of the present invention. Figure 5 A is a diagram of the flow of people, the left is a snapshot of the simulated environment, and the right is a diagram of the agent's trajectory; Figure 5 b is the centralized interaction diagram, the left is a snapshot of the simulated environment, and the right is the trajectory diagram of the agent.

[0134] Navigation was implemented in a simulated environment. This embodiment selected the social force model and the ORCA model as the pedestrian simulation model, the FO-ORCA model as the mean model for the pedestrian trajectory, and the straight line between the temporary target point and the current point obtained after A* algorithm planning as the mean model for the robot. The same kernel function was used for both the pedestrian and the robot, and the generated trajectories were sampled 1000 times each. Experiments were conducted in two interaction scenarios: concentrated interaction and navigating through crowds. Furthermore, a deep learning method was selected to compare metrics such as navigation time, the closest distance to pedestrians during navigation, and the straight-line path ratio.

[0135] Table 1 is a record of the performance metrics of the method and deep reinforcement learning in this embodiment.

[0136] Table 1

[0137] Navigation time (s) 18 20 Path ratio 1.1 1.2 Number of emergency stops 0 0

[0138] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0139] 1. Handling Uncertainty and Avoiding Common Navigation Problems: This embodiment uses a "hybrid Nash equilibrium" to describe the collaborative and game-theoretic relationship between humans and machines, which can effectively handle uncertainty in the interaction. This helps to avoid the "frozen robot problem" (the robot stops moving due to uncertainty) caused by excessive conservatism in traditional methods and the "reciprocal dance" (ineffective behavior of humans and robots avoiding each other) caused by a lack of effective cooperation.

[0140] 2. More realistically simulates pedestrian behavior:

[0141] 1) Bounded rationality modeling: Introducing a "rationality factor" to simulate the imperfectly rational state in pedestrian decision-making, acknowledging that pedestrian behavior may contain randomness or noise, rather than always adopting the optimal strategy. This factor can also be dynamically adjusted according to scene factors such as speed and density, making it more adaptable.

[0142] 2) Modeling policy uncertainty: By modeling the nominal mixed policy of pedestrians through Gaussian process, not only is their trajectory predicted, but the uncertainty of their policy is also captured by the covariance function, which is more advanced than treating people as simple static obstacles.

[0143] 3. Balancing Safety and Intent: Its objective function comprehensively considers both the "desired collision risk" and the "penalty for deviation from the nominal policy." This means that while planning collision-free trajectories (safety), the algorithm also strives to maintain the initial movement intentions of the robot and pedestrians (efficiency and naturalness), achieving a balance between safety and efficiency.

[0144] 4. Explicit convergence: This embodiment provides explicit mathematical conditions for the convergence of the algorithm, ensuring the stability and reliability of the algorithm during the solution process.

[0145] The above embodiments are preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention.

Claims

1. A human-machine collaborative trajectory planning method based on hybrid Nash equilibrium, characterized in that, Includes the following steps: Step 1, Establish a pedestrian interaction model: Construct a pedestrian interaction model to predict the movement intentions of pedestrians before they interact with the robot; Step 2, construct a hybrid strategy set for pedestrians and robots: generate a nominal hybrid strategy for pedestrians based on the predicted trajectory output by the pedestrian interaction model, and generate a nominal hybrid strategy for robots based on the planned trajectory output by the robot motion planner. The nominal hybrid strategy is a probability distribution in the strategy space. Step 3, Establishment of the objective function and game solution: Establish an objective function that integrates the expected collision risk and the penalty for nominal policy deviation, and handle the policy uncertainty in the human-computer interaction process based on the Bayesian update rule. The robot's hybrid Nash equilibrium policy is obtained by solving the objective function. Step 4, Control Execution: The robot's hybrid Nash equilibrium strategy is converted into control commands to drive the robot's movement. In step S3, the joint expected collision risk between the two agents is defined as the joint expectation of the collision risk of the hybrid strategy by the two agents: In the iterative Bayesian approach, in addition to prior beliefs, i.e., the nominal mixed policy, a conditional likelihood function is also needed. Assuming each agent follows the expected utility assumption, the following conditional function can be used to model a given agent. If a hybrid strategy is adopted, then the other agent adopts a pure strategy. The probability magnitude: According to the Bayesian algorithm, after the interaction, the agent... The Bayesian posterior mixture strategy is as follows: in For normalization term, Building upon this, we can further extend it to Bayesian policy updates in multi-agent systems: In the formula Indicates a given division A hybrid strategy involving all agents except those mentioned above. Follow pure strategy The likelihood, In step S3, the objective function is established based on risk collision and intention preservation. To maintain the convergence of the algorithm and considering that pedestrians may not be perfectly rational during interactions, the agent... Establish the following objective function: In the formula This represents the degree of rationality of an intelligent agent. For a pedestrian, who is in a state of bounded rationality, the value is greater than 0 and less than 1, while for a robot, which is in a state of perfect rationality, the value is equal to 1. , Indicates except for intelligent agents For all other agents, this term represents the expected collision risk, and the last term is the penalty for deviating from the nominal policy. Its mathematical expression is: Considering the imperfect rationality of pedestrians, the Bayesian update for pedestrian mixed strategies needs to be modified, as shown in the following mathematical expression: The system-level joint objective function can then be defined as follows: The algorithm converges when the following condition is met: 。 2. The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium according to claim 1, characterized in that: in, In step S1, the pedestrian interaction model is an improved FO-ORCA model based on the optimal reciprocal collision avoidance ORCA algorithm. The improvement includes escape velocity responsibility optimization and model parameters learned through neural networks. Finally, an intention filter is added to obtain the pedestrian interaction model. Extended Kalman filtering and maximum likelihood estimation are then used to estimate the model error to obtain the pedestrian interaction model.

3. The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium according to claim 2, characterized in that: in, The optimization of escape velocity responsibility specifically refers to the optimization of the agent. Introducing optimization of collision avoidance parameters: To maintain their current velocity, the interacting agents construct the following cost function: In the formula, the cost function is defined as follows: ,same, use By calculating and optimizing this cost function, the intelligent agent was obtained. and Responsibility parameters between and , The optimization process of the model parameters learned through the neural network is as follows: Each agent is simulated using optimized ORCA, with the agent operating at a time step centrally defined in the dataset. Continuous movement, total duration is During the selection phase, the following cost functions are used for comparison and optimization: in Represents time in publicly available datasets The actual location coordinates of the pedestrian. In each simulation time The distances between a target pedestrian and its three nearest neighbors are recorded from real data. These distances are used as input features to the parameter dataset, and four optimizations are performed, each producing a different set of optimal parameters. These four results are then used as output labels. The neural network learns the mapping from these input features to the corresponding parameter sets, mathematically represented as follows: In the formula, the function Indicates weight A parameterized neural network takes the input feature vector Mapping to the output parameter set Specifically, These represent the four calibration parameters of the pedestrian dynamics model set.

4. The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium according to claim 1, characterized in that: in, In step S2, the nominal mixing policy is the mixing policy before interaction. It is assumed that each agent has a nominal mixing policy before interacting with other agents, denoted as... ,in For indexing intelligent agents, The hybrid strategy for pedestrian interaction is used as the nominal hybrid strategy for the pedestrians. A trajectory generation method based on Gaussian processes is selected. Specifically, the trajectory predicted by the pedestrian interaction model is set as the mean function. To capture the uncertainty of the pedestrian strategy, the covariance function of the trajectory is set as the RBF kernel function, with the following specific expression: Consideration time Time The above mixing strategy, and through parameters and To control the fluctuation range of the trajectory and the smoothness or correlation decay rate of the function, the trajectory is discretized and sampled to obtain the pedestrian's nominal hybrid strategy. Similarly, for robots using meta-planners to generate time... Time The trajectory from the starting point to the target point is used as the mean function, and the nominal hybrid strategy of the robot is obtained using the kernel function described above. .

5. The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium according to claim 1 or 4, characterized in that: in, In step S2, for pedestrians, the trajectory predicted by FO-ORCA is their mean function. Multiple trajectories are sampled using a kernel function, and then smoothed using a smoothing function. For the robot, the A* algorithm is used as the original planner, and a heuristic function is set: In the formula, Indicates the distance from the starting point to the current node. The actual path cost, Indicates the current node Heuristic cost estimation for the target node Represents a node Based on the overall priority, the algorithm is expanded first. The smallest node is chosen to be in Euclidean space. ,Right now: We obtain the target point that is closest to the current position of the robot, select a straight line as the mean function of the robot, and use a kernel function to form the robot's hybrid strategy.

6. The human-machine cooperative trajectory planning method based on hybrid Nash equilibrium according to claim 1, characterized in that: in, rational factors Dynamically adjusts over time: in They represent time. The rate of change, density, and population size, therefore the function Fitting is performed using regression models or neural networks.

7. A human-machine collaborative trajectory planning system based on hybrid Nash equilibrium, characterized in that, For performing the method as described in claim 1, comprising: Pedestrian interaction model building module: Constructs a pedestrian interaction model to predict the pedestrian's movement intentions before interacting with the robot; Hybrid strategy set construction module: Generates the pedestrian's nominal hybrid strategy based on the predicted trajectory output by the pedestrian interaction model, and generates the robot's nominal hybrid strategy based on the planned trajectory output by the robot motion planner. The nominal hybrid strategy is a probability distribution in the strategy space. Objective function establishment and game solving module: Establish an objective function that integrates expected collision risk and penalty for nominal policy deviation, and handle policy uncertainty in human-computer interaction based on Bayesian update rules. The robot's hybrid Nash equilibrium policy is obtained by solving the objective function. Control execution module: Converts the robot's hybrid Nash equilibrium strategy into control commands for the robot, thereby driving the robot's movement.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1 to 6.

Images