Collision-free coverage path planning method for variable speed curvature limited robot

Abstract

The application discloses a collision-free coverage path planning method for a variable-speed curvature limited robot, aims at solving the safety and efficiency problems introduced by fixed turning radius and speed of an existing coverage path planning method, and has the technical scheme that a coverage path planning system for the variable-speed curvature limited robot is constructed, which is composed of a client, a server and a robot terminal; the client collects target environment information, the server performs unit decomposition on a target area to obtain an endpoint set P of a coverage unit; then the server constructs a potential field representing a collision risk, and calculates a variable-speed collision-free path with minimum time consumption from an endpoint to another endpoint in the set P; finally, a mathematical model is constructed to obtain a Tour with minimum cost sequence of visiting all endpoints in the set P, and a final collision-free path is obtained. The coverage path is optimized by constructing a risk potential field and a variable-speed path, so that the coverage efficiency is effectively improved while the path safety is ensured.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of mobile robots, and particularly relates to a coverage path planning method for a variable-speed curvature-limited robot in a known environment. The present application can be applied to robot offline coverage path planning in scenarios such as environmental monitoring, automated agriculture, and indoor cleaning. BACKGROUND

[0002] As the basis for automated robots to complete applications such as environmental monitoring, map reconstruction, search and rescue, coverage path planning (CPP) aims to design a path that visits all points in the target area, which minimizes the coverage time while avoiding obstacles. In real coverage scenarios, CPP usually involves environments containing complex obstacles and curvature-limited robots. Therefore, it is necessary to design a collision-free coverage path planning method for a curvature-limited robot.

[0003] The most typical dynamic constraints of robots are the limits on the motion curvature and turning speed. For the path planning problem of robots with fixed motion curvature and speed in an obstacle-free environment, Dubins proposed a geometric method to plan the shortest path between two points, which is called Dubins path. Dubins path has the advantages of analytical expression and fast calculation. At present, researchers have proposed a series of Dubins coverage methods based on Dubins path. Dubins coverage method is a typical curvature-constrained robot area coverage method, which is widely used in agricultural automation, patrol and rescue, and seabed detection. For example, the MinNWT algorithm proposed in the literature (Yu, Xin, Thaddeus A. Roppel, and John Y. Hung. "An optimization approach for planning robotic field coverage." IECON 2015-41st Annual Conference of the IEEE Industrial Electronics Society. IEEE, 2015) abstracts the coverage task into a connected graph, and obtains the final coverage path by solving the sequence of the minimum cost of visiting the vertices in the graph. For another example, the semi-BCD algorithm proposed in the literature (Lewis, Jeremy S., et al. "Semi-boustrophedon coverage with a dubins vehicle." 2017 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017) deletes and simplifies the connected graph by using a graph pruning strategy, thereby reducing the calculation time.

[0004] Although Dubins coverage methods have many advantages, they have two problems. The first problem is that Dubins coverage methods assume that the robot can only move at a fixed turning radius and speed, which limits the ability to reduce the time cost by accelerating the robot. In fact, the robot can move at different speeds, and sometimes the change in speed can effectively save time. However, current time-optimal path planning for variable-speed curvature robots mostly focuses on point-to-point path planning, and the time-optimal CPP problem for variable-speed curvature robots has not been solved.

[0005] The second problem is the safety of the robot in the obstacle environment. Currently, most Dubins coverage methods are for obstacle-free environments, or for environments where the robot can move on obstacles. However, in many real-world applications such as area monitoring, battlefield damage assessment, and agricultural automation, there may be obstacles in the target environment or on the boundary that cannot be crossed. Therefore, directly applying Dubins coverage methods to such obstacle environments may produce infeasible results. In addition, some Dubins coverage methods set a buffer area for robot turning between obstacles and the area to be covered to avoid collisions between the robot and the obstacle. Generally, robot turning is considered a non-working area, and a wider buffer area will reduce coverage, while a narrower buffer area will increase the risk of collision between the robot and the obstacle.

[0006] How to solve the coverage path planning problem of variable-speed curvature constrained robots in obstacle environments is a key problem in the field of coverage path planning. SUMMARY

[0007] The technical problem to be solved by the present application is to provide a collision-free coverage path planning method for variable-speed curvature constrained robots, which ensures the safety of the robot while minimizing the coverage time.

[0008] The technical solution of the present application is: first, a coverage path planning system for variable-speed curvature constrained robots is constructed, which consists of a client, a server and a robot terminal. The client collects target environment information, and the server performs unit decomposition on the target area to obtain a set of endpoint P of the coverage unit. Then the server constructs a potential field representing the collision risk, and calculates the time-optimal variable-speed collision-free path from the endpoints in P. Finally, a mathematical model is constructed to obtain the Tour with the minimum cost sequence of visiting all endpoints in P, and then the final collision-free path is obtained.

[0009] The specific technical solution is:

[0010] First, a coverage path planning system for variable-speed curvature constrained robots is constructed. The coverage path planning system consists of a server, a client and a robot terminal.

[0011] The client is a mobile terminal, a PC or a robot. The client has detection sensors such as a laser radar and a binocular camera. The client obtains target area environment information (including the area of the target area, obstacles and boundaries) through the detection sensors, and abstracts the target area environment information into a map G. Elements with a value of 0 in G represent obstacles, and elements with a value of 1 represent grids to be covered. The client sends the map G and a user-specified speed sampling interval Inter to the server.

[0012] The server is a server or a PC, and a coverage path planning system is installed thereon. The coverage path planning system is connected to the client and the robot terminal. It is composed of a region decomposition module, a risk field construction module, a point-to-point path planning module and a sequence generation module.

[0013] The region decomposition module is connected to the detection sensors of the client, the robot terminal, the point-to-point path planning module and the sequence generation module. It receives the target area map information G sent by the client, the user-specified speed sampling interval Inter and the robot parameters sent by the robot terminal, decomposes the target area according to the task sensor coverage radius r1 in the robot parameters, obtains a unit endpoint set P, and sends the unit endpoint set P, the target area map information G and the user-specified speed sampling interval Inter to the point-to-point path planning module and the sequence generation module.

[0014] The risk field construction module is connected to the region decomposition module, the point-to-point path planning module and the sequence generation module. It receives the target area map information G and the robot parameters sent by the region decomposition module, constructs a potential energy field PE representing the collision risk of the robot according to the robot minimum safety distance parameter d safe , and sends the PE to the point-to-point path planning module and the sequence generation module.

[0015] The point-to-point path planning module is connected to the region decomposition module, the risk field construction module and the sequence generation module. It receives the unit endpoint information P, the grid map G and the user-specified speed sampling interval Inter from the region decomposition module, receives the potential energy field PE from the risk field construction module, and receives the robot parameters such as the forward speed s, the maximum angular velocity u max from the sequence generation module. According to the forward speed, the acceleration and the maximum angular velocity of the robot in the robot parameters, the robot forward speed set PS and the unit-to-unit collision-free path are calculated, and then a two-dimensional cost matrix CM representing the path cost and a two-dimensional path matrix CPATH storing the candidate path are obtained. CM and CPATH are sent to the sequence generation module.

[0016] The sequence generation module is connected with the robot terminal, the region decomposition module, the risk field construction module and the point-to-point path planning module. The sequence generation module receives robot parameters from the robot terminal, receives the unit endpoint set P, the target region map information G and the user-specified speed sampling interval Inter from the region decomposition module, receives PE from the risk field construction module, receives the two-dimensional cost matrix CM and the two-dimensional path matrix CPATH from the point-to-point path planning module, models the coverage path planning problem as a traveling salesman problem, generates a collision-free coverage path PATH that visits all coverage units, and sends the coverage path PATH to the robot terminal.

[0017] The robot terminal is a real robot, such as an Ackerman robot. The robot terminal is loaded with: (1) a radar sensor that can detect obstacles in a rectangular area with a width of r2, r2∈(0, 200] meters; the obstacle information detected by the radar sensor is sent to the positioning module, and a common radar sensor is a Sill laser radar that can detect obstacles in an area with a radius of 8 meters; (2) a task sensor for performing a coverage task (such as the side brush of a floor cleaning robot in a house cleaning task, the blade of an automatic lawn mower in a lawn mowing task, etc.), which can cover a rectangular area with a width of r1, r1≤r2; (3) a positioning module for real-time positioning (a commonly used positioning module is the ROS AMCL positioning package (G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid mapping with Rao-Blackwellized particle filters,” IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34–46, Feb 2007. Translated as: G. Grisetti, C. Stachniss, and W. Burgard, Improved techniques for grid mapping with Rao-Blackwellized particle filters, Robotics Journal, February 2007, Vol. 23, pp. 34-46, Ubuntu 16 and later versions are available); (4) a robot path following module, a commonly used path following module is the Pure_Persuit algorithm (reference Coulter, R. Craig. Implementation of the pure pursuit path tracking algorithm. Carnegie-Mellon UNIV Pittsburgh PA Robotics INST, 1992, translated as Coulter, R. Craig. Implementation of the pure pursuit path tracking algorithm. Carnegie-Mellon University Pittsburgh PA Robotics Institute, 1992, Ubuntu 16 and later versions are available) for path tracking; (5) other software and hardware control modules necessary for robot operation, including the robot's inherent operating system, underlying drive system, motion control system, etc., which are inherent configurations of commercial robots. The robot terminal sends the robot's forward speed s (s∈[s min , s max ]), acceleration a (a∈[a min , a max ]), maximum angular velocity u max , minimum safety distance d safe , and r1, r2, etc. parameters to the server.

[0018] Second step, the target area environment information (including the target area, the boundary, the position of the obstacle) is collected by the probe sensor of the client, the grid map G of the target environment is constructed, and the grid map G is sent to the area decomposition module of the server. Let the number of grid rows in the grid map G be A, and the number of grid columns be B.

[0019] Third step, the area decomposition module of the server receives the grid map G sent by the client, the sampling interval Inter specified by the user, and the task sensor coverage radius r1 sent by the robot terminal, decomposes the target area to obtain the unit endpoint set P, and then sends the unit endpoint set P, the grid map G and the sampling interval Inter specified by the user to the point-to-point path planning module and the sequence generation module. The specific method is as follows:

[0020] 3.1 The area decomposition module adopts Semi-BCD decomposition algorithm (Lewis, Jeremy S., et al. "Semi-boustrophedon coverage with a dubins vehicle." 2017 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, i.e. Lewis, Jeremy S., et al. Semi-boustrophedon coverage with a dubins vehicle. "2017 IEEE / RSJ IROS Conference, page 4, section V, paragraph 2 to paragraph 4), which divides the target area into multiple rectangular units according to the grid map G. In each rectangular unit, the width of the edge located on the x-axis is equal to r1, and the edge located on the y-axis intersects with the obstacle or the target area boundary. A single rectangular unit corresponds to a single coverage task, and all rectangular units constitute an initial coverage task set C, C = {c1,..., c n ,..., c N}, N is the total number of coverage tasks, N is a positive integer, and c n represents the nth coverage task in C.

[0021] 3.2 Set the two intersection points of the center line and the upper and lower edges of c n as the upper endpoint p n and the lower endpoint p n，1 of c n，2 . The coordinates of p n，1 are (px, pu), and the coordinates of p n，2 are (px, pd), where px represents the x-axis coordinate of the center line of c n , and pu represents the lower endpoint p n of c n .The y-axis coordinate of the upper end point of the center line, pd represents the y-axis coordinate of the lower end point of the center line. N units in C correspond to generate 2N end points {p 1，1 , p 1，2 ,..., p n，1 , p n，2 ,..., p N，1 , p N，2}, wherein p n，1 , p n，2 represent the upper and lower two end points of the nth unit in C. For the convenience of description, p n，1 and the lower end point p n，2 are simplified to obtain the unit end point set P = {p1,..., p i ,..., p 2N}, 1≤i≤2N, p i is the ith end point in {p 1，1 , p 1，2 ,..., p n，1 , p n，2 ,..., p N，1 , P N，2}. The region decomposition module sends the unit end point set P, the grid map G and the user-specified sampling interval Inter to the point-to-point path planning module and the sequence generation module.

[0022] In the fourth step, the risk field construction module of the server receives the target region grid map G and the minimum safety distance parameter d safe of the robot sent by the region decomposition module, constructs a risk potential field PE representing the collision risk, and sends PE to the point-to-point path planning module and the sequence generation module. The method for constructing the collision risk potential field PE is:

[0023] 4.1 The risk field construction module constructs the risk potential field PE according to the grid map G. PE is a two-dimensional matrix containing AxB elements. The value of each element in the matrix is between 0 and 1. The value of 1 of the matrix element represents that the robot is sure to have a collision risk at the element, and the value of 0 represents that the robot is safe at the element. The larger the value of the element in PE, the greater the possibility of collision between the robot and the obstacle at the element. The method for constructing PE is:

[0024] 4.1.1 Initialize the value of all elements of the risk potential field PE to 0, and initialize the row number a to 1 and the column number b to 1.

[0025] 4.1.2 According to the grid map G, calculate the risk potential pe a，b of the element in the ath row and the bth column of PE a，b The calculation of pe

[0026]

[0027] where d represents the Euclidean distance between the cell G(a, b) in the ath row and bth column in the grid map G and the nearest obstacle cell to G(a, b) in G. Equation (1) shows that if the distance between G(a, b) and the nearest obstacle cell is greater than the minimum safety distance d safe of the robot, the robot is safe at G(a, b) and the risk value pe a，b = 0; otherwise, the robot has a risk of collision with the obstacle at G(a, b) and the risk value is pe a，b .

[0028] 4.1.3 If a = A and b = B, it means that the risk field PE is constructed, go to 4.2; if a = A but b ≠ B, let a = 1 and let b = b + 1, go to 4.1.2; if a ≠ A and b ≤ B, let a = a + 1, go to 4.1.2.

[0029] 4.2 The risk field construction module sends PE to the point-to-point path planning module and the sequence generation module.

[0030] In the fifth step, the point-to-point path planning module of the server receives the set of cell end points P sent by the region decomposition module, and calculates the cost matrix CM and the path matrix CPATH corresponding to P by the following method:

[0031] 5.1 Initialize the cost matrix CM and the path matrix CPATH corresponding to P. The cost matrix CM is a two-dimensional matrix with a dimension of 2N x 2N, and any element cm d，e in the matrix represents the time cost of the collision-free path between the dth end point p d and the eth end point p e in P, d = 1,..., 2N, e = 1,..., 2N. The path matrix CPATH is a two-dimensional matrix with a dimension of 2N x 2N, and any element cpath d，e in the matrix represents the set of waypoints of the collision-free path between p d and p e , which satisfies cpath d，e = {(x f , y f , θ f ), f = 1,..., F}, F being the number of waypoints in CPATH, which is a positive integer, where (x f , y f , θ f ) represents the fth waypoint of the collision-free path between p d and p e , x f , y fx y, θ are the x / y axis coordinates of the robot at the fth waypoint, θ is the orientation of the robot at the fth waypoint. Initialize all elements of cost matrix CM as 0, initialize all elements in CPATH as empty set, initialize row index i as 1, initialize column index j as 1. f x y, θ are the x / y axis coordinates of the robot at the fth waypoint, θ is the orientation of the robot at the fth waypoint. Initialize all elements of cost matrix CM as 0, initialize all elements in CPATH as empty set, initialize row index i as 1, initialize column index j as 1.

[0032] 5.2 Point-to-point path planning module receives P, grid map G and velocity sampling interval Inter sent by region decomposition module, receives robot parameters such as forward velocity, acceleration, maximum angular velocity of the robot sent by sequence generation module, calculates collision-free path best_path from p d to p e and time consumption T min of best_path, let cpath d，e = best_path, let cm d，e = T min . The method is as follows:

[0033] 5.2.1 Point-to-point path planning module receives P from region decomposition module, and obtains the pose information (x d , y d , θ d ) of the robot at p e and the pose information (x e , y e , θ e ) of the robot at p d according to the relative positions of p d and p d , x d , y d are the x / y axis coordinates of the robot at p e , θ e is the orientation of the robot at p e , x e , y e are the x / y axis coordinates of the robot at p d , θ d is the orientation of the robot at p d . The method is as follows:

[0034] 5.2.1.1 According to P sent by the region decomposition module, obtain the x / y axis coordinates (x e , y e ) of the robot at p e , the x / y axis coordinates (x d , y e ) of the robot at p d .

[0035] 5.2.1.2 If p d and pe Belonging to the same unit, see section 5.2.1.3, Computational Robot in p d Orientation θ d , and computational robots in p e Orientation θ e If p d and p e They do not belong to the same unit; refer to section 5.2.1.4, which discusses computational robots in p. d Orientation θ d And calculate the robot's position in p e Orientation θ e .

[0036] 5.2.1.3 At this time, p d and p e If they belong to the same unit, if p d p is the upper endpoint of this unit. e The lower endpoint of this unit indicates that the robot starts from p. d Enter the cell, then cover the cell from top to bottom, and finally from p e Leave, therefore set If p d p is the lower endpoint of this unit. e Set the upper endpoint of this unit. Go to 5.2.1.5.

[0037] 5.2.1.4 At this time, p d and p e Belonging to different units, if p d The upper endpoint of the cell indicates that the robot has covered p from bottom to top. d After the unit it belongs to, from p d Leave p d The cell to which it belongs. When covering a rectangular cell from bottom to top, the robot's orientation is... Therefore, the setting Otherwise If p e The upper endpoint of the cell indicates that the robot has covered p. d After the unit it belongs to, from p e Enter p e The corresponding cell, then covering p from top to bottom. e The cell to which it belongs. When covering a rectangular cell from top to bottom, the robot's orientation is... Therefore, the setting Otherwise Go to 5.2.1.5.

[0038] 5.2.1.5 Instruct the robot to operate in p d The pose is (x dy d θ d ), causing the robot to be in p e The pose is (x e y e θ e ).

[0039] 5.2.2 The point-to-point path planning module receives the robot sampling interval Inter sent by the region decomposition module, and the robot acceleration a (a∈[a]) sent by the sequence generation module. min a max ], a min ≥0, a max >0, a min <a max , where a min and a max (representing the robot's minimum and maximum forward acceleration, respectively), and forward velocity s (s∈[s...)). min s max ], s min ≥0, s max >0, s min <s max , where s min and s max (representing the robot's minimum and maximum forward speeds, and maximum angular velocity u, respectively) max By taking equal-interval samples of the robot's forward velocity based on the robot's parameters, a set of forward velocities S = {s1, ..., s2} is obtained. k , ..., s K}, where K is the number of velocity sampling points, which is a positive integer, and s k It is the k-th velocity sampling point in S, s1 = s min s K =s max Any two velocities in S form a velocity pair, and the K velocities in S correspond to K 2 There are 10 velocity pairs, which constitute the set of forward velocity pairs PS = {(s...} k1 s k2 ), s k1 s k2 ∈S,k1,k2=1,..,K}.

[0040] 5.2.3 The point-to-point path planning module receives the risk potential energy field PE sent by the risk field construction module and calculates p. d and p e The best path (best_path) between the collision-free paths and the time T of that path. min The method is as follows:

[0041] 5.2.3.1 Initialize the minimum path time Tmin Initialize best_path as an empty set, and let k1 = 1, k2 = 1.

[0042] 5.2.3.2 Calculate the set of collision-free variable speed Dubins paths DP corresponding to the speed pair (s k1 , s k2 ). The method is as follows,

[0043] 5.2.3.2.1 Initialize DP as an empty set.

[0044] 5.2.3.2.2 Set the speeds and turning radii of the six Dubins paths in DP. The method is as follows: According to the definition of Dubins paths, Dubins paths include RSL, RSR, LSR, LSL, LRL, and RLR six paths, where R represents a right arc segment, L represents a left arc segment, and S represents a straight line segment. According to whether a straight line segment (S) is included, the above six Dubins paths can be divided into two types: straight line segment Dubins paths (including RSL, RSR, LSR, and LSL four paths) and no straight line segment Dubins paths (including LRL and RLR two paths). Both straight line segment Dubins paths and no straight line Dubins paths include three path segments, and let the turning radii of the first to third path segments be r α , r β , and r γ , and the robot speeds be s α , s β , and s γ .

[0045] 5.2.3.2.2.1 Calculate the speeds and turning radii of straight line segment Dubins paths. Straight line segment Dubins paths include three path segments, which are the first arc segment, the second straight line segment, and the third arc segment. Straight line segment Dubins paths require that the first arc segment and the third arc segment must maintain a fixed turning radius, so the point-to-point path planning module sets the turning radius r α of the first arc segment in the Dubins path as , sets the robot speed s α of the first arc segment in the Dubins path as s k1 , sets the turning radius r γ of the third arc segment in the Dubins path as , and sets the robot speed s γ of the third arc segment in the Dubins path as s k2The second path segment of the Dubins path is a straight segment, and its turning radius is set to r. β If the value is 0, the speed configuration method is adopted. and Jan Faigl. "On finding time-efficient trajectories for fixed-wing aircraft using dubins paths with multiple radii." Proceedings of the 35th Annual ACM Symposium on Applied Computing. 2020. Translated as Petr and Jan Faigl., “Using Durbins paths with multiple radii to find time-efficient tracks for fixed-wing aircraft.” Proceedings of the 35th ACM Conference on Applied Computing, 2020, page 2, section 3.2, paragraphs 1-2. Figure 3 ), calculate the robot's velocity s on the second straight segment. β The method is as follows: Let the lengths of the first arc segment, the second straight line segment, and the third arc segment of the Dubins path be α, β, and γ, respectively.

[0046] When s α <s γ At that time, the robot is made to accelerate at its maximum speed a. max From s α Accelerate to s γ Given a path length of L1, let the robot accelerate to the minimum speed a. min From s max Decelerate to s γ The path length is L2. The robot's velocity s on the second straight segment is... β There are four possibilities: (1) If the length of the straight path segment β is less than L1, it means that the length of the straight path segment is insufficient for the robot to move from s α Start accelerating to s γ Therefore, this path is physically infeasible, so s is set. β (2) If β = L1, it means that the length of the straight line segment is just enough to make the robot start from s α Start accelerating to s γ Therefore, s is set β From s α Start with maximum acceleration a max Accelerate to s γ (3) If L1 < β < L1 + L2, it means that the length of the straight line segment allows the robot to move from s α Accelerate to a speed of s γ Bigger than smax Small speed s t1 s α <s t <s max Then decelerate to s γ Therefore, s is set β From s α Start with maximum acceleration a max Accelerate to s t Then with minimum acceleration a min Decelerate to s γ To make the robot from s α Start with maximum acceleration a max Accelerate to s t1 The path length at that time is AL1, where Make the robot from s t1 Start with minimum acceleration a min Decelerate to s γ The path length at that time is DL1, where AL1 and DL1 satisfy the equation AL1 + DL1 = β, which contains only one variable s. t1 Solving this equation will yield s. t1 (4) If β>L1+L2, it means that the length of the straight line segment allows the robot to move from s α Accelerate to s max Then decelerate to s γ Therefore, s is set β From s α Start with maximum acceleration a max Accelerate to maximum speed s max Then with minimum acceleration a min Decelerate to s γ .

[0047] When s α =s γ At that time, the robot was moved from s α Start with maximum acceleration a max Accelerate to s max Then with minimum acceleration a min Decelerate to s γ At that time, the length of the entire path is L3. The robot's velocity s on the second straight segment is... β There are two possibilities: (1) If the straight path segment β < L3, set s β From s α Start with maximum acceleration a max Accelerate to s t2 Then with minimum acceleration a min Decelerate to s γ To make the robot from s αStart at maximum acceleration a max accelerate to s t2 with path length AL2, where Let the robot start at s t with minimum acceleration a min decelerate to s γ with path length DL2, where AL2and DL2satisfy the equation AL2+ DL2= β, which has only one variable s t2 Solving this equation gives the value of s t2 (2) If β ≥ L3, set s β Start at s α with maximum acceleration a max accelerate to maximum velocity s max and then decelerate to s min with minimum acceleration a γ .

[0048] When s α > s γ , let the robot start at velocity s α with minimum acceleration a min decelerate to s γ with path length L4. Let the robot start at s α with maximum acceleration a max accelerate to s max and then decelerate to s min with minimum acceleration a γ . The total path length is L5. The velocity s β of the robot on the second straight segment has four possibilities: (1) If β < L4, set s β to 0; (2) If β = L4, set s β Start at s α with minimum acceleration a min decelerate to s α from s γ . (3) If L4< β < L5, set s β Start at s α with maximum acceleration a max accelerate to s t3 and then decelerate to s min with minimum acceleration a γ . Let the robot start at s α with maximum acceleration a max accelerate to s t3 with path length AL3, where Let the robot start at s t3Start with the minimum acceleration a min Decelerate to s γ The path length is DL3, where AL3 and DL3 satisfy the equation AL3+DL3=β, which has only one variable s t3 Solving this equation gives the value of s t3 If β>L5, then set s β Start from s α Accelerate to the maximum speed s max With the maximum acceleration a max Then decelerate to s min With the minimum acceleration a γ .

[0049] 5.2.3.2.2.2 Calculate the speed and turning radius of the Dubins path that does not contain a straight line segment. The three path segments in the Dubins path that does not contain a straight line segment are all arc segments, and the turning and robot speed in the three arc segments must be equal, so set the turning radius r α , r β , r γ of the first to third path segments in the Dubins path that does not contain a straight line segment as Set the robot speed s α , s β , s γ of the first to third path segments as s k1 .

[0050] 5.2.3.2.3 Calculate the path point set of the six Dubins paths in DP. The method is as follows: According to the variable-speed Dubins path model (reference J. P. Wilson, K. Mittal, and S. Gupta, “Novel motion models for time-optimal risk-aware motion planning for variable-speed auvs,” in OCEANS 2019 MTS / IEEE SEATTLE. IEEE, 2019, pp. 1-5, translated as J. P. Wilson, K. Mittal, and S. Gupta, “Novel motion models for time-optimal risk-aware motion planning for variable-speed auvs,” in OCEANS 2019 MTS / IEEE Conference, Seattle, pp. 2-4), calculate the poses of the three path segments in the variable-speed Dubins path according to formulas (2), (3), and (4), respectively.

[0051]

[0052]

[0053] S σ (x, y, θ) = (x + σcosθ, y + σsinθ, θ) (4)

[0054] where L σ (x, y, θ), R σ (x, y, θ) and S σ (x, y, θ) represent the coordinate pose of the robot on the left turning arc segment, the right turning arc segment and the straight line segment respectively; σ in formula (2) and formula (3) represents the radian offset of the robot from the start point of the arc segment, and σ of the formula (4) path segment represents the displacement of the robot on the straight line segment, r σ represents the turning radius of the robot on the arc segment.

[0055] According to formula (2)-(4), all the waypoints of the three path segments of a single Dubins path can be obtained, and these waypoints constitute a waypoint set dp r , which satisfies dp r = {(x f , y f , θ f ), f = 1,..., F}, F is the number of navigation points in dp r , and F is a positive integer, where (x f , y f , θ f ) is the fth navigation point in the path dp r , x f , y f , θ f represent the x / y axis coordinates and orientation of the robot at the point; for RSL, RSR, LSR, LSL, LRL and RLR six paths, the waypoint sets of six variable speed Dubins paths are obtained respectively, and the six paths constitute a variable speed Dubins path set DP = {dp r , r = 1,..., 6}, where dp r is the rth path in DP.

[0056] 5.2.3.3 Calculate the time consumption set DT corresponding to each path in the collision-free variable speed Dubins path set DP, DT = {dt r , r = 1,..., 6}, where dt r represents the rth element in DT, and its value is the time consumption of the path dp r . The method is as follows:

[0057] 5.2.3.3.1 Initialize the 6 elements in DT with a very large positive number TA (e.g. TA = 1000000), and let r = 1.

[0058] 5.2.3.3.2 Determine whether there is a collision risk in path dp r . If there is a collision risk in path dp r , there is a waypoint u in path dp r such that the risk potential pe(u) at waypoint u is greater than 0, which means that there is a possibility of collision with the obstacle. Set dt r = TA; otherwise, it means that path dp r is safe and there is no collision risk. In the case where there is no collision risk in path dp r , calculate the corresponding path time consumption dt r of path dp r according to equation (5).

[0059]

[0060] where s α，r , s β，r and s γ，r represent the forward speed in the first path segment, the second path segment and the third path segment in path dp r , respectively; and a r , b r and g r represent the length of the first path segment, the second path segment and the third path segment in path dp r , respectively.

[0061] 5.2.3.3.3 If r = 6, it means that the calculation of time consumption set DT is completed, and go to 5.2.3.4; otherwise, let r = r + 1, and go to 5.2.3.3.2.

[0062] 5.2.3.4 Search for the element DT min with the smallest value in DT, and let DT min = min(DT). Let DT min be the wth element in DT, where 1 < w < 6. If DT min < T min , let T min = DT min , and let the optimal path best_path be the path dP w with the least time consumption in DP without collision, and go to 5.2.3.5; otherwise, keep T min unchanged, and go to 5.2.3.5.

[0063] 5.2.3.5 If k2≠B, let k2=k2+1, go to 5.2.3.2; if k2=B and k1≠A-1, let k2=1, k1=k1+1, go to 5.2.3.2; otherwise, it means that p has been obtained d and p e between them with the minimum time consumption, let cpath d，e =best_path, cm d，e =T min , go to 5.3.

[0064] 5.3 If d=2N and e=2N, it means that the cost matrix CM and CPATH corresponding to P have been calculated, go to 5.4; if d=2N and e≠2N, let d=1, e=e+1, go to step 5.2; if d≠2N and e≤2N, let d=d+1, go to step 5.2.

[0065] 5.4 The point-to-point path planning module sends CM and CPATH to the sequence generation module, go to the sixth step.

[0066] The sixth step, the sequence generation module of the server receives the unit endpoint set P sent by the region decomposition module, the cost matrix CM and CPATH sent by the point-to-point path planning module, and the robot parameters sent by the robot terminal, constructs a coverage path planning mathematical model, obtains a sequence Tour with the minimum cost of visiting all endpoints in P, and further obtains a coverage path PATH. The method is as follows:

[0067] 6.1 Initialize Tour as an empty set.

[0068] 6.2 The sequence generation module receives CM sent by the point-to-point path planning submodule, constructs a coverage path planning model, as shown in formulas (6)-(10):

[0069] Min (∑cm i，j *z i，j ), i=1, 2,..., 2N, j=1, 2,..., 2N, i≠j (6)

[0070] ∑z i，j =2N (7)

[0071]

[0072]

[0073] z 2×n1+1，2×n1+2 +z 2×n1+2，2×n1+1 =1, n1=1,..., N (10)

[0074] Where the independent variable z i，jThis indicates that the robot sequentially accesses endpoint p. i and p j The Boolean value. If z i，j =1 indicates that the robot is accessing endpoint p. i Then, access endpoint p. j If z i，j =0, indicating that the robot is accessing endpoint p. i After that, endpoint p will not be accessed. j Formula (6) represents finding the loop with the minimum cost of visiting all endpoints in P. Formula (7) represents that Tour contains 2N edges. Formula (8) represents that the robot can only enter the same endpoint once. Formula (9) represents that the robot can only leave the same endpoint once. Formula (10) represents that the robot will definitely visit the two endpoints of the same rectangular cell in sequence.

[0075] 6.2 The mathematical model established by formulas (6)-(10) is a traveling salesman problem. The sequence generation module utilizes existing traveling salesman problem solvers (common traveling salesman problem solvers include the LKH solver). https: / / aithub.com / unr-arl / LKH TSP and MATLAB TSP solver https: / / www.mathworks.com / matlabcentral / fileexchange / 64654-travelling-salesman-problem Solving equations (6)-(10) yields the sequence Tour = {t1, ..., t2} that minimizes the cost of visiting all endpoints in P. n , ..., t 2N}, where t n It is the nth endpoint in the Tour.

[0076] 6.3 Generate the overridden path PATH based on the endpoint sequence Tour and CPATH. The method is as follows:

[0077] 6.3.1 Initialize endpoint sequence number n = 1, and initialize the coverage path PATH to an empty set.

[0078] 6.3.2 Let the nth endpoint and the (n+1)th endpoint in Tour be u and v, respectively. Extract the minimum time-consuming collision-free path cpath from the path matrix CPATH. u，v The path is the path between the nth endpoint and the (n+1)th endpoint in the Tour. n Let PATH = PATH∪path n .

[0079] 6.3.3 If n≠2N-1, let n=n+1 and go to step 6.3.2; otherwise, go to 6.3.4 to calculate the path from the 2Nth endpoint of the Tour back to the 1st endpoint of the Tour.

[0080] 6.3.4 Let the 2Nth endpoint and the first endpoint in Tour be uu and vv respectively, and extract cpath from the path matrix CPATH. uu，vv , as the path between the 2Nth endpoint and the first endpoint in the Tour 2N Let PATH = PATH∪path 2N Go to 6.4.

[0081] 6.4 The sequence generation module sends the overwrite path PATH to the robot terminal, proceed to step 7.

[0082] Step 7: The robot terminal receives the coverage path PATH output by the sequence generation module. Based on the path following module, positioning module and robot operation software and hardware control module configured in the robot terminal, it moves according to the coverage path PATH (generally, it moves while performing tasks). This movement trajectory can achieve complete coverage of the target area.

[0083] The following technical effects can be achieved by using this invention:

[0084] 1. This invention can plan a collision-free coverage path with minimal time consumption for robots with variable speed and limited curvature.

[0085] 2. In steps three to five of this invention, a collision risk potential is constructed to ensure the safety of the coverage path, and a variable-speed Dubins path is constructed to ensure the efficiency of the path, thereby obtaining the collision-free path with the minimum time consumption between the two endpoints of any unit.

[0086] 3. The sixth step of this invention models the coverage path planning problem as a traveling salesman problem, obtains the sequence with the minimum cost of visiting all endpoints, and then obtains a safe and collision-free coverage path to visit all points in the target area.

[0087] To verify the feasibility of this invention, simulation experiments and physical tests were conducted to compare this invention with the semi-BCD algorithm and the MinNWT algorithm. Experimental results show that this invention effectively reduces the time consumed by the coverage path and the computation time of the coverage method while maintaining coverage accuracy. Furthermore, this invention is also applicable to Ackerman robots. Attached Figure Description

[0088] Figure 1 This is an overall flowchart of the present invention;

[0089] Figure 2 This is a logical structure diagram of the coverage path planning system for a variable-speed, curvature-constrained robot constructed in the first step of this invention.

[0090] Figure 3The comparison results of the coverage time, collision risk, coverage rate and calculation time consumption of the present application and semi-BCD algorithm and MinNWT algorithm with different advancing speeds in a scene with a size of 20m*20m. Figure 3 (a) is the coverage time comparison result of the present application and semi-BCD algorithm and MinNWT algorithm with an advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2. Figure 3 (b) is the maximum collision risk comparison result of the present application and semi-BCD algorithm and MinNWT algorithm with an advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2. Figure 3 (c) is the coverage rate comparison result of the present application and semi-BCD algorithm and MinNWT algorithm with an advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2. Figure 3 (d) is the calculation time consumption comparison result of the present application and semi-BCD algorithm and MinNWT algorithm with an advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2.

[0091] Figure 4 The comparison results of the coverage time, collision risk, coverage rate and calculation time consumption of the present application and semi-BCD algorithm and MinNWT algorithm with different advancing speeds in a scene with a size of 30m*30m. Figure 4 (a) is the coverage time comparison result of the present application and semi-BCD algorithm and MinNWT algorithm with an advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2. Figure 4 (b) is the maximum collision risk comparison result of the present application and semi-BCD algorithm and MinNWT algorithm with an advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2.Figure 4 (d) is the coverage rate comparison result of the present invention and semi-BCD algorithm and MinNWT algorithm with the advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s, under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2. Figure 4 (d) is the calculation time comparison result of the present invention and semi-BCD algorithm and MinNWT algorithm with the advancing speed of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s, under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2.

[0092] Figure 5 is a screenshot and planned path of the present invention in a real laboratory environment. Figure 5 (a) Laboratory environment and test robot. Figure 5 (b) is the planned coverage path.

[0093] Figure 6 is a process screenshot of the present invention in a real laboratory environment at four time points. The left side of each screenshot shows the planned coverage path and the pose of the robot at the current time, and the right side shows the pose of the robot in the real laboratory environment. DETAILED DESCRIPTION

[0094] As shown in Figure 1 , the present invention comprises the following steps:

[0095] First, a coverage path planning system for variable speed curvature constraint robots is constructed. The coverage path planning system, as shown in Figure 2 , is composed of a server, a client and a robot terminal.

[0096] The client is a mobile terminal, a PC or a robot. The client has detection sensors such as laser radar and binocular camera. The client obtains target area environment information (including target area area, obstacles, boundaries) through the detection sensors, and abstracts the target area environment information into a map G. The elements with value 0 in G represent obstacles, and the elements with value 1 represent the grids to be covered. The client sends the map G and the user-specified speed sampling interval Inter to the server.

[0097] The server is a server or a PC with a coverage path planning system installed. The coverage path planning system is connected to the client and the robot terminal. It is composed of a region decomposition module, a risk field construction module, a point-to-point path planning module and a sequence generation module.

[0098] The area decomposition module is connected with the detection sensor of the client, the robot terminal, the point-to-point path planning module and the sequence generation module, receives the target area map information G sent by the client, the speed sampling interval Inter specified by the user and the robot parameters sent by the robot terminal, decomposes the target area into a plurality of units according to the task sensor coverage radius r1 in the robot parameters, and sends the unit endpoint set P, the target area map information G and the speed sampling interval Inter specified by the user to the point-to-point path planning module and the sequence generation module.

[0099] The risk field construction module is connected with the area decomposition module, the point-to-point path planning module and the sequence generation module, receives the target area map information G and the robot parameters sent by the area decomposition module, constructs a potential energy field PE representing the collision risk of the robot according to the robot minimum safety distance parameter d safe in the robot parameters, and sends the PE to the point-to-point path planning module and the sequence generation module.

[0100] The point-to-point path planning module is connected with the area decomposition module, the risk field construction module and the sequence generation module, receives the unit endpoint P, the grid map G and the speed sampling interval Inter specified by the user from the area decomposition module, receives the potential energy field PE from the risk field construction module, receives the robot parameters such as the forward acceleration a, the forward speed s and the maximum angular velocity u from the sequence generation module, calculates the robot forward speed set PS and the collision-free path between units according to the forward speed, the acceleration and the maximum angular velocity of the robot in the robot parameters, further obtains a two-dimensional cost matrix CM representing the path cost and a two-dimensional path matrix CPATH storing the candidate path, and sends the CM and the CPATH to the sequence generation module. max

[0101] The sequence generation module is connected with the robot terminal, the area decomposition module, the risk field construction module and the point-to-point path planning module, receives the robot parameters from the robot terminal, receives the unit endpoint set P, the target area map information G and the speed sampling interval Inter specified by the user from the area decomposition module, receives the PE from the risk field construction module, receives the two-dimensional cost matrix CM and the two-dimensional path matrix CPATH from the point-to-point path planning module, models the coverage path planning problem into a traveling salesman problem, generates a collision-free coverage path PATH visiting all coverage units, and sends the coverage path PATH to the robot terminal.

[0102] ​The robot terminal is a real robot, and an Ackerman robot is used in this embodiment. The robot terminal is loaded with: (1) a radar sensor that can detect obstacles in a rectangular area with a width of r2, r2∈(0, 200] meters; the obstacle information detected by the radar sensor is sent to the positioning module, and a common radar sensor is a Sill laser radar that can detect obstacles in an area with a radius of 8 meters; (2) a task sensor for performing a coverage task (such as the side brush of a floor cleaning robot in a house cleaning task, the blade of an automatic lawn mower in a lawn mowing task, and the like), which can cover a rectangular area with a width of r1, r1≤r2; (3) a positioning module for real-time positioning (in this embodiment, a ROS AMCL positioning package is used); (4) a robot path following module (in this embodiment, a Pure_Persuit algorithm is used) for path tracking; (5) other software and hardware control modules necessary for the operation of the robot, including the robot's inherent operating system, bottom drive system, motion control system, and the like, which are inherent configurations of commercial robots. The robot terminal sends the robot's forward speed s (s∈[s min , s max ], acceleration a (a∈[a min , a max ], maximum angular velocity u max , minimum safety distance d safe , and r1, r2, and the like to the server.

[0103] In the second step, the detection sensor of the client collects the environmental information of the target area (including the area, boundary, and position of the obstacles of the target area), constructs a grid map G of the target environment, and sends the grid map G to the area decomposition module of the server. Let the number of rows of the grid map G be A and the number of columns be B.

[0104] In the third step, the area decomposition module of the server receives the grid map G sent by the client, the sampling interval Inter specified by the user, and the task sensor coverage radius r1 sent by the robot terminal, decomposes the target area, obtains a unit endpoint set P, and then sends the unit endpoint set P, the grid map G, and the sampling interval Inter specified by the user to the point-to-point path planning module and the sequence generation module. The specific method is as follows:

[0105] 3.1 The area decomposition module uses a Semi-BCD decomposition algorithm to divide the target area into multiple rectangular units according to the grid map G. In each rectangular unit, the width of the edge located on the x-axis is equal to r1, and the edge located on the y-axis intersects with the obstacle or the boundary of the target area. A single rectangular unit corresponds to a single coverage task, and all rectangular units constitute an initial coverage task set C, C={c1,..., c n ,..., c N}, where N is the total number of covered tasks, N is a positive integer, and c n This represents the nth coverage task in C.

[0106] 3.2 Setting c n The two intersection points of the midline and its upper and lower sides are c. n The upper endpoint p n，1 and lower endpoint p n，2 p n，1 The coordinates are (px, pu), p n，2 The coordinates are (px, pd), where px represents c. n The x-axis coordinate of the centerline, pu represents c n The y-coordinate of the endpoint above the centerline, pd represents the y-coordinate of the endpoint below the centerline. N cells in C correspond to 2N endpoints {p 1，1 p 1，2 , ..., p n，1 p n，2 , ..., p N，1 P N，2}, where p n，1 p n2 Let p represent the top and bottom endpoints of the nth unit in C. For ease of description, let p be... n，1 and lower endpoint p n，2 After simplification, we obtain the set of cell endpoints P = {p1, ..., p2}. i , ..., p 2N}, 1≤i≤2N, p i It is {p 1，1 p 1，2 , ..., p n，1 p n，2 , ..., p N，1 p N，2 The i-th endpoint in}. The region decomposition module sends the set of cell endpoints P, the grid map G, and the user-specified sampling interval Inter to the point-to-point path planning module and the sequence generation module.

[0107] Fourth, the risk field construction module on the server side receives the target region grid map G and the robot's minimum safe distance parameter d sent by the region decomposition module. safe A risk potential energy field PE representing collision risk is constructed and sent to the point-to-point path planning module and the sequence generation module. The method for constructing the collision risk potential energy field PE is as follows:

[0108] 4.1 The risk field construction module constructs the risk potential field PE from the grid map G. PE is a two-dimensional matrix containing A x B elements, and the value of each element in the matrix is between 0 and 1. The value of 1 for an element in the matrix means that the robot is certain to have a collision risk at the element, and the value of 0 means that the robot is safe at the element. The greater the value of an element in PE, the greater the possibility of the robot colliding with an obstacle at the element. The method of constructing PE is as follows:

[0109] 4.1.1 Initialize the value of all elements of the risk potential field PE to 0, and initialize the row number a to 1 and the column number b to 1.

[0110] 4.1.2 According to the grid map G, calculate the risk potential pe of the element in the a-th row and the b-th column of PE a，b , pe a，b is calculated as shown in equation (1):

[0111]

[0112] where d represents the Euclidean distance between the element G(a, b) in the a-th row and the b-th column of the grid map G and the nearest obstacle element G(a, b) in G. Equation (1) shows that if the distance between G(a, b) and the nearest obstacle element is greater than the minimum safety distance d safe of the robot, the robot is safe at G(a, b), and the risk value pe a，b = 0; otherwise, the robot has a risk of collision with an obstacle at G(a, b), and the risk value is pe a，b .

[0113] 4.1.3 If a = A and b = B, it means that the construction of the risk field PE is complete, go to 4.2; if a = A but b ≠ B, let a = 1 and let b = b + 1, go to 4.1.2; if a ≠ A and b ≤ B, let a = a + 1, go to 4.1.2.

[0114] 4.2 The risk field construction module sends PE to the point-to-point path planning module and the sequence generation module.

[0115] In the fifth step, the point-to-point path planning module of the server receives the set of cell endpoints P sent by the region decomposition module, and calculates the cost matrix CM and the path matrix CPATH corresponding to P, by the following method:

[0116] 5.1 Initialize the cost matrix CM and the path matrix CPATH corresponding to P. The cost matrix CM is a two-dimensional matrix with a dimension of 2N x 2N, and any element cm d，e in the matrix represents the d-th endpoint p d in P and the e-th endpoint p eThe time cost of a collision-free path between the given values ​​is given by d = 1, ..., 2N, e = 1, ..., 2N. The path matrix CPATH is a two-dimensional matrix of dimension 2N × 2N, where any element cpath is a given path. d，e p d and p e The set of waypoints with no collision paths between them, satisfying cpath d，e ={(x f y f θ f ), f = 1, ..., F}, where F is the number of waypoints in CPATH, which is a positive integer, where (x f y f θ f ) indicates that the robot is in p d and p e The f-th waypoint of the non-collision paths between them, x f y f Let θ be the x / y coordinates of the robot at the f-th waypoint. f Let be the robot's orientation at the f-th waypoint. Initialize all elements of the cost matrix CM to 0, initialize all elements of CPATH to an empty set, initialize row index i to 1, and initialize column index j to 1.

[0117] 5.2 The point-to-point path planning module receives P, the grid map G, and the velocity sampling interval Inter from the region decomposition module, and receives robot parameters such as the robot's forward speed, acceleration, and maximum angular velocity from the sequence generation module, and calculates the path from p... d Departure and arrival at p e The collision-free path best_path, and the time T of best_path. min Let cpath d，e =best_path, let cm d，e =T min The method is as follows:

[0118] 5.2.1 The point-to-point path planning module receives P from the region decomposition module, and plans the path based on P. d and p e The relative position is used to obtain the robot's position at p. d Pose information at (x) d y d θ d ) and p e Pose information at (x) e y e θ e ), x d y d For robots in p dx / y-axis coordinates of the robot at point p d orientation of the robot at point p d x / y-axis coordinates of the robot at point p e , y e orientation of the robot at point p e x / y-axis coordinates of the robot at point p e orientation of the robot at point p e The method is as follows:

[0119] 5.2.1.1 Obtain the x / y-axis coordinates of the robot at point p d (x d , y d ) according to the P sent by the area decomposition module, and the x / y-axis coordinates of the robot at point p e (x e , y e ).

[0120] 5.2.1.2 If p d and p e belong to the same unit, go to 5.2.1.3 to calculate the orientation θ d of the robot at point p e , and calculate the orientation θ e of the robot at point p d ; if p e and p d do not belong to the same unit, go to 5.2.1.4 to calculate the orientation θ d of the robot at point p e , and calculate the orientation θ e of the robot at point p d .

[0121] 5.2.1.3 At this time, p e and p d belong to the same unit, if p e is the upper end point of the unit, and p d is the lower end point of the unit, it indicates that the robot enters the unit from p e , then covers the unit from top to bottom, and finally leaves from p d , so set if p e is the lower end point of the unit, and p d is the upper end point of the unit, set go to 5.2.1.5.

[0122] 5.2.1.4 At this time, p e and p d belong to different units, if p d is the upper end point of the unit, it indicates that the robot covers p dThe unit belongs to p d Leave p d The unit belongs to p Therefore, set Otherwise, let If p e is the upper endpoint of the unit, indicating that the robot has covered p d The unit belongs to p e Enter p e The unit belongs to p e The unit belongs to p Therefore, set Otherwise, let Turn 5.2.1.5.

[0123] 5.2.1.5 Let the pose of the robot at p d be (x d , y d , θ d ), and let the pose of the robot at p e be (x e , y e , θ e ).

[0124] 5.2.2 The point-to-point path planning module receives the robot sampling interval Inter sent by the region decomposition module, the robot acceleration a (a ∈ [a min , a max ], a min ≥ 0, a max > 0, a min < a max , where a min and a max represent the minimum and maximum forward acceleration of the robot, respectively), the forward speed s (s ∈ [s min , s max ], s min ≥ 0, s max > 0, s min < s max , where s min and s max represent the minimum and maximum forward speed of the robot, respectively), and the maximum angular velocity u max and other robot parameters, and performs equal-interval sampling on the forward speed of the robot to obtain a forward speed set S = {s1,..., s k ,..., s K}, K is the number of speed sampling points, which is a positive integer, where s k is the kth speed sampling point in S, s1 = smin , s K max . Any two velocities in S form a velocity pair, K velocity pairs in S correspond to K 2 velocity pairs, which form the set of forward velocity pairs PS = {(s k1 , s k2 ), s k1 , s k2 ∈ S, kl, k2 = 1,.., K}.

[0125] 5.2.3 The point-to-point path planning module receives the risk potential field PE sent by the risk field construction module, calculates the collision-free path best_path between p d and p e and the time cost T min of the path. The method is as follows:

[0126] 5.2.3.1 Initialize the minimum path time cost T min as a very large positive number TA, TA = 1000000, initialize the optimal path best_path as an empty set, and let the sequence numbers kl = 1 and k2 = 1.

[0127] 5.2.3.2 Calculate the set of collision-free variable-speed Dubins paths DP under the condition of the velocity pair (s k1 , s k2 ). The method is as follows,

[0128] 5.2.3.2.1 Initialize DP as an empty set.

[0129] 5.2.3.2.2 Set the velocities and turning radii of the six Dubins paths in DP. The method is as follows: According to the definition of Dubins path, Dubins path includes RSL, RSR, LSR, LSL, LRL, and RLR six paths, where R represents a right turning arc segment, L represents a left turning arc segment, and S represents a straight line segment. According to whether a straight line segment (S) is included, the above six Dubins paths can be divided into two types: straight line segment Dubins paths (including RSL, RSR, LSR, and LSL four paths) and no straight line segment Dubins paths (including LRL and RLR two paths). Both straight line segment Dubins paths and no straight line Dubins paths include three path segments, and let the turning radii of the first to third path segments be r α , r β , and r γ , and the robot velocities be s α , s β , and s γ .

[0130] ​5.2.3.2.2.1 Calculate the speed and turning radius of the Dubins path with straight segments. The Dubins path with straight segments consists of three segments: a first arc segment, a second straight segment, and a third arc segment. The Dubins path with straight segments requires that the first and third arc segments maintain a fixed turning radius; therefore, the point-to-point path planning module sets the turning radius *r* of the first arc segment in the Dubins path. α for Set the robot speed s for the first arc segment in the Dubins path. α For s k1 Set the turning radius r of the third arc segment in the Dubins path. γ for Set the robot speed s for the third arc segment in the Dubins path. γ For s k2 The second path segment of the Dubins path is a straight segment, and its turning radius is set to r. β The value is 0. The velocity s of the robot on the second straight segment is calculated using the velocity configuration method. β The method is as follows: Let the lengths of the first arc segment, the second straight line segment, and the third arc segment of the Dubins path be α, β, and γ, respectively.

[0131] When s α <s γ At that time, the robot is made to accelerate at its maximum speed a. max From s α Accelerate to s γ Given a path length of L1, let the robot accelerate to the minimum speed a. min From s max Decelerate to s γ The path length is L2. The robot's velocity s on the second straight segment is... β There are four possibilities: (1) If the length of the straight path segment β is less than L1, it means that the length of the straight path segment is insufficient for the robot to move from s α Start accelerating to s γ Therefore, this path is physically infeasible, so s is set. β (2) If β = L1, it means that the length of the straight line segment is just enough to make the robot start from s α Start accelerating to s γ Therefore, s is set β From s α Start with maximum acceleration a max Accelerate to s γ (3) If L1 < β < L1 + L2, it means that the length of the straight line segment allows the robot to move from s α Accelerate to a speed of s γLarge but less than s max Small velocity s t1 , s α <s t <s max Then decelerate to s γ Thus set s β Start from s α with maximum acceleration a max to s t Then decelerate to s min with minimum acceleration a γ . Let the path length of the robot from s α with maximum acceleration a max to s t1 be AL1, where Let the path length of the robot from s t1 with minimum acceleration a min to s γ be DL1, where AL1and DL1satisfy the equation AL1+ DL1= β, which has only one variable s t1 Solving this equation gives the value of s t1 (4) If β > L1+ L2, the length of the straight line segment is such that the robot can accelerate from s α to s max and then decelerate to s γ Thus set s β Start from s α with maximum acceleration a max to maximum velocity s max and then decelerate to s min with minimum acceleration a γ .

[0132] When s α = s γ , let the robot start from s α with maximum acceleration a max to s max and then decelerate to s min with minimum acceleration a γ , the length of the entire path is L3. The velocity s β of the robot on the second straight line segment has two possibilities: (1) If the straight line path segment β < L3, set s β Start from s α with maximum acceleration a max to s t2 and then decelerate to s min with minimum acceleration a γ . Let the path length of the robot from sα Start with maximum acceleration a max Accelerate to s t2 The path length at that time is AL2, where Make the robot from s t Start with minimum acceleration a min Decelerate to s γ The path length at that time is DL2, where AL2 and DL2 satisfy the equation AL2 + DL2 = β, which contains only one variable s. t2 Solving this equation will yield s. t2 (2) If β≥L3, set s β From s α Start with maximum acceleration a max Accelerate to maximum speed s max Then with minimum acceleration a min Decelerate to s γ .

[0133] When s α >s γ At that time, the robot was made to start from speed s α Starting with minimum acceleration a min Decelerate to s γ The path length is L4. Let the robot start from s α Start with maximum acceleration a max Accelerate to s max Then with minimum acceleration a min Decelerate to s γ At that time, the length of the entire path is L5. The robot's velocity s on the second straight segment is... β There are four possibilities: (1) If β < L4, set s β (2) If β = L4, set s to 0; β From s α Start with minimum acceleration a min From s α Decelerate to s γ (3) If L4 < β < L5, set s β From s α Start with maximum acceleration a max Accelerate to s t3 Then with minimum acceleration a min Decelerate to s γ To make the robot from s α Start with maximum acceleration a max Accelerate to s t3 The path length at that time is AL3, where Make the robot from st3 Start at minimum acceleration a min decelerate to s γ with path length DL3, where AL3 and DL3 satisfy the equation AL3 + DL3 = β, which has only one variable s t3 Solving this equation gives the value of s t3 (4) If β > L5, then set s β Start at s α with maximum acceleration a max accelerate to maximum speed s max and then decelerate to s min with minimum acceleration a γ .

[0134] 5.2.3.2.2.2 Calculate the speed and turning radius of Dubins path without straight line segment. The three path segments in Dubins path without straight line segment are all arc segments, and the turning and robot speed in the three arc segments must be equal, so set the turning radius r α , r β , r γ in the first to third path segments in Dubins path without straight line segment as Set the robot speed s α , s β , s γ in the first to third path segments as s k1 .

[0135] 5.2.3.2.3 Calculate the path point set of the six Dubins paths in DP. The method is as follows: according to the variable speed Dubins path model, calculate the poses of the three path segments in the variable speed Dubins path according to formula (2), formula (3) and formula (4), respectively.

[0136]

[0137]

[0138] S σ (x, y, θ) = (x + σcosθ, y + σsinθ, θ) (8)

[0139] where, L σ (x, y, θ), R σ (x, y, θ) and S σ(x, y, θ) represent the coordinate and pose of the robot on the left turning arc segment, the right turning arc segment and the straight line segment, respectively; σ in equation (2) and equation (3) represents the radian offset of the robot from the start point of the arc segment, and σ in equation (4) represents the displacement of the robot on the straight line segment, r σ represents the turning radius of the robot on the arc segment.

[0140] According to equations (2)-(4), all the waypoints of the three path segments of a single Dubins path can be obtained, and these waypoints constitute a waypoint set dp r , which satisfies dp r = {(x f , y f , θ f ), f = 1,..., F}, F is the number of navigation points in dp r , and F is a positive integer, wherein (x f , y f , θ f ) is the fth navigation point in the path dp r , x f , y f , θ f represent the x / y axis coordinates and the orientation of the robot at the point; for the six paths of RSL, RSR, LSR, LSL, LRL and RLR, the waypoint sets of six variable-speed Dubins paths are obtained, and the six paths constitute a variable-speed Dubins path set DP = {dp r , r = 1,..., 6}, wherein dp r is the rth path in DP.

[0141] 5.2.3.3 Calculate the time consumption set DT corresponding to each path in the collision-free variable-speed Dubins path set DP, DT = {dt r , r = 1,..., 6}, wherein dt r represents the rth element in DT, and its value is the time consumption of the path dp r . The method is as follows:

[0142] 5.2.3.3.1 Initialize the six elements in DT to a very large positive number TA (such as TA = 1000000), and let the serial number r = 1.

[0143] 5.2.3.3.2 Determine whether there is a collision risk in the path dp r . If there is a waypoint u in the path dp r , and the risk potential pe(u) at the waypoint u is greater than 0, it means that the path dp r may collide with the obstacle, and dt rTA; otherwise, it means that the path dp r is safe and there is no collision risk. In the case that there is no collision risk, the calculation path dp r is calculated according to formula (5) r corresponding path time dt r ,

[0144]

[0145] where s α，r , s β，r and s γ，r represent the forward speed on the first path segment, the second path segment and the third path segment of the path dp r respectively; a r , b r and g r represent the length of the first path segment, the second path segment and the third path segment of the path dp r respectively.

[0146] 5.2.3.3.3 If r = 6, it means that the time consumption set DT is calculated, go to 5.2.3.4; otherwise, let r = r + 1, go to 5.2.3.3.2.

[0147] 5.2.3.4 Search for the element DT min， DT min with the minimum value in DT, and let DT min be the wth element in DT, 1 < w < 6. If DT min < T min , let T min = DT min , and let the optimal path best_path be the path dp w with the least time consumption in DP, go to 5.2.3.5; otherwise, T min remains the original value, go to 5.2.3.5.

[0148] 5.2.3.5 If k2 ≠ B, let k2 = k2 + 1, go to 5.2.3.2; if k2 = B and k1 ≠ A-1, let k2 = 1 and k1 = k1 + 1, go to 5.2.3.2; otherwise, it means that the path best_path with the least time consumption between p d and p e has been obtained, let cpath d，e = best_path and cm d，e = T min , go to 5.3.

[0149] 5.3 If d = 2N and e = 2N, it means that the cost matrix CM and CPATH corresponding to P have been calculated, go to 5.4; if d = 2N and e ≠ 2N, let d = 1 and e = e + 1, go to 5.2; if d ≠ 2N and e ≤ 2N, let d = d + 1, go to 5.2.

[0150] 5.4 The point-to-point path planning module sends CM and CPATH to the sequence generation module, go to the sixth step.

[0151] In the sixth step, the sequence generation module of the server receives the unit endpoint set P sent by the region decomposition module, the cost matrix CM and CPATH sent by the point-to-point path planning module, and the robot parameters sent by the robot terminal, constructs a coverage path planning mathematical model, obtains a sequence Tour with the minimum cost of visiting all endpoints in P, and further obtains a coverage path PATH. The method is as follows:

[0152] 6.1 Initialize Tour as an empty set.

[0153] 6.2 The sequence generation module receives CM sent by the point-to-point path planning submodule, constructs a coverage path planning model, as shown in formulas (6)-(10):

[0154] Min (∑cm i，j *z i，j ), i = 1, 2,..., 2N, j = 1, 2,..., 2N, i ≠ j (6)

[0155] ∑z i，j = 2N (7)

[0156]

[0157]

[0158] z 2×n1+1，2×n1+2 +z 2×n1+2，2×n1+1 = 1, n1 = 1,..., N (10)

[0159] Where the independent variable z i，j represents the Boolean value of the robot sequentially visiting endpoints p i and p j . If z i，j = 1, it means that the robot visits endpoint p i after visiting endpoint p j ; if z i，j = 0, it means that the robot does not visit endpoint p i after visiting endpoint p jEquation (6) represents the solution of the loop with the minimum cost of visiting all endpoints in P, equation (7) represents that Tour contains 2N edges, equation (8) represents that the robot can only enter the same endpoint once, equation (9) represents that the robot can only leave the same endpoint once, and equation (10) represents that the robot must visit two endpoints of the same rectangular cell in sequence.

[0160] 6.2 The mathematical model established by equations (6)-(10) is a traveling salesman problem, and the sequence generation module uses the LKH solver to solve equations (6)-(10) to obtain the sequence Tour = {t1, t2,..., t2N} of visiting all endpoints in P with the minimum cost, where tnis the nth endpoint in Tour. n ,..., t 2N}, where t n is the nth endpoint in Tour.

[0161] 6.3 The coverage path PATH is generated according to the endpoint sequence Tour and CPATH. The method is as follows:

[0162] 6.3.1 Initialize the endpoint sequence number n = 1, and initialize the coverage path PATH as an empty set.

[0163] 6.3.2 Let the nth endpoint and the n+1th endpoint in Tour be u and v respectively, and extract the collision-free path cpath u，v with the minimum time cost from the path matrix CPATH as the path path n between the nth endpoint in Tour and the n+1th endpoint in Tour. Let PATH = PATH U path n .

[0164] 6.3.3 If n ≠ 2N-1, let n = n+1, and go to step 6.3.2; otherwise, go to step 6.3.4 to calculate the path from the 2Nth endpoint in Tour back to the 1st endpoint in Tour.

[0165] 6.3.4 Let the 2Nth endpoint and the 1st endpoint in Tour be u and v respectively, and extract cpath uu，vv from the path matrix CPATH as the path path 2N between the 2Nth endpoint in Tour and the 1st endpoint in Tour. Let PATH = PATH U path 2N . Go to step 6.4.

[0166] 6.4 The sequence generation module sends the coverage path PATH to the robot terminal, and goes to step 7.

[0167] Step 7: The robot terminal receives the coverage path PATH output by the sequence generation module. Based on the path following module, positioning module and robot operation software and hardware control module configured in the robot terminal, it moves according to the coverage path PATH (generally, it moves while performing tasks). This movement trajectory can achieve complete coverage of the target area.

[0168] Figure 3 and Figure 4 The results show a comparison of the coverage time, maximum collision risk, coverage rate, and computational time between this invention and the Semi-BCD and MinNWT algorithms with forward speeds of {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s in 20m×20m and 30m×30m scenarios, respectively, under the condition that the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]×r². Several obstacles with known numbers and boundaries are distributed in both experimental scenarios. The simulation uses a Dubins robot, which is subject to the following kinematic constraints: a forward speed ranging from [0.2, 1] m / s and a forward speed ranging from [0.5, 1.5] m / s. 2 The forward acceleration is 1.0 rad / s, the turning rate is 1.0 rad / s, the minimum turning radius is 0.2 m, and the safety distance is 0.25 m. Both the Semi-BCD and MinNWT algorithms assume a fixed robot speed; therefore, experiments were conducted with speeds of 0.2, 0.36, 0.52, 0.68, 0.84, and 1.0 m / s for the Semi-BCD and MinNWT algorithms. Unlike the Semi-BCD and MinNWT algorithms with fixed speeds, this invention assumes a variable robot speed, which can be any sampled speed in the speed set S. The performance results of this invention are represented by a straight line through all sampled speeds in set S. To obtain comparable results, this invention is compared with all Semi-BCD and MinNWT algorithms with speeds of 0.2, 0.36, 0.52, 0.68, 0.84, and 1.0 m / s.

[0169] first, Figure 3 (a) and Figure 4 (a) shows the coverage time of this invention, the Semi-BCD algorithm, and the MinNWT algorithm when the robot's forward speed is {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s and the buffer width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r². Figure 3 (a) and Figure 4 In (a), the x-axis represents the width of the buffer zone, the y-axis represents the robot's forward speed, and the Z-axis represents the coverage time. Observation Figure 3(a) and Figure 4 (a) shows that, compared with Semi-BCD algorithm and MinNWT algorithm with different speeds, the coverage time obtained by the present application is the smallest or close to the smallest under different buffer region width conditions.

[0170] Figure 3 (b) and Figure 4 (b) shows the maximum collision risk of the present application, Semi-BCD algorithm and MinNWT algorithm when the robot forward speed is {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s and the buffer region width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2, wherein Figure 3 (b) and Figure 4 The x-axis of (b) is the buffer region width, the y-axis is the robot forward speed, and the Z-axis is the maximum collision risk. It can be observed that Figure 3 (b) and Figure 4 (b) shows that, under different buffer region width conditions, the maximum collision risk of the path of the present application is 0, that is, the present application can obtain a safe coverage path. In contrast, Semi-BCD algorithm may cross the obstacle because it assumes that the obstacle can be crossed. The safety of MinNWT algorithm is related to the buffer region width and speed, and a smaller speed requires a narrower buffer region width, and a higher speed requires a wider buffer region.

[0171] Figure 3 (c) and Figure 4 (c) shows the coverage rate of the present application, Semi-BCD algorithm and MinNWT algorithm when the robot forward speed is {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s and the buffer region width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r2, wherein Figure 3 (c) and Figure 4 The x-axis of (c) is the buffer region width, the y-axis is the robot forward speed, and the Z-axis is the coverage rate. It can be observed that Figure 3 (c) and Figure 4 (c) shows that the coverage rate of the present application, Semi-BCD algorithm and MinNWT algorithm is related to the buffer region width. The wider the buffer region, the lower the corresponding coverage rate. When the buffer region width is r2, the coverage rate of the present application, Semi-BCD algorithm and MinNWT algorithm all reaches more than 95%; however, when the buffer region width gradually increases, the coverage rate of the present application, Semi-BCD algorithm and MinNWT algorithm gradually decreases. In general, when the buffer region width is less than or equal to 2r2, the present application can obtain a coverage rate of more than or equal to 95%.

[0172] Figure 3 (d) and Figure 4 (d) shows the computation time of this invention, the Semi-BCD algorithm, and the MinNWT algorithm when the robot's forward speed is {0.2, 0.36, 0.52, 0.68, 0.84, 1.0} m / s and the buffer area width is [1, 1.5, 2.0, 2.5, 3, 3.5, 4.0]*r². Figure 3 (d) and Figure 4 In (d), the x-axis represents the width of the buffer zone, the y-axis represents the robot's forward speed, and the Z-axis represents the calculation time. Observation Figure 5 (d) It can be seen that, in most cases, the computation time of this invention is less than that of the Semi-BCD algorithm and the MinNWT algorithm. Observation Figure 5 (d) It can be seen that the computation time of the present invention is comparable to that of the Semi-BCD algorithm, while the computation time of the MinNWT algorithm is much longer than that of the present invention and the Semi-BCD algorithm.

[0173] Figure 5 (a) is a screenshot of the present invention in a real laboratory environment. Figure 5 (b) is the coverage path outlined in this law. For example... Figure 6 As shown in (a), the experimental environment is a laboratory area measuring 9.5m × 10.5m, with multiple obstacles distributed throughout. The client uses detection sensors to pre-construct a grid map of the target environment. The robotic end-user is a forward-facing Ackerman robot with a forward speed range of [0.3, 0.5] m / s and a forward acceleration range of [0.5, 1.0] m / s. 2 The robot has a minimum turning radius of 0.3m, a maximum angular velocity of 1.0 rad / s, and a safety distance of 0.35m. The one-dimensional velocity set S is set to {0.3, 0.4, 0.5} m / s, and the buffer zone width is set to 0.9m. The robot terminal integrates a ROS AMCL package as a localization module and the Pure_Pursuit algorithm as a path-following module. In addition, the robot is equipped with a SLAMTEC lidar sensor with a detection range of 8 meters and a coverage task sensor with a detection range of 0.9 meters. Based on environmental information and robot parameters, this invention plans... Figure 6 (b) shows the coverage path. The robot's forward speed at each point on the coverage path is encoded by color, with lighter colors representing faster robot speeds and darker colors representing slower robot speeds at that point. When performing the coverage task, the robot starts from the lower left corner, moves along the pre-set coverage path, and eventually returns to the starting point.

[0174] Figure 6The process screenshots in real laboratory environment of the present application are shown. The left part of each screenshot is the recorded robot pose information in RVIZ, and the right side is the corresponding pose of Ackerman robot in the real experimental scene. Figure 6 (a) shows the screenshot when the robot starts from the starting point to begin to perform the coverage; Figure 6 (b)-(d) are screenshots at three times during the coverage process; ​ (e) is the screenshot at the end of the coverage. ​ The 5 screenshots show that the real robot can follow the coverage path planned by the present application, that is, the present application is applicable to real robots.

Claims

1. A collision-free coverage path planning method for a variable-curvature-limited robot, characterized by The method comprises the following steps: The first step is to construct a coverage path planning system for a variable speed and curvature constraint robot; the coverage path planning system is composed of a server, a client and a robot terminal; The client is a mobile terminal, a PC or a robot; the client is provided with detection sensors such as a laser radar and a binocular camera; the client obtains target area environment information through the detection sensors and abstracts the target area environment information into a map G; elements with a value of 0 in the map G represent obstacles, and elements with a value of 1 represent to-be-covered grids; The client sends the map G and a user-specified speed sampling interval Inter to the server; The server is a server or a PC, which is provided with a coverage path planning system; the coverage path planning system is connected with the client and the robot terminal; the server is composed of a region decomposition module, a risk field construction module, a point-to-point path planning module and a sequence generation module; The region decomposition module is connected with the detection sensors of the client, the robot terminal, the point-to-point path planning module and the sequence generation module; the region decomposition module receives the target area map information G, the user-specified speed sampling interval Inter and the robot parameters sent by the client and the robot terminal, decomposes the target area according to a task sensor coverage radius r1 in the robot parameters, obtains a unit endpoint set P, and sends the unit endpoint set P, the target area map information G and the user-specified speed sampling interval Inter to the point-to-point path planning module and the sequence generation module; The risk field construction module is connected with the region decomposition module, the point-to-point path planning module and the sequence generation module, receives the target region map information G and the robot parameters sent by the region decomposition module, constructs a potential field PE representing the robot collision risk according to the robot minimum safety distance parameter d in the robot parameters, and sends the PE to the point-to-point path planning module and the sequence generation module. safe , constructs a potential field PE representing the robot collision risk according to the robot minimum safety distance parameter d in the robot parameters, and sends the PE to the point-to-point path planning module and the sequence generation module. The point-to-point path planning module is connected with the region decomposition module, the risk field construction module and the sequence generation module; the point-to-point path planning module receives the unit endpoint set P, the grid map G and the user-specified speed sampling interval Inter from the region decomposition module, receives the potential energy field PE from the risk field construction module, receives the robot parameters from the sequence generation module, calculates a robot forward speed set PS and a unit-to-unit non-collision path according to the forward speed, the acceleration and the maximum angular velocity of the robot in the robot parameters, further obtains a two-dimensional cost matrix CM representing a path cost and a two-dimensional path matrix CPATH storing candidate paths, and sends the CM and the CPATH to the sequence generation module; The sequence generation module is connected with the robot terminal, the region decomposition module, the risk field construction module and the point-to-point path planning module; the sequence generation module receives the robot parameters from the robot terminal, receives the unit endpoint set P, the target area map information G and the user-specified speed sampling interval Inter from the region decomposition module, receives the PE from the risk field construction module, receives the two-dimensional cost matrix CM and the two-dimensional path matrix CPATH from the point-to-point path planning module, models the coverage path planning problem into a traveling salesman problem, generates a non-collision coverage path PATH visiting all coverage units, and sends the coverage path PATH to the robot terminal; The robot terminal is a real robot, and the robot terminal is loaded with: a radar sensor for detecting obstacles in a rectangular area with a width of r2 and sending the detected obstacle information to a positioning module; a task sensor for performing a coverage task, covering a rectangular area with a width of r1; a positioning module for real-time positioning; a robot path following module; other software and hardware control modules necessary for robot operation, including the robot's inherent operating system, bottom drive system, and motion control system; the robot terminal sends the robot's forward speed s, acceleration a, maximum angular velocity u max , minimum safety distance d safe and r1, r2 to the server; The second step is that the detection sensors of the client collect target area environment information, construct a grid map G of the target environment, and send the grid map G to the region decomposition module of the server. Let the number of grid rows in the grid map G be A, and the number of grid columns be B; the regional environment information includes the area, boundary and obstacle position of the target region; Thirdly, the regional decomposition module of the server receives the grid map G sent by the client, the sampling interval Inter specified by the user and the task sensor coverage radius r1 sent by the robot terminal, decomposes the target region to obtain the unit endpoint set P, and then sends the unit endpoint set P, the grid map G and the sampling interval Inter specified by the user to the point-to-point path planning module and the sequence generation module; the method is as follows: 3.1 The regional decomposition module adopts Semi-BCD decomposition algorithm, and divides the target region into multiple rectangular units according to the grid map G; in each rectangular unit, the width of the edge located on the x-axis is equal to r1, and the edge located on the y-axis intersects with the obstacle or the boundary of the target region; a single rectangular unit corresponds to a single coverage task, and all the rectangular units constitute an initial coverage task set C, C={c1,…,cN}, N is the total number of coverage tasks, N is a positive integer, and cn represents the nth coverage task in C; n … N} n ​ 3.2 Set c n The two intersection points of the middle line and the upper and lower edges are c n The upper endpoint of c n,1 The lower endpoint of c n,2 The coordinates of c n,1 The coordinates of c n,2 px represents the x-axis coordinate of the middle line of c n pu represents the y-axis coordinate of the upper endpoint of the middle line of c n pd represents the y-axis coordinate of the lower endpoint of the middle line of c 1,1 N units in C correspond to 2N endpoints {p 1,2 ,p n,1 ,…,p n,2 ,p N,1 ,…,p N,2} are generated, where p n,1 ,p n,2 represent the upper and lower endpoints of the nth unit in C; for the convenience of description, p n,1 and the lower endpoint p n,2 are simplified to obtain the unit endpoint set P = {p1,…,p i ,…,p 2N}, 1≤i≤2N, p i is the ith endpoint in {p 1,1 ,p 1,2 ,…,p n,1 ,p n,2 ,…,p N,1 ,p N,2} The regional decomposition module sends the unit endpoint set P, the grid map G and the sampling interval Inter specified by the user to the point-to-point path planning module and the sequence generation module; Fourth, the risk field construction module on the server side receives the target region grid map G and the robot's minimum safe distance parameter d sent by the region decomposition module. safe A risk potential energy field PE representing collision risk is constructed and sent to the point-to-point path planning module and the sequence generation module. The method for constructing the collision risk potential energy field PE is as follows: 4.1 The risk field construction module constructs the risk potential field PE according to the grid map G, PE is a two-dimensional matrix containing AxB elements, and the value of each element in the matrix is between 0 and 1; the value of the matrix element is 1, which means that the robot will definitely collide at this element, and the value is 0, which means that the robot is safe at this element; the larger the value of the element in PE, the greater the possibility of collision between the robot and the obstacle at the element; 4.2 The risk field construction module sends PE to the point-to-point path planning module and the sequence generation module; Fifthly, the point-to-point path planning module of the server receives the unit endpoint set P sent by the regional decomposition module, calculates the cost matrix CM and the path matrix CPATH corresponding to P, and the method is as follows: 5.1 initialize cost matrix CM and path matrix CPATH corresponding to P; cost matrix CM is a two-dimensional matrix with dimension 2N x 2N, any element cm d,e in the matrix represents the time cost of collision-free path between the dth end point p d and the eth end point p e in P, d = 1, …, 2N, e = 1, …, 2N; path matrix CPATH is a two-dimensional matrix with dimension 2N x 2N, any element cpath d,e in the matrix represents the set of waypoints of collision-free path between p d and p e , satisfying cpath d,e = {(x f , y f , θ f ), f = 1, …, F}, F is the number of waypoints in CPATH, which is a positive integer, where (x f , y f , θ f ) represents the fth waypoint of the robot on the collision-free path between p d and p e , x f , y f are the x / y axis coordinates of the robot at the fth waypoint, θ f is the orientation of the robot at the fth waypoint; initialize all elements of cost matrix CM to 0, initialize all elements in CPATH to empty set, initialize row index i to 1, and initialize column index j to 1; 5.2 The point-to-point path planning module receives the P, the grid map G and the velocity sampling interval Inter sent by the region decomposition module, receives the robot parameters such as the forward velocity, the acceleration and the maximum angular velocity of the robot sent by the sequence generation module, calculates the collision-free path best_path from p d to p e , and the time consumption T min of the best_path, sets cpath d,e = best_path, sets cm d,e = T min ; the method is as follows: 5.2.1 The point-to-point path planning module receives P from the region decomposition module, and according to p d and p e relative positions, obtains the pose information (x d , y d , θ d ) of the robot at p d and the pose information (x e , y e , θ e ) of the robot at p e , x d , y d are the x / y axis coordinates of the robot at p d , θ d is the orientation of the robot at p d , x e , y e are the x / y axis coordinates of the robot at p e , θ e is the orientation of the robot at p e ; 5.2.2 The point-to-point path planning module receives the robot sampling interval Inter sent by the region decomposition module, the robot acceleration a, the forward velocity s and the maximum angular velocity u sent by the sequence generation module max , a e [a min , a max ], a min > 0, a max > 0, a min < a max , where a min and a max represent the minimum and maximum forward acceleration of the robot respectively; s e [s min , s max ], s min > 0, s max > 0, s min < s max , where s min and s max represent the minimum and maximum forward velocity of the robot respectively; By sampling the robot's forward velocity at equal intervals, we obtain the forward velocity set S = {s1, ..., s2}. k ,…,s K }, where K is the number of velocity sampling points, which is a positive integer, and s k It is the k-th velocity sampling point in S, s1 = s min ,s K =s max Any two velocities in S form a velocity pair, and the K velocities in S correspond to K 2 There are 10 velocity pairs, which constitute the set of forward velocity pairs PS = {(s...} k1 ,s k2 ),s k1 ,s k2 ∈S,k1,k2=1,..,K}; 5.2.3 The point-to-point path planning module receives the risk potential field PE sent by the risk field construction module, calculates the collision-free path best_path between p d and p e and the time consumption T min of the path; the method is as follows: 5.2.3.1 Initialization of minimum path cost T min Initialize the optimal path best_path to the empty set, and let the sequence numbers k1 = 1 and k2 = 1 for a positive number TA. 5.2.3.2 Calculate the corresponding collision-free variable-speed Dubins path set DP under the condition of speed pair (s k1 , s k2 ) ; DP contains a total of six Dubins paths in it, including RSL, RSR, LSR, LSL, LRL and RLR, where R represents a right arc line segment, L represents a left arc line segment, and S represents a straight line segment; DP = {dp r ,r = 1, …, 6}, dp r is the rth path in DP, dp r = {(x f ,y f ,θ f ), f = 1, …, F}, F is the number of navigation points in dp r , F is a positive integer, (x f ,y f ,θ f ) is the fth navigation point in dp r , x f , y f , θ f respectively represent the x / y axis coordinates and orientation of the robot at the point; 5.2.3.3 Calculate the set of time consumption DT corresponding to each path in DP, DT = {dt r , r = 1, …, 6}, where dt r represents the rth element in DT, whose value is the time consumption of path dp r . 5.2.3.4 Search for the element with the minimum value in DT DT min , DT min = min(DT); let DT min be the wth element in DT, 1≤w≤6; if DT min < T min , let T min = DT min , let the optimal path best_path be the path dp w in DP with the least time consumption, go to 5.2.3.5; otherwise, T min remains the same, go to 5.2.3.5; 5.2.3.5 If k2≠B, let k2=k2+1, go to 5.2.3.2; if k2=B and ki≠A-1, let k2=1, ki=ki+1, go to 5.2.3.2; otherwise, it means that p has been obtained d and p e between them with the minimum time cost, let cpath d,e =best_path, cm d,e =T min , go to 5.3; 5.3 If d=2N and e=2N, it means that the cost matrix CM and CPATH corresponding to P have been calculated, go to 5.4; if d=2N, e≠2N, let d=1, e=e+1, go to 5.2; if d≠2N, e≤2N, let d=d+1, go to 5.2; 5.4 The point-to-point path planning module sends CM and CPATH to the sequence generation module, and goes to the sixth step; Sixthly, the sequence generation module of the server receives the unit endpoint set P sent by the regional decomposition module, the cost matrix CM and CPATH sent by the point-to-point path planning module, and the robot parameters sent by the robot terminal, constructs a coverage path planning mathematical model, obtains a sequence Tour with the minimum cost of visiting all endpoints in P, and further obtains a coverage path PATH; the method is as follows: 6.1 Initialize Tour as an empty set; 6.2 The sequence generation module receives CM sent by the point-to-point path planning submodule, and constructs a coverage path planning model, as shown in formulas (6)-(10): Min (∑cm i,j *z i,j ), i = 1, 2,..., 2N, j = 1, 2,..., 2N, i≠j (6)∑z i,j = 2N (7) z 2×n1+1,2×n1+2 +z 2×n1+2,2×n1+1 = 1, n1= 1,..., N (10) where the independent variable z i,j represents the Boolean value of whether the robot will visit endpoint p i after visiting endpoint p j ; if z i,j = 1, it means that the robot will visit endpoint p i after visiting endpoint p j ; if z i,j = 0, it means that the robot will not visit endpoint p i after visiting endpoint p j ; formula (6) means to find the tour with the minimum cost of visiting all endpoints in P, formula (7) means that the tour contains 2N edges, formula (8) means that the robot can only enter the same endpoint once, formula (9) means that the robot can only leave the same endpoint once, and formula (10) means that the robot will definitely visit the two endpoints of the same rectangular cell in sequence. 6.2 Solving equations (6)-(10) gives the sequence Tour = {t1,...,t n ,…,t 2N} of the n endpoints in P with the minimum cost of access, where t n is the nth endpoint in Tour. 6.3 Generate the coverage path PATH according to the endpoint sequence Tour and CPATH; 6.4 The sequence generation module sends the coverage path PATH to the robot terminal, and goes to the seventh step; Seventhly, the robot terminal receives the coverage path PATH output by the sequence generation module, and based on the path following module, the positioning module and the robot running software and hardware control module configured by the robot terminal, moves and executes the task according to the coverage path PATH, to realize complete coverage of the target region. 2.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein The robot terminal adopts an Ackerman robot; the radar sensor detects a rectangular area with a width r2∈(0, 200] meters; the radar sensor adopts a Sillan laser radar; the task sensor requires a coverage width r1≤r2; the positioning module adopts a ROS AMCL positioning package; and the path following module adopts a pure pursuit path tracking algorithm, namely, a Pure_Persuit algorithm, for path tracking. 3.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein 4.1 The method for constructing the risk potential field PE according to the grid map G by the risk field construction module in step 4.1 is: 4.1.1 initializing the value of all elements of the risk potential field PE as 0, initializing the row number a as 1, and initializing the column number b as 1; 4.1.2 According to the grid map G, the risk potential pe of the cell in the ath row and bth column in the PE is calculated a,b , pe a,b The calculation is shown in formula (1): where d represents the Euclidean distance between the cell G(a, b) in the a-th row and b-th column in the grid map G and the nearest obstacle cell to G(a, b) in G; formula (1) indicates that if the distance between G(a, b) and the nearest obstacle cell is greater than the minimum safety distance d of the robot safe , then the robot is safe at G(a, b), and the risk value pe a,b = 0. Conversely, the robot is at risk of colliding with the obstacle at G(a,b) with a risk value of pe a,b ; 4.1.3 if a=A and b=B, it is indicated that the risk field PE is constructed, and the process is ended; if a=A but b≠B, a=1 is set, b=b+1 is set, and step 4.1.2 is returned; if a≠A and b≤B, a=a+1 is set, and step 4.1.2 is returned. 4.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein 5.2.1 The point-to-point path planning module described in step 2.1.1 is based on p d and p e The relative position, the pose information of the robot at p d (x d , y d , θ d ) and the pose information at p e (x e , y e , θ e ) can be obtained by the following method: 5.2.1.1 Based on P sent by the region decomposition module, obtain the robot's position at p. d x / y axis coordinates (x d ,y d The robot is in p e x / y axis coordinates (x e ,y e ); 5.2.1.2 If p d and p e belong to the same cell, go to 5.2.1.3 and compute the orientation θ d of the robot at p d , and the orientation θ e of the robot at p e ; if p d and p e do not belong to the same cell, go to 5.2.1.4 and compute the orientation θ d of the robot at p d , and the orientation θ e of the robot at p e ; 5.2.1.3 This time p d and p e belong to the same cell if p d is the upper endpoint of the cell, p e is the lower endpoint of the cell, indicates that the robot enters the cell from p d , covers the cell from top to bottom, and leaves from p e , set if p d is the lower endpoint of the cell, p e is the upper endpoint of the cell, set go to 5.2.1.5; 5.2.1.4 This time p d and p e belongs to a different cell, if p d is the upper endpoint of a cell, indicates that the robot covers p d after it has finished covering the cell to which p d belongs, from p d leaves the cell to which p e belongs; when covering a rectangular cell from bottom to top, the orientation of the robot is set otherwise let if p d is the upper endpoint of a cell, indicates that the robot covers p e after it has finished covering the cell to which p e belongs, enters the cell to which p e belongs, and then covers p d from top to bottom; when covering a rectangular cell from top to bottom, the orientation of the robot is set otherwise go to 5.2.1.5; 5.2.1.5 Let the pose of the robot at p d be (x d ,y d ,θ d ), and let the pose of the robot at p e be (x e ,y e ,θ e ). 5.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein 5.2.3.1 TA=1000000 in step 5.2.3.

1. 6.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein 5.2.3.2 The calculation of the corresponding collision-free variable-speed Dubins path set DP under the condition of speed pair (s k1 ,s k2 ) is as follows: 5.2.3.2.1 initializing DP as an empty set; 5.2.3.2.2 Set the speed and turning radius of the six Dubins paths in DP; the method is as follows: According to the definition of Dubins path, Dubins path contains RSL, RSR, LSR, LSL, LRL, RLR six paths, where R represents right arc segment, L represents left arc segment, and S represents straight line segment; According to whether it contains straight line segment, the six Dubins paths are divided into four paths with straight line segment, namely RSL, RSR, LSR, LSL, and two paths without straight line segment, namely LRL and RLR; The Dubins path with straight line segment and the Dubins path without straight line both contain three path segments, Let the turning radius of the first to the third path segment be r α , r β and r γ , and the speed of the robot is s α , s β and s γ ; 5.2.3.2.2.1 Calculate the velocity and turning radius of the straight-line Dubins path; the straight-line Dubins path includes three path segments, which are the first arc segment, the second straight-line segment and the third arc segment respectively; the straight-line Dubins path requires that the first arc segment and the third arc segment must maintain a fixed turning radius, and the point-to-point path planning module sets the turning radius r of the first arc segment in the Dubins path α For Set the robot velocity s of the first arc segment in the Dubins path α For s k1 Set the turning radius r of the third arc segment in the Dubins path γ For Set the robot velocity s of the third arc segment in the Dubins path γ For s k2 The second path segment of the straight-line Dubins path is a straight-line segment, and the turning radius of the straight-line segment is set to r β For 0, calculate the robot velocity s on the second straight-line segment β ; 5.2.3.2.2.2 Calculate the velocity and turning radius of the Dubins path without straight line segment; the three path segments in the Dubins path without straight line segment are all arc segments, set the turning radius r of the first to third path segments in the Dubins path without straight line segment α , β , γ are all Set the robot velocity s of the first to third path segments α , β , γ s k1 ; 5.2.3.2.3 calculating the path point set of the 6 Dubins paths in DP, which is: according to the variable-speed Dubins path model, the poses of the three path segments in the variable-speed Dubins path are calculated according to formula (2), formula (3) and formula (4) respectively; L σ (x,y,θ) = (x - r σ sinθ + r σ sin(θ + σ), (2) y + r σ cos θ - r σ cos (θ + σ), θ + σ) R σ (x, y, θ) = (x + r σ sin θ - r σ sin(θ - σ), (3) y - r σ cos θ + r σ cos (θ - σ), θ - σ) S σ (x,y,θ) = (x + σcosθ, y + σsinθ, θ) (4) where L σ (x,y,θ), R σ (x,y,θ) and S σ (x,y,θ) represent the coordinate position and orientation of the robot on the left turning arc segment, the right turning arc segment and the straight line segment, respectively; σ in equations (2) and (3) represents the arc offset of the robot from the start point of the arc segment, and σ of the path segment in equation (4) represents the displacement of the robot on the straight line segment, r σ represents the turning radius of the robot on the arc segment. According to formulas (2)-(4), all the path points of the three path segments of a single Dubins path are obtained, which constitute a path point set dp r , satisfying dp r ={(x f ,y f ,θ f ),f=1,…,F} r , and the path point sets of six variable-speed Dubins paths are obtained for the six paths of RSL, RSR, LSR, LSL, LRL and RLR, respectively, which constitute a variable-speed Dubins path set DP={dp r ,r=1,…,6}. 7.The collision-free coverage path planning method for a variable-curvature-limited robot according to claim 6, wherein 5.2.3.2.2.1 The computer calculates the speed s of the robot on the second straight line segment β The velocity configuration method is adopted: let the lengths of the first arc segment, the second straight line segment and the third arc segment of the Dubins path be α, β and γ respectively; When s α <s γ , let the robot accelerate from s max to s α with the maximum acceleration a γ . The path length is L1. Let the robot decelerate from s min to s max with the minimum acceleration a γ . The path length is L2. The velocity s β of the robot on the second straight line segment has four cases: (1) If the length of the straight path segment β is less than L1, set s β to 0; (2) If β = L1, set s β to start from s α and accelerate to s max with the maximum acceleration a γ ; (3) If L1 < β < L1 + L2, set s β to start from s α and accelerate to s​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​ When s α = s γ , let the robot start from s α , accelerate to s max with maximum acceleration a max , decelerate to s min with minimum acceleration a γ , the length of the whole path is L3; the velocity of the robot on the second straight line segment s β has two kinds: (1) if the straight line path segment β < L3, set s β from s α , accelerate to s max with maximum acceleration a t2 , then decelerate to s min with minimum acceleration a γ ; let the robot start from s α , accelerate to s max with maximum acceleration a t2 , the path length is AL2, where the robot starts from s t , decelerates to s min with minimum acceleration a γ , the path length is DL2, where AL2 and DL2 satisfy the equation AL2 + DL2 = β, solve the equation to obtain the value of s t2 ; (2) if β ≥ L3, set s β from s α , accelerate to maximum velocity s max with maximum acceleration a max , then decelerate to s min with minimum acceleration a γ ; When s α > s γ , let the robot start from the speed s α and decelerate to s min with the minimum acceleration a γ . The path length is L4; let the robot start from s α and accelerate to s max with the maximum acceleration a max , then decelerate to s min with the minimum acceleration a γ . The length of the whole path is L5; there are four cases for the speed s β of the robot on the second straight line segment: (1) If β < L4, set s β to 0; (2) If β = L4, set s β to start from s α and decelerate to s min with the minimum acceleration a α ; (3) If L4 < β < L5, set s γ to start from s β and accelerate to s α with the maximum acceleration a max , then decelerate to s[[ID=4D]]<D000347>with the minimum acceleration a min ; Let the robot start from s γ and accelerate to s α with the maximum acceleration a max . The path length is AL3, where Let the robot start from s t3 and decelerate to s min with the minimum acceleration a γ . The path length is DL3, where AL3 and DL3 satisfy the equation AL3 + DL3 = β, and solve this equation to obtain the value of s t3 ; (4) If β > L5, set s β to start from s α and accelerate to the maximum speed s max with the maximum acceleration a max , then decelerate to s min with the minimum acceleration a γ .​​ 8.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein 5.2.3.3 the method for calculating the time consumption set DT corresponding to each path in DP in step 5.2.3.3 is: 5.2.3.3.1 initializing the 6 elements in DT as TA, and setting the serial number r=1; 5.2.3.3.2 Determine path dp r whether there is a collision risk; if path dp r exists a waypoint u, the risk potential pe(u) at waypoint u is greater than 0, indicating that path dp r has a possibility of collision with the obstacle, set dt r as TA; otherwise, path dp r is safe, there is no collision risk, and the corresponding path time consumption dt r of the calculated path dp r is calculated according to formula (5), wherein s α,r , s β,r and s γ,r represent the forward speed on the first path segment, the second path segment, the third path segment of the path dp r , respectively; a r , b r , g r denote the length of the first path segment, the second path segment, the third path segment of the path dp r , respectively. 5.2.3.3.3 if r=6, it is indicated that the time consumption set DT is calculated, and the process is ended; otherwise, r=r+1 is set, and step 5.2.3.3.2 is returned. 9.The collision-free coverage path planning method for a variable-curvature-limited robot of claim 1, wherein 6.3 The method for generating the coverage path PATH according to the endpoint sequence Tour in step 6.3 is: 6.3.1 initializing the endpoint serial number n=1, and initializing the coverage path PATH as an empty set; 6.3.2 Let u and v be the nth and (n+l)th end points in Tour, respectively, and let cpath be the collision-free path of minimum duration extracted from path matrix CPATH u,v , as the path path n between the nth end point in Tour and the (n+l)th end point in Tour; let PATH = PATH U path n ; 6.3.3 if n≠2N-1, n=n+1 is set, and step 6.3.2 is returned; otherwise, step 6.3.4 is returned to calculate the path from the 2Nth endpoint to the 1st endpoint in Tour. 6.3.4 Let cpath be the path from the (2N)th end point to the first end point in Tour, extracted from the path matrix CPATH uu,vv , as path 2N between the (2N)th end point and the first end point in Tour; let PATH = PATH U path 2N .

Images