A robot dynamic obstacle avoidance method and system

Abstract

The present invention discloses a robot dynamic obstacle avoidance method and system, including: S1 detecting the surrounding environment, initializing and determining the starting point and target point, and generating a point cloud; S2 inputting the point cloud into a synchronous positioning and mapping module, generating the robot posture, and maintaining a three-dimensional probability map; S3 generating a global path according to a fast random search tree and a three-dimensional probability map, and generating a local map and an obstacle prediction trajectory according to the point cloud; S4 generating a local trajectory according to the obstacle prediction trajectory and the global path, and sending a motion instruction to the robot; S5 the robot moves along the local trajectory according to the motion instruction; the present invention predicts the trajectory of the obstacle by generating a local map and an obstacle prediction trajectory according to the point cloud, and generating a local trajectory according to the obstacle prediction trajectory and the global path, so that avoidance actions can be made in advance, which has better safety; compared with the method of model predictive control + obstacle curve fitting, the obstacle prediction of the present invention is more accurate.

Description

Technical Field

[0001] The present invention relates to the field of robot motion planning, and in particular to a robot dynamic obstacle avoidance method and system. Background Art

[0002] A robust, safe, and efficient path planning algorithm is needed to enable autonomous vehicles to plan collision-free paths around static obstacles like streetlights and roadblocks, as well as dynamic obstacles common in everyday life, such as pedestrians, vehicles, and dogs. Therefore, developing a path planning algorithm that avoids dynamic obstacles and applying it to autonomous vehicles will have significant application value in scenarios such as shopping malls, campuses, airports, and office spaces.

[0003] Robot motion planning can generally be divided into global planning and local planning. Global planning refers to generating an optimal (e.g., shortest or least time-consuming) reference path in large-scale static scenarios (e.g., larger than 100m*100m). Local planning refers to avoiding complex obstacles and generating a collision-free trajectory in smaller-scale scenarios (e.g., smaller than 100m*100m). Global planning is relatively time-consuming to generate reference trajectories, resulting in lower update frequencies. Local planning, on the other hand, ensures robot safety by generating real-time motion commands to drive the robot's motion.

[0004] Local planning can be categorized into search-based, sampling-based, and optimization-based methods based on their implementation methods. Search-based algorithms include the hybrid A-star algorithm and the dynamic window approach (DWA); sampling-based algorithms include the hybrid RRT algorithm (Rapidly-exploring Random Trees) and the hybrid RRT-star algorithm; and optimization-based algorithms include the MPC algorithm (Model Predictive Control) and the LQR algorithm (Linear–quadratic regulator).

[0005] The prior art discloses an AGV (Automated Guided Vehicle) path planning method based on an improved DWA algorithm in a dynamic environment, which belongs to the technical field of AGV path planning. The method first designs a local DWA algorithm by obtaining local map information around the AGV, classifies dynamic obstacles in the local map, evaluates the speed of dynamic obstacles, improves the AGV's obstacle avoidance judgment ability for dynamic obstacles, and reduces the degree to which the AGV deviates from the shortest path. Secondly, in order to solve the problem that the DWA algorithm needs to adjust the direction when moving to the path point, the DWA evaluation function is optimized to shorten the adjustment time of the AGV at the path point, and the inflection point of the AGV global path is optimized by the improved DWA algorithm to improve the AGV transportation efficiency. Finally, simulation verification is carried out, and the simulation results show that the improved DWA algorithm proposed by the method can ensure real-time obstacle avoidance of the AGV in an environment with complex dynamic conditions, while shortening the transportation time of the AGV and improving the transportation efficiency.

[0006] However, the DWA algorithm primarily samples multiple speeds in velocity space and simulates the trajectory of these speeds over a certain period of time. This sampling mechanism doesn't guarantee optimal paths, and its judgment of dynamic obstacle states is too crude, preventing accurate prediction of the obstacle's future velocity and position. This compromises safety during obstacle avoidance.

[0007] The prior art also provides a local trajectory planning method for indoor mobile robots based on dynamic obstacle motion information. This method, characterized by using a single-line laser radar (LIDAR) to acquire the position information of all obstacle point clouds based on the AMCL (adaptive Monte Carlo Localization) algorithm, then segmenting and linearly fitting all laser point clouds, removing known obstacles from the map, and marking newly appearing potential dynamic obstacles with appropriately sized circles. Dynamic obstacle detection is then performed by changing the coordinates of the circle centers. The coordinates of the dynamic obstacle centers are continuously acquired and their motion equations are solved using least squares. Finally, the dynamic obstacle velocity and direction cost map is expanded, and the dynamic obstacle avoidance is achieved safely by combining it with the DWA algorithm. Current local trajectory planning algorithms treat dynamic obstacles as transient static obstacles, lacking both proactiveness and safety. This method can achieve safer obstacle avoidance.

[0008] However, the method of fitting obstacles using the least squares algorithm lacks robustness. When the obstacle position noise is relatively large, inaccurate predictions often occur. The solution of directly placing the obstacle trajectory into the cost map and then using the DWA algorithm for planning is too conservative and ignores the real-time nature of obstacles. It can ensure safety but reduces the optimality of the path. Summary of the Invention

[0009] The purpose of the present invention is to solve the problem of improving the safety of dynamic obstacle avoidance of a robot.

[0010] To this end, the present invention proposes a robot dynamic obstacle avoidance method, comprising the following steps: S1, detecting the surrounding environment, initializing and determining the starting point and target point, and generating a point cloud; S2, inputting the point cloud into a synchronous positioning and mapping module to generate the robot posture and maintain a three-dimensional probability map; S3, generating a global path based on a fast random search tree and a three-dimensional probability map, and generating a local map and an obstacle prediction trajectory based on the point cloud; S4, generating a local trajectory based on the obstacle prediction trajectory and the global path, and sending motion instructions to the robot; S5, the robot moves along the local trajectory according to the motion instructions.

[0011] In some embodiments of the present invention, step S3 further includes generating a local map based on the point cloud; parameterizing the obstacle onto the local map through clustering to generate a minimum obstacle envelope ellipse; estimating and predicting the state of the minimum obstacle envelope ellipse through Kalman filtering to generate an obstacle prediction trajectory in the forward time domain.

[0012] In some embodiments of the present invention, step S4 further includes performing motion planning based on the obstacle prediction trajectory and the global path, combining a dynamic obstacle control function with a model predictive control algorithm to generate the local trajectory, and sending a motion instruction.

[0013] In some embodiments of the present invention, the state of the obstacle minimum envelope ellipse includes shape and position; the position covariance R of the obstacle minimum envelope ellipse is p The modified parameter k is proposed, and its expression is as follows:

[0014]

[0015] where R p，max and R p，min represents the boundary of the position variance, Ξ min，crit and Ξ max，crit The index Ξ that represents the confidence level of the minimum enveloping ellipse position of the obstacle p According to the estimated state and variance value of the obstacle minimum envelope ellipse, the state of the obstacle minimum envelope ellipse in the future time domain is predicted, and the expression is as follows:

[0016] x k =Ax k-1 ;

[0017] P k =AP k-1 A T +Q;

[0018] Where A is the obstacle motion equation, x k is the state of the obstacle at time k, x k-1 is the state of the obstacle at time k-1, P k is the variance of the obstacle at time k, P k-1 is the variance of the obstacle at time k-1, and Q is the covariance of the system noise.

[0019] In some embodiments of the present invention, an indicator Ξ for evaluating the MBE position confidence is defined: p , the expression is as follows:

[0020]

[0021] where x ob，t-i，t and Denote the time t, r is a superscript, and represents the actual minimum envelope ellipse position and the actual position of the obstacle at the i-th step in the backward time domain; define Ξ η To evaluate the shape change of the minimum envelope ellipse, the time domain is set to m steps, Ξ η The estimates are as follows:

[0022]

[0023] in represents the average value of η, η To estimate Ξ p , the formula is as follows:

[0024]

[0025] Where κ and γ are the proportionality coefficient and the power coefficient, respectively.

[0026] In some embodiments of the present invention, the ellipse is expanded with uncertainty. On both axes, it is magnified by σ to Where σ is the extension of the ellipse axis; define r = r p +r η To describe the uncertainty of the ellipse, where r p and r η are obtained from the covariance of the position and shape of the ellipse respectively; in order to find the minimum ellipse of the envelope Minkowski sum, the minimum value of σ is obtained by solving the following equation:

[0027]

[0028] For the obstacle O(t:t+N|t) in the prediction time domain, when k increases from 0 to N, the uncertainty of the obstacle increases and the corresponding ellipse also expands accordingly.

[0029] In some embodiments of the present invention, the dynamic control barrier function expression is as follows:

[0030]

[0031] Where X is a variable and h is a continuously differentiable function. The expression is as follows:

[0032]

[0033] The safety set C is the lower level set of the continuously differentiable function, and its expression is

[0034] in is a state set; the dynamic control barrier function is for all There is an extension class Function γ, the expression is as follows:

[0035]

[0036] Where u is the input and st is the setting.

[0037] In some embodiments of the present invention, the center of the robot is connected to the minimum enveloping ellipse of the obstacle, and the distance between the minimum enveloping ellipse outline of the obstacle and the robot is calculated; the set of distances between the minimum enveloping ellipse outline of the obstacle and the robot sweeps through the control obstacle function area and is constrained by the boundary of the control obstacle function to prevent the robot from approaching the obstacle too quickly;

[0038] The dynamic control barrier function expression is as follows:

[0039] Δh(X k ,u k )≥-γh(X k ), 0<γ<1;

[0040] Δh(X k ,u k ):=h(X k+1 )-h(X k );

[0041] where u k is the controller input, X k is the state variable, γ is the extended class function; each obstacle is represented by an area The minimum enveloping ellipse is expressed as follows:

[0042]

[0043] At time k, the distance between the robot and the periphery of the minimum enveloping ellipse of the obstacle is calculated using the following expression:

[0044]

[0045] where l i (k) is the distance between the robot and the periphery of the minimum enveloping ellipse of the obstacle, a i (k) is the semi-major axis of the obstacle ellipse, b i (k) is the semi-minor axis of the obstacle ellipse, and δ represents the angle formed by the straight line and the major axis of the ellipse. The quadratic form of the dynamic control obstacle function is expressed as follows:

[0046]

[0047] Where p(k) is the state of the robot at time k, is the position of the obstacle’s minimum envelope ellipse at time k, d safe For a safe distance.

[0048] In some embodiments of the present invention, the fast random search tree in step S3 adopts a fast random search tree based on plane fitting in a non-flat terrain navigation framework based on plane fitting.

[0049] In some embodiments of the present invention, a model predictive control algorithm is combined with the dynamic control barrier function to establish a model predictive control algorithm and a dynamic control barrier function framework. The model predictive control algorithm and the dynamic control barrier function framework are expressed as follows:

[0050]

[0051] in is the state set, is the input state set, is the terminal state set, J is the optimization objective function, x t+1 =f(x t ,u t ) is the dynamic model of the robot, and the reference path given by the global planner is The terminal cost is The process cost is Among them, P, Q, R, S are weight matrices, The system is The state of being, Represents the control input of the robot.

[0052] The present invention also provides a robot dynamic obstacle avoidance system, comprising: a sensor, which can implement the above-mentioned robot dynamic obstacle avoidance method, and also comprising: a synchronous positioning and mapping module, a global planning module, a local perception module, and a local planning module; the sensor is used to detect the surrounding environment and generate a point cloud; the synchronous positioning and mapping module generates a robot posture according to the point cloud and maintains a three-dimensional probability map; the global planning module generates a global path according to a fast random search tree and a three-dimensional probability map; the local perception module generates a local map and an obstacle prediction trajectory according to the point cloud; the local planning module generates a local trajectory according to the obstacle prediction trajectory and the global path, and sends motion instructions to the robot.

[0053] The present invention has the following beneficial effects:

[0054] The present invention generates a local map and obstacle prediction trajectory based on the point cloud, and generates a local trajectory based on the obstacle prediction trajectory and the global path. This predicts the trajectory of the obstacle, allowing evasive action to be taken in advance, which provides better safety. Compared with the method of model predictive control + obstacle curve fitting, the obstacle prediction accuracy of the present invention is higher.

[0055] Other beneficial effects of the embodiments of the present invention will be further described below. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 This is a schematic diagram of the workflow in an embodiment of the present invention;

[0057] Figure 2 Schematic diagram of system composition in an embodiment of the present invention;

[0058] Figure 3 is a local perception flow chart in an embodiment of the present invention;

[0059] Figure 4a Schematic diagram of fitting results in an embodiment of the present invention;

[0060] Figure 4b is a curve diagram showing the change of confidence of a cylindrical obstacle under static conditions in an embodiment of the present invention;

[0061] Figure 4c is a curve diagram showing the change in confidence of a cylindrical obstacle under motion conditions in an embodiment of the present invention;

[0062] Figure 5 is a schematic diagram of an example of MPC-D-CBF in an embodiment of the present invention;

[0063] Figure 6a This is a front view of the algorithm robot deployed in an embodiment of the present invention;

[0064] Figure 6bis a side view of the deployment of the algorithm robot in an embodiment of the present invention;

[0065] Figure 7 2 is a diagram showing the obstacle avoidance effect of the robot in an embodiment of the present invention.

[0066] The reference numerals are as follows:

[0067] 1 is RS-Helios 32-line laser radar, 2 is WiFi, 3 is onboard computer, 4 is power supply DETAILED DESCRIPTION

[0068] The present invention will be further described below with reference to the accompanying drawings and in combination with preferred embodiments. It should be noted that, unless there is a conflict, the embodiments and features in the embodiments of the present application can be combined with each other.

[0069] It should be noted that the directional terms such as left, right, up, and down in this embodiment are merely relative concepts or are based on the normal use state of the product and should not be considered as restrictive.

[0070] Local planning can be categorized into search-based, sampling-based, and optimization-based methods based on their implementation methods. Search-based algorithms include the hybrid A-star algorithm and the dynamic window approach (DWA); sampling-based algorithms include the hybrid RRT algorithm (Rapidly-exploring Random Trees) and the hybrid RRT-star algorithm; and optimization-based algorithms include the MPC algorithm (Model Predictive Control) and the LQR algorithm (Linear–quadratic regulator). Search-based algorithms are relatively time-consuming but can yield an optimal path solution. Sampling-based algorithms generally lack path optimality but are less time-consuming. The hybrid RRT-star algorithm, for example, achieves asymptotic optimality through continuous path iteration. Optimization-based algorithms use gradient descent to determine the robot's future trajectory and, by defining the range of optimization parameters, can avoid various obstacles. With the advancement of computing power, these algorithms have become increasingly popular in recent years.

[0071] Before introducing the specific implementation methods, the idea of ​​the present invention is first introduced as follows:

[0072] Research on safety-critical optimal planning and control for mobile robots has been actively pursued in recent decades. By improving the robustness and efficiency of existing autonomous navigation solutions, autonomous robot navigation has been widely applied in many fields. However, autonomous navigation in dynamic and unstructured environments remains a challenge. The main difficulties are reflected in the following aspects:

[0073] 1. Stable detection and prediction of obstacles in unstructured environments;

[0074] 2. Parameter representation and uncertainty analysis of obstacles;

[0075] 3. Motion planning algorithm for real-time motion trajectory of obstacles.

[0076] The present invention combines global planning and local planning to generate a two-level motion control framework, which can ensure that the robot can avoid obstacles and ensure safety while maintaining the optimal path during movement. The robot dynamic obstacle avoidance method proposed in this invention can achieve the following functions:

[0077] 1. The ability to detect and track obstacles in real time;

[0078] 2. The ability to estimate the shape, position and speed of obstacles in real time;

[0079] 3. The ability to predict the state of obstacles in real time;

[0080] 4. The ability to avoid static and dynamic obstacles while following the global path in real time;

[0081] 5. The ability to change the minimum safe distance between the robot and obstacles according to specific application scenarios.

[0082] The following embodiment of the present invention proposes a dynamic obstacle avoidance method for a robot.

[0083] The workflow of the robot dynamic obstacle avoidance method according to the embodiment of the present invention is as follows: Figure 1 As shown:

[0084] S1. Detect the surrounding environment, initialize the starting point and target point, and generate a point cloud;

[0085] S2. Input the point cloud into a simultaneous localization and mapping module to generate a robot pose and maintain a three-dimensional probability map;

[0086] S3. Generate a global path based on a fast random search tree and a three-dimensional probability map, and generate a local map and obstacle prediction trajectory based on the point cloud;

[0087] S4, generating a local trajectory according to the obstacle prediction trajectory and the global path, and sending a motion instruction to the robot;

[0088] S5. The robot moves along the local trajectory according to the motion instruction.

[0089] The embodiment of the present invention provides a robot dynamic obstacle avoidance system, including a simultaneous localization and mapping module (SLAM) module, a global planning module, a local planning module, and a local perception module. Figure 2 As shown, from left to right: The LiDAR sensor detects the surrounding environment and generates a point cloud. The SLAM module receives the point cloud and generates a 3D probability map for the robot's position and posture, maintaining it in real time. The global planning module generates a reference path based on the PF-RRT* in the PUTN algorithm and the map. The point cloud is used to generate a local map. Obstacles are parameterized onto the local map through clustering to generate the MBE. The Kalman filter estimates the current motion state of the MBE, generating a corresponding motion trajectory in the forward time domain. The predicted trajectory and reference path are sent to the local planning module for motion planning. This module combines D-CBF and MPC to generate a safe trajectory for the robot and sends motion commands in real time. The UGV receives the motion commands and moves along the trajectory.

[0090] An embodiment of the present invention also provides a robot dynamic obstacle avoidance system, comprising: a sensor, which can implement the robot dynamic obstacle avoidance method in the above embodiment, and also comprising: a synchronous positioning and mapping module, a global planning module, a local perception module, and a local planning module; the sensor is used to detect the surrounding environment and generate a point cloud; the synchronous positioning and mapping module generates a robot posture according to the point cloud and maintains a three-dimensional probability map; the global planning module generates a global path according to a fast random search tree and a three-dimensional probability map; the local perception module generates a local map and an obstacle prediction trajectory according to the point cloud; the local planning module generates a local trajectory according to the obstacle prediction trajectory and the global path, and sends motion instructions to the robot.

[0091] The following further describes the robot dynamic obstacle avoidance method according to an embodiment of the present invention with reference to the accompanying drawings:

[0092] 1. Obstacle Prediction

[0093] like Figure 3 As shown, local perception, while taking into account observation uncertainty, predicts obstacle trajectories and delineates safe and obstacle areas in real time. In this process, a high-level graph is generated from the real-time point cloud. Gradient and step-height processing are then performed on the high-level graph to create a local map. Obstacle identification and data association are then performed within the local map.

[0094] Due to the noise in the point cloud and the variation in the obstacle detection angle, the shape η of the MBEob = [a, b, θ], where a, b, θ represent the semi-major axis, semi-minor axis, and offset angle of the ellipse, respectively. Rapid and large changes will occur, resulting in the position x of the MBE ob =[x ob ,y ob ] deviates from the true value. To address this problem, the embodiment of the present invention proposes an indicator Ξ for evaluating the MBE position confidence p .

[0095]

[0096] where x ob，t-i，t and They represent the MBE position and the real position of the obstacle at the i-th step in the backward time domain at time t (r is a superscript, indicating real). η To evaluate the degree of shape change of the MBE, the time domain is set to m steps. In general, the shape of the obstacle does not change rapidly (for example, rapid expansion). Therefore, within the limited area of ​​the local map, the shape of the obstacle can be considered to remain unchanged. η The estimates are as follows:

[0097]

[0098] in represents the average value of η.

[0099] because It is impossible to measure accurately in the actual environment. η Can be used to estimate Ξ p Through data analysis, the embodiment of the present invention uses the formula To complete this estimation task, κ and γ are the proportional coefficient and power coefficient respectively. and Ξ p The embodiment of the present invention conducts experiments on obstacles of different speeds and sizes, such as Figure 4a As shown, it can be seen that when [κ, γ] = [5.5, 1.3], the formula can well estimate obstacles with a radius less than 1.5m and a speed less than 0.9m / s.

[0100] Figure 4a When κ is 5.5 and γ is 1.3 and Ξ p The correlation coefficient of varies with the speed and size of the obstacle, where the horizontal axis represents the size of the obstacle, the vertical axis represents the speed of the obstacle, and the vertical axis represents the and Ξ p The correlation coefficient of . Figure 4b 、 Figure 4care the true values ​​of the cylindrical obstacle under static and moving conditions, respectively. p and estimated value The change of , where the horizontal axis T is the time interval and the vertical axis is the variance. In the experiment, the cylinder diameter is set to 1m and the motion state is set to uniform linear motion at 0.5m / s. p and The correlation coefficient is 0.9133 when the obstacle is at rest, and 0.9059 when the obstacle is at a constant speed.

[0101] In order to reduce the influence of point cloud noise, the embodiment of the present invention uses KF to update the state of the obstacle and uses MBEξ as observation. The state variable is set as a vector The state transfer matrix and observation matrix for 9-dimensional variables are:

[0102]

[0103] Where T is the time interval, I and O are the identity matrix and the zero matrix respectively. The change of the MBE position is caused by two reasons: (a) the movement of the obstacle itself, and (b) the MBE shape η ob Based on the previous analysis, the position covariance R p A correction parameter k is proposed to reduce the deviation of anomalies:

[0104]

[0105] where R p，max and R p，min represents the boundary of the position variance, Ξ min，crit and Ξ max，crit Indicates Ξ p Thus, when the shape of the MBE changes rapidly, R p Increase to reduce the vibration of the MBE position. According to the current estimated state and the corresponding variance value, x k =Ax k-1 and P k =AP k-1 A T +Q is used to predict the state in the future time domain, where Q is the covariance of the system noise.

[0106] In order to improve security, the embodiment of the present invention uses uncertainty to expand the ellipse. On both axes, it is magnified by σ to Where σ is the extension of the ellipse axis. Define r = r p +r η To describe the uncertainty of the ellipse, where rp and r η In order to find the minimum ellipse of the envelope Minkowski sum, the minimum value of σ is obtained by solving the following equation

[0107]

[0108] For the obstacle O(t:t+N|t) in the prediction time domain, when k increases from 0 to N, the uncertainty of the obstacle increases and the corresponding ellipse also expands accordingly.

[0109] 2. Local Planning Algorithm

[0110] It has been proven that CBF (Control Barrier Functions) can be used for UGV obstacle avoidance. However, the safety of traditional CBF for dynamic obstacles is still a challenge. Unlike CBF, D-CBF (Dynamic Control Barrier Function) considers obstacles to be movable. The robot position, obstacle position, and obstacle shape are defined as For criticality safety control, the set C is defined as a continuously differentiable function The lower level set of :

[0111]

[0112] In this invention, the embodiment of the present invention refers to C as a security set. Referring to the definition of CBF, D-CBF can be defined as: For all And there is an extension class Function γ:

[0113]

[0114] By definition, a D-CBF is a CBF on χ. Due to the CBF theorem, the safety set C is forward-invariant and asymptotically stable. Therefore, the set of components in C corresponding to the robot positions is also forward-invariant and asymptotically stable, which means that the control system is safe.

[0115] like Figure 5 As shown, the robot x(t:t+N|t) avoids and predicts obstacles O i (t:t+N|t) is the optimal path in the next N steps. Connect the center of the robot to the ellipse, and the distance between the ellipse outline and the robot can be calculated as d i (t: t+N|t). This distance set sweeps across the CBF area and is constrained by the CBF boundary to prevent the robot from approaching obstacles too quickly. In a discrete system, D-CBF can be expressed as

[0116] Δh(X k ,uk )≥-γh(X k ),0<γ≤1

[0117] Where, Δh(X k ,u k ):=h(X k+1 )-h(X k ).

[0118] Each dynamic obstacle is represented as a region The ellipse can be described and At time step t, the set of all obstacles is described as From the derivation of Kalman prediction in the previous section, we can know that the obstacle O i The forward time domain N state prediction is O i (t: t+N|t). In order to simplify the symbols, the embodiment of the present invention uses k to replace t+k|t. i (k) and the center of the robot p(k) = [x(k), y(k)], at time k, by solving the simultaneous equations of the line and the ellipse, calculate l i (k) From O i (k) Distance to its periphery:

[0119]

[0120] where δ represents the angle between the line and the major axis of the ellipse. Therefore, D-CBF is expressed in quadratic form as follows:

[0121]

[0122] where d safe For a safe distance.

[0123] In order to plan the robot's future posture and achieve a collision-free motion trajectory, the embodiment of the present invention uses MPC to achieve this and combines D-MPC to establish an MPC-D-CBF framework. To establish MPC, the embodiment of the present invention first uses the discrete time equation x t+1 =f(x t ,u t ) to describe the dynamic model of the robot, where The system is The state of being. Represents the control input of the robot

[0124]

[0125] in and They are the state set, input state set and terminal state set, J is the optimization objective function, and the reference path given by the global planner is Terminal price The process cost is Where P, Q, R, and S are weight matrices.

[0126] 3. Robot Deployment

[0127] The algorithm of the embodiment of the present invention works under the ROS Melodic operating system. The local perception module works at a frequency of 10 Hz to 20 Hz, and the local planning module works at a frequency of 10 Hz. Figure 6a 、 Figure 6b As shown, Scout 2.0 is a four-wheel differential mobile robot equipped with a power source 4 and Wi-Fi 2. The only sensor used is an RS-Helios 32-line lidar 1 with a 70° vertical field of view and a 55° field of view below the horizon to eliminate blind spots. Two onboard computers 3 are Intel@NUCs (i5 2.4GHz CPU and 16GB of RAM), used to run the planning and SLAM algorithms, respectively.

[0128] like Figure 7 As shown, the dotted line represents the global path generated by PF-RRT*. The boxes represent the robot's poses planned by MPC-D-CBF. The scene shows the trajectories of two pedestrians and an electric vehicle. The uncertainty of the MBE is represented by increasing color from small to large. The sequence of elliptical cylinders represents the predicted obstacle trajectories.

[0129] In this scenario, when the robot encounters an obstacle (pedestrian, electric car), the algorithm of the present invention generates a trajectory based on the predicted trajectory of the obstacle. For example, in (a), when the pedestrian is still on the robot's left side, the algorithm of the present invention predicts that its state in the next N steps will be a trajectory to the right. Therefore, the robot adopts an avoidance strategy from the left. This not only improves the safety of the robot's movement, but also allows it to reach the target point in a shorter time.

[0130] The embodiments of the present invention have the following advantages:

[0131] 1. The robot can avoid complex dynamic obstacles under the control of the algorithm of the embodiment of the present invention;

[0132] 2. The robot can optimize the path while minimizing time while avoiding obstacles;

[0133] 3. Predicting the trajectory of obstacles and the trajectory of the robot improves safety.

[0134] Compared with traditional model predictive control and model predictive control methods based on traditional control obstacle functions, the present invention predicts the trajectory of obstacles, so that avoidance actions can be taken in advance, which has better safety; compared with the model predictive control + obstacle curve fitting method, the obstacle prediction accuracy of the present invention is higher.

[0135] The embodiments of the present invention also have the following advantages:

[0136] 1. The proposed D-CBF (Dynamic Control Barrier Function) extends the safety of traditional CBF.

[0137] 2. Use D-CBF and model predictive control to complete the task of dynamic obstacle avoidance.

[0138] 3. Estimate and predict the shape and position of obstacles through Kalman filtering with uncertainty.

[0139] 4. A function is proposed to estimate the uncertainty of MBE position through the change of MBE morphology.

[0140] 5. The pf-RRT* algorithm and MPC-D-CBF are combined to complete the coordination of global planning and local planning.

[0141] The abbreviations and key terms used in the embodiments of the present invention are defined as follows:

[0142] Simultaneous Localization And Mapping (SLAM)

[0143] Plane-fitting based Uneven Terrain Navigation Framework (PUTN)

[0144] Plane-Fitting based Optimal Rapidly-Exploring Random Trees (PF-RRT*)

[0145] Control Barrier Function (CBF)

[0146] Dynamic Control Barrier Function (D-CBF)

[0147] Model Predictive Control (MPC)

[0148] Fast lidar odometry (FAST-LO)

[0149] Octomap

[0150] Minimum bounding ellipses (MBE)

[0151] Unmanned Ground Vehicle (UGV)

[0152] Kalman Filter (KF)

[0153] The above is a further detailed description of the present invention in conjunction with specific preferred embodiments, and the specific implementation of the present invention should not be considered to be limited to these descriptions. Those skilled in the art will recognize that several equivalent substitutions or obvious variations can be made without departing from the scope of the present invention, and that any equivalent performance or application should be considered to fall within the scope of protection of the present invention.

Claims

1. A robot dynamic obstacle avoidance method, characterized in that: The following steps are involved: S1. Detect the surrounding environment, initialize the starting point and target point, and generate a point cloud; S2. Input the point cloud into a simultaneous localization and mapping module to generate a robot pose and maintain a three-dimensional probability map; S3, generating a global path based on a fast random search tree and a three-dimensional probability map, and generating a local map and an obstacle prediction trajectory based on the point cloud; S4, generating a local trajectory according to the obstacle prediction trajectory and the global path, and sending a motion instruction to the robot; S5, the robot moves along the local trajectory according to the motion instruction; Step S3 includes generating a local map based on the point cloud; parameterizing obstacles onto the local map through clustering to generate a minimum obstacle envelope ellipse; estimating and predicting the state of the minimum obstacle envelope ellipse through Kalman filtering to generate an obstacle prediction trajectory in the forward time domain; The state of the obstacle minimum envelope ellipse includes shape and position; the position covariance R of the obstacle minimum envelope ellipse p The modified parameter k is proposed, and its expression is as follows: where R p,max and R p,min represents the boundary of the position variance, Ξ min,crit and Ξ max，crit The index Ξ that represents the confidence level of the minimum enveloping ellipse position of the obstacle p the boundaries; According to the estimated state and variance value of the obstacle minimum envelope ellipse, the state of the obstacle minimum envelope ellipse in the future time domain is predicted, and the expression is as follows: x k =Ax k-1 ; P k =AP k-1 From T +Q; Where A is the obstacle motion equation, x k is the state of the obstacle at time k, x k-1 is the state of the obstacle at time k-1, P k is the variance of the obstacle at time k, P k-1 is the variance of the obstacle at time k-1, and Q is the covariance of the system noise.

2. The method according to claim 1, wherein Step S4 includes performing motion planning based on the obstacle prediction trajectory and the global path, combining the dynamic obstacle control function with the model predictive control algorithm to generate the local trajectory, and sending motion instructions.

3. The method according to claim 1, wherein Define the indicator Ξ used to evaluate the confidence of the minimum enveloping ellipse position p , the expression is as follows: where x ob，t-i，t and They represent the minimum envelope ellipse position and true position of the obstacle at the i-th step in the backward time domain at time t, respectively. r is a superscript, indicating true; Definition η To evaluate the shape change of the minimum envelope ellipse, the time domain is set to m steps, Ξ η The estimates are as follows: in represents the average value of η, η To estimate Ξ p , the formula is as follows: Where κ and γ are the proportionality coefficient and the power coefficient, respectively.

4. The method according to claim 3, wherein Expand the ellipse with uncertainty, the ellipse On both axes, it is magnified by σ to Where σ is the extension of the ellipse axis length; Define r = r p +r η To describe the uncertainty of the ellipse, where r p and r η are obtained from the covariance of the position and shape of the ellipse respectively; in order to find the minimum ellipse of the envelope Minkowski sum, the minimum value of σ is obtained by solving the following equation: For the obstacle O(t:t+N|t) in the prediction time domain, when k increases from 0 to N, the uncertainty of the obstacle increases and the corresponding ellipse also expands accordingly.

5. The method according to claim 2, wherein The dynamic control barrier function expression is as follows: Where X is a variable and h is a continuously differentiable function. The expression is as follows: The safety set C is the lower level set of the continuously differentiable function, and its expression is in is the state set; The dynamic control barrier function is There is an extension class Function γ, the expression is as follows: Where u is the input and st is the setting.

6. The method according to claim 2, wherein Connecting the center of the robot to the minimum enveloping ellipse of the obstacle, and calculating the distance between the minimum enveloping ellipse outline of the obstacle and the robot; the set of distances between the minimum enveloping ellipse outline of the obstacle and the robot sweeps across the dynamic control obstacle function area and is constrained by the boundary of the dynamic control obstacle function to prevent the robot from approaching the obstacle too quickly; The dynamic control barrier function expression is as follows: where u k is the controller input, X k is the state variable, γ is the extended class function; Each obstacle is represented as a region The minimum enveloping ellipse is expressed as follows: At time k, the distance between the robot and the periphery of the minimum enveloping ellipse of the obstacle is calculated using the following expression: where l i (k) is the distance between the robot and the periphery of the minimum enveloping ellipse of the obstacle, a i (k) is the semi-major axis of the obstacle ellipse, b i (k) is the semi-minor axis of the obstacle ellipse, and δ represents the angle formed by the straight line and the major axis of the ellipse; The quadratic form of the dynamic control barrier function is expressed as follows: Where p(k) is the state of the robot at time k, x i ob (k) is the position of the obstacle’s minimum envelope ellipse at time k, l i (k) is the distance from the robot position to the outer edge of the obstacle's minimum enveloping ellipse at time k, d safe For a safe distance.

7. The method according to claim 1, wherein The fast random search tree in step S3 adopts a fast random search tree based on plane fitting in a non-flat terrain navigation framework based on plane fitting.

8. The method according to claim 2, wherein The model predictive control algorithm is combined with the dynamic control barrier function to establish a model predictive control algorithm and a dynamic control barrier function framework. The model predictive control algorithm and the dynamic control barrier function framework are expressed as follows: in is the state set, is the input state set, is the terminal state set, J is the optimization objective function, x t+1 =f(x t ,u t ) is the dynamic model of the robot, and the reference path given by the global planner is The terminal cost is The process cost is Among them, P, Q, R, S are weight matrices, The system is The state of being, Represents the control input of the robot.

9. A robot dynamic obstacle avoidance system, comprising: The sensor is characterized in that it can implement a robot dynamic obstacle avoidance method according to any one of claims 1 to 8, further comprising: a synchronous positioning and mapping module, a global planning module, a local perception module, and a local planning module; The sensor is used to detect the surrounding environment and generate a point cloud; The simultaneous localization and mapping module generates the robot pose based on the point cloud and maintains a three-dimensional probability map; The global planning module generates a global path based on a fast random search tree and a three-dimensional probability graph; The local perception module generates a local map and obstacle prediction trajectory based on the point cloud; The local planning module generates a local trajectory according to the obstacle prediction trajectory and the global path, and sends a motion instruction to the robot.

Images