Method and device for safe motion planning of mobile robots in dynamic scenes

Abstract

The present invention relates to a method and device for planning safe motion of a mobile robot in a dynamic scene. The method includes: in a dynamic environment, obtaining the state information of the mobile robot through a positioning module, obtaining the state information of the surrounding dynamic obstacles through a perception module, constructing obstacle avoidance constraints based on the current state information of the mobile robot and the information of the dynamic obstacles, combining the control obstacle function and the model predictive control, converting the obstacle avoidance constraints into obstacle avoidance costs, adding them to the cost equation of the model predictive control, constructing a one-step safety constraint based on the relative degree of the obstacle avoidance constraint to the system control input as a safety guarantee constraint of the system, solving the optimization problem of the model predictive control, and sending the optimal solution of the optimization problem as the control input to the motion control module of the mobile robot. The present invention is applied to simulation scenes in dynamic environments, which is conducive to ensuring that the mobile robot can reach the destination more efficiently under the premise of safety.

Description

Technical Field

[0001] The invention relates to a method and a device for safe motion planning of a mobile robot in a dynamic scene, and belongs to the technical field of motion planning of mobile robots. Background Art

[0002] Robots navigating in dense crowds should react accordingly based on the movements of nearby pedestrians, but improperly designed navigation algorithms may result in unsafe, inefficient, and unrecognizable robot movements. Existing robot navigation work in dynamic environments can be divided into three categories: reaction-based methods, learning-based methods, and optimization-based methods.

[0003] In the reaction-based method, the robot takes a step forward and makes the current optimal action according to the information of dynamic obstacles in the current environment. The classic method is represented by velocity obstacle (VO), which divides the velocity space according to the speed of all dynamic obstacles in the environment to obtain the current optimal robot velocity action. In some existing technologies, RVO (Reciprocal Velocity Obstacle) is proposed based on VO, which evenly distributes the obstacle avoidance responsibility to two agents to reduce oscillations during obstacle avoidance. In addition, ORCA (Optimal Reciprocal Collision Avoidance) is proposed based on RVO, which adds the concept of time window, increases the feasible velocity space of the robot, and thus makes the robot's action better. However, this method only uses the current environmental information, which will lead to short-sightedness, oscillation and unnatural behavior, which is not conducive to understanding the robot's movement intention.

[0004] In learning-based approaches, robots attempt to reproduce appropriate navigation strategies to mimic pedestrian interactions, thereby navigating dense crowds in a more socially appropriate manner. Among them, reinforcement learning (RL) is often used to train computationally efficient navigation strategies that implicitly encode interactions and collaborations between pedestrians. Significant progress has been made in recent learning-based work, which often models the collective influence of the crowd through simplified aggregation of pairwise interactions, such as the existing techniques of minimax operators or LSTM, but which may reduce the performance of cooperative planning in complex crowded scenarios. For example, in some existing techniques, self-attention mechanisms are added to human-robot pairwise interactions, and human-robot and human-human interactions are jointly modeled in a reinforcement learning framework to enhance the understanding of crowd behavior. However, traditional learning-based methods rely solely on offline training, are highly dependent on scene attributes, face the problem of generalization from simulation to the real world, and cannot guarantee performance in situations different from the training data.

[0005] In the optimization-based method, it usually consists of two consecutive steps for each time step, namely prediction and planning. First, the pedestrian motion model is used to predict the motion of nearby pedestrians, and then the appropriate trajectory is planned through optimization. The robot's kinematic constraints and static / dynamic collision constraints can be integrated into an optimization problem. For example, some existing technologies propose a local motion planning method based on model predictive contour control (MPCC), which can perform motion planning in an unstructured dynamic environment while considering the obstacle avoidance problem of static and moving obstacles. For another example, some existing technologies propose a motion planner based on safety corridor model predictive contour control (CMPCC) based on MPCC to solve the problem of generating dynamically feasible and collision-free trajectories for drones in the face of unpredictable interference. For another example, some existing technologies combine the control obstacle function on the basis of MPCC to achieve dynamic obstacle avoidance of safety-critical control based on lidar. These optimization-based methods have proven to be robust and safe in robot obstacle avoidance, but a big challenge is that if the predicted motion is different from the actual motion of pedestrians, the robot may not be able to find a feasible path in a dense crowd. If the optimization problem is not designed properly, there will be certain safety risks for the robot in a dense crowd.

[0006] In recent years, methods based on control barrier functions (CBFs) have been widely used in safety-critical control systems. For example, some existing technologies have proposed a safety-critical controller (CLF-CBF-QP), which unifies the control Lyapunov function (CLF) that ensures stability and the control barrier function (CBF) that ensures safety through quadratic programming to achieve continuous time-domain adaptive cruise control. This method couples the potentially conflicting control objectives and safety criteria in the optimization problem, and coordinates the convergence and safety of the system in a balanced position, thereby ensuring that the autonomous system of the mobile robot meets the safety requirements while reaching the target position as quickly as possible. Model predictive control takes into account the future state information of the system, so that a relatively non-greedy control strategy can be obtained. For another example, some existing technologies have proposed a safety-critical model predictive control strategy (MPC-CBF) based on model predictive control and discrete-time control barrier function, which aims to ensure system safety and achieve optimal performance through model predictive control. Compared with using only one-step prediction information, this method shows early avoidance of obstacles and improves the safety of the autonomous system.

[0007] In summary, since the optimization-based method has the advantage of being able to incorporate future environmental estimation information and safety constraints, it is more likely to plan a reasonable route that can be understood by pedestrians around it. However, when facing complex situations with uncertain environmental perception, mobile robots will still make dangerous behaviors, causing collisions between mobile robots and surrounding obstacles. However, the latest research directly uses the control obstacle function as a hard constraint for the optimization problem, and directly acting as a hard constraint in the prediction time domain may lead to an unsolvable optimization problem. Therefore, how to combine the control obstacle function to construct the optimization problem becomes the main difficulty. Therefore, when using optimization-based methods, it is very important to properly construct the optimization problem to improve the safety of the system. Summary of the invention

[0008] The present invention provides a method and device for safe motion planning of a mobile robot in a dynamic scene, aiming to solve at least one of the technical problems existing in the prior art.

[0009] The technical solution of the present invention relates to a method for safe motion planning of a mobile robot in a dynamic scene. The method according to the present invention comprises the following steps:

[0010] S100, in a dynamic environment, obtaining state information of the mobile robot through a positioning module, and obtaining state information of surrounding dynamic obstacles through a perception module;

[0011] S200, in a dynamic environment, constructing obstacle avoidance constraints based on current state information of the mobile robot and information of dynamic obstacles by combining the obstacle control function and model predictive control;

[0012] S300, converting the obstacle avoidance constraint into an obstacle avoidance cost, and adding the cost equation into the model predictive control;

[0013] S400, constructing a one-step safety constraint according to the relative degree of the obstacle avoidance constraint with respect to the system control input, so as to serve as a safety guarantee constraint of the system;

[0014] S500, solving the optimization problem of model predictive control, and sending the optimal solution of the optimization problem as control input to the motion control module of the mobile robot.

[0015] Furthermore, the step S600 also includes:

[0016] If there is no feasible solution to the optimization problem in step S400, the braking device of the mobile robot is activated.

[0017] Further, in step S100,

[0018] At the tth moment, the mobile robot state information is expressed as Among them, (x t,y t ) represents the two-dimensional coordinates of the mobile robot in the current map, θ t represents the orientation angle of the mobile robot, r represents the radius of the mobile robot, Indicates the current speed of the mobile robot along the x-axis and y-axis respectively;

[0019] The state variables of the dynamic obstacle are expressed as Among them, the superscript i represents the i-th dynamic obstacle, the first two variables represent the two-dimensional coordinates of the dynamic obstacle, and the last two variables represent the speed of the dynamic obstacle along the x-axis and y-axis respectively.

[0020] Further, in step S200,

[0021] At time t, the optimization problem based on the control barrier function and model predictive control is expressed as follows:

[0022]

[0023] stfor all k=0,...,N-1:

[0024] x t+k+1|t =f(x t+k|t ,u t+k|t ),

[0025]

[0026]

[0027] x t|t =x t ,

[0028]

[0029] Among them, N represents the prediction time domain size; x t+k|t Indicates that from the current state x t The predicted state at the beginning of time t+k; input sequence u t+k|t = {u t|t , ..., u t+N-1|t}; The first term p of the cost function equation represents the terminal cost, and the second term q represents the process cost; The first constraint x t+k+1|t is the kinematic constraint of the system; the second constraint x t+k|t and the third constraint u t+k|t They represent the state constraints and input constraints of the robot along the prediction time domain respectively; the fourth constraint x t|t is the initial state constraint; the fifth constraint h i It is a distance constraint based on the control barrier function;

[0030] in, r i is the radius of the mobile robot and the dynamic obstacle, γ∈(0,1] is the conservative coefficient;

[0031] Among them, the optimal solution at time t is the input sequence, which is represented by u t+k|t = {u t|t , ..., u t+N-1|t}, the first element of the optimal solution input sequence is sent to the mobile robot as the control input.

[0032] Further, in step S300,

[0033] At time t, the distance constraint based on the control obstacle function is converted into obstacle avoidance cost by adding slack variables using the exact penalty function method, and then added into the cost equation of model predictive control to obtain .

[0034] Further, in step S300,

[0035] After converting the obstacle avoidance constraint into the obstacle avoidance cost, the optimization problem is expressed as follows:

[0036]

[0037] stfor all k=0,...,N-1:

[0038] x t+k+1|t =f(x t+k|t ,u t+k|t ),

[0039]

[0040]

[0041] x t|t =x t ,

[0042]

[0043] in, is the added slack variable, its specific form is N o is the number of dynamic obstacles;

[0044] Among them, the last constraint is a soft constraint, and its specific form is as follows:

[0045]

[0046] Further, in step S400,

[0047] The relative degree d is the control input u t Appears in the system state constraint h(x t ) in the delay step;

[0048] After using the generalized control barrier function as a single-step constraint to join the model predictive control optimization problem, the safety guarantee constraint of the system is obtained as follows:

[0049]

[0050] Among them, the conservative coefficient η∈(γ, 1].

[0051] Further, in step S500,

[0052] At time t, the final form of the optimization problem based on the control barrier function and model predictive control is expressed as follows:

[0053]

[0054] stfor all k=0,...,N-1:

[0055] x t+k+1|t =f(x t+k|t ,u t+k|t ),

[0056]

[0057]

[0058] x t|t =x t ,

[0059]

[0060]

[0061] By solving the above optimization problem, the optimal solution sequence at time t is obtained, which is expressed as follows:

[0062] u t+k|t = {u t|t , ..., u t+N-1|t}

[0063] The first element of the optimal solution sequence is issued to the mobile robot as a control input.

[0064] 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.

[0065] The technical solution of the present invention also relates to a mobile robot safe motion planning device, wherein the system includes a computer device, and the computer device includes the above-mentioned computer-readable storage medium.

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

[0067] The present invention is a safe motion planning method and device for a mobile robot applied in a dynamic scene. It is a safe motion planning method that combines a control obstacle function and a model predictive control. It is applied in a simulation scene in a dynamic environment, which is beneficial to ensure that the mobile robot can reach the destination more efficiently under the premise of safety.

[0068] When the optimization problem encounters a violation of the original hard constraints, the present invention will reflect it in the optimization problem in the form of a cost rather than a solution failure, thereby ensuring that the optimization problem is feasible for any input sequence in the control input constraints. This makes it unnecessary to ensure that every solution obtained satisfies the safety constraints in motion planning, and the mobile robot can more actively explore new paths in crowded areas.

[0069] In the present invention, only the first element of the optimal solution sequence will be issued to the robot as a control input, thereby achieving the goal of avoiding dynamic obstacles while enabling the robot to reach the destination efficiently.

[0070] In order to avoid possible collisions, the present invention can ensure that the robot stops completely by activating the built-in brakes on the robot when the optimization problem is not feasible, rather than just setting the control input to zero or maintaining the control input at the last moment. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 is a basic flow chart of the method according to the present invention.

[0072] Figure 2 Schematic diagram of the control module structure according to the method of the present invention.

[0073] Figure 3 It is a schematic diagram of the experimental results of ORCA simulation test using the ORCA method.

[0074] Figure 4 It is a schematic diagram of the experimental results of ORCA simulation test using the MPC-DC method.

[0075] Figure 5 It is a schematic diagram of the experimental results of ORCA simulation test using the MPC-CBF method.

[0076] Figure 6 It is a schematic diagram of the simulation test results using the method of the present invention. DETAILED DESCRIPTION

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

[0078] 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 "a", "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 commonly understood by those skilled in the art. The terms used in this specification are intended only to describe specific embodiments 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.

[0079] 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.

[0080] Reference Figure 1 to Figure 2 In some embodiments, the mobile robot safe motion planning method according to the present invention comprises at least the following steps:

[0081] S100, in a dynamic environment, obtaining state information of the mobile robot through a positioning module, and obtaining state information of surrounding dynamic obstacles through a perception module;

[0082] S200, in a dynamic environment, constructing obstacle avoidance constraints based on current state information of the mobile robot and information of dynamic obstacles by combining the obstacle control function and model predictive control;

[0083] S300, converting the obstacle avoidance constraint into an obstacle avoidance cost, and adding the cost equation into the model predictive control;

[0084] S400, constructing a one-step safety constraint according to the relative degree of the obstacle avoidance constraint with respect to the system control input, so as to serve as a safety guarantee constraint of the system;

[0085] S500, solving the optimization problem of model predictive control, and sending the optimal solution of the optimization problem as control input to the motion control module of the mobile robot.

[0086] The present invention uses a model predictive control method combined with a control obstacle function to obtain a safe motion planning method and device for safe robot actions according to the state of the mobile robot and the surrounding dynamic obstacles, which can not only improve the safety of the motion planning of the mobile robot in a dynamic environment, but also reduce the probability of solution failure, thereby achieving the effect of improving the efficiency of the mobile robot to reach the destination. The method of the present invention can enable the mobile robot to avoid crowded areas, stop in time to avoid obstacles in motion, and finally successfully reach the destination.

[0087] Specific implementation of step S100

[0088] In a dynamic environment, the method of the present invention obtains the status information such as the position of the mobile robot itself through the positioning module, and observes the status information such as the position and speed of the surrounding dynamic obstacles through the perception module.

[0089] Among them, the positioning module uses the AMCL (Adaptive Monte Carlo Localization) positioning function package in ROS (Robot Operating System) and the Kalman filter to estimate the robot's position information and speed information. Specifically, the mobile robot uses the AMCL positioning function package to process the current map, lidar, odometer and other data, and preliminarily estimates the two-dimensional coordinates, heading angle, linear velocity and angular velocity information of the mobile robot relative to the world coordinate system. Finally, the linear Kalman filter is used to process these data to estimate more accurate robot position information and speed information, and publish this information to the downstream module for further processing.

[0090] In some specific embodiments, the position information and speed information of the mobile robot are combined into the state information of the mobile robot under the current map. The state information of the mobile robot at the tth time is expressed as Where (x t ,y t ) represents the two-dimensional coordinates of the mobile robot in the current map, θ t represents the orientation angle of the mobile robot, r represents the simplified radius of the mobile robot, Indicates the current speed of the mobile robot along the x-axis and y-axis respectively.

[0091] Among them, the perception module uses the three-dimensional target detection PointPillars algorithm (a three-dimensional target detection algorithm based on point cloud) to detect pedestrians in the environment, and then uses the Hungarian algorithm to match different dynamic obstacle trajectories, and finally estimates the position information and speed information of dynamic obstacles based on the Kalman filter. Specifically, first, the mobile robot uses the PointPillars algorithm to detect according to the point cloud data transmitted by the current lidar, obtains the position information of pedestrians in the environment, and generates the corresponding three-dimensional detection box as the dynamic obstacle of the downstream module. Then, the Hungarian algorithm is used to match different dynamic obstacle trajectories. Finally, the linear Kalman filter is used to process these data to estimate more accurate obstacle position information and speed information, and publish this information to the downstream module for the next step of processing.

[0092] In some specific embodiments, the detection result of a pedestrian in the surrounding environment at time t is defined as n k is the number of detected targets, D k Each element in represents a tuple (x, y, z, θ, l, w, h, s), including the center coordinates of the target (x, y, z), the length, width and height information of the target detection box (l, w, h), the heading angle θ, and the confidence s. The target with high confidence in the detection result is selected as the dynamic obstacle, and the state variable of the dynamic obstacle is selected according to the principle of Kalman filter as The superscript i represents the i-th dynamic obstacle, the first two variables represent the two-dimensional coordinates of the dynamic obstacle, and the last two variables represent the speed of the dynamic obstacle along the x-axis and y-axis respectively.

[0093] Specific implementation of step S200

[0094] In a dynamic environment, the present invention constructs safe obstacle avoidance constraints based on the current state information of the mobile robot and the information of dynamic obstacles, combined with the control obstacle function. Specifically, at time t, the optimization problem based on the control obstacle function and model predictive control is as follows:

[0095]

[0096] stfor all k=0,...,N-1:

[0097] x t+k+1|t =f(x t+k|t ,u t+k|t ),

[0098]

[0099]

[0100] x t|t =x t ,

[0101]

[0102] Where N represents the prediction time domain size; x t+k|t Indicates that from the current state x t The predicted state at the beginning of the t+kth moment; the first term p of the cost function equation represents the terminal cost, and the second term q represents the process cost; the first constraint x t+k+1|t is the kinematic constraint of the system; the second constraint x t+k|t and the third constraint u t+k|t They represent the state constraints and input constraints of the robot along the prediction time domain respectively; the fourth constraint x t|t is the initial state constraint; the fifth constraint h i is a distance constraint based on a control barrier function.

[0103] in, r i is the radius of the mobile robot and the dynamic obstacle, γ∈(0,1] is the conservative coefficient. The smaller the value of the conservative coefficient, the safer it is, but the corresponding constraints will be stricter.

[0104] The optimal solution of the above optimization problem at time t is an input sequence, which is represented by u t+k|t = {u t|t , ..., u t+N-1|t}, only the first element of the optimal solution sequence will be used as control input to the mobile robot.

[0105] Specific implementation of step S300

[0106] The present invention converts the obstacle avoidance constraint into the obstacle avoidance cost and adds it into the cost equation of model predictive control. Specifically, at time t, the distance constraint based on the control obstacle function is converted into the obstacle avoidance cost by adding slack variables and adding it into the cost equation of model predictive control. The optimization problem becomes as follows:

[0107]

[0108] stfor all k=0,...,N-1:

[0109] x t+k+1|t =f(x t+k|t ,u t+k|t ),

[0110]

[0111]

[0112] x t|t =x t ,

[0113]

[0114] in, is the added slack variable, the specific form is N o is the number of dynamic obstacles.

[0115] Among them, the last item of the obstacle avoidance constraint is a soft constraint, and its specific form is as follows:

[0116]

[0117] Finally, the obstacle avoidance constraint will be added to the cost function of the model predictive control optimization problem in the form of an obstacle avoidance cost through a slack variable, where α is the penalty weight. When the optimization problem encounters a violation of the original hard constraint, it will be reflected in the optimization problem in the form of a cost, rather than a solution failure. This ensures that the optimization problem is feasible for any input sequence in the control input constraint. Constructing the optimization problem in this way not only ensures that the optimization problem is feasible at every moment, but also that the control effect is closest to the effect when the original obstacle avoidance constraint is a hard constraint.

[0118] Specific implementation of step S400

[0119] The present invention constructs a one-step safety constraint as a safety guarantee constraint of the system according to the relative degree of the obstacle avoidance constraint to the system control input. In step S300, the optimization problem is always feasible by converting the hard constraint into a soft constraint. Therefore, in motion planning, it is not necessary to ensure that each solution obtained satisfies the safety constraint, so that the robot will more actively explore new paths in crowded areas.

[0120] In order to make the trajectory generated by the controller after adding slack variables more effective in safety guarantee, the present invention uses the concept of extended generalized control barrier function (GCBF) as a single-step constraint added to the model predictive control optimization problem as the safety guarantee of the controller. Specifically, before designing the single-step safety constraint, it is necessary to obtain the relative degree of the constraint relative to the system input according to the definition of relative degree, where the definition of relative degree is as follows:

[0121] The state constraint of the system h(x t ) relative to the control input u t With relative degree d, if satisfy:

[0122]

[0123] That is, the relative degree d is the control input u t Appears in h(x t ), then it is valid to impose safety constraints at time step d, but not at time step j. By incorporating single-step state constraints at time d, the optimization problem can benefit from a wider feasible region and improved computational efficiency, rather than including state constraints in all d steps. Finally, the GCBF is extended to the case of dynamic obstacles, and its specific form is as follows:

[0124]

[0125] Among them, the conservative coefficient η∈(γ, 1]. The reason why the lower limit of η is γ is that we do not want the newly added hard constraints to be stricter than the original soft constraints.

[0126] Specific implementation of step S500

[0127] Solve the optimization problem of model predictive control, and send the optimal solution of the optimization problem as the control input to the motion control module of the mobile robot. Specifically, at time t, the final form of the optimization problem based on the control obstacle function and model predictive control is as follows:

[0128]

[0129] stfor allk=0,...,N-1:

[0130] x t+k+1|t =f(x t+k|t ,u t+k|t ),

[0131]

[0132]

[0133] x t|t =x t ,

[0134]

[0135]

[0136] By solving the above optimization problem, we can get the optimal solution sequence at time t, that is, u t+k|t = {u t|t , ..., u t+N-1|t}, only the first element of the optimal solution sequence will be issued to the robot as control input, so that the robot can avoid dynamic obstacles and reach the destination efficiently.

[0137] Specific implementation of step S600

[0138] If there is no feasible solution to the optimization problem of S400, the brake device of the mobile robot is activated. It should be noted that the single-step hard constraint as a safety guarantee will also bring about the problem of infeasibility, and the single-step hard constraint is much weaker than the original soft constraint. Therefore, the hard constraint will only be activated when the soft constraint is violated. When the hard constraint is violated, it means that the mobile robot is no longer able to generate a sufficiently safe path. Therefore, in order to avoid possible collisions, when the optimization problem is infeasible, the brake on the robot can be activated to ensure that the robot stops completely, rather than simply setting the control input to zero or maintaining the control input at the previous moment, which is conducive to providing safety.

[0139] Furthermore, the above steps S100 to S600 are repeated at the next moment.

[0140] The method and device for safe motion planning of a mobile robot in a dynamic scene of the present invention is a safe motion planning method combining a control obstacle function and a model predictive control, which is applied to a simulation scene in a dynamic environment, and is conducive to ensuring that the mobile robot can reach the destination more efficiently under the premise of safety. A specific embodiment is used to illustrate this.

[0141] For example, the simulation scene of the dynamic environment is detailed as follows. The mobile robot and the dynamic obstacle are both simplified by using two-dimensional circles in the simulation scene. The mobile robot and the dynamic obstacle both move in a two-dimensional square field with a side length of 10 meters. Assuming that the Cartesian coordinates of the center of the field are (0,0), the mobile robot always moves from the starting point coordinate (-4,0) to the end point coordinate (4,0), and the starting points of the dynamic obstacles are randomly distributed on a circle with a radius of 4 meters and a center of (0,0), and move to the end point on the other side of the circle. In the simulation experiment, the motion model of the dynamic obstacle is an omnidirectional motion model, and ORCA is used as the control strategy, which ensures that there will be no collision between dynamic obstacles, and the motion model of the mobile robot can be an omnidirectional motion model or a bicycle model. The mobile robot uses the safe motion planning method proposed by the present invention that combines the control obstacle function and model predictive control as the control strategy. In addition to the dynamic obstacles, there are no other obstacles in the simulation scene. In order to fully evaluate the safety and effectiveness of the proposed method, we set the robot to be invisible, that is, the dynamic obstacles in the simulation scene will only avoid collisions with each other, but will not actively avoid the robot's direction of travel. Therefore, the mobile robot needs to actively avoid all dynamic obstacles, otherwise a collision will occur. In addition, the mobile robot and the dynamic obstacles have the same radius of 0.3m and the same reference speed of 1m / s. During the movement of the mobile robot to the end point, the robot can only observe the position and speed information of the dynamic obstacles, but cannot observe other information.

[0142] In the simulation environment configured above, the reaction-based method ORCA, the traditional optimization-based method MPC-DC (model predictive control algorithm based on distance constraints), the method MPC-CBF (model predictive control algorithm based on control obstacle function constraints) after adding the control obstacle function, and the method of the present invention were simulated and tested, and 500 random tests were performed on each. All control strategies are implemented in Python, with Casadi as the modeling language and IPOPT as the solver. The simulation test experiment was carried out on a computer running Ubuntu 20.04 system, which uses an Intel Core i5-12490f processor and 16GB of memory. The comparison indicators of the simulation experiment are success rate, collision rate, average navigation time and average solution time. Success rate: The percentage of mobile robots that safely reach the destination. Collision rate: The percentage of mobile robots that collide with dynamic obstacles in the process of reaching the destination. Average navigation time: The average time it takes for the mobile robot to safely reach the destination, in seconds. Average solution time: The average time required for the method of the present invention to solve the model predictive control optimization problem each time, in milliseconds.

[0143] Specific simulation test experimental results are shown in Table 1. It can be seen from the table that in a dynamic environment, due to the violation of the reciprocity assumption, the reaction-based method ORCA performs poorly due to short-sightedness. The traditional model predictive controller MPC-DC based on distance constraints cannot avoid dynamic obstacles in advance, so it is easy to enter high-risk areas and cause collisions. Although MPC-CBF, which adds CBF as a distance constraint, is a hard constraint, it can significantly improve the safety of motion planning, but it will also lead to a decrease in navigation efficiency due to solution failure. The method provided by the present invention can not only improve the safety of motion planning of mobile robots in dynamic environments, but also reduce the probability of solution failure, thereby achieving the effect of improving the efficiency of mobile robots reaching their destinations.

[0144] Table 1

[0145] method Success rate Collision rate Average navigation time Average solution time ORCA 47.0％ 52.6％ 11.04 - MPC-DC 36.2％ 63.6％ 12.97 45.11 MPC-CBF 73.6％ 26.2％ 16.73 42.47 Method of the present invention 99.6％ 0.4％ 14.35 55.08

[0146] See also Figures 3 to 6 ,In the 330th test case, the navigation paths of these controller methods are compared.,It can be seen from the visualized trajectories of the mobile robot and the,dynamic obstacles that the method provided by the present invention enables the,mobile robot to avoid crowded areas, and stop in time to avoid moving obstacles,,and finally successfully reach the destination.

[0147] 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.

[0148] 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.

[0149] 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.

[0150] 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.

[0151] 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 mobile robot safe motion planning method, characterized in that: The method comprises the following steps: S100, in a dynamic environment, obtaining state information of the mobile robot through a positioning module, and obtaining state information of surrounding dynamic obstacles through a perception module; S200, in a dynamic environment, constructing obstacle avoidance constraints based on current state information of the mobile robot and information of dynamic obstacles by combining the obstacle control function and model predictive control; S300, converting the obstacle avoidance constraint into an obstacle avoidance cost, and adding the cost equation into the model predictive control; S400, constructing a one-step safety constraint according to the relative degree of the obstacle avoidance constraint with respect to the system control input, so as to serve as a safety guarantee constraint of the system; S500, solving the optimization problem of model predictive control, and sending the optimal solution of the optimization problem as control input to the motion control module of the mobile robot; Among them, at the tth moment, the state information of the mobile robot is expressed as In the formula, (x t ,y t ) represents the two-dimensional coordinates of the mobile robot in the current map, θ t represents the orientation angle of the mobile robot, r represents the radius of the mobile robot, represents the current speed of the mobile robot along the x-axis and y-axis respectively; the state variable of the dynamic obstacle is expressed as In the formula, the superscript i represents the i-th dynamic obstacle, the first two variables represent the two-dimensional coordinates of the dynamic obstacle, and the last two variables represent the speed of the dynamic obstacle along the x-axis and y-axis respectively; Among them, at time t, the distance constraint based on the control obstacle function uses the method of exact penalty function, and after adding slack variables to convert the obstacle avoidance constraint into obstacle avoidance cost, it is added to the cost equation of model predictive control; after converting the obstacle avoidance constraint into obstacle avoidance cost, its optimization problem is expressed as follows: st for all k=0,...,N-1: x t+k+1|t =f(x t+k|t ,u t+k|t ), x t|t =x t , Where N represents the prediction time domain size; x t+k|t Indicates that from the current state x t The predicted state at the beginning of time t+k; input sequence u t+k|t = {u t|t , ..., u t+N-1|t }; The first term p of the cost function equation represents the terminal cost, and the second term q represents the process cost; is the added slack variable, its specific form is N o is the number of dynamic obstacles; α is the penalty weight; The first constraint x t+k+1|t is the kinematic constraint of the system; the second constraint x t+k|t and the third constraint u t+k|t They represent the state constraints and input constraints of the robot along the prediction time domain respectively; the fourth constraint x t|t is the initial state constraint; the last item of the constraint is a soft constraint, and its specific form is as follows: In the formula, r i is the radius of the mobile robot and the dynamic obstacle, and γ∈(0, 1] is a conservative coefficient.

2. The method according to claim 1, characterized in that The step S600 also includes: If there is no feasible solution to the optimization problem in step S400, the braking device of the mobile robot is activated.

3. The method according to claim 1, characterized in that In the step S200, At time t, the optimization problem based on the control barrier function and model predictive control is expressed as follows: stfor allk=0,...,N-1: x t+k+1|t =f(x t+k|t ,u t+k|t ), x t|t =x t , 4. The method according to claim 3, characterized in that In the step S400, The relative degree d is the control input u t Appears in the system state constraint h(x t ) in the delay step; After using the generalized control barrier function as a single-step constraint to join the model predictive control optimization problem, the safety guarantee constraint of the system is obtained as follows: Among them, the conservative coefficient η∈(γ, 1].

5. The method according to claim 4, characterized in that In step S500, At time t, the final form of the optimization problem based on the control barrier function and model predictive control is expressed as follows: st for all k=0,...,N-1: x t+k+1|t =f(x t+k|t ,u t+k|t ), x t|t =x t , By solving the above optimization problem, the optimal solution sequence at time t is obtained, which is expressed as follows: in t+k|t ={in t|t ,...,in t+N-1|t } The first element of the optimal solution sequence is issued to the mobile robot as a control input. 6 . A computer-readable storage medium having program instructions stored thereon, wherein the program instructions, when executed by a processor, implement the method according to any one of claims 1 to 5.

7. A mobile robot safe motion planning device, characterized in that: include: A computer device comprising a computer readable storage medium according to claim 6.

Images