Implicit space social courtesy navigation method and system based on interaction point arrival time

Abstract

The application discloses a hidden space social courtesy navigation method and system based on interaction point arrival time and belongs to the technical field of robot autonomous navigation. The current sensor observation data of a robot is used to generate a current time hidden state; a hidden space dynamics model is combined to deduce and generate a candidate set of multiple candidate action sequences and hidden state trajectories of the candidate set and input the candidate set to a hidden space social evaluator to obtain a social courtesy cost, a threshold trigger flag and a lower limit of execution length; each candidate action sequence is scored based on a comprehensive score function which is fused with the social courtesy cost, a trajectory smoothness cost and an immediate reward and a terminal value calculation; the candidate action sequence with the highest score is selected; and action sequence optimization is further performed according to the threshold trigger flag and the lower limit of execution length. The method can not only maintain the calculation advantage of hidden space deduction in a high-dimensional environment, but also introduce social courtesy related constraints and signals, so that local planning which takes into account short-term safety, long-term goals and social compliance is realized.

Description

Technical Field

[0001] This invention relates to the field of robot autonomous navigation technology, and in particular to a latent space social courtesy navigation method and system based on the arrival time of interaction points. Background Technology

[0002] In autonomous robot navigation tasks, the design of local planning methods always faces the dual challenges of dynamic environment complexity and real-time computation. Traditional geometry-based planning methods usually rely on explicit modeling and prediction of obstacle trajectories. However, in highly dynamic scenarios such as crowds, explicit prediction requires processing high-dimensional position and velocity information of multiple moving bodies, which results in high computational overhead, rapid accumulation of prediction errors, and difficulty in maintaining stability in practical deployments.

[0003] In recent years, learning-based latent space planning methods have attracted attention. These methods compress sensor observations into low-dimensional latent states using an encoder, and then combine this with a learned dynamic model for rolling deduction, avoiding explicit prediction of the original high-dimensional environment. Latent space deduction improves computational efficiency and generalization, while also providing new possibilities for combining with trajectory optimization algorithms.

[0004] However, existing methods generally have the following shortcomings:

[0005] The evaluation signal is singular: most tasks rely solely on immediate rewards or endpoint value to assess the trajectory. Immediate rewards often focus on short-term obstacle avoidance and path length, neglecting long-term interaction effects; while endpoint value, although reflecting the achievement of long-term goals, struggles to guarantee local safety and courteous behavior. Using either alone can easily lead to poor navigation performance.

[0006] Lack of social factors: When navigating in densely populated environments, robots not only need to avoid collisions but also must exhibit courteous behavior that conforms to human social norms. Existing reinforcement learning or trajectory optimization methods often only consider physical obstacle avoidance and efficiency, lacking modeling of interactive conflicts, priority, and social distance, resulting in robots exhibiting behaviors such as rushing, frequent pauses, or unnatural movements.

[0007] Insufficient trajectory stability: In dynamic and complex environments, if the optimizer does not have appropriate social constraints, it is prone to frequent action switching, resulting in trajectory jitter or swaying, which affects actual usability.

[0008] Therefore, a new method is urgently needed that can maintain the computational advantages of latent space deduction in high-dimensional environments, while introducing social courtesy-related constraints and signals to achieve local planning that balances short-term security, long-term goals, and social compliance. Summary of the Invention

[0009] To address the problems of complex explicit prediction calculations, limited evaluation signals, and lack of social courtesy modeling in existing methods, this invention proposes a latent space social courtesy navigation method and system based on the arrival time of interaction points. This method utilizes privileged information to generate explicit social tags and transfers this social knowledge to a latent space representation through a distillation mechanism. Then, in the latent space, trajectory rolling deduction and social evaluation are jointly performed, incorporating multi-step immediate rewards, terminal value, and smoothness constraints, and employing an iterative trajectory optimization method for trajectory optimization. This invention's solution significantly enhances the robot's social compliance and trajectory smoothness in crowd environments while maintaining the efficiency of latent space methods, balancing short-term safety and long-term goals. To achieve the above objectives, the technical solution adopted by this invention is as follows:

[0010] In a first aspect, this invention proposes a latent space social courtesy navigation method based on the arrival time of interaction points, comprising the following steps:

[0011] The robot acquires current sensor observation data, processes it using a feature extraction network, and obtains the robot's low-dimensional hidden state at the current moment. Then, based on the hidden state at the current moment and the hidden space dynamics model, a candidate set containing multiple future candidate action sequences and their hidden state trajectories is generated.

[0012] The hidden state trajectory corresponding to each future candidate action sequence in the candidate set is input into the latent space social evaluator to obtain the social courtesy cost, threshold trigger flag, and lower limit of execution length. The latent space social evaluator is obtained by supervising distillation learning pre-training the initialized evaluator using explicit social supervision labels. The explicit social supervision labels include the location of the interaction conflict point, the arrival time difference state, the priority state, the direction-sensitive safety margin, and the threshold trigger flag.

[0013] Each candidate action sequence is scored based on a comprehensive scoring function, which integrates the social courtesy cost, trajectory smoothness cost, and the immediate reward and terminal value predicted by the reward and value network. The candidate action sequence with the highest score is selected. If the threshold trigger flag is activated, the selected action sequence is adjusted to favor the deceleration or yielding direction, and the final action sequence is output.

[0014] Control the robot to execute the final action sequence, and the execution time step is not less than the lower limit of the execution length; after execution, take the robot's latest state as the starting point and continuously replan the final action sequence until the robot reaches the target point.

[0015] Furthermore, the process of obtaining the robot's current low-dimensional hidden state includes:

[0016] Acquire time-series laser scanning data, robot speed information, and robot position information relative to the target point;

[0017] Motion compensation is performed on the time-series laser scanning data, and the data is stacked to generate a grayscale image of the obstacle.

[0018] The spatial features of the obstacle grayscale image, the dynamic motion features of the velocity information, and the target features of the relative target point position information are extracted by the neural network, and the three are fused into the hidden state at the current moment.

[0019] Furthermore, the process of generating the candidate set includes:

[0020] By inputting the current hidden state into the hidden space dynamics model, a basic future action sequence and the corresponding hidden state trajectory are derived.

[0021] Sampling is performed around the basic future action sequence to generate multiple future action sequences and their corresponding hidden state trajectories, which together form a candidate set containing candidate action sequences and their hidden state trajectories.

[0022] Furthermore, the latent space dynamics model includes a dynamic prediction function and a policy network; the dynamic prediction function takes the known latent state as input and generates the action at the corresponding time; the policy network takes the known action and latent state as input and generates the latent state at the next time.

[0023] Furthermore, the training process of the latent space social evaluator introduces a privileged information supervision module, which is used to calculate and generate explicit social supervision labels based on the trajectory information of future candidate action sequences and dynamic obstacles, including:

[0024] The intersection of the predicted future trajectories of the robot and dynamic obstacles, or the area where the minimum distance is less than a preset interaction threshold, is used as the interaction conflict point;

[0025] Calculate the arrival time difference between the two parties at the point of interaction conflict, and generate the arrival time difference state of the robot, which is either leading or lagging.

[0026] The robot's priority status is determined based on the arrival time difference and preset courtesy rules, wherein the priority status is either pass or yield;

[0027] Calculate the orientation-sensitive safety margin between the robot and the obstacle based on the relative azimuth angle and relative velocity;

[0028] The threshold trigger flag is set based on the arrival time difference and relative speed. If the arrival time difference is less than the threshold and the relative speed exceeds the threshold, the threshold trigger flag is 1; otherwise, it is 0.

[0029] Furthermore, during the training phase, the latent space social evaluator uses the latent state trajectory corresponding to each future candidate action sequence and the explicit social supervision label as input to predict the social courtesy cost, threshold trigger flag, and lower bound of execution length.

[0030] Furthermore, the comprehensive scoring function is expressed as:

[0031] ;

[0032] in, It is a comprehensive score of the candidate action sequence. It is the time decay factor. It is the immediate reward at the current time t+i. It is the terminal value at time step H. It is the price of social courtesy. It is a scaling factor. It is the smoothness cost of the candidate action sequence.

[0033] Furthermore, the reward and value network is trained through supervised learning and includes a reward prediction network for predicting immediate rewards and a value prediction network for predicting terminal value. The supervision label for immediate rewards consists of the sum of the target reward, collision penalty, and social reward. The supervision label for terminal value is calculated using the temporal difference method based on the supervision label for immediate rewards.

[0034] Secondly, the present invention also provides a latent space social courtesy navigation system based on interaction point arrival time prediction, for implementing the aforementioned latent space social courtesy navigation method based on interaction point arrival time prediction.

[0035] The beneficial effects of this invention include:

[0036] (1) The present invention introduces a pre-trained latent space social evaluator to obtain the social courtesy cost, threshold trigger flag and execution length lower limit. The latent space social evaluator will use explicit social supervision labels to internalize in its network parameters during the pre-training process. The social courtesy judgment can be obtained by inputting the latent state trajectory into the evaluator for one forward propagation. The calculation speed is fast and the robot’s behavior naturally reflects social intelligence. It directly guides the trajectory optimization process, making the optimized sequence more in line with the natural courtesy behavior expected by human social interaction, and significantly improving its acceptability and naturalness of interaction in the crowd.

[0037] (2) By introducing the lower limit of the execution length constraint, the robot is forced to execute actions of multiple time steps continuously, instead of replanning and potentially switching to completely different trajectories at each step. This mechanism, combined with the trajectory smoothness cost and the threshold triggering mechanism, effectively suppresses trajectory jitter, swaying or sudden turning caused by frequent trajectory switching, ensuring the continuity and stability of the robot's motion and improving the reliability of control.

[0038] (3) The comprehensive scoring function of the present invention does not rely solely on a single indicator. It integrates multiple decision-making factors, including reward value, social courtesy cost and trajectory smoothness cost, to ensure that the planned trajectory can simultaneously take into account short-term safety, long-term navigation goals and smooth and fluid movements. Attached Figure Description

[0039] Figure 1 This is an overall framework diagram of the implicit space social courtesy navigation system based on the arrival time of interaction points provided by the present invention.

[0040] Figure 2 This is an overall flowchart of the training phase of this invention.

[0041] Figure 3 This is the overall flowchart of the reasoning stage of this invention. Detailed Implementation

[0042] The present invention will be further described and illustrated below with reference to specific embodiments. The embodiments described are merely examples of the content of this disclosure and do not limit the scope of the invention. The technical features of each embodiment in the present invention can be combined accordingly, provided that there is no mutual conflict.

[0043] like Figure 1 As shown, this invention discloses a latent space social courtesy navigation system based on the arrival time of interaction points, used to implement a latent space social courtesy navigation method based on the arrival time of interaction points. Its overall process is divided into a training phase and an inference phase. The training phase includes latent space encoding of input observation information, latent space dynamics deduction, privileged information supervision signal distillation, and latent space social evaluator training. The inference phase includes latent space encoding of input observation information, latent space dynamics deduction, latent space social evaluator inference, and distributed sampling trajectory optimization and execution control. During the training phase, explicit social tags are generated through the privileged information supervision module, and these tags are distilled into the implicit output of the latent space evaluator during the training-inference phase. This avoids the need to progressively calculate explicit indicators such as interaction conflict points, arrival time differences, and safety margins during runtime. The inference phase relies solely on latent space features to complete courtesy judgment and trajectory scoring, significantly reducing computational overhead, improving real-time performance in dynamic scenarios, and ensuring stable operation on resource-constrained robot platforms.

[0044] The Latent Space Social Courtesy Navigation System includes a Latent State Encoding Module, a Latent Space Dynamics Module, a Privilege Information Monitoring Module, a Reward and Value Prediction Module, a Latent Space Social Evaluator, and a Trajectory Optimization Module. Each module is described below.

[0045] (a) Hidden State Encoding Module

[0046] The hidden state encoding module transforms high-dimensional observation information from sensors and the robot itself into a low-dimensional, compact representation, facilitating efficient deduction and evaluation in the hidden space. Specifically, it includes the following steps:

[0047] (1) Input Information Acquisition: The two-dimensional lidar on the robot acquires a ring of scanning data centered on the robot at each moment, and stacks multiple consecutive frames (e.g., 8 frames) of laser scanning data from the past moments to form a time-series observation matrix. At the same time, the robot acquires the velocity information provided by the robot's underlying controller and converts it into vector form, including linear velocity and angular velocity. The position of the robot relative to the target point (defined as relative position information) is calculated by odometry or positioning system and represented in polar coordinates or Cartesian coordinates.

[0048] (2) Motion compensation and temporal stacking: After motion compensation, the temporal observation matrix formed by multiple frames of laser data is stacked into a grayscale image of obstacles that can distinguish between static and dynamic obstacles. This image contains both the spatial distribution of static obstacles in the environment and reflects the motion trend of dynamic obstacles.

[0049] Motion compensation specifically includes: using the robot's spatial position in the latest frame of the matrix as a reference point, and employing odometry information to perform reverse displacement compensation on each historical frame, unitizing it with the coordinates of the latest frame in the matrix, thereby eliminating the influence of the robot's own motion. In the stacked grayscale image, static obstacles appear as deep black because their positions are stable across multiple frames, while dynamic obstacles, due to relative motion, undergo displacement between frames, appearing as areas of gradual grayscale change.

[0050] (3) Feature extraction and fusion: The grayscale image is processed by a convolutional network Spatial features are extracted; the velocity vector is passed through a fully connected network. Extracting dynamic motion features; the relative positions of the robot and the target point are processed through a fully connected network. The target features are formed; finally, the three are combined to form the low-dimensional hidden state at time t. This is used for subsequent robot trajectory simulation and evaluation. The calculation formula is as follows:

[0051]

[0052] in, This represents the grayscale image of the obstacle at time t; This represents the robot's velocity information at time t, including linear velocity and angular velocity; The position of the robot relative to the target point at time t, and the three together constitute the robot's state information.

[0053] This module significantly compresses the original observation dimensions, but still retains key spatial layout, dynamic status, and mission objective information, ensuring the efficiency and accuracy of subsequent trajectory extrapolation and social assessment.

[0054] (II) Hidden Space Dynamics Module

[0055] The Latent Space Dynamics module is used to simulate the motion evolution of a robot over a future period in a low-dimensional latent space. This module contains dynamic prediction functions. and policy network The core idea of ​​the dynamic model is to utilize the dynamic prediction function learned during the training phase. The robot recursively generates future hidden state trajectories based on its current hidden state and action input. The implementation process is as follows:

[0056] (1) Action sequence sampling: Several candidate action sequences are sampled in the action space. Each candidate action sequence contains the robot action sequence from time t to time t+H. Each of the actions Composed of linear velocity and angular velocity, these actions may originate from a Gaussian distribution or other distributed sampling strategies.

[0057] (2) Hidden state rolling deduction: Constructing a dynamic prediction function and policy network As the dynamic model of the backbone network, for each candidate action sequence, based on the hidden state at time t... Recursively predict robot actions at time t And the hidden state of the future moment:

[0058]

[0059]

[0060]

[0061] By iterating in this way, the hidden state trajectory of the candidate action sequence at time H in the future is obtained. .

[0062] In this invention, different strategies are employed in the generation of candidate action sequences and hidden state trajectories during the training and inference phases:

[0063] During the training phase, a known real robot action sequence with a time step of H is selected as a reference action sequence. This sequence is perturbed using sampling strategies such as Gaussian distribution, thereby deriving multiple action sequences that vary within a reasonable range from the real sequence. These, along with the reference action sequence, constitute candidate action sequences. Each candidate action sequence corresponds to... The same, calculated by the hidden state encoding module. The candidate action sequence is input into the dynamic model in (2), and the dynamic prediction function is used. By performing rolling deduction, the hidden state trajectory corresponding to each candidate action sequence is obtained. At the same time, the policy network is analyzed using the reference action sequence and the corresponding hidden state trajectory. Parameter optimization;

[0064] During the inference phase, the dynamic model relies entirely on the dynamic prediction function trained within the model. and policy network Make autonomous decisions. The current hidden state of the robot. Input dynamics model, through policy network and dynamic model The robot's action sequence and corresponding hidden state trajectory for a future time step H are derived. Using the predicted robot action sequence as the center, multiple neighboring candidate action sequences are generated through sampling strategies such as Gaussian distribution for subsequent evaluation and selection. The hidden state trajectories of the candidate action sequences are generated in the same way as during the training phase.

[0065] Compared to directly predicting high-dimensional sensor observations, performing robot motion inference in the latent space significantly reduces the computational load and avoids the uncertainty of explicitly modeling the trajectory of each dynamic body, enabling trajectory optimization to be completed within the real-time control cycle. At the same time, the dynamics module, through interaction with the real environment or training with simulation data, ensures that its prediction results are consistent with the real system in a statistical sense, guaranteeing the usability and generalization of the inferred trajectory.

[0066] (III) Privilege Information Supervision Module

[0067] This module is only enabled during the training phase. By aggregating and annotating multi-agent interactions, it generates explicit courtesy rules and priority judgments. This allows the latent space social evaluator to implicitly reflect human-like behavioral patterns during reasoning, outputting a single courtesy or pass judgment, avoiding contradictory or unstable decisions caused by conflicts in multi-party interactions. When facing complex scenarios with multiple moving objects simultaneously, the system maintains a consistent courtesy strategy. For example, in dense crowds, narrow passages, and intersections, the robot exhibits "proactive avoidance and reasonable courtesy," reducing the sense of conflict and abruptness with crowds, significantly improving the robot's robustness in dynamic and complex environments and its acceptability in public environments.

[0068] During the training phase, using the explicit robot relative position and velocity information from time t to time t+H, and through a series of computable geometric and physical rules, the following precise social supervision labels are generated and input into the latent space social evaluator along with the hidden state trajectory sequence:

[0069] Interaction Conflict Point Location: Predicting the area where the robot and dynamic obstacles may encounter each other in the future. First, short-term predictions are made about the future trajectories of the robot and each nearby dynamic obstacle. For the robot, its trajectory sequence is derived in the latent space dynamics module; for the dynamic obstacles, their trajectories can be simulated using a simple uniform velocity model or a more complex model based on real trajectories. Since the time period selected in the training phase is from time t to time t+H in the past, the trajectory sequences of the dynamic obstacles are known and do not need to be derived or predicted.

[0070] The robot's predicted trajectory is spatially intersected with the predicted trajectory of each dynamic obstacle, and the region where the two trajectories are closest in two-dimensional space is calculated.

[0071] The spatial region that meets the following conditions is defined as the interaction conflict point: the minimum distance between the robot and the predicted trajectory of the dynamic obstacle is less than the human-preset interaction threshold. The coordinates of the conflict point can be the geometric center of the spatial region corresponding to the minimum distance, or the robot's spatial position at the minimum distance.

[0072] Arrival Time Difference Status: Calculate the order in which both parties arrive at the conflict point. For each identified interaction conflict point, record the timestamps of the robot's trajectory and the dynamic obstacle's trajectory first entering the location of that interaction conflict point. and arrival time difference The calculation formula is: Supervisory labels indicating whether the robot is "leading" or "lagging" based on the time difference;

[0073] Priority status: Based on the arrival time difference and preset yielding rules (such as left yielding to right, weak yielding to strong), priority is determined, and a "pass" or "yield" status is generated.

[0074] Orientation-sensitive safety margin: Calculate the relative position vector between the robot and the nearest dynamic obstacle, transform this vector into the robot's body coordinate system to obtain the relative azimuth angle, and define a safety margin function that varies with the azimuth angle. The forward region of the robot has the most stringent requirements, and the safety margin function has the largest value; the lateral region has medium requirements, and the safety margin function has a medium value; the backward region has the least stringent requirements, and the safety margin function has the smallest value.

[0075] Threshold trigger flag: If a potential high-risk conflict is detected (the expected arrival times of both parties at the point of interaction conflict are very close, the distance between the robot and the current dynamic obstacle is less than the manually set emergency threshold, or the approach speed of the dynamic obstacle relative to the robot exceeds the manually set safety threshold), a forced deceleration or detour flag will be triggered.

[0076] (iv) Latent Space Social Evaluator

[0077] The core of the latent space social evaluator is a latent space model composed of a fully connected network. During the training phase, the evaluator learns how to map the social supervision labels of the privileged information supervision module to the latent space model network through a mapping function via a supervised distillation mechanism, thus obtaining a trained latent space model. During the inference phase, the trained latent space social evaluator only needs to input a sequence of latent state trajectories to output implicit costs and control signals related to social interaction, thereby achieving natural courtesy navigation.

[0078] (1) Supervised distillation mechanism: During the training phase, the hidden state trajectory sequence from time t to time t+H is input. The privileged information supervision module provides explicit social supervision labels, such as interaction conflict points, arrival time differences, priority determination, direction-sensitive safety margins, and threshold triggering flags. The evaluator transforms these supervision labels into a mapping between latent space features and model output through supervised learning. Essentially, it learns a mapping function that maps the input latent state trajectory sequence to the corresponding social supervision label, thereby distilling and internalizing social knowledge and mapping it into the latent space model.

[0079] The output of the latent space model is as follows:

[0080] The cost of social courtesy Used to punish those who fail to yield to others;

[0081] Threshold trigger flag A binary signal indicating whether forced deceleration or yielding is required;

[0082] Execution length lower limit The constraint trajectory must be executed at least several steps to avoid instability caused by frequent switching.

[0083] The output results are transmitted to the trajectory optimization module.

[0084] (2) Input and output of the inference stage: In the inference stage, the hidden space state at time t is... and the hidden state trajectory sequence at time H in the future The input is fed into a pre-trained latent space model. Forward prediction is then performed using the pre-trained mapping function within the model. Only one forward propagation is needed to achieve the output of socially relevant costs and control signals, relying solely on the latent state input. The model also outputs the social courtesy cost. Threshold trigger flag and execution length lower limit The results are then transmitted to the trajectory optimization module.

[0085] Because the latent space social evaluator distills the explicit social supervision labels of the privileged information supervision module during the training phase, there is no need to explicitly calculate the arrival time difference or safety margin during the inference phase, thus saving computational resources. The output signal of the trained latent space social evaluator not only contains continuous cost values ​​but also discrete trigger signals, combining the advantages of fine-grained optimization and hard constraints. It can be extended to multi-agent scenarios, and the evaluator can automatically aggregate the interaction effects of multiple obstacles and output a single courtesy judgment.

[0086] (v) Rewards and Value Prediction Module

[0087] To ensure that the optimal trajectory selected by the subsequent trajectory optimization module satisfies both short-term safety and long-term goals, thus providing a quantitative basis for trajectory optimization, the reward and value prediction module of this invention comprehensively considers both short-term immediate rewards and long-term terminal value, and utilizes the reward prediction network learned during the training phase. and value prediction network This allows for efficient evaluation of the short-term gains and long-term value of trajectories within the latent space.

[0088] Specifically, it includes:

[0089]

[0090]

[0091] in, This represents the predicted instantaneous reward at time t. This is a reward prediction network consisting of fully connected layers, used to predict the immediate reward function in the latent space. This represents the predicted terminal value at time t. It is a value prediction network consisting of fully connected layers, used to predict terminal value in the latent space.

[0092] During training, an immediate reward function based on real-world feedback is used. Rewards based on goals Collision penalty Social rewards The formula is as follows: The three parts are added together.

[0093]

[0094] in, Let t be the state of the robot, including , , .

[0095] (1) Target Reward This reward system provides sparse terminal rewards, as well as dense guiding signals based on the reduction in distance to the target to encourage the robot to move toward the target and reward it for successfully reaching the target.

[0096]

[0097] in, and These are the robot's spatial positions at time t-1 and time t, respectively. For a fixed target point location, A threshold set by an individual. Scaling factor The reward for reaching the goal is set by the user and is set to 1 in this specific embodiment.

[0098] (2) Collision penalty To prevent robots from unsafely approaching or directly colliding with any obstacles, the closer the robot gets to an obstacle (especially within the critical safety boundary), the greater the penalty.

[0099]

[0100] in, This refers to the minimum distance, measured by the lidar, from the robot to surrounding obstacles (including static and dynamic obstacles). The robot's safety radius is set by humans. Scaling factor The collision penalty, which is set manually, is set to -1 in this specific embodiment.

[0101] (3) Social rewards It is used to evaluate the robot's future interactions with dynamic obstacles, thereby guiding the robot to act safely and courteously in crowded environments.

[0102] If the robot arrives too early at the point of interaction conflict and "cuts in," it receives a negative reward; if it yields or arrives late, it receives a positive reward. The formula is as follows:

[0103]

[0104] in, The minimum distance between the robot and the dynamic obstacle at the predicted point of interaction conflict. Artificially set social thresholds This is the scaling factor.

[0105] During the training phase, an immediate reward function is used. The true reward value based on the robot's hidden state at time t is calculated, and then the terminal value is calculated through temporal difference error. Supervised learning and minimizing the temporal difference error are used to drive the robot's performance. and Optimize the fit so that it can accurately predict immediate rewards. With end value .

[0106] In the reasoning phase, the results obtained from the training phase... Immediate reward function in the latent space A replacement was made.

[0107] (vi) Trajectory Optimization Module

[0108] This module is used to calculate the hidden state trajectory sequence corresponding to each candidate action sequence. The comprehensive scoring function.

[0109] Comprehensive scoring function for trajectory sequences It is obtained by the following multi-objective weighting:

[0110]

[0111] in, The time decay factor, and They are respectively The t+i power and the H power, The social courtesy cost of this trajectory sequence is output by the hidden-state social evaluator. Scaling factor The smoothness cost of this trajectory sequence, used to penalize abrupt turns, is calculated as follows:

[0112]

[0113] in, and These represent the actions corresponding to the trajectory sequence at times t+i+1 and t+i, respectively, including linear velocity and angular velocity.

[0114] The trajectory scores of different candidate action sequences are ranked, and the trajectory sequence with the highest score is the optimal trajectory sequence of the robot at the current time t.

[0115] After obtaining the optimal trajectory sequence, the optimal trajectory is further adjusted based on the threshold trigger flag output by the hidden state social evaluator. When the threshold trigger flag output by the evaluator... At that time, each action of the optimal trajectory sequence from time t to time t+H is adjusted. By modifying the linear velocity and angular velocity in the action, the robot's motion trend is made to decelerate or detour, so as to actively avoid social conflicts.

[0116] The system uses the adjusted optimal trajectory as the execution trajectory and sends the corresponding motion control commands to the robot's underlying controller, based on the lower limit of the execution length output by the latent space social evaluator. Enforcement from arrive The trajectory sequence is adjusted at different times to avoid frequent switching and jitter.

[0117] By employing both threshold trigger flags and minimum execution length constraints, the system effectively suppresses common issues in trajectory optimization such as short-frequency switching and jitter. When the intensity of interaction conflicts exceeds the threshold, the system automatically triggers the threshold, biasing the sampling distribution to a more stable yielding direction. Simultaneously, the minimum execution length constraint ensures that the robot maintains continuous motion over a period of time, thereby avoiding frequent corrections and sudden turns, and improving the continuity of the trajectory and the stability of execution.

[0118] Based on the above system, a latent space social courtesy navigation method based on the arrival time of interaction points is implemented, including a training phase and an inference phase.

[0119] like Figure 2 As shown, the training phase mainly includes the following steps:

[0120] S1, Constructing a hidden state trajectory sequence based on hidden space dynamics

[0121] Step 1.1: Construct a sequence of target robot actions from time t to time t+H, where each action consists of linear velocity and angular velocity.

[0122] Step 1.2: Select the known past action sequence of the target robot with a time step of H as the reference action sequence. Use sampling strategies such as Gaussian distribution to derive several action sequences from the reference action sequence. These action sequences and the reference action sequence together form a candidate action sequence set.

[0123] Step 1.3: Based on the sensor scan data of N consecutive frames from time tN to time t, after motion compensation and temporal stacking, a grayscale image of the obstacle is obtained, the target robot's velocity information at time t, and the relative position of the target robot relative to the target point at time t. Spatial features, dynamic motion features, and target features are extracted by a feature extraction network and encoded as the hidden state of the target robot at time t.

[0124] Step 1.4: Input the candidate action sequences one by one into the dynamic model, and perform rolling deduction in combination with the hidden state of the target robot at time t to obtain the hidden state trajectory sequence from time t to time t+H corresponding to each candidate action sequence.

[0125] S2, Privileged Information Supervision

[0126] Step 2.1: Using the robot's relative position and velocity information from time t to time t+H in the candidate action sequence, generate social supervision labels through geometric and physical rules and input them into the latent space model in the latent space social evaluator. The social supervision labels include interaction conflict points, arrival time difference, priority determination, direction-sensitive safety margin, and threshold trigger flag.

[0127] S3, Hidden Space Social Assessment

[0128] Step 3.1: Input the hidden state trajectory sequence from time t to time t+H corresponding to each candidate action sequence and the social supervision label into the latent space model, output the social courtesy cost, threshold trigger flag and lower limit of execution length, and input them into the trajectory optimization module.

[0129] S4, Rewards and Value Incentives

[0130] Step 4.1: Calculate the real instant reward at time t based on the hidden state trajectory sequence generated from the reference action sequence, and iteratively optimize the reward prediction network and value prediction network based on the real instant reward at time t to obtain the trained reward prediction network and value prediction network.

[0131] S5, Trajectory Optimization

[0132] Step 5.1: For each candidate action sequence, calculate the trajectory score of the candidate action sequence based on the real instant reward at time t, the terminal value at time step H calculated by the value prediction network, the social courtesy cost obtained in step 3.1, and the smoothness cost of two adjacent actions.

[0133] Step 5.2: Select the hidden state trajectory sequence corresponding to the candidate action sequence with the highest trajectory score as the optimal trajectory.

[0134] Step 5.3: Determine whether the threshold is triggered based on the threshold trigger flag in Step 3.1. If the threshold is triggered, pre-adjust the linear velocity and angular velocity of each action in the action sequence corresponding to the optimal trajectory to achieve deceleration or turning.

[0135] Step 5.4: Compare the adjusted action sequence with the reference action sequence, and repeat S1-S5 until all network parameters converge.

[0136] Step 5.5: Obtain the trained feature extraction network, dynamic model, latent space model, reward prediction network, and value prediction network.

[0137] like Figure 3 As shown, the reasoning stage mainly includes the following steps:

[0138] S1, Constructing a hidden state trajectory sequence based on hidden space dynamics

[0139] Step 1.1: Based on the sensor scan data of N consecutive frames from time tN to time t, after motion compensation and temporal stacking, a grayscale image of the obstacle is obtained, the target robot's velocity information at time t, and the relative position of the target robot relative to the target point at time t. Spatial features, dynamic motion features, and target features are extracted by the trained feature extraction network and encoded as the hidden state of the target robot at time t.

[0140] Step 1.2: Input the robot's hidden state at time t into the trained dynamic model and perform rolling deduction to obtain a basic action sequence and a corresponding basic latent space trajectory sequence from time t to the future time t+H. Generate multiple action sequences from the basic action sequence using sampling strategies such as Gaussian distribution. These action sequences together with the basic action sequence form a candidate action sequence set.

[0141] Step 1.3: Input the action sequences generated by sampling strategies such as Gaussian distribution into the trained dynamic model one by one, and perform rolling deduction in combination with the hidden state of the target robot at time t to obtain the hidden state trajectory sequence from time t to the future time t+H corresponding to each candidate action sequence.

[0142] S2, Latent Space Social Assessment

[0143] Step 2.1: Input the hidden state trajectory sequence from time t to the future time t+H corresponding to each candidate action sequence into the trained latent space model, output the social courtesy cost, threshold trigger flag and lower limit of execution length, and input them into the trajectory optimization module.

[0144] S3, Trajectory Optimization

[0145] Step 3.1: For each candidate action sequence, calculate the instantaneous reward at time t and the terminal value at time step H based on the trained reward prediction network and value prediction network. Combine the social courtesy cost obtained in Step 2.1 and the smoothness cost of two adjacent actions to calculate the comprehensive score of the candidate action sequence.

[0146] Step 3.2: Select the candidate action sequence with the highest comprehensive score as the optimal sequence.

[0147] Step 3.3: Determine whether the threshold is triggered based on the threshold trigger flag in Step 2.1. If the threshold is triggered, pre-adjust the linear velocity and angular velocity of each action in the optimal sequence to achieve deceleration or turning.

[0148] Step 3.4: Execute the action corresponding to the lower limit of the execution length in step 4.1, and then repeat steps S1-S3 until the target point is reached.

[0149] In one specific embodiment of the present invention, to verify the effectiveness of the method, several representative experimental scenarios were constructed in the IRSim simulation platform and compared with existing methods. The experimental environment included:

[0150] Open scenario: Dynamic obstacles move in an open area to test the robot's obstacle avoidance capabilities at different obstacle speeds and modes;

[0151] Corridor scenario: Set up wide, medium, narrow and extremely crowded corridor passages to test the robot's interactive obstacle avoidance and path keeping capabilities in confined spaces.

[0152] In the experiments, the obstacle's movement patterns included both unimodal and multimodal modes: unimodal movement indicated that the obstacle only moved towards a fixed target; multimodal movement indicated that the obstacle had a probability of switching targets or retreating during its movement, thus introducing randomness and uncertainty. Through these experimental scenarios, the robustness and social compliance of the method of this invention under different complexity environments could be systematically tested.

[0153] The experimental results are shown in Tables 1 and 2. In open scenarios, the method of this invention outperforms the comparative methods in terms of success rate, path efficiency (SPL), and trajectory smoothness. Especially in multimodal obstacles and high-speed dynamic environments, it maintains a success rate close to 1.0, while the performance of the comparative methods significantly declines. In corridor-constrained scenarios, the method of this invention maintains a high task completion rate and low inference time in extremely narrow environments, indicating that it possesses stronger interaction modeling capabilities and better trajectory stability in confined spaces.

[0154] Table 1 Comparative Experiments in Open Scenarios

[0155]

[0156] Table 2 Comparative Experiments in Enclosed Corridor Scenarios

[0157]

[0158] In addition, ablation experiments (Table 3) were conducted to test the effect of the latent space social evaluator in this invention. When the social evaluator was turned off, the success rate decreased from 0.91 to 0.74, the SPL decreased from 0.83 to 0.67, and the number of times the robot unsafely crossed in front of pedestrians increased from 2.32 to 6.11, indicating that the lack of social distillation makes the robot more prone to taking aggressive actions and lacking courtesy. After enabling the social evaluator, the robot can effectively slow down in advance and actively avoid pedestrians, exhibiting more natural social compliance behavior.

[0159] Table 3 Ablation Experiments of Social Evaluators

[0160]

[0161] In summary, the method of this invention exhibits higher success rates, smoother trajectories, and fewer unsafe interaction events in various environments. This result demonstrates that the design of distilling social rules through privileged information supervision and using a latent space social evaluator to output courtesy costs and trigger flags during the inference phase can significantly improve the robot's safety, stability, and social compliance in dynamic crowd environments.

[0162] This invention's method is entirely based on sensor observation and latent space deduction, without relying on external maps or explicit obstacle trajectory prediction, thus making it applicable to various indoor and outdoor scenarios. It maintains good performance in both autonomous exploration in unknown environments and interactive navigation among crowds using existing maps. Furthermore, this framework is scalable to different types of robot platforms, including differentially driven robots and humanoid robots, providing versatility and portability for a wide range of applications.

[0163] Based on the same inventive concept, this embodiment also provides a latent space social courtesy navigation system based on interaction point arrival time, used to implement the above-mentioned latent space social courtesy navigation method based on interaction point arrival time. The system includes:

[0164] The hidden state encoding module is used to acquire the robot's current sensor observation data, process it using a feature extraction network, and obtain the robot's low-dimensional hidden state at the current moment.

[0165] The latent space dynamics module, based on the current latent state and the latent space dynamics model, infers and generates a candidate set containing multiple future candidate action sequences and their latent state trajectories;

[0166] The latent space social evaluator is used to evaluate the latent state trajectory corresponding to each future candidate action sequence and output the social courtesy cost, threshold trigger flag and lower limit of execution length.

[0167] The trajectory optimization module scores each candidate action sequence based on a comprehensive scoring function, selects the candidate action sequence with the highest score, and determines whether the threshold trigger flag is activated. If so, the selected action sequence is adjusted to favor the deceleration or yielding direction, and the final action sequence is output; continuous replanning is performed.

[0168] The iterative execution module is used to control the robot to execute the final action sequence, and the execution time step is not less than the lower limit of the execution length. After execution, the robot's latest state is used as the starting point to continuously replan the final action sequence until the robot reaches the target point.

[0169] The privileged information supervision module is used to calculate and generate explicit social supervision labels based on the trajectory information of future candidate action sequences and dynamic obstacles, and to use the explicit social supervision labels to perform supervised distillation learning on the latent space social evaluator.

[0170] For the system embodiments, since they basically correspond to the method embodiments, relevant details can be found in the descriptions of the method embodiments; the implementation methods of the remaining modules will not be repeated here. The system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of the present invention according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0171] The system embodiments of the present invention can be applied to any device with data processing capabilities, such as a computer or other similar device. The system embodiments can be implemented in software, hardware, or a combination of both. Taking software implementation as an example, as a logical device, it is formed by the processor of any data processing device loading the corresponding computer program instructions from non-volatile memory into memory for execution.

[0172] The above examples are merely specific embodiments of the present invention. Obviously, the present invention is not limited to the above embodiments and many variations are possible. All variations that can be directly derived or conceived by those skilled in the art from the disclosure of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A latent spatial social courtesy navigation method based on the arrival time of interaction points, characterized in that, Including the following steps: The robot acquires current sensor observation data, processes it using a feature extraction network, and obtains the robot's low-dimensional hidden state at the current moment. Then, based on the hidden state at the current moment and the hidden space dynamics model, a candidate set containing multiple future candidate action sequences and their hidden state trajectories is generated. The hidden state trajectory corresponding to each future candidate action sequence in the candidate set is input into the latent space social evaluator to obtain the social courtesy cost, threshold trigger flag, and lower limit of execution length. The latent space social evaluator is obtained by supervising distillation learning pre-training the initialized evaluator using explicit social supervision labels. The explicit social supervision labels include the location of the interaction conflict point, the arrival time difference state, the priority state, the direction-sensitive safety margin, and the threshold trigger flag. Each candidate action sequence is scored based on a comprehensive scoring function, which integrates the costs of social courtesy, trajectory smoothness, and immediate rewards and terminal values ​​predicted by the reward and value network. Select the candidate action sequence with the highest score. If the threshold trigger flag is activated, adjust the selected action sequence to favor the deceleration or yielding direction, and output the final action sequence. Control the robot to execute the final action sequence, and the execution time step is not less than the lower limit of the execution length; after execution, take the robot's latest state as the starting point and continuously replan the final action sequence until the robot reaches the target point.

2. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 1, characterized in that, The process of obtaining the robot's current low-dimensional hidden state includes: Acquire time-series laser scanning data, robot speed information, and robot position information relative to the target point; Motion compensation is performed on the time-series laser scanning data, and the data is stacked to generate a grayscale image of the obstacle. The spatial features of the obstacle grayscale image, the dynamic motion features of the velocity information, and the target features of the relative target point position information are extracted by the neural network, and the three are fused into the hidden state at the current moment.

3. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 1, characterized in that, The process of generating the candidate set includes: By inputting the current hidden state into the hidden space dynamics model, a basic future action sequence and the corresponding hidden state trajectory are derived. Sampling is performed around the basic future action sequence to generate multiple future action sequences and their corresponding hidden state trajectories, which together form a candidate set containing candidate action sequences and their hidden state trajectories.

4. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 3, characterized in that, The latent space dynamics model includes a dynamic prediction function and a policy network; the dynamic prediction function takes the known latent state as input and generates the action at the corresponding time; the policy network takes the known action and latent state as input and generates the latent state at the next time.

5. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 1, characterized in that, The training process of the latent space social evaluator incorporates a privileged information supervision module, which calculates and generates explicit social supervision labels based on future candidate action sequences and trajectory information of dynamic obstacles, including: The intersection of the predicted future trajectories of the robot and dynamic obstacles, or the area where the minimum distance is less than a preset interaction threshold, is used as the interaction conflict point; Calculate the arrival time difference between the two parties at the point of interaction conflict, and generate the arrival time difference state of the robot, which is either leading or lagging. The robot's priority status is determined based on the arrival time difference and preset courtesy rules, wherein the priority status is either pass or yield; Calculate the orientation-sensitive safety margin between the robot and the obstacle based on the relative azimuth angle and relative velocity; The threshold trigger flag is set based on the arrival time difference and relative speed. If the arrival time difference is less than the threshold and the relative speed exceeds the threshold, the threshold trigger flag is 1; otherwise, it is 0.

6. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 5, characterized in that, During the training phase, the latent space social evaluator takes the latent state trajectory corresponding to each future candidate action sequence and the explicit social supervision label as input to predict the social courtesy cost, threshold trigger flag, and lower bound of execution length.

7. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 1, characterized in that, The comprehensive scoring function is expressed as follows: ； in, It is a comprehensive score of the candidate action sequence. It is the time decay factor. It is the immediate reward at the current time t+i. It is the terminal value at time step H. It is the price of social courtesy. It is a scaling factor. It is the smoothness cost of the candidate action sequence.

8. The implicit space social courtesy navigation method based on the arrival time of interaction points according to claim 1, characterized in that, The reward and value network is trained through supervised learning and includes a reward prediction network for predicting immediate rewards and a value prediction network for predicting terminal value. The supervision label for immediate rewards consists of the sum of the target reward, collision penalty, and social reward. The supervision label for terminal value is calculated using the temporal difference method based on the supervision label for immediate rewards.

9. A latent space social courtesy navigation system based on the arrival time of interaction points, used to implement the latent space social courtesy navigation method based on the arrival time of interaction points as described in claim 1, characterized in that, The system includes: The hidden state encoding module is used to acquire the robot's current sensor observation data, process it using a feature extraction network, and obtain the robot's low-dimensional hidden state at the current moment. The latent space dynamics module, based on the current latent state and the latent space dynamics model, infers and generates a candidate set containing multiple future candidate action sequences and their latent state trajectories; The latent space social evaluator is used to evaluate the latent state trajectory corresponding to each future candidate action sequence and output the social courtesy cost, threshold trigger flag and lower limit of execution length. The trajectory optimization module scores each candidate action sequence based on a comprehensive scoring function, selects the candidate action sequence with the highest score, and determines whether the threshold trigger flag is activated. If so, the selected action sequence is adjusted to favor the deceleration or yielding direction, and the final action sequence is output; continuous replanning is performed. The iterative execution module is used to control the robot to execute the final action sequence, and the execution time step is not less than the lower limit of the execution length. After execution, the robot's latest state is used as the starting point to continuously replan the final action sequence until the robot reaches the target point.

10. The implicit spatial social courtesy navigation system based on the arrival time of interaction points according to claim 9, characterized in that, Also includes: The privileged information supervision module is used to calculate and generate explicit social supervision labels based on the trajectory information of future candidate action sequences and dynamic obstacles, and to use the explicit social supervision labels to perform supervised distillation learning on the latent space social evaluator.

Images