Safe navigation method and device for mobile robot in dense crowd environment

Abstract

The present invention relates to a safe navigation method and device for a mobile robot in a dense crowd environment. The method includes: estimating the joint state of a mobile robot and a pedestrian, obtaining a safe state constraint through a value network based on deep reinforcement learning, obtaining a safe action constraint through a discrete generalized speed obstacle, solving an optimization problem for a safety filter according to the safe state constraint and the safe action constraint, so as to obtain an optimal reference action after correcting a reference input, and when there is no feasible solution to the optimization problem, by adding a local navigation target as a soft constraint and canceling the safe action constraint, re-solving a new optimization problem, and when there is still no feasible solution to the optimization problem, canceling the safe state constraint, re-solving a new optimization problem, and finally inputting the corrected optimal reference action into the mobile robot. The present invention improves the safety of a learning-based navigation method by correcting the optimal action solved by a learning-based navigation method.

Description

Technical Field

[0001] The invention relates to a safe navigation method and device for a mobile robot in a densely populated environment, and belongs to the technical field of artificial intelligence. Background Art

[0002] The navigation goal of a mobile robot is to plan a safe trajectory with high efficiency and no collision. In a densely populated environment, a mobile robot needs to observe the surrounding environment, accurately estimate the position and speed of pedestrians around it, and make decisions to plan a trajectory that not only meets the robot's own motion constraints, but also can safely avoid pedestrians and reach the destination in a shorter time.

[0003] There are two types of methods here. One is a reaction-based navigation method, and the other is a learning-based navigation method. The former treats pedestrians as ordinary dynamic obstacles, and mainly uses the concept of speed obstacles VO (VelocityObstacles) to limit the speed action space of the mobile robot, solves the optimization problem online, makes the optimal collision-free action based on the current state of the mobile robot and pedestrians, and executes the action. Among them, the RVO (Reciprocal Velocity Obstacles) method achieves the collision-free requirement by selecting the optimal action in the collision-free speed action space, and improves the VO method. The reaction-based navigation method does not consider the future state of the surrounding pedestrians, is short-sighted, and leads to problems such as long navigation time and collisions.

[0004] Learning-based navigation methods can effectively solve the short-sightedness problem of reaction-based navigation methods. This type of method can generally be divided into two steps. First, the pedestrian status is encoded, and then the optimal action strategy is trained through deep reinforcement learning. When dealing with a variable number of pedestrians, some existing methods only consider obstacle avoidance for pedestrians closest to the robot, or encourage social interaction between the robot and pedestrians in the reward function so that the value network learns to choose socially interactive actions during training. However, since the value network uses a fully connected network, its input size is fixed, so it can only handle obstacle avoidance problems for up to four neighboring pedestrians. Alternatively, the states of all pedestrians and robots are aggregated using an LSTM network, and the interactive information useful for robot navigation is expressed in the form of hidden variables. However, LSTM-RL only uses the distance between the robot and each pedestrian as the input order of the LSTM network. The closer the state of the pedestrian is to the robot, the later it is input into the LSTM network. This means that LSTM-RL believes that the closer the pedestrian is to the robot, the greater the impact on the robot's navigation. In fact, the impact of pedestrians on robot navigation is not necessarily inversely proportional to the distance. Factors such as the pedestrian's walking speed and direction and the robot's own target orientation should also be considered. Alternatively, a SARL method is proposed, which uses a self-attention mechanism to replace the LSTM network to jointly encode the interactions between pedestrians and pedestrians, and between pedestrians and robots. During training, the influence weights of all pedestrians on the robot can be learned, and the pedestrians with the greatest impact on the robot's navigation can be avoided first. Alternatively, a graph neural network based on an attention mechanism is used to encode the interactions between pedestrians and pedestrians, and between pedestrians and robots, and to predict the future trajectories of pedestrians. Finally, the PPO algorithm is used to train the navigation strategy.

[0005] The idea of ​​the above method is to obtain hidden variables or predict trajectories by better encoding the interaction between pedestrians and pedestrians, pedestrians and robots, so as to improve the success rate of deep reinforcement learning and reduce the collision rate. However, deep reinforcement learning cannot guarantee safety and lacks explicit and explainable safety constraints. Although the navigation method based on learning has achieved relatively good simulation results, it often cannot ensure the satisfaction of safety constraints in practical applications. In addition, when designing the reward function of deep reinforcement learning, the penalty for collision behavior is considered. When the mobile robot faces unknown and uncertain situations, it will still make dangerous behaviors, causing the mobile robot to collide with pedestrians. Therefore, it is very important to improve the safety of the navigation method based on learning. Summary of the invention

[0006] The present invention provides a safe navigation method and device for a mobile robot in a dense crowd environment, aiming to solve at least one of the technical problems existing in the prior art.

[0007] The technical solution of the present invention relates to a safe navigation method for a mobile robot in a dense crowd environment, which is applied to a mobile robot, wherein the mobile robot is provided with a perception module, a positioning module, a laser radar and a sensor. The method according to the present invention comprises the following steps:

[0008] S100, acquiring data from the laser radar and the sensor, and inputting the data into the perception module and the positioning module, estimating and outputting the state of the mobile robot and the state of the surrounding pedestrians, so as to obtain the joint state of the mobile robot and the pedestrians;

[0009] S200, according to the joint state of the mobile robot and the pedestrian, obtaining the optimal action of the mobile robot through a value network based on deep reinforcement learning, so as to serve as a reference input of the safety filter;

[0010] S300, constructing a polyhedral state constraint of the mobile robot to obtain a control invariant set as a safety state constraint;

[0011] S400, filtering out safe actions from discrete action space through discrete generalized speed barriers to obtain safe action constraints;

[0012] S500, solving an optimization problem for the safety filter according to the safety state constraint and the safety action constraint to obtain an optimal reference action after modifying the reference input;

[0013] S600, when there is no feasible solution to the optimization problem in step S500 and the number of discrete actions constrained by the safety action is zero, a new optimization problem is solved again by adding a local navigation target as a soft constraint and canceling the safety action constraint to obtain a new corrected optimal reference action;

[0014] S700, when there is no feasible solution to the optimization problem in step S600 and the state of the mobile robot at the next moment does not satisfy the safety state constraint, the safety state constraint is cancelled based on the optimization problem in step S600, and a new optimization problem is solved again to obtain a new corrected optimal reference action;

[0015] S800: Input the finally obtained corrected optimal reference action into the mobile robot as an execution instruction of the mobile robot.

[0016] Furthermore, in step 200, the reference input of the security filter is represented as follows:

[0017]

[0018] In the formula, u d represents the optimal action of the mobile robot obtained based on deep reinforcement learning, represents the value function of the value network, represents the joint state of the mobile robot and the pedestrian, where the optimal strategy π is obtained through deep reinforcement learning * :

[0019] Furthermore, in step 300, the multi-faceted state constraints of the mobile robot are expressed as follows:

[0020]

[0021] In the formula, represents the safety state constraint of the mobile robot at the kth sampling moment, x k represents the three-dimensional mobile robot state, x k =[p x ,p y ,θ] T , where (p x , p y ) represents the two-dimensional position coordinates of the mobile robot, θ represents the heading angle of the mobile robot, R 3 Represents three-dimensional space, T k With t k Represents the position of the mobile robot [p x ,p y ] T The matrix of security restrictions.

[0022] Furthermore, in step 300, the future state of the mobile robot is obtained by the following system equation:

[0023] x i∣k =Ax i∣k +Bu 0∣k ,u 0∣k =u k

[0024] In the formula, x i∣k represents the future state quantity derived from the discrete linear system at the kth sampling moment, u 0∣k represents the action at the kth moment, u k represents the optimal action of the security filter at the kth sampling moment, and A and B are system matrices.

[0025] Furthermore, in step S400, the future state of the pedestrian in the discrete generalized speed obstacle is obtained by the following system equation:

[0026]

[0027] In the formula, represents the future state of the jth pedestrian, represents the future action of the jth pedestrian.

[0028] Furthermore, in step S500, the optimization problem of the security filter is expressed as follows:

[0029]

[0030] In the formula, the first term of the objective function is To minimize the deviation between the safe action and the reference action, the second term ρ of the objective function above is 0 ||x 1|k ―u goal || 2 It is used to encourage the robot to reach the navigation target point faster. The parameter ρ0 is used to adjust the credibility of the first and second terms of the objective function in the above formula. represents the optimal reference action of the security filter, u 0∣k represents the action at the kth sampling moment, x 1∣k represents the state of the kth sampling moment recursively pushed forward into the future, x goal represents the navigation target point of the mobile robot, u i∣k represents the action used when the discrete linear system is recursively extended to the future based on the kth sampling moment, x 0∣k represents the state at the kth sampling moment, x k represents the state at the kth sampling moment, is the safety state constraint at the kth sampling moment, represents the safety action constraint, x 1∣k It represents the state of the k-th sampling moment recursively pushed forward into the future. represents the control invariant set, Represents the safety action constraints within n sampling periods.

[0031] Furthermore, in step S600, after adding the local navigation target and canceling the safety action constraint, the new optimization problem of the safety filter is expressed as follows:

[0032]

[0033] In the formula, x local_goal Represents the local navigation target point of the mobile robot. The parameter ρ1 is used to adjust the credibility of the first and second terms of the objective function in the above formula. represents a discrete action space.

[0034] Furthermore, in step S700, after canceling the security state constraint, the new optimization problem of the security filter is expressed as follows:

[0035]

[0036] Wherein, the parameter ρ2 is used to adjust the credibility of the first and second terms of the objective function in the above formula.

[0037] The technical solution of the present invention also relates to a computer-readable storage medium on which program instructions are stored, and the above-mentioned method is implemented when the program instructions are executed by a processor.

[0038] The technical solution of the present invention also relates to a safe navigation device for a mobile robot in a dense crowd environment, wherein the system includes a computer device, and the computer device includes the above-mentioned computer-readable storage medium.

[0039] The beneficial effects of the present invention are as follows:

[0040] The safe navigation method and device of a mobile robot in a dense crowd environment of the present invention, in a simulation scene of a dense crowd, uses the safety filter of the learning-based navigation method proposed by the present invention to add safety constraints, and can correct the optimal action solved by the learning-based navigation method, so that it meets the safety constraints, thereby improving the safety of the learning-based navigation method. The safe navigation method based on the safety filter is a post-module of the learning-based navigation method, which uses the optimal action given by the learning-based navigation method as a reference, and determines whether the optimal action is safe by solving an optimization problem. If it is not safe, an action with the minimum deviation from the optimal action and meeting the safety constraints is output, so that the mobile robot knows how to avoid crowded people or let pedestrians go first and reach the destination. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 is an overall flow chart of the navigation method according to the present invention.

[0042] Figure 2 It is a schematic diagram of the design mode of the state constraint according to the method of the present invention.

[0043] Figure 3 It is a schematic diagram of the design method of the safety action constraint according to the method of the present invention.

[0044] Figure 4 It is a motion trajectory simulation comparison diagram of whether the CADRL method uses the method proposed by the present invention.

[0045] Figure 5 It is a motion trajectory simulation comparison chart of whether the LSTM-RL method uses the method proposed by the present invention.

[0046] Figure 6 It is a motion trajectory simulation comparison diagram of whether the SARL method uses the method proposed by the present invention. DETAILED DESCRIPTION

[0047] The concept, specific structure and technical effects of the present invention will be clearly and completely described below in combination with the embodiments and drawings to fully understand the purpose, scheme and effect of the present invention.

[0048] It should be noted that, unless otherwise specified, when a feature is referred to as being "fixed" or "connected" to another feature, it may be directly fixed or connected to another feature, or it may be indirectly fixed or connected to another feature. The singular forms of "," "said" and "the" used herein are also intended to include the plural forms, unless the context clearly indicates otherwise. In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as those generally understood by those skilled in the art. The terms used in this specification are intended to describe specific embodiments only and are not intended to limit the invention. The term "and / or" used herein includes any combination of one or more of the related listed items.

[0049] It should be understood that, although the term first, second, third etc. may be adopted to describe various elements in the present disclosure, these elements should not be limited to these terms. These terms are only used to distinguish the same type of elements from each other. For example, without departing from the scope of the present disclosure, the first element may also be referred to as the second element, and similarly, the second element may also be referred to as the first element. The use of any and all examples or exemplary language ("for example", "such as" etc.) provided herein is only intended to better illustrate embodiments of the present invention, and unless otherwise required, the scope of the present invention will not be limited.

[0050] Reference Figure 1 , the method of the embodiment of the present invention is based on a mobile robot, which is provided with a perception module, a positioning module, a laser radar and a sensor. The control system of the ROS mobile robot sends out the three-dimensional point cloud data of the laser radar, inputs the above data into the perception module, and then uses the deep network of PointPillars to detect pedestrians, and obtains the three-dimensional position of the pedestrian and the length, width and height of the detection box. In addition, the control system of the ROS mobile robot sends out sensor data such as the laser radar and the odometer, and inputs the above data into the positioning module. Then the amcl3d positioning function package will use particle filtering to process the existing three-dimensional map and sensor data such as the laser radar and the odometer, and preliminarily estimate the robot's position, heading angle, linear velocity and angular acceleration, and finally use Kalman filtering to further estimate the robot's position and heading angle.

[0051] Reference Figures 1 to 3 In some embodiments, the safe navigation method of a mobile robot in a dense crowd environment according to the present invention comprises at least the following steps:

[0052] S100, acquiring data from the laser radar and the sensor, and inputting the data into the perception module and the positioning module, estimating and outputting the state of the mobile robot and the state of the surrounding pedestrians, so as to obtain the joint state of the mobile robot and the pedestrians;

[0053] S200, according to the joint state of the mobile robot and the pedestrian, obtaining the optimal action of the mobile robot through a value network based on deep reinforcement learning, so as to serve as a reference input of the safety filter;

[0054] S300, constructing a polyhedral state constraint of the mobile robot to obtain a control invariant set as a safety state constraint;

[0055] S400, filtering out safe actions from discrete action space through discrete generalized speed barriers to obtain safe action constraints;

[0056] S500, solving an optimization problem for the safety filter according to the safety state constraint and the safety action constraint to obtain an optimal reference action after modifying the reference input;

[0057] S600, when there is no feasible solution to the optimization problem in step S500 and the number of discrete actions constrained by the safety action is zero, a new optimization problem is solved again by adding a local navigation target as a soft constraint and canceling the safety action constraint to obtain a new corrected optimal reference action;

[0058] S700, when there is no feasible solution to the optimization problem in step S600 and the state of the mobile robot at the next moment does not satisfy the safety state constraint, the safety state constraint is cancelled based on the optimization problem in step S600, and a new optimization problem is solved again to obtain a new corrected optimal reference action;

[0059] S800: Input the finally obtained corrected optimal reference action into the mobile robot as an execution instruction of the mobile robot.

[0060] Specific implementation of step S100

[0061] The method of the embodiment of the present invention obtains the status of the mobile robot itself and the surrounding pedestrians through the perception module and the positioning module.

[0062] In one embodiment, the perception module of the embodiment of the present invention is used for pedestrian detection and pedestrian state estimation, including pedestrian detection based on PointPillars and pedestrian state estimation based on Kalman and Hungarian algorithms, so as to complete the estimation of multiple pedestrian states. Specifically, first, the control system of the ROS mobile robot sends out the three-dimensional point cloud data of the laser radar, and then uses the deep network of PointPillars to detect pedestrians, and obtains the three-dimensional position of the pedestrian and the length, width and height of the detection box; then the linear Kalman is used to estimate the pedestrian state, and the prediction result of the uniform speed model and the matching result of the Hungarian algorithm are used to update the Kalman estimator; finally, the prediction result of the Kalman estimator is used as the optimal estimate of the pedestrian state and output.

[0063] In one embodiment, the positioning module of the embodiment of the present invention is used for positioning and state estimation of the mobile robot, which includes the amcl3d positioning function package and the robot state estimation based on Kalman, so as to complete the estimation of the robot state. Specifically, first, the ROS mobile robot system sends sensor data such as laser radar and odometer; then the amcl3d positioning function package uses particle filtering to process the existing three-dimensional map and sensor data such as laser radar and odometer, and preliminarily estimates the position, heading angle, linear velocity and angular velocity of the mobile robot; finally, the Kalman filter is used to further estimate the position and heading angle of the mobile robot and output them.

[0064] In one embodiment, in the mobile robot coordinate system, the mobile robot's own state and the state of the surrounding pedestrians are combined to form a joint state Specifically, the mobile robot coordinate system is defined as: the mobile robot position is the origin of the coordinate system, and the x-axis points to the target point of the mobile robot. In the above mobile robot coordinate system, the joint state is expressed as Among them, the pedestrian state is expressed as and the mobile robot state is represented as s t =[d g ,v pref ,θ,r,v x ,v y ],in (p x , p y ) is the current position coordinate of the mobile robot in this coordinate system; (g x , g y ) is the target position coordinate of the mobile robot in this coordinate system; v pref is the reference speed value of the mobile robot; θ is the heading angle of the mobile robot in this coordinate system; r is the simplified circular radius of the mobile robot; v x ,v yis the velocity value of the mobile robot along the x and y axes of the mobile robot coordinate system, so that the position d in the linear velocity state of the mobile robot can be obtained through the positioning module. g , heading angle θ, linear velocity v x With v y , and the remaining parameters can be set by yourself. Furthermore, the observable state of the i-th pedestrian is in The rest are consistent with the meaning of the state quantities of the mobile robot, that is, all the state quantities of the i-th pedestrian can be obtained through the perception module.

[0065] Specific implementation of step S200

[0066] The method of the embodiment of the present invention is a navigation method based on learning. The optimal action of the mobile robot is obtained by using the value network of deep reinforcement learning as the reference input of the safety filter. The value network of the embodiment of the present invention is a neural network composed of fully connected layers. The navigation strategy is trained by using the DeepV-learning method in a dense crowd environment. Specifically, the value network approximately represents the value function According to the joint state of the mobile robot and the pedestrian Find an optimal strategy π through Deep V-learning * : To maximize the value function, that is:

[0067]

[0068] Wherein, is the reference input of the security filter of the embodiment of the present invention, and is also the optimal action of the mobile robot output by the value network. d represents the optimal action of the mobile robot obtained based on deep reinforcement learning, represents the value function of the value network, represents the joint state of the mobile robot and the pedestrian, where the optimal strategy π is obtained through deep reinforcement learning * :

[0069] Specific implementation of step S300

[0070] The method of the embodiment of the present invention sets time-varying safety constraints according to the dynamic and random characteristics of dense crowds. The safety constraints are divided into safety state constraints and safety action constraints. The safety state constraints are represented by polyhedrons, and the safety state constraints at each sampling moment are constructed by the control invariant set at the current moment, so as to have control invariance.

[0071] Among them, the discrete linear system of the mobile robot is expressed as follows:

[0072]

[0073] Among them, the state of the mobile robot is x k =[p x ,p y ,θ] T , which represents the position and heading angle, and the action of the mobile robot is u k =[v,w] T , which represents the linear velocity and angular velocity. The mobile robot needs to satisfy the action constraints and state constraints. Among them, x i∣k represents the future state quantity derived from the discrete linear system at the kth sampling moment, for example, x 0∣k represents the state at the kth sampling moment, x 1∣k Indicates the state of the k-th sampling moment recursively pushed forward into the future. i∣k represents the action used when the discrete linear system based on the kth sampling moment is recursively extended to the future, for example, u 0∣k represents the action at the kth sampling moment, u 1∣k Indicates the k-th sampling moment to the future state x 1∣k The action taken during recursion. Among them, △T is the sampling period of the discrete system; T k and t k is the matrix of linear inequalities, that is, T k p k ≤t k , which is called a polyhedron in convex optimization theory, is a convex set, where p k Refers to the two-dimensional position of the robot, T k p k ≤t k It is a two-dimensional graph, such as Figure 2 The blue area, T k and t k The parameters of are composed of the boundaries of the blue area, which are related to the relative position between the robot and the pedestrian. It should be noted that in the above mobile robot system equation, formula (1) is a single-vehicle model, which belongs to a non-holonomic constraint, and A and B in formula (2) are the system equation matrices of the robot.

[0074] In one embodiment, according to the control invariant set implemented in the present invention, at the kth sampling time, a polyhedron set about the system state is can be called a control invariant set if Make in represents any integer greater than or equal to 0; A and B are system matrices; It is the action constraint in the physical sense; is the safety state constraint at the kth sampling moment, which is obtained by calculation using the method of the embodiment of the present invention.

[0075] In one embodiment, the state constraint at the kth sampling time of the embodiment of the present invention is The state constraint is obtained by the following calculation: For computing control invariant sets See also Figure 2 , the red circle represents the mobile robot, the green circle represents the pedestrian, and the light blue area represents the state constraint The light blue area is based on the two yellow circular barriers and Specifically, assuming the position of the mobile robot is p k =[p x ,p y ] T , the radius is r, and the position of the i-th pedestrian is The radius is r i , then the circular obstacle formed by the i-th pedestrian Will be is the center of the circle and its radius is r+r i The boundaries of the blue area are The tangent line is also from p k point to The blue area is the vertical line of the mobile robot position p k The safety limit of is represented by a two-dimensional polyhedron as Then, according to the unrestricted heading angle of the mobile robot, ―π≤θ≤π, the two-dimensional polyhedron represented by the blue area is Upgrading to a three-dimensional polyhedron, that is, representing the three-dimensional mobile robot state x k =[p x ,p y ,θ] T The safety restrictions of the mobile robot can be expressed as follows:

[0076]

[0077] In the formula, represents the safety state constraint of the mobile robot at the kth sampling moment, x k represents the three-dimensional mobile robot state, x k =[p x ,p y ,θ] T , where (p x, p y ) represents the two-dimensional position coordinates of the mobile robot, θ represents the heading angle of the mobile robot, R 3 Represents three-dimensional space, T k With t k Represents the position of the mobile robot [p x ,p y ] T The matrix of safety constraints, in other words, the two-dimensional position of the mobile robot [p x ,p y ] T The linear constraint can be expressed as

[0078] Finally, the multi-faceted state constraint of the mobile robot obtained based on the above formula in the embodiment of the present invention is The control invariant set at the kth sampling time is obtained by iterative method

[0079] Specific implementation of step S400

[0080] The method of the embodiment of the present invention adopts discrete generalized velocity obstacles (DGVOs, Dispersed Generalized Velocity Obstacles) to obtain a set of actions that will cause collision with pedestrians if a certain action is continuously performed in the future, and excludes the dangerous actions of these DGVOs, thereby ensuring that the robot is collision-free in the future to a certain extent.

[0081] In one embodiment, according to the discrete generalized speed barrier of the embodiment of the present invention, in a dense crowd of n pedestrians, within the next N sampling periods, an action u k As a discrete generalized speed barrier, if Make in, represents the Euclidean distance, r and r j are the radius of the mobile robot and the radius of the jth pedestrian respectively. The future state of the mobile robot is calculated using the system equation of formula (1): To obtain the future state of the jth pedestrian, the system equation (2) is used To obtain, where represents the future state of the jth pedestrian, represents the future action of the jth pedestrian, where is the current speed of the jth pedestrian

[0082]

[0083] In the formula, x i∣k represents the future state quantity derived from the discrete linear system at the kth sampling moment, u 0∣k represents the action at the kth moment, u k It is necessary to explain that, in the above-mentioned pedestrian system equation, equation (2) belongs to the complete constraint, where A and B in equation (2) are the pedestrian system equation matrices.

[0084] The method of the embodiment of the present invention simply determines the continuous execution of the discrete action space through the discrete generalized speed obstacle Whether an action in will collide with pedestrians in the next N sampling periods. Figure 3 , and is the speed of the first and second pedestrians, and the action of the mobile robot is continuously executed over a period of time After that, since the mobile robot and the pedestrian collided at the position of the red cross, is a discrete generalized velocity barrier, and and All actions that are discrete generalized speed barriers constitute a dangerous discrete action space. In the embodiment, the safety action constraint at the kth moment Defined as a set of collision-free, safe discrete actions, none of which are discrete generalized speed barriers and all of which belong to the total discrete action space Therefore, the safe action constraint The total discrete action space can be calculated and The set difference is obtained.

[0085] Specific implementation of step S500

[0086] The method of the embodiment of the present invention obtains the optimal action u based on the learning-based navigation method d Correction is performed to obtain the optimal action u in step S200 d As the reference input of the security filter, and according to the security state constraint obtained in step S300 and the security action constraint obtained in step S400, the security filter is optimized. The optimization problem of the security filter is a quadratic programming problem with linear constraints, which is expressed as follows:

[0087]

[0088] In the formula, It indicates that the security filter obtains the optimal action as the output reference in step S200 at the kth sampling moment. The first term of the objective function in the above formula is It is used to minimize the deviation between the safe action and the reference action. The second term of the objective function ρ0||x 1|k ―u goal || 2 It is used to encourage the robot to reach the navigation target point faster. The parameter ρ0 is used to adjust the credibility of the first and second terms of the objective function in the above formula. represents the optimal reference action of the security filter, u 0∣k represents the action at the kth sampling moment, x 1∣k represents the state of the kth sampling moment recursively pushed forward into the future, x goal represents the navigation target point of the mobile robot, u i∣k represents the action used when the discrete linear system is recursively extended to the future based on the kth sampling moment, x k represents the state at the kth sampling moment, x k As the basis of state recursion, it can be written as x 0∣k , is the safety state constraint at the kth sampling moment, represents the safety action constraint, x 1∣k It represents the state of the k-th sampling moment recursively pushed forward into the future. represents the control invariant set, Represents the safety action constraints within N sampling periods.

[0089] The first term of the objective function of the optimization problem in the embodiment of the present invention The second term of the objective function is ρ 0 ||x 1| k―u goal || 2 It will encourage the robot to reach the navigation target point faster. Among them, the parameter ρ0 can adjust the credibility of the first and second terms of the objective function, so that when the learning-based navigation method performs poorly, the safety filter can choose a larger ρ0 to improve the quality of navigation.

[0090] In the embodiment of the present invention, since the optimization variables of the security filter optimization problem are finite discrete actions, the security filter optimization problem only needs to directly traverse all feasible discrete actions and find a discrete action that can minimize the objective function as the solution to the security filter optimization problem.

[0091] Specific implementation of step S600

[0092] In the method of the embodiment of the present invention, when there is no feasible solution to the safety filter optimization problem of step S500 and the number of discrete actions in the set of safety action constraints is zero, the embodiment of the present invention replaces the global navigation target point in the objective function with a heuristic local navigation target point and cancels the restrictions on discrete actions by the safety action constraints of step S400, thereby alleviating the situation where there is no feasible solution.

[0093] Specifically, the heuristic local target point at the kth sampling time can be obtained by finding the heuristic function H*(x 1|k ) is obtained by taking the minimum value of , which can be described as an optimization problem:

[0094]

[0095] stx 1|k =Ax 0|k +Bu 0|k ,

[0096] x 0|k =x k ,

[0097]

[0098] Among them, x 1∣k As a heuristic local goal x local_goal The heuristic function H(x) of the embodiment of the present invention 1|k ) is given by the gravitational function U att (x 1|k ) and the repulsion function U rep (x 1|k ), which is expressed as follows:

[0099]

[0100] U arr (x 1|k )=||x 1|k -x goal ||,

[0101]

[0102] Where K is the scale parameter, is the radius of the circular area where the repulsive force acts, D = ‖x 1∣k -x 0∣k,i ‖ is the distance between the robot and the i-th pedestrian.

[0103] The method of the embodiment of the present invention uses a heuristic local target x local_goalAfter canceling the safety action constraint, the new optimization problem is solved by adding the local navigation target as a soft constraint and canceling the safety action constraint. The new safety filter optimization problem is expressed as follows:

[0104]

[0105] In the formula, x local_goal Represents the local navigation target point of the mobile robot. The parameter ρ1 is used to adjust the credibility of the first and second terms of the objective function in the above formula. represents a discrete action space.

[0106] The method of the embodiment of the present invention traverses all discrete actions and searches for a discrete action that minimizes the objective function when satisfying the safety state constraint as a solution to the safety filter optimization problem.

[0107] Specific implementation of step S700

[0108] In the method of the embodiment of the present invention, when the optimization problem of step S600 still has no feasible solution, and the state of the mobile robot at the next moment does not satisfy the safety state constraint, the restriction of the safety state constraint on the optimization variable is cancelled, and a new optimization problem of the safety filter is obtained, which is expressed as follows:

[0109]

[0110]

[0111] Wherein, the parameter ρ2 is used to adjust the credibility of the first and second terms of the objective function in the above formula.

[0112] The method of the embodiment of the present invention traverses all discrete actions and searches for a discrete action that can minimize the objective function as a solution to the optimization problem.

[0113] Specific implementation of step S800

[0114] The method of the embodiment of the present invention uses the optimal solution of the optimization problem of the safety filter of step S500, step S600 or step S700 as the input instruction of the chassis control module of the mobile robot, that is, the linear velocity and angular velocity instructions. Finally, the mobile robot executes the input instruction. It should be noted that the chassis control module of the mobile robot is not involved in the present invention, and different mobile robots should have different chassis control modules.

[0115] Experimental verification of the method and system according to the present invention

[0116] The embodiment of the present invention sets the simulation scene of dense crowd as follows: the shapes of the mobile robot and the surrounding pedestrians are replaced by circles. The mobile robot and the pedestrians move in a 10X10 square area; the navigation scene is a circular scene, that is, the mobile robot always navigates from the starting point coordinate (-4,0) to the end point coordinate (4,0); and the starting point of the pedestrian is randomly distributed on a circle with a radius of 4 and a center coordinate of (0,0), and their end point is on the other side of the circle. In the simulation experiment, the motion model of the pedestrian is fully constrained, and the pedestrian uses ORCA as a navigation strategy, while the motion model of the mobile robot is non-holonomically constrained (bicycle model), and the mobile robot uses a learning-based navigation method as a navigation strategy. In addition to pedestrians being dynamic obstacles, there are no other obstacles in the dense crowd environment. The mobile robot is set to be invisible, that is, all pedestrians will not actively avoid the mobile robot when walking towards their respective target points. The mobile robot needs to actively avoid pedestrians, otherwise a collision will occur. When the mobile robot is navigating, the planned optimal action can be executed immediately and the required speed can be reached instantly. The position, speed and radius of all agents (pedestrians and mobile robots) can be observed by other agents, but their target points cannot be observed. The radius and reference speed of pedestrians are random, and their random value ranges are r∈[0.1,0.4],v pref ∈[0,1.5], in the training phase of the learning-based navigation method, the number of pedestrians is 5, but in the test phase, the number of pedestrians is random, that is, N∈[5,10], N + .

[0117] In the simulation scenario configured above, simulation experiments were conducted on whether the three learning-based navigation methods CADRL, LSTM-RL and SARL used the safe navigation method based on the safety filter of the present invention, and 500 random tests were performed on each. The simulation experiments were all completed under the Ubuntu 20.04 system, and the specific hardware configuration was Intel Core i7-8700 processorwith 16GB RAM. The comparison indicators of the simulation experiment are success rate, collision rate, average navigation time, average expected reward and danger rate. Success rate: the number of test samples in which the robot reaches the navigation target point without collision, accounting for the proportion of the total number of test samples. Collision rate: the number of test samples in which the robot collides with pedestrians, accounting for the proportion of the total number of test samples. Average navigation time: the average time required for the robot to reach the target point in the test samples that successfully reach the navigation target point. Average expected reward: the average discounted cumulative reward obtained by the robot. Danger rate: the proportion of the number of states in which the distance between the robot and the nearest pedestrian is less than 0.2 in all test samples to the total number of states of all test samples.

[0118] The specific simulation experiment results are shown in the following table. The * indicates that the safety navigation method based on the safety filter proposed by the present invention is used. The upward arrow indicates that the higher the value, the better. The downward arrow indicates that the lower the value, the better. For the three learning-based navigation methods, after adding the safety filter, their success rate, collision rate and danger rate are all improved, that is, the safety is improved.

[0119]

[0120] See also Figures 4 to 6 Through the visualized trajectories of the mobile robot and the pedestrians, it can be seen that the original method generally causes a collision at the most crowded place of the crowd. After adding the method of the embodiment of the present invention, the mobile robot knows how to avoid the crowded crowd or let the pedestrians go first and reach the destination.

[0121] It should be appreciated that the method steps in the embodiments of the present invention can be implemented or implemented by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer readable memory. The method can use standard programming techniques. Each program can be implemented in a high-level process or object-oriented programming language to communicate with a computer system. However, if necessary, the program can be implemented in an assembly or machine language. In any case, the language can be a compiled or interpreted language. In addition, the program can be run on a programmed ASIC for this purpose.

[0122] Furthermore, the operations of the processes described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by context. The processes described herein (or variations and / or combinations thereof) may be performed under the control of one or more computer systems configured with executable instructions, and may be implemented as code (e.g., executable instructions, one or more computer programs, or one or more applications) that is executed collectively on one or more processors, by hardware, or a combination thereof. The computer program includes a plurality of instructions that may be executed by one or more processors.

[0123] Further, the method can be implemented in any type of computing platform that is operably connected to a suitable computer, including but not limited to a personal computer, a minicomputer, a mainframe, a workstation, a network or distributed computing environment, a separate or integrated computer platform, or in communication with a charged particle tool or other imaging device, etc. Various aspects of the present invention can be implemented in machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, an optical read and / or write storage medium, an RSM, a ROM, etc., so that it can be read by a programmable computer, and when the storage medium or device is read by the computer, it can be used to configure and operate the computer to perform the process described herein. In addition, the machine-readable code, or portions thereof, can be transmitted via a wired or wireless network. When such media includes instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor, the invention described herein includes these and other different types of non-transitory computer-readable storage media. When programmed according to the methods and techniques of the present invention, the present invention can also include the computer itself.

[0124] The computer program can be applied to input data to perform the functions described herein, thereby converting the input data to generate output data stored in a non-volatile memory. The output information can also be applied to one or more output devices such as a display. In a preferred embodiment of the present invention, the converted data represents physical and tangible objects, including specific visual depictions of physical and tangible objects produced on the display.

[0125] The above is only a preferred embodiment of the present invention. The present invention is not limited to the above implementation. As long as the technical effect of the present invention is achieved by the same means, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the scope of protection of the present invention. Within the scope of protection of the present invention, its technical scheme and / or implementation method may have various modifications and changes.

Claims

1. A safe navigation method for a mobile robot in a dense crowd environment, characterized in that: Applied to a mobile robot, the mobile robot is provided with a perception module, a positioning module, a laser radar and a sensor; wherein the method comprises the following steps: S100, acquiring data from the laser radar and the sensor, and inputting the data into the perception module and the positioning module, estimating and outputting the state of the mobile robot and the state of the surrounding pedestrians, so as to obtain the joint state of the mobile robot and the pedestrians; S200, according to the joint state of the mobile robot and the pedestrian, obtaining the optimal action of the mobile robot through a value network based on deep reinforcement learning, so as to serve as a reference input of the safety filter; S300, constructing a polyhedral state constraint of the mobile robot to obtain a control invariant set as a safety state constraint; S400, filtering out safe actions from discrete action space through discrete generalized speed barriers to obtain safe action constraints; S500, solving an optimization problem for the safety filter according to the safety state constraint and the safety action constraint to obtain an optimal reference action after modifying the reference input; S600, when there is no feasible solution to the optimization problem in step S500 and the number of discrete actions constrained by the safety action is zero, a new optimization problem is solved again by adding a local navigation target as a soft constraint and canceling the safety action constraint to obtain a new corrected optimal reference action; S700, when there is no feasible solution to the optimization problem in step S600 and the state of the mobile robot at the next moment does not satisfy the safety state constraint, the safety state constraint is cancelled based on the optimization problem in step S600, and a new optimization problem is solved again to obtain a new corrected optimal reference action; S800, inputting the finally obtained corrected optimal reference action into the mobile robot as an execution instruction of the mobile robot; Wherein, in step 300, the multi-faceted state constraint of the mobile robot is expressed as follows: In the formula, represents the safety state constraint of the mobile robot at the kth sampling moment, x k represents the three-dimensional mobile robot state at the kth sampling moment, x k =[p x ,p y ,θ] T , where (p x , p y ) represents the two-dimensional position coordinates of the mobile robot, θ represents the heading angle of the mobile robot, R 3 Represents three-dimensional space, T k With t k Represents the position of the mobile robot [p x ,p y ] T The matrix of safety constraints; Wherein, in step 300, the future state of the mobile robot is obtained by the following system equation: x i∣k =Ax i∣k +Bu 0∣k ,u 0∣k =u k In the formula, x i∣k represents the future state quantity derived from the discrete linear system at the kth sampling moment, u 0∣k represents the action at the kth sampling moment, u k represents the optimal action of the safety filter at the kth sampling moment, A and B are system matrices; Wherein, in the step S400, the future state of the pedestrian in the discrete generalized speed obstacle is obtained by the following system equation: In the formula, represents the future state of the jth pedestrian, represents the future action of the jth pedestrian; In step S500, the optimization problem of the security filter is expressed as follows: In the formula, the first term of the objective function is It is used to minimize the deviation between the safe action and the reference action. The second term of the objective function ρ0||x 1|k -u goal || 2 It is used to encourage the robot to reach the navigation target point faster. The parameter ρ0 is used to adjust the credibility of the first and second terms of the objective function in the above formula. represents the optimal reference action of the security filter, u 0∣k represents the action at the kth sampling moment, x 1∣k represents the state of the kth sampling moment recursively pushed forward into the future, x goal represents the navigation target point of the mobile robot, u i∣k represents the action used when the discrete linear system is recursively extended to the future based on the kth sampling moment, x 0∣k represents the state at the kth sampling moment, represents the safety action constraint, represents the control invariant set, Represents the safety action constraints within N sampling periods.

2. The method according to claim 1, characterized in that In step S200, the reference input of the security filter is represented as follows: In the formula, u d represents the optimal action of the mobile robot obtained based on deep reinforcement learning, represents the value function of the value network, represents the joint state of the mobile robot and the pedestrian, where the optimal strategy π is obtained through deep reinforcement learning * :

3. The method according to claim 1, characterized in that In step S600, after adding the local navigation target and canceling the safety action constraint, the new optimization problem of the safety filter is expressed as follows: In the formula, x local_goal Represents the local navigation target point of the mobile robot. The parameter ρ1 is used to adjust the credibility of the first and second terms of the objective function in the above formula. represents a discrete action space.

4. The method according to claim 3, characterized in that In step S700, after canceling the security state constraint, the new optimization problem of the security filter is expressed as follows: Wherein, the parameter ρ2 is used to adjust the credibility of the first and second terms of the objective function in the above formula. 5 . A computer-readable storage medium having program instructions stored thereon, wherein the program instructions are executed by a processor to implement the method according to claim 1 .

6. A safe navigation device for a mobile robot in a dense crowd environment, characterized in that: include: A computer device comprising a computer readable storage medium according to claim 5.

Images