Path planning method based on spatial transformation

Abstract

The invention provides a path planning method based on spatial transformation, and belongs to the technical field of unmanned vehicle path planning. Comprising the following steps: pre-processing a real map, and converting the real map with obstacles into a barrier-free space through spatial transformation; establishing and storing a dynamic mapping relation between the barrier-free space and a route area of the aircraft; acquiring a starting point and an ending point in the barrier-free space, and acquiring path discrete points between the starting point and the ending point; and converting the path discrete points into a real map to obtain path planning points in a real map coordinate system. The path planning method based on spatial transformation provided by the invention has the characteristics of few iteration and search processes, less calculation resource occupation and high calculation efficiency.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle path planning, and in particular to a path planning method based on space transformation. Background Art

[0002] When an unmanned aerial vehicle is performing a navigation task, due to the existence of obstacles or navigation boundaries in reality, its navigation path needs to be planned to complete the mission while avoiding collisions. In order to reduce the workload of people during use, automatic path planning is generally required.

[0003] For small and medium-sized unmanned aerial vehicles, such as drones or underwater unmanned aerial vehicles, due to limitations in size, weight, energy, etc., unmanned aerial vehicles usually use small embedded computer boards to perform multiple tasks such as sensor data analysis, communication, decision-making, motion control, emergency response, etc. Therefore, as part of the decision-making task, its automatic path planning should minimize the consumption of local computing resources to improve the reliability of the system, and speed up the calculation speed to shorten the system response time to improve the efficiency of task completion. Most of the current path planning algorithms use iteration or search, such as breadth-first, A* and other search algorithms and their variants, artificial potential field methods, etc., which have the disadvantages of relatively high computing power consumption and relatively long task planning time, especially when obstacles are relatively dense.

[0004] Therefore, based on the above situation, it is very necessary to propose a path planning method based on spatial transformation to efficiently solve the path planning task. Summary of the invention

[0005] In view of this, the present invention proposes a path planning method based on space transformation, which can be applied to both static and dynamic obstacle environments, has fewer iterations and search processes, occupies less computing resources, and has high computing efficiency.

[0006] The present invention provides a path planning method based on space transformation, comprising the following steps:

[0007] Real map pre-processing: converting the real map with obstacles into an obstacle-free space through spatial transformation; establishing and saving the dynamic mapping relationship between the obstacle-free space and the routeable area of ​​the aircraft;

[0008] Get the starting point and the end point in the barrier-free space, and get the discrete points of the path between the starting point and the end point;

[0009] Transform the discrete points of the path into the real map to obtain the path planning points in the real map coordinate system.

[0010] On the basis of the above technical solutions, preferably, the dynamic mapping relationship between the obstacle-free space and the drivable area of ​​the aircraft is established and saved by adding obstacles one by one in the obstacle-free space, and each obstacle repels non-obstacle points in an adjacent range, and the repulsion produces a coordinate transformation on the non-obstacle points; after transforming the repulsive distance of all points in the obstacle-free space, a mapping F is formed from the coordinates of all non-obstacle points in the obstacle-free space to the real map coordinates of the drivable space of the unmanned aircraft in the real map: X→X, where X is the real map coordinate and X′ is the coordinate of the non-obstacle point in the obstacle-free space.

[0011] Preferably, the calculation method for excluding the coordinate transformation of non-obstruction points is: b represents the coordinate set of obstacle points belonging to obstacles; P b is the exclusion distance of a certain obstacle point to the non-obstacle point; δX′ b is the coordinate difference between the non-obstruction point and the obstacle point, δX′ b =X′-X′ b ; Repulsion distance P b It is inversely proportional to the distance between the non-obstruction point and the obstacle point.

[0012] Further preferably, the repulsion distance P b The repulsion distance P is expressed as a polynomial by series expansion. b The coordinate distance x = ||δX′ b || polynomial basis [x a , x b , x c , ...] and the inner product of the polynomial coefficient vector C:

[0013] Further preferably, the spatial transformation relationship between the real map and the barrier-free space is solved as follows:

[0014] The coordinate to be transformed is marked as y, and the transformed coordinate is y′; let the neighborhood range of coordinate y be ΔL;

[0015] Find the set of reference coordinate points {P L};

[0016] If the set of reference coordinate points {P L} is not greater than the map dimension d+1, then the radius of ΔL is enlarged; if the set of reference coordinate points {P L} is greater than the map dimension d+1, then select the set {P L} is the origin, the reference point coordinates and transformed coordinates of the origin are x0 and x′0 respectively; the corresponding sets of the remaining reference coordinate points and transformed coordinates are {x}, {x′};

[0017] Through the mapping relationship A: x→x′, solve the linear transformation matrix A, A(x-x0)=x′-x′0;

[0018] Calculate the transformed coordinate y′=A(y-x0)+x0 to obtain the transformed coordinate y.

[0019] Still further preferably, the dimension d of the real map is two-dimensional or three-dimensional.

[0020] Further preferably, in the set {X′} of non-obstruction point coordinates in the barrier-free space, discrete points on the path between the starting point and the end point are connected by a straight line, a broken line or a curve.

[0021] Further preferably, the fitting rule of the path discrete points is:

[0022] S31, obtaining at least one path discrete point M' of the starting point and the end point of the obstacle-free space; making the normal offset of M' cL, where c is the offset coefficient, and L is the starting point x'0 and the end point x' of the obstacle-free space end The length of the connection;

[0023] S32, perform spatial transformation on the path discrete point M′ to obtain the coordinate M after spatial transformation; determine whether the position of the path discrete point before and after spatial transformation is abnormal: if there is no position abnormality, execute the next step; if there is a position abnormality, adjust the value of the offset coefficient, and repeat steps S31-S32;

[0024] S33, constructing a connection plan {x1} for the starting point x0 and the path discrete point M′, respectively, and constructing a connection plan {x2} for the end point and the path discrete point; the priority of the connection methods of different path discrete points is straight line connection> broken line connection> curve connection;

[0025] S34. If a path obtained by connecting different discrete points of the path with a straight line passes through an obstacle area, is not connected to an adjacent path, or passes through an area outside the map limit, the straight line connection mode is replaced with a broken line or a curved line connection mode;

[0026] S35. Merge the obtained connection plans {x1} and {x2}, and end.

[0027] Further preferably, step S32 determines whether the position of the path discrete point before and after the spatial transformation is abnormal, that is, determines whether the path discrete point M′ and its coordinates M after spatial transformation are located in the obstacle of the real map and its adjacent dangerous area W, as well as the coverage area W of the spatial transformation corresponding to the dangerous area.

[0028] More preferably, the value range of the offset coefficient is 0.01-0.5.

[0029] The path planning method based on space transformation provided by the present invention has the following beneficial effects compared with the prior art:

[0030] The path planning method of the present invention is based on space transformation, and the real map is influenced by the repulsive force mode to realize space transformation; then the coordinate transformation relationship between the real map and the barrier-free space is mapped, and the legitimacy of the path discrete points is verified by adding the path discrete points and further constructing the path in the barrier-free space, and the connection mode between different path discrete points is reasonably planned, and the shortest path avoiding obstacles is constructed, and it is reversely mapped to the real map to obtain the planned path, and the planned path is further verified by the time and length requirements, and finally the required planned path is obtained; compared with the conventional iteration or search method, the method of the present invention greatly shortens the planning time. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0032] Figure 1 This is a principle flow chart of a path planning method based on space transformation of the present invention;

[0033] Figure 2 A mapping relationship between an obstacle-free space and a navigable area in a path planning method based on space transformation according to the present invention;

[0034] Figure 3 A schematic diagram of the steps of obtaining a path discretization of a real map in a path planning method based on space transformation according to the present invention;

[0035] Figure 4 A schematic diagram of path planning of a path planning method based on space transformation according to the present invention;

[0036] Figure 5 A flowchart of static planning correction of a path planning method based on space transformation according to the present invention;

[0037] Figure 6 A schematic diagram of calculation results of an embodiment of a path planning method based on space transformation of the present invention;

[0038] Figure 7 The present invention is a schematic diagram of the inverse transformation of a path planning result of a path planning method based on space transformation. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solution and advantages of the embodiments of the present disclosure clearer, the technical solution of the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings of the embodiments of the present disclosure. Obviously, the described embodiments are part of the embodiments of the present disclosure, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0040] Most current path planning algorithms use iteration or search, which have the disadvantages of relatively high computing power consumption and relatively long task planning time. Figure 1 As shown, the present invention provides a path planning method based on space transformation, comprising the following steps:

[0041] S1: Real map pre-processing, converting the real map with obstacles into an obstacle-free space through spatial transformation; establishing and saving the dynamic mapping relationship between the obstacle-free space and the routeable area of ​​the aircraft.

[0042] The transformation here is the first transformation, that is, the transformation from the real map to the obstacle-free space. The dynamic mapping relationship between the obstacle-free space and the navigable area of ​​the aircraft is established and saved by adding obstacles one by one in the obstacle-free space. Each obstacle repels the non-obstruction points in the adjacent range, and the repulsion produces a coordinate transformation for the non-obstruction points. After the repulsion distance of all points in the obstacle-free space is transformed, a mapping is formed from the coordinates of all non-obstruction points in the obstacle-free space to the real map coordinates of the drivable space of the unmanned aircraft in the real map. F : X → X , where X is the coordinate of the real map and X′ is the coordinate of the non-obstruction point in the barrier-free space. Figure 1 , Figure 1 The real map in the figure is a rectangle. There are planned starting points and end points in the real map. The space transformation caused by the repulsion effect obtains a barrier-free space with a certain degree of deformation. The corresponding starting points and end points exist in the barrier-free space. Then obstacles are added to the barrier-free space. After the planned path is obtained in the barrier-free space, the planned path connecting the starting point and the end point in the real map is obtained through the inverse transformation of the path.

[0043] Specifically, the calculation method for eliminating the coordinate transformation of non-obstacle points is: b represents the coordinate set of obstacle points belonging to obstacles; P b is the exclusion distance of a certain obstacle point to the non-obstacle point; δX′ b is the coordinate difference between the non-obstruction point and the obstacle point, δX′ b =X′-X′ b ; Repulsion distance P b It is inversely proportional to the distance between the non-obstruction point and the obstacle point, which can be expressed as a negative exponential function. To characterize, where w k and w e are the proportional coefficient and exponential coefficient respectively, and both are greater than 0. The values ​​of the proportional coefficient and exponential coefficient are determined according to the test results of the map environment, with the principle that the safe distance of the feasible domain formed around the obstacle is equivalent to the obstacle avoidance capability or requirement of the unmanned aerial vehicle, and there is no spatial overlap in the map. The principle of constructing the mapping relationship is as follows: Figure 2 shown.

[0044] Repulsion distance P b The repulsion distance P is expressed as a polynomial by series expansion. b The coordinate distance x = ||δX′ b || polynomial basis [x a , x b , x c , ...] and the inner product of the polynomial coefficient vector C: In this embodiment, when calculating the repulsion distance, the coordinate distance ||δX′ b ||You can choose different vector paradigm calculation methods, such as the maximum absolute value of coordinate difference, the sum of absolute values ​​of coordinate difference, and the standard deviation between coordinates; you can also use a series approximate expansion form to represent the coordinate distance ||δX′ b ||: n is the order.

[0045] S2: Obtain the starting point and the end point in the obstacle-free space, and obtain the path discrete points between the starting point and the end point.

[0046] refer to Figure 3 and Figure 4 ,Under the mapping relationship F: X′→X, during the task planning process, the actual coordinates may not completely coincide with the coordinate points of the known mapping relationship, and an approximate conversion is performed between the real map coordinates and the barrier-free space coordinates.

[0047] The spatial transformation relationship between the real map and the barrier-free space is solved as follows:

[0048] Mark the coordinate to be transformed as y, and the coordinate after transformation as y; let the neighborhood range of coordinate y be ΔL;

[0049] Find the set of reference coordinate points {P L};

[0050] If the set of reference coordinate points {P L} is not greater than the map dimension d+1, then the radius of ΔL is enlarged; if the set of reference coordinate points {P L} is greater than the map dimension d+1, then select the set {P L} is the origin, the reference point coordinates and transformed coordinates of the origin are x0 and x′0 respectively; the corresponding sets of the remaining reference coordinate points and transformed coordinates are {x}, {x′};

[0051] Through the mapping relationship A: x→x′, solve the linear transformation matrix A, A(x-x0)=x′-x′0;

[0052] Calculate the transformed coordinate y′=A(y-x0)+x0 to obtain the transformed coordinate y. The corresponding starting point, end point and point corresponding to the obstacle disappearing position of the barrier-free space can be obtained by the above linear transformation method. As a preferred embodiment, the dimension d of the real map in the present invention is two-dimensional or three-dimensional.

[0053] S3: Transform the discrete points of the path into the real map to obtain the path planning points in the real map coordinate system.

[0054] In the set {X′} of non-obstruction point coordinates in the barrier-free space, discrete points on the path between the starting point and the end point are connected by a straight line, a broken line or a curve.

[0055] The fitting rule of the path discrete points is:

[0056] S31, obtaining at least one path discrete point M' of the starting point and the end point of the obstacle-free space; making the normal offset of M' cL, where c is the offset coefficient, and L is the starting point x'0 and the end point x' of the obstacle-free space end In the present invention, the value range of the offset coefficient c is 0.01-0.5. The offset direction is to deviate outward away from the obstacle.

[0057] S32. Perform spatial transformation on the discrete point M′ of the path to obtain the coordinate M after spatial transformation; determine whether the position of the discrete point of the path before and after the spatial transformation is abnormal: if there is no position abnormality, execute the next step; if there is a position abnormality, adjust the value of the offset coefficient and repeat steps S31-S32; the initial value of the offset coefficient c is 0.01-0.1. If adjustment is required, adjust it by increasing it by 0.05 each time, and the maximum number of adjustments shall not exceed 9 times.

[0058] Step S32 determines whether the position of the path discrete point before and after the spatial transformation is abnormal, that is, whether the path discrete point M and its coordinates M after spatial transformation are located in the obstacle of the real map and its adjacent dangerous area W, as well as the coverage area W′ of the spatial transformation corresponding to the dangerous area.

[0059] S 33 , respectively construct a connection plan {x1} for the starting point x′0 and the path discrete point M′, and construct a connection plan {x2} for the end point and the path discrete point; the priority of the connection methods of different path discrete points is straight line connection> broken line connection> curve connection;

[0060] S34. If a path obtained by connecting different discrete points of the path with a straight line passes through an obstacle area, is not connected to an adjacent path, or passes through an area outside the specified range of the map, the straight line connection mode is replaced with a broken line or a curved line connection mode; Figure 5 In the example, there is a circular danger zone in the vicinity of the obstacle. If the path connected by the straight line intersects with the circular ring, the straight line will enter the danger zone and there is a risk of contact with the obstacle area. The size of the circular danger zone is related to the speed and collision radius of the aircraft.

[0061] S 35 , merge the obtained connection plans {x1} and {x2}, and end.

[0062] Example: Figure 6 and Figure 7 As shown in the figure, for a two-dimensional map planning task, the distance unit is normalized to 1. In the 100×100 map space, there is a prohibited area obstacle 1 and a physical obstacle 2, with position coordinates of (49.5, 60) and (50, 10), which can be regarded as circles with diameters of 35 and 7, respectively. There is a planning task with a starting point coordinate of (0, 50) and an end point coordinate of (100, 50). The line connecting them passes through obstacle 1, and path planning is required. This method is used for pathing, the thickness of the danger zone is set to 2 and the obstacle is included, and the negative exponential function is used to calculate the exclusion distance for spatial transformation, and the coefficient w k and w eThe values ​​are 10 and 1 respectively. Python 3.6 programming and single-core serial mode are used for single path planning. Straight path planning is used in obstacle-free space, and the number of path midpoints is 100. The path planning result is transformed as follows Figure 7 As shown in the figure, the path avoids all obstacles and the planning calculation time is less than 0.2s. It meets the path planning requirements and consumes less computing resources.

[0063] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A path planning method based on space transformation, characterized in that: The steps include: Real map pre-processing: converting the real map with obstacles into an obstacle-free space through spatial transformation; establishing and saving the dynamic mapping relationship between the obstacle-free space and the routeable area of ​​the aircraft; Get the starting point and the end point in the barrier-free space, and get the discrete points of the path between the starting point and the end point; Transform the discrete points of the path into the real map to obtain the path planning points in the real map coordinate system.

2. A path planning method based on spatial transformation according to claim 1, characterized in that: The dynamic mapping relationship between the obstacle-free space and the navigable area of ​​the aircraft is established and saved by adding obstacles one by one in the obstacle-free space, and each obstacle repels the non-obstacle points in the adjacent range, and the repulsion produces a coordinate transformation for the non-obstacle points; after the repulsion distance of all points in the obstacle-free space is transformed, a mapping F: X→X is formed from the coordinates of all non-obstacle points in the obstacle-free space to the real map coordinates of the drivable space of the unmanned aircraft in the real map, where X is the real map coordinate and X′ is the coordinate of the non-obstacle point in the obstacle-free space.

3. A path planning method based on spatial transformation according to claim 2, characterized in that: The calculation method for the coordinate transformation of the non-obstruction point is: b represents the coordinate set of obstacle points belonging to obstacles; P b is the exclusion distance of a certain obstacle point to the non-obstacle point; δX′ b is the coordinate difference between the non-obstruction point and the obstacle point, δX′ b =X′-X′ b ; Repulsion distance P b It is inversely proportional to the distance between the non-obstruction point and the obstacle point.

4. A path planning method based on space transformation according to claim 3, characterized in that: Repulsion distance P b The repulsion distance P is expressed as a polynomial by series expansion. b The coordinate distance x = ||δX′ b || polynomial basis [x a , x b , x c , ...] and the inner product of the polynomial coefficient vector C:

5. The path planning method based on space transformation according to claim 4, characterized in that: The spatial transformation relationship between the real map and the barrier-free space is solved as follows: Mark the coordinate to be transformed as y, and the coordinate after transformation as y; let the neighborhood range of coordinate y be ΔL; Find the set of reference coordinate points {P L }; If the set of reference coordinate points {P L } is not greater than the map dimension d+1, then the radius of ΔL is enlarged; if the set of reference coordinate points {P L } is greater than the map dimension d+1, then select the set {P L } is the origin, the reference point coordinates and transformed coordinates of the origin are x0 and x′0 respectively; the corresponding sets of the remaining reference coordinate points and transformed coordinates are {x}, {x′}; Through the mapping relationship A: x→x′, solve the linear transformation matrix A, A(x-x0)=x′-x′0; Calculate the transformed coordinate y′=A(y-x0)+x′0 to obtain the transformed coordinate y.

6. A path planning method based on space transformation according to claim 5, characterized in that: The dimension d of the real map is two-dimensional or three-dimensional.

7. A path planning method based on space transformation according to claim 3, characterized in that: In the set {X′} of non-obstruction point coordinates in the obstacle-free space, between the starting point x′0 and the end point x′ end The paths between discrete points are connected by straight lines, broken lines or curves.

8. The path planning method based on space transformation according to claim 5, characterized in that: The fitting rule of the path discrete points is: S31, obtaining at least one path discrete point M' of the starting point and the end point of the obstacle-free space; making the normal offset of M' cL, where c is the offset coefficient, and L is the starting point x'0 and the end point x' of the obstacle-free space end The length of the connection; S32, perform spatial transformation on the path discrete point M′ to obtain the coordinate M after spatial transformation; determine whether the position of the path discrete point before and after spatial transformation is abnormal: if there is no position abnormality, execute the next step; if there is a position abnormality, adjust the value of the offset coefficient, and repeat steps S31-S32; S33, constructing a connection plan {x1} for the starting point x0 and the path discrete point M′, respectively, and constructing a connection plan {x2} for the end point and the path discrete point; the priority of the connection methods of different path discrete points is straight line connection> broken line connection> curve connection; S34. If a path obtained by connecting different discrete points of the path with a straight line passes through an obstacle area, is not connected to an adjacent path, or passes through an area outside the map limit, the straight line connection mode is replaced with a broken line or a curved line connection mode; S35. Merge the obtained connection plans {x1} and {x2}, and end.

9. The path planning method based on space transformation according to claim 8, characterized in that: Step S32 determines whether the position of the path discrete point before and after the spatial transformation is abnormal, that is, whether the path discrete point M′ and its coordinate M after spatial transformation are located in the obstacle of the real map and its adjacent dangerous area W, as well as the coverage area W′ of the spatial transformation corresponding to the dangerous area.

10. The path planning method based on space transformation according to claim 8, characterized in that: The offset coefficient ranges from 0.01 to 0.5.

Images