A human-machine collaborative autonomous navigation system and method based on multi-constraint optimization

Abstract

The application relates to the technical field of man-machine cooperation, in particular to a man-machine cooperative autonomous navigation system and method based on multi-constraint optimization, which comprises a navigation action generation module, an artificial interaction module, a confidence evaluation module and a man-machine cooperative optimization module, wherein the man-machine cooperative optimization module is used for constructing an action fusion optimization target according to the machine navigation action, the artificial navigation action, the safety confidence mean value and the action executed at the previous moment, and solving a fusion execution action in a feasible region containing linear velocity constraints, angular velocity constraints, curvature constraints and safety velocity constraints; the application further comprises a Tube MPC correction module and a weighted experience priority sampling module. In the optimization solver, velocity constraints, curvature constraints and dynamic safety velocity constraints based on collision time are explicitly introduced, the physical limits of the machine and the environment risks are simultaneously brought into the optimization feasible region, and the solved action has sufficient safety under the premise of physical realizability.

Description

Technical Field

[0001] This invention relates to the field of human-machine collaboration technology, specifically to a human-machine collaborative autonomous navigation system and method based on multi-constraint optimization. Background Technology

[0002] With the rapid development of deep reinforcement learning algorithms, their application scenarios have experienced explosive growth, and the field of path planning has gradually introduced deep reinforcement learning technology to achieve intelligent decision-making. Building on this, human decision-making has been further integrated into the control loop, forming a human-machine collaborative path planning method that combines human prior knowledge with the machine's autonomous learning capabilities. However, traditional navigation methods based on deep reinforcement learning still have inherent flaws. On the one hand, this method mainly uses a reward function to quantitatively evaluate the agent's actions to select the most suitable obstacle avoidance action in the current environment. However, the reward function is mainly designed around the navigation goal and is difficult to cope with sudden abnormal situations in the environment. This causes the agent to be unable to adjust its decisions in time when facing unfamiliar scenes, easily leading to danger. On the other hand, the action generation process of deep reinforcement learning relies to some extent on a random exploration mechanism. This exploration behavior lacks regularity and is difficult to control effectively, greatly increasing the probability of dangerous collisions in real physical environments, causing irreversible losses.

[0003] Existing research rarely considers the risk assessment of decision-making safety by navigation systems. For example, robots cannot predict the confidence level of their current decisions or assess the safety status of their environment when performing actions. Furthermore, existing research often treats autonomous machine decision-making and human-machine collaborative decision-making as two separate modules, failing to fully explore their complementary and collaborative relationship, resulting in a lack of organic integration between human and machine intelligence. At the optimization objective level, existing research mostly focuses on single indicators such as shortest path or reaching local target points, lacking a comprehensive consideration of multiple objective factors such as motion smoothness, dynamic safety constraints, and human-machine weight allocation. This is crucial for navigation systems that balance operational efficiency and decision-making safety in complex dynamic environments. Additionally, existing optimization problem-solving methods often employ linear weighting or simple switching mechanisms. These methods have poor adaptability in dynamic environments, making it difficult to dynamically adjust decision-making strategies online according to environmental changes, thus limiting the robustness of the system in complex scenarios. Summary of the Invention

[0004] The purpose of this invention is to provide a human-machine collaborative autonomous navigation system and method based on multi-constraint optimization, so as to solve the problems of low decision-making security and inability to cope with sudden anomalies caused by the limitations of reward function design and random exploration mechanism in the existing technology; at the same time, it overcomes the shortcomings of existing research that separate human-machine decision-making, lack of multi-objective overall optimization, poor adaptability and insufficient robustness in dynamic environments.

[0005] On one hand, the present invention provides a human-machine cooperative autonomous navigation system based on multi-constraint optimization, comprising:

[0006] The navigation action generation module is used to output machine navigation actions based on the environmental state of the mobile robot;

[0007] The human interaction module is used to receive human operation instructions and convert them into human navigation actions. It is also used to generate experience type labels that distinguish between human-machine collaborative experience and pure machine experience.

[0008] The confidence assessment module includes a Bayesian neural network with a random deactivation inference mechanism, which is used to output the mean of safety confidence and the variance of prediction uncertainty based on the environmental state, action features, obstacle distance features, predicted collision time features and dynamic obstacle features.

[0009] The human-machine collaborative optimization module is used to construct an action fusion optimization target based on the machine navigation action, the human navigation action, the average safety confidence value and the action executed at the previous moment, and solve the fused execution action within a feasible region that includes linear velocity constraints, angular velocity constraints, curvature constraints and safety velocity constraints;

[0010] The Tube MPC correction module is used to generate a reference trajectory with the fused execution action as a reference input, construct a bounded perturbation set based on the prediction uncertainty variance, construct a robust invariant set and a compaction constraint based on the bounded perturbation set, and output the final control command through nominal control sequence solution and state deviation feedback compensation.

[0011] The weighted experience priority sampling module is used to store human-machine collaborative experience and pure machine experience generated during navigation into the same experience playback buffer, determine dynamic sampling weights based on experience type labels and immediate rewards, and determine sampling probabilities according to the dynamic sampling weights to update the navigation action generation module and the confidence evaluation module.

[0012] Specifically, the navigation action generation module is used to input the environmental state into the navigation action generation network as the distance between the current position of the mobile robot and the target point, the relative angle between the current orientation of the mobile robot and the direction of the target point, the current linear velocity of the mobile robot, the current angular velocity of the mobile robot, and the distance of the nearest obstacle in multiple lidar sectors, and output the machine navigation action containing the machine linear velocity component and the machine angular velocity component.

[0013] Specifically, the navigation action generation network is a deep reinforcement learning network built based on the TD3 algorithm. The deep reinforcement learning network includes a policy network for outputting the machine navigation actions. During the training process, the policy network is trained by taking the smaller target value through a dual-value network, delaying policy updates, and smoothing the target actions.

[0014] Specifically, the human interaction module is used to convert the first type of human operation command into a linear velocity increment adjustment amount, the second type of human operation command into an angular velocity increment adjustment amount, and the third type of human operation command into a human-machine collaboration mode activation / deactivation signal, so as to form the human navigation action based on the linear velocity increment adjustment amount and the angular velocity increment adjustment amount, and generate the experience type label based on the human-machine collaboration mode activation / deactivation signal and whether the human operation command participates in action fusion.

[0015] Specifically, the confidence assessment module is used to form an input feature vector from the standard deviation of the action or action feature output by the policy network in the navigation action generation module, the minimum obstacle distance, the predicted collision time, the proportion of dynamic obstacles, and the number of dynamic obstacles. The number of dynamic obstacles is determined based on the number of sectors where the change in distance between LiDAR sectors at adjacent sampling times is greater than the sum of the mobile robot's own displacement and a preset tolerance. The proportion of dynamic obstacles is determined based on the ratio of the number of sectors identified as dynamic obstacles to the total number of sectors participating in the detection.

[0016] Specifically, the confidence assessment module is used to perform multiple random inactivation forward inferences on the same input feature vector to obtain multiple security prediction values, and the average of the multiple security prediction values ​​is used as the security confidence mean, and the sample variance of the multiple security prediction values ​​relative to the security confidence mean is used as the prediction uncertainty variance.

[0017] Specifically, the human-machine collaborative optimization module is used to construct an action fusion optimization target that includes machine action deviation, previous action smoothing, and safety deceleration when no human operation command is detected, and to construct an action fusion optimization target that includes machine action deviation, human action deviation, previous action smoothing, and safety deceleration when a human operation command is detected. The weight of the machine action deviation is positively correlated with the mean of the safety confidence level, while the weights of the human action deviation and the safety deceleration are negatively correlated with the mean of the safety confidence level.

[0018] Specifically, the human-machine collaborative optimization module is used to solve the action fusion optimization objective by using the projection gradient descent method combined with Armijo line search, and when the iteratively updated action does not satisfy the linear velocity constraint, angular velocity constraint, curvature constraint or safe velocity constraint, the iteratively updated action is projected back to the feasible region jointly defined by the linear velocity constraint, angular velocity constraint, curvature constraint and safe velocity constraint.

[0019] Specifically, the Tube MPC correction module is used to construct the bounded disturbance set as a set whose disturbance norm is no greater than the product of the prediction uncertainty variance and the scaling factor. Based on the closed-loop system matrix formed by the system matrix, the input matrix, and the feedback gain, and the bounded disturbance set, a robust invariant set is constructed. The actual state constraints are compressed according to the robust invariant set to obtain the nominal state constraints. The actual input constraints are compressed according to the feedback gain and the robust invariant set to obtain the nominal input constraints. The nominal control sequence is then solved under the compressed constraints.

[0020] On the other hand, the present invention also provides a method for a human-machine cooperative autonomous navigation system based on multi-constraint optimization as described above, comprising the following steps:

[0021] S1. Obtain the environmental state of the mobile robot at the current moment, and input the environmental state into the navigation action generation network to obtain the machine navigation action;

[0022] S2. Obtain manual operation instructions and convert the manual operation instructions into manual navigation actions that include linear velocity components and angular velocity components;

[0023] S3. Input the environmental state, the action characteristics of the machine navigation action, the obstacle distance characteristics, the predicted collision time characteristics, and the dynamic obstacle characteristics into a Bayesian neural network with a random inactivation inference mechanism. Obtain a safety prediction sequence through multiple forward inferences, and calculate the mean safety confidence and the variance of prediction uncertainty based on the safety prediction sequence.

[0024] S4. Construct an action fusion optimization target based on whether the manual operation instruction is detected. The action fusion optimization target includes a machine action deviation item, a smoothing item for the action executed at the previous moment, and a safety deceleration item. When the manual operation instruction is detected, it further includes a manual action deviation item. The weight of the machine action deviation item is positively correlated with the mean of the safety confidence level, and the weights of the manual action deviation item and the safety deceleration item are negatively correlated with the mean of the safety confidence level.

[0025] S5. Within the feasible region that includes linear velocity constraints, angular velocity constraints, curvature constraints, and safe velocity constraints based on predicted collision time, the action fusion optimization objective is solved by projection gradient descent to obtain the fused execution action.

[0026] S6. Using the fusion execution action as a reference input, generate a reference trajectory in the prediction time domain, and construct a bounded perturbation set based on the prediction uncertainty variance;

[0027] S7. Construct a robust invariant set based on the bounded perturbation set and feedback gain, and compress the nominal state constraints and nominal input constraints according to the robust invariant set;

[0028] S8. Solve the Tube MPC optimization problem under nominal dynamic constraints, compressed nominal state constraints, and compressed nominal input constraints to obtain the nominal control sequence.

[0029] S9. Obtain the real-time actual state of the mobile robot, calculate the state deviation between the real-time actual state and the nominal state, and superimpose the first control quantity in the nominal control sequence with the feedback correction quantity generated based on the state deviation to obtain the final control command sent to the mobile robot.

[0030] S10. Store the state transition data generated during navigation into the same experience replay buffer. The state transition data includes the current environment state, fused execution action, immediate reward, next moment environment state, round termination flag, experience type label, and dynamic sampling weight. Determine the dynamic sampling weight according to the experience type label and the immediate reward, and determine the sampling probability according to the dynamic sampling weight, so as to update the navigation action generation network and the Bayesian neural network based on the experience obtained by sampling.

[0031] The beneficial effects of this invention are as follows:

[0032] 1. This invention explicitly introduces velocity constraints, curvature constraints, and dynamic safety velocity constraints based on collision time into the optimization solver, simultaneously incorporating the robot's physical limits and environmental risks into the optimization feasible region, ensuring that the solved actions have sufficient safety under the premise of physical realizability.

[0033] 2. This invention introduces the uncertainty variance of the BNN output into the Tube MPC framework to dynamically construct perturbation boundaries, enabling the optimization process to automatically adjust robustness and conservatism based on the perceived level of uncertainty. The higher the uncertainty, the wider the perturbation boundary used in the optimization, thereby achieving closed-loop optimization from perceived uncertainty to control robustness.

[0034] 3. The upper-level optimization solver of this invention realizes intelligent fusion of human and machine instructions under the guidance of confidence, while the lower-level TubeMPC performs robust re-optimization of the initial actions under the drive of uncertainty. The two optimization layers cooperate with each other to ensure both the effectiveness of the decision-making level and the robustness of the control level. Attached Figure Description

[0035] Figure 1 This is a flowchart of the human-machine collaborative navigation optimization solution process of the present invention;

[0036] Figure 2 This is a module framework diagram of the human-machine collaborative navigation system of the present invention;

[0037] Figure 3 This is a framework diagram of the human-machine collaboration optimization model of the present invention;

[0038] Figure 4 This is a diagram of the TubeMPC optimization model of the present invention;

[0039] Figure 5 This is a flowchart of the TD3 process of the present invention;

[0040] Figure 6 This is a flowchart of the confidence assessment process of the present invention. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be described in further detail below with reference to the accompanying drawings.

[0042] Example 1:

[0043] Please see Figures 1-6 As shown, a human-machine cooperative autonomous navigation system based on multi-constraint optimization includes:

[0044] The navigation action generation module is used to output machine navigation actions based on the environmental state of the mobile robot;

[0045] The human interaction module is used to receive human operation instructions and convert them into human navigation actions. It is also used to generate experience type labels that distinguish between human-machine collaborative experience and pure machine experience.

[0046] The confidence assessment module includes a Bayesian neural network with a random deactivation inference mechanism, which is used to output the mean of safety confidence and the variance of prediction uncertainty based on the environmental state, action features, obstacle distance features, predicted collision time features and dynamic obstacle features.

[0047] The human-machine collaborative optimization module is used to construct an action fusion optimization target based on the machine navigation action, the human navigation action, the average safety confidence value and the action executed at the previous moment, and solve the fused execution action within a feasible region that includes linear velocity constraints, angular velocity constraints, curvature constraints and safety velocity constraints;

[0048] The Tube MPC correction module is used to generate a reference trajectory with the fused execution action as a reference input, construct a bounded perturbation set based on the prediction uncertainty variance, construct a robust invariant set and a compaction constraint based on the bounded perturbation set, and output the final control command through nominal control sequence solution and state deviation feedback compensation.

[0049] The weighted experience priority sampling module is used to store human-machine collaborative experience and pure machine experience generated during navigation into the same experience playback buffer, determine dynamic sampling weights based on experience type labels and immediate rewards, and determine sampling probabilities according to the dynamic sampling weights to update the navigation action generation module and the confidence evaluation module.

[0050] Specifically, the human-machine collaborative optimization steps are as follows: human navigation instructions are acquired in real time through an external interface and input into the optimization solver along with the machine navigation actions output by the deep reinforcement learning decision-making step, the average confidence value output by the confidence evaluation step, and the collision time calculated by the environment model. The optimization solver aims to minimize the weighted deviation between the machine actions and human actions while maintaining smooth action, and solves for the optimized execution action within the feasible region that satisfies the robot's speed range, maximum curvature, and dynamic safety speed constraints based on the collision time.

[0051] Tube MPC Safety Smoothing Correction Steps: Construct a Tube MPC corrector, using the optimized execution action output from the human-machine collaborative optimization step as a reference input, and the uncertainty variance output from the confidence assessment step as the main basis for the dynamic disturbance boundary; Under the conditions of satisfying nominal dynamics and state constraints, solve for the optimal trajectory, and combine feedback correction based on real-time state deviation to output the final execution action;

[0052] Deep reinforcement learning decision-making steps: A navigation agent is constructed based on the TD3 algorithm. The agent receives environmental state information and outputs machine navigation actions.

[0053] Confidence assessment steps: Construct a BNN network model, using the robot's sensor data, state information, action features, and dynamic obstacle indicators as inputs, perform forward propagation using the Monte Carlo Dropout method, and output the mean confidence score representing the safety of the current navigation environment and the variance of uncertainty representing the reliability of the prediction.

[0054] Experience management and learning steps: Design a unified experience replay buffer, store the state transition data generated by the final execution action into the buffer, and mark it as human-machine collaborative experience or pure machine experience, while assigning different initial sampling weights; perform priority sampling based on the weights to update the parameters of the deep reinforcement learning agent and the Bayesian neural network model;

[0055] Navigation execution and iteration steps: Execute the final execution action in a simulation or real environment, observe the new state and obtain a reward, and repeat the above steps until the preset navigation performance target is reached.

[0056] 1. Human-Machine Collaboration Optimization Module

[0057] Human operation commands and machine actions are based on collaborative decision-making based on optimization theory. By receiving human navigation commands in real time and weighing multiple objectives with machine actions and environmental confidence, optimization objectives are constructed under the premise of satisfying speed, angular velocity constraints, curvature constraints and safety constraints, and the final executed machine actions are solved using the projection descent method.

[0058] The human-computer interaction interface maps discrete keyboard keys to robot motion commands, enabling real-time human intervention in robot navigation. Specifically, keys W and S accelerate and decelerate the robot's linear velocity in the forward direction, respectively, with the mapping relationship as follows:

[0059] , ;

[0060] Buttons A and D are used to adjust the robot's angular velocity, specifically as follows:

[0061] , ;

[0062] in , These are constant linear velocity and angular velocity increments, respectively. This represents the robot's current actual linear velocity. This is the maximum speed limit for the robot's linear velocity. To obtain the smaller of the two values, This represents the robot's current turning angular velocity. These are the minimum and maximum angular velocity limits for robot turning, respectively.

[0063] In addition, buttons E and Q represent the on and off of the human-machine collaboration mode, respectively. By recognizing the buttons, we can accurately distinguish whether the experience belongs to human-machine collaboration or pure machine experience, which will prepare for subsequent experience label classification.

[0064] Human-machine system strategies should be dynamically adjusted according to different stages of deep reinforcement learning to help the model train faster and more efficiently.

[0065] In the early stages of training: Because the policy network is not yet fully developed, the robot's initial movement distribution is random, often failing to directly face the target. Human guidance at this stage typically prioritizes attitude adjustment actions to quickly align with the target, followed by obstacle avoidance navigation. However, this intuitive guidance strategy may dominate the early training phase, causing the robot's policy network to converge to a rigid "turn first, then navigate" behavior, neglecting the crucial feature of the target's relative azimuth in the initial observation state. This results in a lack of generalization ability across different initial postures. Therefore, it is crucial to minimize the introduction of human experience at this stage, giving the navigation system more opportunities for autonomous exploration.

[0066] Mid-training stage: During this stage, the policy network gradually converges, and the robot has initially acquired the ability to plan paths and avoid obstacles. At this time, for dynamic obstacles with high uncertainty in the environment, human experience can be appropriately enhanced to intervene in specific situations, or when the robot is stuck in a local optimum and cannot continue to move, human experience can be introduced to provide the robot with a route to the target position.

[0067] Late Training Stage: In the later stages, the policy network has matured. At this point, the weight of human intervention should be gradually reduced until the robot makes decisions entirely autonomously. Even without human assistance, the robot still needs to conduct extensive autonomous exploration and trial and error in the current environment to further improve the system's stability and robustness in dynamic environments.

[0068] To enable machines to perform optimal actions during navigation, an optimization solver based on the projective gradient descent method is constructed, balancing human commands, machine actions, and environmental safety confidence as the metric. Depending on whether human keyboard commands are detected, the solver employs two different forms of optimization objective functions.

[0069] Optimize the objective function when no human instructions are detected. Defined as:

[0070] ;

[0071] in, The final action to be solved. For machine navigation actions output by deep reinforcement learning networks, For the aforementioned human navigation instructions. This refers to the final action executed at the previous moment. The mean of the security confidence level of the confidence level assessment module. , , These are the weight coefficients for the corresponding items;

[0072] This objective demonstrates that even without human intervention, the final action will adaptively adjust based on environmental confidence: fully trusting machine planning when the environment is safe (high confidence), and resorting to other methods when the environment is dangerous or uncertain (low confidence). Actively reduce speed to ensure safe operation of the machine in unknown environments, while smoothing out items. This ensures the continuity of the movements.

[0073] When human keyboard input is detected, optimize the objective function. Defined as:

[0074] ;

[0075] in, The human navigation instructions are described. This objective indicates that in a human-machine collaborative mode, the final action will dynamically balance between machine actions and human instructions based on environmental confidence: when the environment is safe (high confidence), the system tends to execute machine actions; when the environment is dangerous or uncertain (low confidence), the system tends to execute human instructions.

[0076] To find a solution to the above optimization problem, the feasibility and safety of the final action must be guaranteed. The feasible region should include three types of constraints:

[0077] The first basic motion constraint, the linear velocity must meet the following requirements:

[0078] ;

[0079] The angular velocity must satisfy:

[0080] ; This is the robot's final output angular velocity.

[0081] The second curvature constraint, to prevent the robot from tipping over due to excessive angular velocity, requires that the instantaneous curvature during the execution of the action satisfy the following:

[0082] ;

[0083] in For the maximum permissible curvature, This is to avoid constants with a denominator of 0. To optimize the final actual output linear velocity of the robot obtained from the solution;

[0084] The third safety speed constraint is to ensure that the robot has enough time to react when a collision is imminent, based on the current collision time. and safe collision time Dynamically limit the maximum allowable linear velocity The piecewise function is calculated as follows:

[0085] ;

[0086] To solve this constrained optimization problem, a projective gradient descent method combined with Armijo line search is employed. This method iteratively searches for the objective function within the feasible region that satisfies multiple constraints. Minimize the optimal action. In each iteration, first calculate the current action point. Objective function gradient of action variable This gradient indicates the direction of the steepest descent of the function value. To determine a suitable iteration step size, the algorithm does not use a fixed step size, but instead searches for the step size along the negative gradient direction to ensure that each iteration achieves a sufficient descent of the function value.

[0087] The specific step size search process follows the Armijo line search criterion, which requires the selected step size to be... The following inequality conditions must be met:

[0088] ;

[0089] in To search for constants first, Let L2 be the gradient vector. This inequality represents the target values ​​corresponding to the updated linear and angular velocities. It must be at the current target value At least down This ensures the stability of the iterative process;

[0090] If the current step size satisfies the Armijo condition, then retain the step size and update the action:

[0091] ;

[0092] If not satisfied, then according to the contraction factor. Update step size; The Armijo conditions are then re-examined until a step size that satisfies them is found. However, considering the waste of precise calculations at every step in the environment, a maximum number of iterations, M, is preset. If a feasible step size is still not found after M iterations, the current optimal step size should be output to update the action parameters. While updating the step size, it is also necessary to check whether the action satisfies preset velocity constraints, curvature constraints, and dynamic safety constraints. If the set constraints are violated, the action is pulled back into the feasible region.

[0093] 2. Tube MPC calibration module

[0094] As a safety defense module for end-action actions, the safety smooth correction first takes the optimized action output by the human-machine collaborative optimization module as a reference input, and simultaneously receives the uncertainty variance output by the confidence assessment module to dynamically construct the disturbance boundary. On this basis, it constructs an optimization problem that includes nominal system prediction, robust invariant set constraints and feedback correction control, solves the optimal control sequence, and combines real-time state deviation for feedback compensation. Finally, it outputs a smooth control command with anti-interference capability, ensuring that even under model mismatch and environmental disturbances, the actual motion trajectory of the agent is still constrained within the safety channel.

[0095] Uncertainty exists in both the navigation system and sensor perception due to discrepancies between the simplified motion model and real dynamics, sensor noise, and the unpredictable behavior of environmental disturbances (especially dynamic obstacles). To handle such bounded uncertainties, the system model is formulated as a discrete time-varying linear system with added perturbations:

[0096] ;

[0097] in, For the actual state of the system, For the control input of the actual system, For the bounded perturbations at the input end, this set covers perturbation factors such as model mismatch and external disturbances. A and B are matrices.

[0098] The core of Tube MPC lies in the fact that it does not precisely predict future state trajectories, but rather constrains the system's actual state trajectory within a trajectory "pipeline." The center of this pipeline is the nominal trajectory, and its cross-sectional area is the robust invariant set. This represents the maximum range of deviation of the system from its nominal trajectory under disturbance.

[0099] This approach, which transforms the model's understanding of its own predictions into robust control parameters, achieves a shift from high-level uncertainty perception to low-level safety control, thereby ensuring that even the worst-case scenario can be considered and constrained within bounded safety constraints, thus guaranteeing basic safety.

[0100] In each control cycle, Tube MPC first needs to obtain the robot's state estimate for the current moment, which is typically represented as a vector:

[0101] ;

[0102] in, Global X-axis coordinates of the robot chassis Global Y-axis coordinates of the robot chassis Robot heading angle, The robot's current actual linear velocity, The robot's current actual angular velocity includes the robot's position coordinates, orientation angle, and current linear and angular velocity information in the global coordinate system. This data is obtained in real time through airborne sensors such as odometry and lidar.

[0103] While acquiring the current state, the system also obtains the optimized execution actions from the human-machine collaboration optimization module. This action reflects the decision-making outcome after weighing machine actions against human instructions under confidence guidance. Based on this, the system needs to generate future... reference trajectory of the step :

[0104] ;

[0105] As the ideal path for Tube MPC to track. The reference heading angle for the k-th step is used. The reference trajectory is generated by recursively extrapolating the kinematic model. At that moment, take This refers to the current actual state; for subsequent moments, the control input remains unchanged, maintaining the optimized action. , And calculate the position change according to the following formula:

[0106] ;

[0107] This process is repeated until... This will give you the complete reference trajectory sequence. The reference state at each moment includes position, orientation, and velocity information;

[0108] To ensure the smoothness of the control input during the optimization process and avoid sudden changes in action that could impact the robot, it is also necessary to obtain the actual control input executed at the previous moment. This data is read directly from the records of the control system and will serve as the benchmark for the smoothing term in the subsequent objective function.

[0109] After completing the above preparatory work, the core optimization problem of Tube MPC can be formally constructed. The decision variable of this problem is the future... The nominal control sequence of the step and its corresponding nominal state sequence ,in Output from the human-machine collaboration optimization module The actual state value measured by the sensor at the current moment has been determined. The nominal state and the control quantity must satisfy nominal dynamic constraints. For a differential-driven mobile robot, its discrete-time kinematic model can be expressed as:

[0110] ;

[0111] Where the matrix and The specific form depends on the current state. Specifically, let's note:

[0112] , ;

[0113] The dynamic equations then expand to:

[0114] ;

[0115] This reflects the dynamic characteristics of the actuator. The above relationship can also be written in matrix form, i.e.:

[0116] ;

[0117] Based on satisfying the dynamic constraints, the optimization objective aims to balance trajectory tracking accuracy and control stability. Therefore, the following quadratic objective function is constructed. :

[0118] ;

[0119] The objective function consists of three parts: the first part The deviation between the nominal trajectory and the reference trajectory is measured to guide the robot to move along the desired path; Part Two Dramatic changes in the amount of punishment control, among which Take the actual input from the previous moment to ensure the continuity of the action; Part Three As a terminal cost, we strive to make the state at the predicted time-domain endpoint as close as possible to the desired state set by the global target point:

[0120] ; Global navigation endpoint X coordinate, The Y-coordinate of the global navigation endpoint;

[0121] Wherein the orientation angle:

[0122] .

[0123] The magnitude of the Tube MPC perturbation is determined by the uncertainty variance of the BNN network output. Dynamically determined, specifically, the perturbation set is defined as:

[0124] ;

[0125] in This is the scaling factor. The more uncertain the BNN's prediction of the safety of the current environment (i.e., the more uncertain it is...), the more likely it is to become a scaling factor. When the value is larger, the system believes that the possible disturbance is also larger. Therefore, a more conservative disturbance boundary is adopted for control, reserving a larger safety margin for high uncertainty scenarios.

[0126] Based on this perturbation boundary, a robust invariant set can be constructed. To satisfy Γ is a pre-designed feedback gain matrix, obtained by solving the Riccati equation for the nominal system, and is a robust invariant set. It can be obtained through iterative calculation: First, initialize... Then according to Perform a recursive process, which involves adding all elements from both sets. Repeat this iterative process until... And if the set does not expand further, then the minimum robust invariant set can be obtained. It represents the disturbance Under the influence of the feedback control law, the system state is... The range of steady-state deviations that can be achieved;

[0127] To ensure that the actual state and input still satisfy the physical constraints even in the presence of disturbances, it is necessary to impose more stringent constraints on the nominal state and input than on the actual constraints:

[0128] ;

[0129] in These are the actual physical constraints of the robot. The above-mentioned tight constraints ensure that for all possible perturbations... Actual state and actual input Always confined to a safe area, among which This represents the state deviation.

[0130] Therefore, the complete Tube MPC optimization problem can be formulated as follows: Given nominal dynamic constraints and compacted state and input constraints, find the nominal control sequence that minimizes the objective function. This problem falls under the category of convex optimization and can be solved iteratively using a standard quadratic programming solver. The solver starts from an initial guess (assuming all...) Starting from the current action (keeping it unchanged), we enter an iterative optimization loop. In each iteration, based on the current nominal control sequence... Through the nominal dynamic equation The corresponding nominal state sequence is calculated recursively. ,in Given the current state, the solver then uses the objective function... Evaluate the quality of the current solution and check whether it satisfies the state constraints. With input constraints If the current solution does not satisfy the constraints, or although it satisfies the constraints but has not yet converged to the optimum, the solver will adjust the entire control sequence in the direction that reduces the objective function, generate a new control sequence, and then repeat the above recursive, evaluation, and checking process. This process is repeated until a set of optimal nominal control sequences that satisfies all constraints and minimizes the objective function is found. .

[0131] After optimization, only the first element of the nominal control sequence is taken. As a feedforward control variable. However, due to model instability and the presence of external disturbances, the robot's actual state... It often deviates from the nominal state. Therefore, a feedback correction mechanism needs to be introduced. After obtaining the current real state in real time, the state deviation is calculated. and compare it with the feedback gain matrix The product is used to obtain a correction term, which is then added to the feedforward control quantity to form the final action sent to the robot.

[0132] ;

[0133] This feedback item can correct the state deviation caused by disturbances in real time, pulling the actual state back to the center of the "pipeline", thereby ensuring that no matter how the disturbance changes, the state trajectory of the closed-loop system is always constrained within the preset safety pipeline.

[0134] 3. Navigation Action Generation Module

[0135] like Figure 5 As shown, the deep reinforcement learning decision-making steps employ the TD3 algorithm, training the algorithm network through machine interaction in the environment. During training, the agent acquires the robot's global position and real-time linear velocity via odometry topics. With angular velocity The distance to obstacles ahead is obtained through lidar point cloud data.

[0136] The environmental information perceived by the intelligent agent includes: the Euclidean distance between the robot's current position and the global target point. The relative angle between the robot's current orientation and the target point. The linear velocity of the robot in its current state With angular velocity And the nearest obstacle distance detected by the lidar in multiple pre-divided sectors.

[0137] After receiving the aforementioned state information, the agent inputs it into a deep reinforcement learning network built based on the TD3 algorithm to calculate navigation actions. The TD3 algorithm uses two independent Critic networks (denoted as Q1 and Q2) to estimate the value of "state-action" pairs. Each Critic network has a corresponding target network. When calculating the target Q-value used to update the Critic network, the minimum of the two target Q-network outputs is taken. This effectively counteracts the overestimation problem that a single Q-network might produce, as the policy network tends to select actions with overestimated values. Secondly, the updates to the policy network and target network are delayed, allowing the Q-network a buffer time to stabilize and preventing policy updates due to unstable value estimations. Finally, a small amount of noise is added to the target action and pruned to smooth it out, reducing sharp peaks and overfitting in the Q-network on certain actions, thereby reducing estimation errors and variance, and thus improving the stability and efficiency of learning. Through these three mechanisms, the stability and convergence efficiency of the agent learning navigation strategies in complex environments are significantly improved.

[0138] To guide the agent's navigation behavior in a location-specific environment in a way that is both efficient and safe, the reward function comprehensively considers navigation efficiency, motion balance, obstacle avoidance safety, and task completion, in order to optimize the robot's autonomous navigation performance. The specific design is as follows:

[0139] Reward for achieving the goal: The Euclidean distance from the target point when the robot successfully reaches the target point. Less than the threshold The system will provide a large, fixed positive reward. This reward aims to incentivize robots to complete navigation tasks as quickly as possible, thereby improving the overall task completion rate.

[0140] Collision penalty: When the robot collides with an obstacle during navigation, the distance to the obstacle is less than a threshold. The system will immediately apply a large fixed negative reward. This penalty signal prompts the robot to learn to avoid dangerous behaviors, thereby reducing the probability of collisions and improving the safety of system operation;

[0141] Motion smoothness reward: Under normal circumstances where neither the target is reached nor a collision occurs, to ensure navigation efficiency and robot stability, the reward function performs a two-way balance on motion smoothness: considering the linear velocity component of the robot's motion. Positive weighting is applied to encourage the robot to maintain a reasonable forward speed and avoid task delays caused by low speed; the angular velocity component in the robot's movements is also considered. Negative weighting of absolute values This is to suppress unnecessary sharp turns and optimize motion stability and path feasibility;

[0142] Obstacle avoidance safety bonus: The minimum distance between the robot and obstacles is obtained through laser sensors. Construct the distance penalty function:

[0143] ;

[0144] This function only takes effect when the obstacle is less than 1 meter away, by introducing a penalty term. Enhance the robot's active obstacle avoidance capabilities to prevent collisions;

[0145] The complete reward function is defined below and is used to systematically optimize the robot's autonomous navigation performance:

[0146] ;

[0147] 4. Confidence Assessment Module

[0148] like Figure 6 As shown, confidence assessment is a quantitative estimate of the safety of actions generated by an agent. Through multivariate feature input analysis, it outputs the mean confidence score, which characterizes the safety level of the current environment, and the variance of its prediction uncertainty.

[0149] The model takes a 5-dimensional feature vector as input. This vector integrates the robot's state, real-time perception information, and dynamic obstacle information, where... The standard deviation of the Actor network action characteristics in TD3 reflects the exploratory and uncertain nature of the agent's decision-making in the current state; This indicates the minimum obstacle distance in the current lidar scan data, indirectly indicating the threat level of the obstacle to the agent. This indicates the predicted collision time based on the current speed and distance to the obstacle. Indicates the number of dynamic obstacles. This indicates the proportion of dynamic obstacles. The determination of dynamic obstacles is based on the following rules: For the i-th lidar sector, if the distance change between two consecutive sampling times... Greater than the machine's own displacement With a tiny tolerance The sum of, among which If so, the obstacle in that sector is determined to be dynamic;

[0150] The core of confidence assessment is the use of a Bayesian neural network with Monte Carlo Dropout. This network randomly drops some neurons during both the training and inference phases, approximating Bayesian posterior sampling of the network weights, thus enabling the model to estimate prediction uncertainty.

[0151] Let the BNN model be a function ,in These are network weights. For the same input... Perform N independent forward propagations. Due to the randomness of Dropout, each inference is equivalent to sampling a sub-model from the weight posterior distribution, thus obtaining N scalar outputs of safety prediction. ,in A higher confidence level indicates that the model considers the current environment safer. The final output is the mean confidence score. Defined as the average of these N predictions:

[0152] ;

[0153] The representative model represents the average expectation of security.

[0154] Meanwhile, the variance of the uncertainty in these N predictions is:

[0155] ;

[0156] This variance directly reflects the degree of disagreement between the prediction results of different sub-models, that is, the model's confidence in this prediction. The larger the variance, the higher the uncertainty.

[0157] During the training of this Bayesian neural network, the training dataset }, where the tag The immediate safety value is automatically generated by heuristic rules based on the current reward and state characteristics. If a collision occurs, then... If the goal is reached, then Other cases are determined according to Normalize it to For intervals, the closer the distance, the closer the true value of the safety factor is to 1;

[0158] The training objective of BNN is to minimize the predicted value. Safety label The mean squared error between the values ​​is calculated, while Dropout implicitly performs Bayesian inference on the weights. Its loss function... for:

[0159] ;

[0160] By iteratively optimizing this loss function using stochastic gradient descent, prediction errors are reduced while also improving the network weights. The updated model can map the multidimensional features of the input into a robust confidence estimate. The mean confidence and variance of the output will provide key decision-making basis for subsequent human-machine collaborative optimization and Tube MPC security correction.

[0161] 5. Weighted Experience Priority Sampling Module

[0162] The single experience pool priority sampling mechanism is specifically manifested in the differentiated storage and preference learning of human-machine collaborative experience and pure machine experience. By dynamically assigning differentiated sampling weights to experiences of different types and qualities in a unified experience replay buffer, and performing priority sampling according to the weights, the deep reinforcement learning network is guided to learn from high-quality experiences more efficiently.

[0163] To achieve efficient integration and utilization of human-machine collaboration and pure machine experience, during the experience storage phase, both types of experience are uniformly stored in a fixed maximum capacity. Experience replay buffer It is used to store all state transition data generated during navigation. One type is human-machine collaborative optimization experience (generated by human-assisted decision-making, containing more valuable action instructions), and the other is purely machine autonomous exploration experience (generated by the agent through independent trial and error, exploring a wider range of environmental scenarios). Each experience... Encapsulate this into a 7-tuple data structure, denoted as:

[0164] ;

[0165] in Indicates the current state of the environment. This refers to the final action actually performed (including optimized human-machine collaboration and pure machine decision-making). This indicates the immediate reward obtained after performing the action. This indicates the state to which the transition occurs. Indicates whether the round ends. This indicates the experience type label, used to distinguish whether the experience originated from human-computer collaborative decision-making. ) and pure machine experience ( ), This represents the dynamic sampling weight assigned to this experience, which determines the probability of this experience being sampled during the training process;

[0166] The initialization and dynamic adjustment of sampling weights are crucial. To give greater emphasis to human-computer collaboration experience during training, the first step is to base the weights on the type of experience. Weights Assignment:

[0167] ;

[0168] in This indicates that the initial weight of human-machine collaborative experience is significantly higher than that of pure machine experience. This ensures that in the early stages of training, human-machine collaborative experience receives higher sampling attention because it usually contains better or safer prior human knowledge.

[0169] To further refine the differentiation of experience quality, especially in screening human-machine collaborative experience, the system introduces a reward-based weight fine-tuning mechanism. For each piece of human-machine collaborative experience, its final weight... It will be based on the instant rewards it receives. Perform dynamic scaling:

[0170] ;

[0171] in The basic weighting coefficients for human-machine collaboration experience , These represent the weighting coefficients for enhancing and weakening human-computer experience, respectively. , Successful human-machine collaboration experiences that yield positive rewards will be given greater importance, while collaboration experiences that yield negative rewards will be suppressed.

[0172] This allows deep reinforcement learning to effectively learn from human-interventional experiences using this experience pool, focusing more on learning efficient and safe strategies, thereby accelerating the learning process and improving strategy performance.

[0173] During training, from the buffer When sampling a small batch of experience to update network parameters, the system employs a weight-based priority sampling strategy. For the current data in the buffer... 1 experience, each experience Probability of being sampled By its weight The proportion of all empirical weights in the total is determined by:

[0174] ;

[0175] Ensuring that human-machine collaborative experience with higher weights is more likely to be used in network training allows the model to learn optimization strategies brought about by human intervention more efficiently in complex and dynamic environments, ultimately improving the robot's target navigation ability and robustness in complex scenarios.

[0176] This probability distribution ensures that high-weighted experiences (typically high-quality human-machine collaborative experiences) have a higher probability of being selected for training. The sampled experience data will be used simultaneously to update the TD3 network parameters of the deep reinforcement learning agent and the parameters of the Bayesian neural network model. Through this biased learning, the agent can not only absorb effective human navigation experience more quickly, but also learn from autonomous navigation and local exploration, ultimately enabling the navigation system to converge to a high-performance, highly robust strategy more quickly in complex dynamic environments.

[0177] Example 2:

[0178] The present invention also provides a method for applying to the human-machine cooperative autonomous navigation system based on multi-constraint optimization as described above, comprising the following steps:

[0179] S1. Obtain the environmental state of the mobile robot at the current moment, and input the environmental state into the navigation action generation network to obtain the machine navigation action;

[0180] S2. Obtain manual operation instructions and convert the manual operation instructions into manual navigation actions that include linear velocity components and angular velocity components;

[0181] S3. Input the environmental state, the action characteristics of the machine navigation action, the obstacle distance characteristics, the predicted collision time characteristics, and the dynamic obstacle characteristics into a Bayesian neural network with a random inactivation inference mechanism. Obtain a safety prediction sequence through multiple forward inferences, and calculate the mean safety confidence and the variance of prediction uncertainty based on the safety prediction sequence.

[0182] S4. Construct an action fusion optimization target based on whether the manual operation instruction is detected. The action fusion optimization target includes a machine action deviation item, a smoothing item for the action executed at the previous moment, and a safety deceleration item. When the manual operation instruction is detected, it further includes a manual action deviation item. The weight of the machine action deviation item is positively correlated with the mean of the safety confidence level, and the weights of the manual action deviation item and the safety deceleration item are negatively correlated with the mean of the safety confidence level.

[0183] S5. Within the feasible region that includes linear velocity constraints, angular velocity constraints, curvature constraints, and safe velocity constraints based on predicted collision time, the action fusion optimization objective is solved by projection gradient descent to obtain the fused execution action.

[0184] S6. Using the fusion execution action as a reference input, generate a reference trajectory in the prediction time domain, and construct a bounded perturbation set based on the prediction uncertainty variance;

[0185] S7. Construct a robust invariant set based on the bounded perturbation set and feedback gain, and compress the nominal state constraints and nominal input constraints according to the robust invariant set;

[0186] S8. Solve the Tube MPC optimization problem under nominal dynamic constraints, compressed nominal state constraints, and compressed nominal input constraints to obtain the nominal control sequence.

[0187] S9. Obtain the real-time actual state of the mobile robot, calculate the state deviation between the real-time actual state and the nominal state, and superimpose the first control quantity in the nominal control sequence with the feedback correction quantity generated based on the state deviation to obtain the final control command sent to the mobile robot.

[0188] S10. Store the state transition data generated during navigation into the same experience replay buffer. The state transition data includes the current environment state, fused execution action, immediate reward, next moment environment state, round termination flag, experience type label, and dynamic sampling weight. Determine the dynamic sampling weight according to the experience type label and the immediate reward, and determine the sampling probability according to the dynamic sampling weight, so as to update the navigation action generation network and the Bayesian neural network based on the experience obtained by sampling.

[0189] This invention's intelligent agent acquires environmental information through sensors. On one hand, a deep reinforcement learning module generates preliminary actions based on the current state; on the other hand, a confidence module quantifies the uncertainty of the current environment and outputs the confidence level of the current environment. Subsequently, based on keyboard input (whether there is human intervention), the objective function is divided and optimized. When no human intervention is present, the objective function is constructed primarily based on tracking machine actions. When a human issues a command, then in the objective function... Human experience terms are incorporated, and the weights of machine actions and human commands are dynamically balanced based on confidence levels. Building upon this, the projective gradient descent method is used to solve for physically feasible optimal actions under multiple constraints, including velocity, curvature, and dynamic safety velocity. To further improve the robustness of the execution layer, this method introduces a Tube MPC correction module to... Using the uncertainty variance of the confidence level assessment output as a reference input, a dynamic perturbation boundary is constructed, and the final action is then obtained through optimization. This ensures that even under environmental disturbances, the actual state remains constrained within a safe conduit. The above process iterates continuously until the agent reaches the target endpoint, thus achieving safe collaborative navigation from perception and decision-making to execution.

[0190] Example 4:

[0191] This embodiment is applied to the autonomous navigation scenario of mobile robots. The method outputs machine navigation actions through the navigation action generation module, generates human navigation actions and experience type labels through the human interaction module, and outputs the mean of safety confidence and the variance of prediction uncertainty through the confidence assessment module. The mean of safety confidence is used for human-machine collaborative optimization, and the variance of prediction uncertainty is used for robust correction of tubular model predictive control. At the same time, the weighted experience priority sampling module performs weighted sampling on the experience data generated during the navigation process to update the navigation action generation module and the confidence assessment module, thereby forming a closed-loop navigation process.

[0192] Within a control cycle, the mobile robot first acquires the current environmental state, which includes the distance between the robot's current position and the target point, the relative angle between the robot's current orientation and the target point's direction, the robot's current linear velocity, current angular velocity, and the distance to the nearest obstacle within multiple LiDAR sectors. The navigation action generation module inputs this environmental state into the navigation action generation network to obtain the machine's navigation actions. These actions include linear velocity and angular velocity components, representing the forward and turning speeds that the navigation action generation module recommends the mobile robot to execute under the current environmental state.

[0193] The human interaction module receives human operation instructions and converts them into human navigation actions.

[0194] Specifically, the human interaction module increases or decreases the linear velocity component in the manual navigation action according to the linear velocity increment adjustment command; and increases or decreases the angular velocity component in the manual navigation action according to the angular velocity increment adjustment command. Thus, the manual navigation action includes a manual linear velocity component and a manual angular velocity component, used to represent the forward speed and turning speed that the operator wants the mobile robot to execute at the current moment.

[0195] The human interaction module also generates experience type labels based on the human-machine collaboration mode activation / deactivation signals and whether human operation commands participate in action fusion. When the human-machine collaboration mode is enabled and the human operation commands actually participate in subsequent action fusion, the experience generated in the corresponding control cycle is marked as human-machine collaboration experience; when no human operation commands are detected participating in action fusion, the experience generated in the corresponding control cycle is marked as pure machine experience. Through this experience type label, the subsequent weighted experience priority sampling module can distinguish between experience formed with human assistance and experience formed through autonomous machine exploration.

[0196] The confidence assessment module constructs input features based on the current environmental state, action characteristics, obstacle distance characteristics, predicted collision time characteristics, and dynamic obstacle characteristics. These input features include the discreteness of the actions or action characteristics output by the policy network in the navigation action generation module, the minimum obstacle distance in the LiDAR scan data, the predicted collision time obtained based on the current speed and obstacle distance, the proportion of dynamic obstacles, and the number of dynamic obstacles.

[0197] For any LiDAR sector, the confidence assessment module compares the distance change of that sector at adjacent sampling times with the displacement of the mobile robot itself. When the distance change of that sector is greater than the sum of the mobile robot's displacement and a preset tolerance, it is determined that a dynamic obstacle exists within that sector. The number of dynamic obstacles is determined based on the number of sectors identified as having dynamic obstacles; the proportion of dynamic obstacles is determined based on the ratio between the number of sectors identified as having dynamic obstacles and the total number of sectors participating in the detection.

[0198] The confidence assessment module includes a Bayesian neural network with a random inactivation inference mechanism. For the same input feature, the Bayesian neural network performs multiple forward inferences. Since the neurons randomly inactivated in each forward inference process are different, multiple inferences will yield multiple safety prediction results. The confidence assessment module averages the multiple safety prediction results and uses the average value as the safety confidence mean. Simultaneously, the confidence assessment module calculates the dispersion of the multiple safety prediction results relative to this average value and uses this dispersion as the prediction uncertainty variance. The safety confidence mean represents the safety level of the current environment; a higher value indicates a safer environment. The prediction uncertainty variance represents the degree of confidence the Bayesian neural network has in the current safety prediction result; a larger variance indicates greater prediction uncertainty.

[0199] The human-machine collaborative optimization module receives machine navigation actions, manual navigation actions, the average safety confidence score, and the actions executed at the previous moment, and constructs an action fusion optimization target. This target is used to balance machine navigation actions, manual navigation actions, action continuity, and safety deceleration to obtain the fused execution actions at the current moment.

[0200] When no human intervention commands are detected, the human-machine collaborative optimization module constructs a motion fusion optimization objective that does not include human action deviations. This objective includes machine action deviations, a smoothing term for the action executed in the previous moment, and a safety reduction term. Specifically, the machine action deviations constrain the difference between the fused action and the machine navigation action; the smoothing term for the action executed in the previous moment constrains the magnitude of change between the current fused action and the action executed in the previous moment; and the safety reduction term suppresses the amplitude of the fused action when the environmental safety level is low. A higher mean safety confidence level results in a larger proportion of machine action deviations, making the fused action closer to the machine navigation action; a lower mean safety confidence level results in a larger proportion of safety reduction terms, causing the mobile robot to actively reduce its movement speed.

[0201] When a manual operation command is detected, the human-machine collaborative optimization module constructs an action fusion optimization objective that includes a human action deviation term. This objective comprises machine action deviation, human action deviation, smoothing of the action executed at the previous moment, and safety deceleration term. The human action deviation term constrains the difference between the fused execution action and the manual navigation action. A higher mean safety confidence level results in a larger proportion of machine action deviation, making the fused execution action more inclined towards machine navigation actions; a lower mean safety confidence level results in a larger proportion of human action deviation and safety deceleration term, making the fused execution action more inclined towards manual navigation actions, while simultaneously suppressing excessively large action amplitudes. Therefore, the system can rely more on autonomous machine decision-making when the environment is safe, and refer more to manual operation commands when the environment is dangerous or uncertain.

[0202] In the process of human-machine collaborative optimization, the fused execution actions need to meet feasible region constraints. These feasible region constraints include linear velocity constraints, angular velocity constraints, curvature constraints, and safe velocity constraints. Linear velocity constraints limit the linear velocity components in the fused execution actions; angular velocity constraints limit the angular velocity components; curvature constraints limit the instantaneous curvature during the mobile robot's turning process; and safe velocity constraints dynamically limit the maximum permissible linear velocity based on the predicted collision time. A shorter predicted collision time indicates a higher risk of collision between the mobile robot and the obstacle, and a lower maximum permissible linear velocity limited by the safe velocity constraint; conversely, a longer predicted collision time indicates a lower collision risk, and a higher maximum permissible linear velocity allowed by the safe velocity constraint.

[0203] The human-machine collaborative optimization module employs a projective gradient descent method combined with a sufficient descent line search criterion to solve for the action fusion optimization objective. In each iteration, the module first calculates the descent direction of the action fusion optimization objective relative to the linear and angular velocity components at the current action point, then updates the action along this descent direction and checks whether the updated action satisfies the linear velocity constraint, angular velocity constraint, curvature constraint, and safe velocity constraint. If the updated action does not satisfy any constraint, the module pulls the updated action back into the feasible region jointly defined by the above constraints. After multiple iterations, the module obtains a fused execution action that satisfies multiple constraints and minimizes the action fusion optimization objective.

[0204] The tubular model prediction control correction module receives the fusion execution action and uses the fusion execution action as a reference input to generate a reference trajectory in the prediction time domain.

[0205] Specifically, the tubular model predictive control correction module uses the current actual state as the initial state of the reference trajectory, maintains the fused execution action as the reference control input in the prediction time domain, and progressively recursively obtains the reference state for each prediction step through the mobile robot's kinematic model. The reference state for each prediction step includes the mobile robot's position, orientation, linear velocity, and angular velocity.

[0206] Simultaneously, the tubular model predictive control correction module receives the prediction uncertainty variance output by the confidence assessment module and constructs a bounded perturbation set based on this variance. This bounded perturbation set represents the range of state deviations that may be caused by model mismatch, sensor noise, and dynamic obstacle disturbances. A larger prediction uncertainty variance indicates greater uncertainty in the Bayesian neural network's prediction of current environmental safety; the tubular model predictive control correction module accordingly expands the perturbation boundary to make the control strategy more conservative. Conversely, a smaller prediction uncertainty variance indicates more stable current prediction results; the tubular model predictive control correction module accordingly narrows the perturbation boundary to reduce the impact of overly conservative control on navigation efficiency.

[0207] After constructing a bounded perturbation set, the tubular model predictive control correction module constructs a robust invariant set based on the closed-loop system relationship formed by the system matrix, input matrix, and feedback gain, combined with the bounded perturbation set. This robust invariant set represents the maximum possible deviation range of the actual state from the nominal state under conditions where the perturbation persists and the feedback correction continues. The tubular model predictive control correction module then contracts the actual state constraints based on the robust invariant set to obtain the nominal state constraints; and contracts the actual input constraints based on the feedback gain and the robust invariant set to obtain the nominal input constraints. Through this contraction process, even if the actual state and actual input are affected by perturbations, they can still remain within the robot's actual safety constraints.

[0208] Under the compressed nominal state and input constraints, the tubular model predictive control correction module solves the optimization problem in the prediction time domain, obtaining a set of nominal control sequences. This optimization problem aims to simultaneously reduce the deviation between the nominal trajectory and the reference trajectory, suppress drastic changes between adjacent control inputs, and make the state at the end of the prediction time domain as close as possible to the desired state. After optimization, the tubular model predictive control correction module takes the first control variable in the nominal control sequence as the feedforward control variable.

[0209] Subsequently, the tubular model predictive control correction module acquires the real-time actual state of the mobile robot and calculates the state deviation between the real-time actual state and the current nominal state. Based on the state deviation and feedback gain, the tubular model predictive control correction module generates a feedback correction amount and adds it to the feedforward control amount to obtain the final control command. This final control command is sent to the mobile robot for execution. Through this feedback correction method, even with model mismatch, sensor noise, or dynamic obstacle disturbances, the actual movement trajectory of the mobile robot can still be confined within a preset safety channel.

[0210] After the mobile robot executes the final control command, the weighted experience priority sampling module stores the state transition data generated in the current control cycle into the same experience replay buffer. Each state transition data includes the current environment state, the actual action executed, the immediate reward, the environment state at the next moment, the round termination flag, the experience type label, and the dynamic sampling weight. The actual action executed can be the fused action output by the human-robot collaborative optimization module, or it can be the final control command after correction by the tubular model predictive control, depending on the training objective of the experience replay buffer.

[0211] The weighted experience priority sampling module first determines the basic sampling weights based on the experience type label. When the experience type label indicates that the experience is human-machine collaborative experience, its basic sampling weight is set higher than that of pure machine experience; when the experience type label indicates that the experience is pure machine experience, its basic sampling weight is set to the default weight. Therefore, human-machine collaborative experience receives higher sampling attention in the early stages of training.

[0212] Based on the basic sampling weights, the weighted experience priority sampling module adjusts the dynamic sampling weights of human-machine collaborative experience according to the immediate reward. When the immediate reward corresponding to the human-machine collaborative experience is positive, it indicates that the human intervention has a positive effect on the navigation effect, and the weighted experience priority sampling module increases the dynamic sampling weight of the experience according to a preset enhancement rule. When the immediate reward corresponding to the human-machine collaborative experience is negative, it indicates that the human intervention may lead to collisions, deviation from the target, or inefficient movement, and the weighted experience priority sampling module decreases the dynamic sampling weight of the experience according to a preset weakening rule. For pure machine experience, the weighted experience priority sampling module retains its basic sampling weights, allowing the mobile robot to still learn from autonomous exploration experience.

[0213] During training, the weighted experience priority sampling module determines the sampling probability based on the dynamic sampling weights of each experience in the experience replay buffer. The higher the dynamic sampling weight of any experience, the larger its proportion in the total sum of all experience weights, and the higher its probability of being selected for training; conversely, the lower the dynamic sampling weight of any experience, the smaller its proportion in the total sum of all experience weights, and the lower its probability of being selected for training. The weighted experience priority sampling module extracts small batches of experience from the same experience replay buffer according to the above sampling probabilities.

[0214] The small-batch experience is used on the one hand to update the navigation action generation network in the navigation action generation module, so that the navigation action generation network learns the navigation strategy based on the environmental state, the action performed, the immediate reward, and the environmental state at the next moment; on the other hand, it is used to update the Bayesian neural network in the confidence evaluation module, so that the Bayesian neural network learns the safety prediction relationship based on the state features, action features, obstacle distance features, predicted collision time features, and dynamic obstacle features.

[0215] For the navigation action generation network, the mini-batch experience is used to update its policy network and value network, enabling the policy network to gradually output machine navigation actions that yield higher rewards and meet navigation safety requirements. For the Bayesian neural network, the mini-batch experience is used to generate or update safety labels, and the network parameters are updated based on the difference between the safety prediction results and the safety labels, allowing the Bayesian neural network to continuously refine the estimation results of the mean safety confidence and the variance of prediction uncertainty.

[0216] In this embodiment, confidence assessment results are incorporated into both human-machine collaborative optimization and tubular model predictive control robustness correction, and navigation execution results are fed back to the same experience playback buffer. Weighted experience priority sampling simultaneously updates the navigation action generation network and the Bayesian neural network, thus forming a closed-loop navigation process of "action generation, confidence assessment, action fusion, robustness correction, and experience update." This closed-loop process enables the mobile robot to adaptively adjust its control strategy based on the current environmental safety level and predictive uncertainty, even under conditions of dynamic obstacles and environmental uncertainty. Furthermore, prioritizing the learning of high-quality experience improves the training efficiency and operational stability of the navigation strategy.

[0217] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A human-machine collaborative autonomous navigation system based on multi-constraint optimization, characterized in that, include: The navigation action generation module is used to output machine navigation actions based on the environmental state of the mobile robot; The human interaction module is used to receive human operation instructions and convert them into human navigation actions. It is also used to generate experience type labels that distinguish between human-machine collaborative experience and pure machine experience. The confidence assessment module includes a Bayesian neural network with a random deactivation inference mechanism, which is used to output the mean of safety confidence and the variance of prediction uncertainty based on the environmental state, action features, obstacle distance features, predicted collision time features and dynamic obstacle features. The human-machine collaborative optimization module is used to construct an action fusion optimization target based on the machine navigation action, the human navigation action, the average safety confidence value and the action executed at the previous moment, and solve the fused execution action within a feasible region that includes linear velocity constraints, angular velocity constraints, curvature constraints and safety velocity constraints; The Tube MPC correction module is used to generate a reference trajectory with the fused execution action as a reference input, construct a bounded perturbation set based on the prediction uncertainty variance, construct a robust invariant set and a compaction constraint based on the bounded perturbation set, and output the final control command through nominal control sequence solution and state deviation feedback compensation. The weighted experience priority sampling module is used to store human-machine collaborative experience and pure machine experience generated during navigation into the same experience playback buffer, determine dynamic sampling weights based on experience type labels and immediate rewards, and determine sampling probabilities according to the dynamic sampling weights to update the navigation action generation module and the confidence evaluation module.

2. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 1, characterized in that, The navigation action generation module is used to input the distance between the current position of the mobile robot and the target point, the relative angle between the current orientation of the mobile robot and the direction of the target point, the current linear velocity of the mobile robot, the current angular velocity of the mobile robot, and the distance of the nearest obstacle in multiple lidar sectors as the environmental state into the navigation action generation network, and output the machine navigation action containing the machine linear velocity component and the machine angular velocity component.

3. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 2, characterized in that, The navigation action generation network is a deep reinforcement learning network built based on the TD3 algorithm. The deep reinforcement learning network includes a policy network for outputting the machine navigation actions. During the training process, the policy network is trained by taking the smaller target value through a dual-value network, delaying policy updates, and smoothing the target actions.

4. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 1, characterized in that, The human interaction module is used to convert the first type of human operation command into linear velocity increment adjustment, the second type of human operation command into angular velocity increment adjustment, and the third type of human operation command into human-machine collaboration mode activation / deactivation signal, so as to form the human navigation action based on the linear velocity increment adjustment and the angular velocity increment adjustment, and generate the experience type label based on the human-machine collaboration mode activation / deactivation signal and whether the human operation command participates in action fusion.

5. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 1, characterized in that, The confidence assessment module is used to form an input feature vector by combining the standard deviation of the action or action feature output by the policy network in the navigation action generation module, the minimum obstacle distance, the predicted collision time, the proportion of dynamic obstacles, and the number of dynamic obstacles. The number of dynamic obstacles is determined based on the number of sectors where the change in distance between LiDAR sectors at adjacent sampling times is greater than the sum of the mobile robot's own displacement and a preset tolerance. The proportion of dynamic obstacles is determined based on the ratio of the number of sectors identified as dynamic obstacles to the total number of sectors participating in the detection.

6. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 5, characterized in that, The confidence assessment module is used to perform multiple random inactivation forward inferences on the same input feature vector to obtain multiple security prediction values, and the average of the multiple security prediction values ​​is used as the security confidence mean, and the sample variance of the multiple security prediction values ​​relative to the security confidence mean is used as the prediction uncertainty variance.

7. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 1, characterized in that, The human-machine collaborative optimization module is used to construct an action fusion optimization target that includes machine action deviation, previous action smoothing, and safety deceleration when no human operation command is detected, and to construct an action fusion optimization target that includes machine action deviation, human action deviation, previous action smoothing, and safety deceleration when a human operation command is detected. The weight of the machine action deviation is positively correlated with the mean of the safety confidence level, while the weights of the human action deviation and the safety deceleration are negatively correlated with the mean of the safety confidence level.

8. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 7, characterized in that, The human-machine collaborative optimization module is used to solve the action fusion optimization objective by using the projection gradient descent method combined with Armijo line search. When the iteratively updated action does not satisfy the linear velocity constraint, angular velocity constraint, curvature constraint or safe velocity constraint, the iteratively updated action is projected back to the feasible region jointly defined by the linear velocity constraint, angular velocity constraint, curvature constraint and safe velocity constraint.

9. The human-machine cooperative autonomous navigation system based on multi-constraint optimization according to claim 1, characterized in that, The Tube MPC correction module is used to construct the bounded disturbance set as a set whose disturbance norm is no greater than the product of the prediction uncertainty variance and the scaling factor. Based on the closed-loop system matrix formed by the system matrix, the input matrix, and the feedback gain, and the bounded disturbance set, a robust invariant set is constructed. The actual state constraints are compressed according to the robust invariant set to obtain the nominal state constraints. The actual input constraints are compressed according to the feedback gain and the robust invariant set to obtain the nominal input constraints. The nominal control sequence is then solved under the compressed constraints.

10. A method for applying to the human-machine cooperative autonomous navigation system based on multi-constraint optimization as described in any one of claims 1-9, characterized in that, Includes the following steps: S1. Obtain the environmental state of the mobile robot at the current moment, and input the environmental state into the navigation action generation network to obtain the machine navigation action; S2. Obtain manual operation instructions and convert the manual operation instructions into manual navigation actions that include linear velocity components and angular velocity components; S3. Input the environmental state, the action characteristics of the machine navigation action, the obstacle distance characteristics, the predicted collision time characteristics, and the dynamic obstacle characteristics into a Bayesian neural network with a random inactivation inference mechanism. Obtain a safety prediction sequence through multiple forward inferences, and calculate the mean safety confidence and the variance of prediction uncertainty based on the safety prediction sequence. S4. Construct an action fusion optimization target based on whether the manual operation instruction is detected. The action fusion optimization target includes a machine action deviation item, a smoothing item for the action executed at the previous moment, and a safety deceleration item. When the manual operation instruction is detected, it further includes a manual action deviation item. The weight of the machine action deviation item is positively correlated with the mean of the safety confidence level, and the weights of the manual action deviation item and the safety deceleration item are negatively correlated with the mean of the safety confidence level. S5. Within the feasible region that includes linear velocity constraints, angular velocity constraints, curvature constraints, and safe velocity constraints based on predicted collision time, the action fusion optimization objective is solved by projection gradient descent to obtain the fused execution action. S6. Using the fusion execution action as a reference input, generate a reference trajectory in the prediction time domain, and construct a bounded perturbation set based on the prediction uncertainty variance; S7. Construct a robust invariant set based on the bounded perturbation set and feedback gain, and compress the nominal state constraints and nominal input constraints according to the robust invariant set; S8. Solve the Tube MPC optimization problem under nominal dynamic constraints, compressed nominal state constraints, and compressed nominal input constraints to obtain the nominal control sequence. S9. Obtain the real-time actual state of the mobile robot, calculate the state deviation between the real-time actual state and the nominal state, and superimpose the first control quantity in the nominal control sequence with the feedback correction quantity generated based on the state deviation to obtain the final control command sent to the mobile robot. S10. Store the state transition data generated during navigation into the same experience replay buffer. The state transition data includes the current environment state, fused execution action, immediate reward, next moment environment state, round termination flag, experience type label, and dynamic sampling weight. Determine the dynamic sampling weight according to the experience type label and the immediate reward, and determine the sampling probability according to the dynamic sampling weight, so as to update the navigation action generation network and the Bayesian neural network based on the experience obtained by sampling.

Images