Path planning method and device for unstructured environment passable area

Abstract

The invention provides a path planning method and device for a passable area in an unstructured environment, and the method comprises the steps: inputting a two-dimensional grid map of the passable area, enabling the grid map to represent the passable area and an obstacle through pixels, and extracting the boundary of the passable area; acquiring a first track point meeting a first condition with the current position of the moving body from the track point set, and adjusting the first track point according to the passable area boundary to obtain a target track point; calculating an optimal path from the current position to the target trajectory point as a candidate trajectory; and adding the current position, the target trajectory point, the candidate trajectory and the passable area boundary into a hypergraph, establishing a target constraint by using passable area boundary constraint, dynamic constraint and kinematics constraint, and performing optimization to obtain an optimized trajectory from the current position to the target trajectory point. According to the method, the safety and the real-time performance of path planning in an unstructured environment scene are improved, and then the accuracy of path planning is improved.

Description

Technical Field

[0001] The present invention belongs to the field of path planning in unstructured environments, and in particular relates to a path planning method and device for a traversable area in an unstructured environment. Background Art

[0002] Path planning in unstructured environments such as rural roads and rugged terrain is a challenging task, as these scenes lack clear driving boundaries or lane lines, and drivable areas are often difficult to identify. In addition, vehicles or mobile robots often encounter irregularly shaped obstacles such as bushes, trees, and mounds, which can pose potential hazards to navigation.

[0003] In the prior art, the global path planning method can plan a complete trajectory from the starting point to the end point at one time. However, when facing an unstructured environment, due to the lack of local maps and incomplete environmental information, this method cannot handle obstacles that are not on the map, nor can it receive real-time sensor observations. Methods based on sensor observation neural networks include reinforcement learning methods and imitation learning methods. Among them, reinforcement learning methods rely on interaction with the environment, but direct training in a real-world environment may be dangerous, and training in a simulation environment will lead to problems from simulation to reality. Imitation learning methods require a large amount of training data, and it costs a lot to label this data. Among the local path planning methods, the dynamic window approach (DWA) is a sampling-based method that samples from the motion space and complies with kinematic constraints, but DWA is not forward-looking. Therefore, the prior art still lacks a method for accurately implementing path planning in unstructured environment scenarios. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention proposes a path planning method and device for traversable areas in unstructured environments, which improve the security and real-time performance of path planning in unstructured environment scenarios, thereby improving the accuracy of path planning.

[0005] To achieve the above objectives, the present invention discloses, on one hand, a path planning method for a traversable area in an unstructured environment, comprising:

[0006] Input a two-dimensional grid map of the traversable area, where the grid map represents the traversable area and obstacles in pixels, and extract the boundary of the traversable area;

[0007] Acquire a first trajectory point that satisfies a first condition with the current position of the moving body from the trajectory point set, and adjust the first trajectory point according to the boundary of the passable area to obtain a target trajectory point;

[0008] Calculate the optimal path from the current position to the target trajectory point as a candidate trajectory;

[0009] The current position, the target trajectory point, the candidate trajectory and the traversable area boundary are added to the hypergraph, and the target constraint is established using the traversable area boundary constraint, dynamic constraint and kinematic constraint, and the optimized trajectory from the current position to the target trajectory point is optimized.

[0010] In some embodiments, extracting the traversable area boundary includes:

[0011] Traversing the grid map, if a pixel point is identified as representing an obstacle and is located in a traversable area, the pixel point is added to the first grid point set;

[0012] If the pixel point is identified as representing a passable area, and at least one of the other four pixels around the pixel point is identified as representing a non-passable area, then the pixel point is added to the first grid point set;

[0013] Clustering the first grid point set to obtain a number of clusters, where each pixel point in the same cluster represents a point belonging to the same polygon after clustering;

[0014] Extract the contour of each cluster, and perform polygonal approximation on each contour to fit the vertices of the polygon. All contours are fitted with polygons to obtain the initial passable area boundary;

[0015] The initial passable area boundary is converted from the pixel coordinate system to the world coordinate system to obtain the final extracted passable area boundary.

[0016] In some embodiments, the method further comprises:

[0017] Use the global path planning algorithm to plan the global path and generate the trajectory point set, including:

[0018] On the global map, given a global starting point and a global end point, a global path from the global starting point to the global end point is planned using the global path planning algorithm using the road network information of the global map, and the global starting point and the global end point are added to the trajectory point set;

[0019] Taking the first preset distance as a reference and the global starting point as an initial reference point for traversal, if the distance between the current traversal point and the initial reference point is greater than or equal to the first preset distance, then adding the current traversal point to the trajectory point set, and setting the current traversal point as a new reference point;

[0020] Continue traversing until all points in the global path are traversed to obtain a complete set of trajectory points.

[0021] In some embodiments, the first condition is:

[0022] Obtaining the trajectory point closest to the current position of the moving body from the trajectory point set;

[0023] Judging whether the closest trajectory point is in front of the moving body according to the steering angle of the moving body and the angle between the current position of the moving body and the closest trajectory point, comprises:

[0024] If the difference between the steering angle and the included angle is less than π / 2, the closest trajectory point is in front of the moving body, and the closest trajectory point is updated to the first trajectory point;

[0025] If the difference between the steering angle and the included angle is greater than or equal to π / 2, the closest trajectory point is not in front of the moving body, and the next trajectory point of the closest trajectory point is updated to the first trajectory point.

[0026] In some embodiments, adjusting the first track point according to the passable area boundary to obtain the target track point includes:

[0027] Convert the current position and the first track point from the world coordinate system to the pixel coordinate system to obtain the current position and the first track point in the pixel coordinate system;

[0028] In the pixel coordinate system, the distance between the first trajectory point and the edge formed by every two vertices of the polygonal vertices of the boundary of the passable area is calculated to determine the edge with the closest distance;

[0029] Extending a line between the first trajectory point and the first intersection point of the edge closest to the distance to obtain a second intersection point with the polygon, and calculating a first distance between a midpoint of the line between the first intersection point and the second intersection point and the first trajectory point;

[0030] Comparing the first distance with a second preset distance, if the first distance is less than or equal to the second preset distance, adjusting the first trajectory point to the midpoint;

[0031] If the first distance is greater than the second preset distance, a circle is drawn with the first trajectory point as the center and the second preset distance as the radius to obtain a third intersection point between the midpoint of the line connecting the first intersection point and the second intersection point and the line connecting the first trajectory point and the circle, and the first trajectory point is adjusted to the third intersection point;

[0032] The pixel coordinate system is converted into a world coordinate system, and the adjusted first track point is determined as a target track point in the world coordinate system.

[0033] In some embodiments, the first distances obtained in each iteration process are summarized to obtain a first distance average, and the second preset distance is updated according to the first distance average.

[0034] In some embodiments, a global path planning algorithm is used to calculate the optimal path from the current position to the target trajectory point to obtain the candidate trajectory.

[0035] In some embodiments, the method further comprises:

[0036] According to the current position and the passable area boundary, the passable area boundary constraint is established, including:

[0037] The distance between the candidate trajectory and each edge of the traversable area boundary is calculated in turn as the second distance. If the second distance is greater than or equal to the third preset distance, the traversable area boundary constraint is determined according to the speed of the moving body, the second distance, and the third preset distance.

[0038] In some embodiments, a spatial clustering algorithm is used to cluster the first grid point set according to the compactness of the samples.

[0039] In some embodiments, a polygon fitting algorithm is used to perform a polygonal approximation on each contour.

[0040] In some embodiments, the unstructured environment includes different environments such as land, water, and air, and the corresponding moving bodies include vehicles or wheeled mobile robots for use in land environments, ships or underwater robots for use in water environments, and airplanes or flying robots for use in air environments.

[0041] On the other hand, the present invention also discloses a path planning device for a traversable area in an unstructured environment, which adopts the above-mentioned path planning method for a traversable area in an unstructured environment, and the device at least comprises:

[0042] A passable area extraction optimization module is used to input a two-dimensional grid map of the passable area, where the grid map represents the passable area and obstacles in pixels, and extract the boundary of the passable area;

[0043] A global target trajectory point dynamic adjustment module is used to obtain a first trajectory point that satisfies a first condition with the current position of the moving body from the trajectory point set, and adjust the first trajectory point according to the passable area boundary to obtain a target trajectory point;

[0044] A hypergraph optimization module is used to calculate the optimal path from the current position to the target trajectory point as a candidate trajectory; and to add the current position, the target trajectory point, the candidate trajectory and the traversable area boundary to the hypergraph, and to establish the target constraint using the traversable area boundary constraint, the dynamic constraint and the kinematic constraint, and to optimize the optimized trajectory from the current position to the target trajectory point.

[0045] Another aspect of the present invention discloses a computer-readable storage medium having a computer program stored thereon, which implements the steps of the above method when executed by a processor.

[0046] Another aspect of the present invention discloses a computer program product, comprising a computer program, which implements the steps of the above method when executed by a processor.

[0047] It can be seen from the above scheme that the advantages of the present invention are:

[0048] The path planning method for the traversable area in an unstructured environment disclosed by the present invention adds global target point adjustment, adjusts the trajectory point to the middle of the traversable area as much as possible, generates the optimal candidate trajectory using the adjusted target point, and adds acceleration constraints; uses the traversable area of ​​a two-dimensional grid, adopts clustering and polygon approximation methods, represents obstacles as polygons, and reduces the computational cost. And using G2O hypergraph optimization, the optimization of the optimal candidate trajectory makes the generated trajectory avoid optimization falling into the local minimum as much as possible, uses the traversable area boundary constraint instead of the obstacle constraint, reduces the hypergraph optimization time, and thus improves the real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 The overall flow chart of a path planning method for a traversable area in an unstructured environment disclosed in an embodiment of the present invention;

[0050] Figure 2 The schematic procedure for optimizing the extraction of traversable areas is shown;

[0051] Figure 3 The schematic procedure of dynamic adjustment of global target point is shown;

[0052] Figure 4 A structural diagram of a path planning device is shown;

[0053] Figure 5 This is a schematic diagram of the application environment structure of the first electronic device of the present invention;

[0054] Figure 6 It is a schematic diagram of the structure of a second electronic device of the present invention.

[0055] Reference numerals:

[0056] 200: A path planning device for a traversable area in an unstructured environment;

[0057] 210: Traversable area extraction optimization module;

[0058] 220: global target trajectory point dynamic adjustment module;

[0059] 230: hypergraph optimization module;

[0060] 300: first electronic device;

[0061] 310: data acquisition equipment;

[0062] 320: information display device;

[0063] 400: second electronic device;

[0064] 410: computing unit;

[0065] 420: ROM;

[0066] 430: RAM;

[0067] 440: input unit;

[0068] 450: output unit;

[0069] 460: communication unit;

[0070] 470: Storage medium. DETAILED DESCRIPTION

[0071] It should be noted that, in this application, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprises" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0072] Without more constraints, an element defined by the phrase "comprising a..." does not exclude the existence of other identical elements in the process, method, article or apparatus comprising the element.

[0073] Timed-Elastic-Band (TEB) is a multi-objective path optimization algorithm that can incorporate different constraints into path planning problems. However, the existing TEB method has shortcomings in terms of safety and real-time performance when facing unstructured environments. Among them, for the safety problem, TEB requires multiple candidate trajectories for optimization, but if the initial trajectory intersects with obstacles, it will fall into a local minimum and the trajectory optimization will become unsafe. The trajectory points are planned on a given map by global path planning. Because there is an error between the map and the actual position, there is a large error in the trajectory points (usually 5-6m on average). In extreme cases, the trajectory points will be on the outside of the road. For the real-time problem, TEB needs to traverse the entire cost map when dealing with obstacles, and add the pixel points representing obstacles in the cost map to the algorithm in the form of point obstacles. Doing so will make the algorithm consider all points during optimization, ignoring the original shape of the obstacle and greatly increasing the amount of calculation. This will lead to a long decision time for the algorithm, resulting in the vehicle or mobile robot being unable to obtain information about changes in the surrounding environment in a timely manner, thereby increasing the frequency of manual intervention and reducing the accuracy of path planning.

[0074] When the inventors were studying the TEB planning method in an unstructured environment, they found that this defect in the prior art was caused by two aspects. The first is that the deviation of the trajectory point from the road position will affect the effect of the local path planning algorithm; the second is the problem of high time cost in obstacle extraction with irregular boundaries. The reason for the first defect is mainly because when the global map information is not accurate enough, there will be a large deviation in the trajectory of global path planning on the offline map, so it is necessary to dynamically adjust the global target point in combination with the real-time road passable area information. For the second defect, in unstructured scenarios, the road boundaries and obstacle shapes are irregular. Among the existing methods that use polygonal approximation for obstacles, the convex polygon approximation method has a large error, and the concave polygon approximation method has a high time cost. Based on this, the embodiment of the present invention improves the existing TEB path planning method, proposes a passable area extraction optimization module, and uses clustering and polygon approximation methods to efficiently use polygons to represent the passable area boundaries in the two-dimensional grid map; proposes a global target point dynamic adjustment module to adaptively correct future trajectory points and obtain the optimal candidate trajectory, so that the moving body can better travel along the middle of the road, thereby reducing the possibility of falling into a local minimum; uses the passable area boundary constraint to replace the original obstacle constraint in the TEB, and adds acceleration constraints to reduce the G2O optimization time while allowing the moving body to travel more safely. The path planning method of the present invention for the passable area in an unstructured environment will be described in detail below.

[0075] See also Figure 1 As shown, Figure 1A schematic diagram showing the overall process of a path planning method for a traversable area in an unstructured environment according to an embodiment of the present invention is shown;

[0076] A path planning method for a traversable area in an unstructured environment, the method comprising the following steps:

[0077] Step S1: input a two-dimensional grid map of a traversable area, wherein the grid map represents the traversable area and obstacles in pixels, and extract the boundary of the traversable area.

[0078] In this embodiment, the purpose is to solve the path planning problem in unstructured environment scenarios. The unstructured scenarios here are not limited to land environments, but can actually be different environments such as water and air. The corresponding moving bodies here are not limited to vehicles, wheeled mobile robots, etc. in land environments, but can also be ships, underwater robots, etc. in water environments, and airplanes, flying robots, etc. in air environments. The present invention does not make specific limitations.

[0079] In this embodiment, a two-dimensional grid map of the passable area is input, wherein the grid map represents the information of the passable area and obstacles in pixels. By traversing the grid map, if a pixel point is identified as representing an obstacle and is located in the passable area, the pixel point is added to the first grid point set; if a pixel point is identified as representing a passable area, and at least one of the remaining four pixels around the pixel point is identified as representing a non-passable area, the non-passable area includes obstacles and other parts that do not belong to the passable area.

[0080] Then the pixel point is also added to the first grid point set. Then, the first grid point set is clustered. In one embodiment, the DBSCAN density-based spatial clustering algorithm is used to cluster the first grid point set according to the density of the samples. After clustering the first grid point set, several clusters are obtained, and each pixel point in the same cluster represents a point belonging to the same polygon after clustering. Further, an edge detection algorithm is used to extract the edge of each cluster to obtain the contour. And polygonal approximation is performed on each contour to fit the vertices of the polygon. All contours are fitted by polygons to obtain the initial passable area boundary b. In one embodiment, the Douglas-Peucker algorithm is used to perform polygonal approximation on each contour. The algorithm can well fit the passable area boundary and adjust the fitting accuracy; and the algorithm does not require additional time overhead.

[0081] Finally, the initial passable area boundary b is further converted from the pixel coordinate system to the world coordinate system to obtain the final extracted passable area boundary b w Specifically, the initial passable area boundary b is calculated by (X g , Y g) is converted to a point in the world coordinate system (X w , Y w ), the coordinates p of the current position of the known moving body in the world coordinate system w Yes(x w ,y w ), the coordinates in the pixel coordinate system are (x g ,y g ), the calculation formula is as follows:

[0082]

[0083] Where P represents the rotation matrix, which is composed of x w ,y w and the current steering angle β of the moving body w After all points in the initial passable area boundary are converted to the world coordinate system, the passable area boundary b extracted in the world coordinate system is obtained. w .

[0084] The above-mentioned process of extracting the boundary of the passable area can be actually based on Figure 2 The program flow shown is implemented.

[0085] In addition, in some embodiments, further according to the current position p of the moving body w 、The boundary of the passable area b w , establish the boundary constraints f of the traversable area b The passable area boundary constraint is designed to make better use of the passable area boundary information. This constraint takes into account both the velocity v of the moving body and the distance from the passable area boundary, because the faster the velocity, the greater the risk of the moving body deviating from the road. Therefore, it can prevent the moving body from going out of the boundary and keep it within the passable area. Specifically, the second distance d from the candidate trajectory to each edge of the passable area boundary is calculated. b , if the second distance d b Greater than or equal to the third preset distance d minb , then according to the speed v of the moving body, the second distance d b , the third preset distance d minb , determine the boundary constraints f of the traversable area b =v*e(-d b , -d minb ,ε,S,N), where e represents the penalty function, ∈ is the offset, S is the scaling factor, and N is the polynomial order.

[0086] The traversable area boundary constraint may be used in the hypergraph optimization process in step S4. For details, see the corresponding content of step S4, which will not be described in detail here.

[0087] Step S2: acquiring a first trajectory point that satisfies a first condition with respect to the current position of the moving body from the trajectory point set, and adjusting the first trajectory point according to the boundary of the passable area to obtain a target trajectory point.

[0088] In this embodiment, a global path planning algorithm is used to plan a global path and generate the trajectory point set. Specifically, on a global map, given a global starting point and a global end point, the A* algorithm is used to plan a global path W from the global starting point to the global end point using the road network information of the global map. global , add the global starting point and the global end point to the trajectory point set g, and mark the global starting point and the global end point as having been traversed. Then, with the first preset distance dis global As a reference, the global starting point is used as the initial reference point for traversal. If the distance between the current traversal point and the initial reference point is greater than or equal to the first preset distance dis global , then add the current traversal point to the trajectory point set g, update the trajectory point set, and set the current traversal point as the new reference point. Each traversed point is marked as traversed, and the traversal continues until all points in the global path are traversed, and the complete trajectory point set g is obtained. The points in g are the coordinates corresponding to the x-axis and y-axis in the world coordinate system.

[0089] After obtaining the trajectory point set, further obtain the first trajectory point that satisfies the first condition with the current position of the moving body from the trajectory point set. Here, the first condition is specifically set as: judging whether the closest trajectory point is in front of the moving body according to the steering angle of the moving body and the angle between the current position of the moving body and the trajectory point closest to the current position. Specifically, by calculating the angle between each trajectory point in the trajectory point set g and the current position p of the moving body, the first trajectory point satisfies the first condition. w distance to obtain the trajectory point p closest to the current position of the moving body close If the steering angle β w The angle ∠p w p close If the difference between them is less than π / 2, it means that the closest trajectory point is in front of the moving body. close Update to the first trajectory point p. If the steering angle β w The angle ∠p w p close If the difference between them is greater than or equal to π / 2, it means that the closest trajectory point p close If the moving object is not in front of the moving object, the next trajectory point of the closest trajectory point in the trajectory point set is updated to the first trajectory point p. The first trajectory point p and the current position p are recorded. w The third distance between c .

[0090] In this embodiment, after obtaining the first trajectory point, the first trajectory point is further adjusted to be close to the center of the passable area according to the boundary of the passable area to obtain the target trajectory point. Specifically, the world coordinate system is first converted into a pixel coordinate system. The current position p in the world coordinate system is w The first trajectory point is converted into the pixel coordinate system and is represented as p w_grid With p grid At this time, in the pixel coordinate system, calculate the first trajectory point p grid The distance between the edge formed by each two vertices of the polygon vertex of the passable area boundary is determined to determine the closest edge. grid The line connecting the first intersection point p1 of the edge closest to the distance is extended to obtain the second intersection point p2 of the polygon, and the midpoint p1p2 of the line connecting the first intersection point and the second intersection point is calculated. tmp With the first trajectory point p grid The first distance dis between them. Using the second preset distance dis threshold To limit the adjustment distance of the first track point, the distance is initially set to a value. grid If the global point is placed at the midpoint of the passable area, the global point may shake violently left and right, and the large shaking will affect the processing of sensor information. This deviation may affect the segmentation of the road ahead, resulting in increasing errors. Therefore, in this embodiment, by comparing the first distance dis with the second preset distance dis threshold , if the first distance dis is less than or equal to the second preset distance dis threshold , adjust the first trajectory point p grid The midpoint p tmp , which means that the first trajectory point p grid Updated to be closer to the midpoint p of the center of the traversable area tmp If the first distance is greater than the second preset distance, it means that it is closer to the midpoint p of the center of the passable area. tmp With the first trajectory point p grid The distance is too far, exceeding the threshold, and the distance needs to be limited. At this time, the first trajectory point p grid is the center of the circle, the second preset distance dis threshold Draw a circle with radius p to get the midpoint p tmp The line p between the first trajectory point tmp p grid The third intersection point p3 with the circle adjusts the first trajectory point p grid At this time, after the adjustment of the first track point is completed, the pixel coordinate system is reconverted into the world coordinate system, and the adjusted first track point is determined as the target track point p′ in the world coordinate system.

[0091] In addition, in this embodiment, the second preset distance dis threshold A value is set after initialization, and the value is adaptively adjusted according to the calculated historical distance dis. Specifically, the first distance obtained in each iteration process is summarized, for example, 10 times, to obtain the first distance average, and the second preset distance is updated according to the first distance average.

[0092] Repeat the above adjustment process of the first track point until the adjustment number num is greater than the adjustment number threshold maxnum or the third distance dis between the recorded current position and the first track point is greater than c Less than dis min , stop adjusting the trajectory point. The purpose of this is to prevent the final position from deviating too far from the initial position due to repeated adjustment of the trajectory point in some environments. Whenever a trajectory point in the trajectory point set g stops adjusting, switch to the next trajectory point and repeat the above process.

[0093] The dynamic adjustment process of the target trajectory point described above can be based on Figure 3 The program flow shown is implemented.

[0094] Step S3: Calculate the optimal path from the current position to the target trajectory point as a candidate trajectory.

[0095] In some embodiments, the Theta* algorithm is used to calculate the optimal path from the current position to the target trajectory point to obtain the candidate trajectory.

[0096] Step S4: add the current position, the target trajectory point, the candidate trajectory and the traversable area boundary to the hypergraph, establish the target constraint using the traversable area boundary constraint, the dynamic constraint and the kinematic constraint, and optimize to obtain a safe, comfortable and dynamically feasible optimized trajectory from the current position to the target trajectory point.

[0097] In step S4, the current position, the target trajectory point, the candidate trajectory and the traversable area boundary are added to the hypergraph, and the optimized trajectory from the current position to the target trajectory point is obtained by using the hypergraph optimization. Specifically, the traversable area boundary constraint, the dynamic constraint and the kinematic constraint are added to the target constraint condition to establish the target constraint.

[0098] In the hypergraph optimization process, the traversable area boundary b, the target trajectory point p′ and the candidate trajectory Traj are used, and the G2O framework is used to solve the sparse optimization problem. The state s of the moving body is used to represent the planned trajectory at the i-th moment. i express:

[0099] s i =[xi ,y i , β i ] T

[0100] Among them, x i ,y i , β i Represent the coordinates and steering angle of the moving body at time i, respectively, where the x-axis represents the direction of the moving body and the y-axis represents the left side of the moving body. The time interval between two adjacent states s is ΔT i Indicates that the total time T can be expressed by the following formula:

[0101]

[0102] The optimization goal is to minimize the total travel time while satisfying the kinematic and dynamic constraints of the moving body. The optimization problem can be expressed by the following formula:

[0103] B:={s1,ΔT1,s2,ΔT2,…,s n-1 , ΔT n-1 ,s n}

[0104]

[0105] Among them, B represents the parameter to be optimized, f(B) is the objective function, B\{s1,s n} means that the starting point and the end point are fixed and not optimized. During the optimization process, the penalty function e is used to penalize the behavior that violates the constraint conditions:

[0106]

[0107] Among them, z is related to the status of each constraint, z r is the reference value, ∈ is the offset, S is the scaling factor, and N is the polynomial order.

[0108] Constraints need to be added to the G2O hypergraph, including traversable area boundary constraints, kinematic constraints, and dynamic constraints. Kinematic constraints include non-holonomic kinematic constraints, and dynamic constraints include minimum turning radius constraints, speed constraints, acceleration constraints, time constraints, jerk constraints, etc. The maximum and minimum speeds and accelerations of the AUV are considered in path planning, so that the planned path will be dynamically feasible. In addition, path planning may produce sharp turns, which is not dynamically feasible. Therefore, the path needs to be smoothed to ensure that the turning radius meets the minimum turning radius requirements of the AUV. Combine path planning with time to ensure that the dynamic constraints are met at every time point along the entire path. However, this division method is not absolute, and there is a certain degree of flexibility in the specific classification method, depending on its application scenario and the focus of attention.

[0109] Among them, each constraint condition is expressed as follows:

[0110] Nonholonomic kinematic constraints f h :Nonholonomic kinematics h i (s i+1 ,s i ) requires that two adjacent configurations need to be located on a common arc of constant curvature, that is, the current state s i and direction d i The angle θ between i Must be equal to the next moment state s i+1 The corresponding angle θ i+1 , nonholonomic kinematic constraints f h The formula is as follows:

[0111]

[0112] f h =e(h i (s i+1 ,s i ), 0,∈,S,N)

[0113] Minimum turning radius constraint f r :For a moving body, its movement is also limited by the minimum turning radius between two consecutive states. r represents the minimum turning radius constraint, d r Indicates the turning radius, d minr Indicates the minimum turning radius.

[0114] f r =e(-d r , -d minr ,∈,S,N)

[0115] Speed ​​constraint f v : velocity vector vi Including linear velocity v and angular velocity w, the velocity vector can be calculated from two consecutive states:

[0116]

[0117] Here, ||·|| represents the second norm.

[0118] Acceleration constraint f a : acceleration vector a i Including linear acceleration a v，i and angular acceleration a w，i , the acceleration vector can be calculated from two consecutive velocity vectors:

[0119]

[0120] Time Constraint time : To ensure that the time of the entire trajectory is as short as possible, the time constraint minimizes the sum of the squares of all time differences:

[0121]

[0122] Jerk constraint f jerk : In order to prevent the moving body from producing a series of unstable phenomena, such as sudden sprinting and braking, in one embodiment, a jerk constraint is further introduced. max Maximum jerk reference vector, jerk i It can be expressed as:

[0123]

[0124] The corresponding penalty function can be expressed as:

[0125] f jerk =e(jerk,jerk max ,∈,S,N)

[0126] The boundary constraint of the traversable area f b : In this embodiment, the above-mentioned constraint on the boundary of the traversable area is used to replace the original obstacle constraint, which can better utilize the boundary information of the traversable area and constrain the moving body to travel within the traversable area as much as possible. The optimization time can be reduced by reducing the number of edges that need to be optimized in the G2O hypergraph, thereby improving real-time performance.

[0127] f b =v*e(-d b , -d minb ,∈,S,N)

[0128] Among them, d b Indicates p wThe distance from each edge of the polygon, d minb Indicates the threshold value.

[0129] After defining the above objective constraints, construct the objective function f(B) containing various constraints. The objective function can be written as the weighted sum of different terms:

[0130] f(B)=W r *f r +W h *f h +W time *f time +W v *f v +W a *f a +W jerk *f jerk +W b *f b

[0131] Among them, W is the weight of each constraint, and f is the penalty function corresponding to the constraint.

[0132] In addition, it should be noted that in different application scenarios, kinematic constraints and dynamic constraints will also be different. For example, the underwater environment is often changing and contains many unknown or unpredictable elements, such as dynamic water flow, moving obstacles (such as other underwater robots or marine life) and complex terrain changes. Water flow, eddy currents and other hydrodynamic effects will affect the stability and capabilities of AUVs. At this time, the dynamic constraints and kinematic constraints of underwater robots may be different from those of mobile robots in land environments. For example, water flow changes are not limited to non-holonomic kinematics, minimum turning radius, speed, acceleration, time, jerk and other factors.

[0133] In the process of hypergraph optimization, initialization is first performed, and then the hypergraph is constructed. In this process, the target point trajectory p′ and the candidate trajectory Traj are used to generate a trajectory with equal time intervals as the vertex of the hypergraph. The state s of the hypergraph contains two parts of information: posture and motion time. Then, the state s is used to add constraint information such as the boundary of the passable area and the speed acceleration as the edge of the hypergraph. After the hypergraph is constructed, the hypergraph is optimized. Set the starting state s1 and the end state s n In order to avoid oscillation, the number of iterations can be set during the optimization process. In each iteration, new points are dynamically added, previous points are deleted, or the values ​​of existing points are optimized, with the goal of minimizing the total objective function. At the same time, a tolerable error range is set for each point in the time interval to avoid oscillation. After the hypergraph is optimized, the optimized trajectory from the current position to the target trajectory point is obtained.

[0134] Finally, after obtaining the final optimized trajectory, a trajectory point at a certain moment can be selected as a control command based on the optimized trajectory and the current position of the moving body, and the generated control command can be sent to the controller through an appropriate communication protocol (such as Wi-Fi, Bluetooth, wired connection, etc.). Ensure the stability and security of the communication link to prevent data loss or tampering. At the same time, after receiving the control command, the controller adjusts the motion state of the moving body according to the command. The controller also monitors the actual motion of the moving body and collects relevant status data (such as position, speed, acceleration, etc.) and sends it to the cloud or remote host computer through a communication network (such as 4G / 5G, Wi-Fi, satellite communication, etc.). After receiving the data, the cloud or remote host computer stores, processes and analyzes it to monitor the state of the moving body in real time and make necessary adjustments. The controller further corrects and optimizes the trajectory of the moving body in real time based on the feedback data to improve the control accuracy and efficiency. In addition, a user interface is developed on the cloud or remote host computer to display the real-time position, trajectory, status and other information of the moving body. In addition, during the entire process, fault warning processing is performed so that the system can be quickly identified, isolated and restored when a fault occurs.

[0135] In summary, the path planning method for the traversable area in an unstructured environment disclosed by the present invention adds global target point adjustment, adjusts the trajectory point to the middle of the traversable area as much as possible, generates the optimal candidate trajectory using the adjusted target point, and adds acceleration constraints; using the traversable area of ​​a two-dimensional grid, clustering and polygon approximation methods are used to represent obstacles as polygons, reducing the computational cost. And using G2O hypergraph optimization, the optimization of the optimal candidate trajectory makes the generated trajectory avoid optimization falling into the local minimum as much as possible, uses the traversable area boundary constraint instead of the obstacle constraint, reduces the hypergraph optimization time, and thus improves the real-time performance.

[0136] In addition, it should be noted that the path planning method disclosed in the present invention is not limited to being executed in the order of the above steps S1-S4. The steps can be reordered, added or deleted. As long as the expected results of the technical solution of the present invention can be achieved, the present invention is not limited here.

[0137] In addition, corresponding to the above-mentioned embodiment of the path planning method, the present invention also provides an embodiment of a path planning device for a traversable area in an unstructured environment.

[0138] See also Figure 4 , Figure 4 The schematic diagram of the structure of the path planning device for the traversable area of ​​the unstructured environment is shown. The path planning device 200 for the traversable area of ​​the unstructured environment at least comprises:

[0139] The traversable area extraction optimization module 210 is used to input a two-dimensional grid map of the traversable area, wherein the grid map represents the traversable area and obstacles in pixels, and extract the boundary of the traversable area.

[0140] The global target trajectory point dynamic adjustment module 220 is used to obtain a first trajectory point that satisfies a first condition with the current position of the moving body from the trajectory point set, and adjust the first trajectory point according to the passable area boundary to obtain a target trajectory point.

[0141] A hypergraph optimization module 230 is used to calculate an optimal path from the current position to the target trajectory point as a candidate trajectory; and

[0142] The current position, the target trajectory point, the candidate trajectory and the traversable area boundary are added to the hypergraph, and the target constraint is established using the traversable area boundary constraint, dynamic constraint and kinematic constraint, and the optimized trajectory from the current position to the target trajectory point is optimized.

[0143] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of each module of the described device can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.

[0144] like Figure 5 As shown, the present invention also proposes a first electronic device 300 in another embodiment, such as Figure 5 As shown, the first electronic device 300 can also be connected to the data acquisition device 310 and the information display device 320 through a wired or wireless information transmission scheme. The data acquisition device 310 is used to collect and obtain real-time information of the moving body, including location, etc., as well as information such as passable areas, and the information display device 320 is used to display the trajectory results obtained by the analysis of the present invention.

[0145] The information display device 320 can process the data output by the first electronic device 300 based on the information display mechanism to improve the readability of the data output by the first electronic device A. The information display mechanism can be manually preset, for example, the data output by the first electronic device A is visually displayed, which can be based on the display parameters and / or attributes set by the user. The display parameters can be, for example, the display data range, and the display attributes can be, for example, the display font, color, whether to scroll and play, etc. The user is presented with the key information specified by the user, such as the specific location, running trajectory, etc. The user can understand this information more timely without having to access the secondary page or scroll the page, saving the user's operation. Or the information display mechanism can be an artificial intelligence AI display model, which can learn the user's key information based on the user's previous usage habits, such as viewing time, number of clicks, number of edits, etc., and then automatically present rich and necessary key information to the user.

[0146] The present invention also provides a computer program product, which includes a computer program. The computer program can be stored in a readable storage medium. When the computer program is executed by a processor, the computer can execute the path planning methods provided by the above methods.

[0147] The present invention also proposes a storage medium in another embodiment for storing a computer program for executing the path planning method. It should be understood that the storage medium in the embodiment of the present invention may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic random access memory (DRAM), synchronous DRAM (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link DRAM (SLDRAM), and direct RAM bus random access memory (DR RAM).

[0148] Figure 6A schematic block diagram of a second electronic device 400 that can be used to implement an embodiment of the present invention is shown. The second electronic device 400 electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The second electronic device 400 can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples, and are not intended to limit the implementation of the present invention described herein and / or required. The second electronic device 400 may be the same or different from the first electronic device 300.

[0149] The second electronic device 400 includes a computing unit 410, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory 420 (ROM) or a computer program loaded from a storage medium VIII into a random access memory (RAM) 430. In the RAM 430, various programs and data required for the operation of the device 1000 can also be stored. The computing unit 410, the ROM 420, and the RAM 430 are connected to each other via a bus. An input / output (I / O) interface is also connected to the bus.

[0150] A number of components in the second electronic device 400 are connected to the I / O interface, including: an input unit 440, such as a keyboard, a mouse, etc.; an output unit 450, such as various types of displays, speakers, etc.; a storage medium, such as a disk, an optical disk, etc.; and a communication unit 460, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 460 allows the second electronic device 400 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0151] The computing unit 410 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 410 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 410 performs the various methods and processes described above, such as method steps S1-S4. For example, in some embodiments, the method may be implemented as a computer software program, which is tangibly contained in a machine-readable medium 470, such as a storage medium. In some embodiments, part or all of the computer program may be loaded and / or installed on the device 400 via the ROM 420 and / or the communication unit 460. When the computer program is loaded into the RAM 430 and executed by the computing unit 410, one or more steps of the method described above may be performed. Alternatively, in other embodiments, the computing unit 410 may be configured to execute the method in any other appropriate manner (e.g., by means of firmware).

[0152] Although the embodiments of the present invention have been disclosed as above, they are not limited to the applications listed in the specification and the implementation modes, and they can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and the scope of equivalents, the present invention is not limited to the specific details and the illustrations shown and described herein.

Claims

1. A path planning method for a traversable area in an unstructured environment, characterized in that: Include: Input a two-dimensional grid map of the traversable area, where the grid map represents the traversable area and obstacles in pixels, and extract the boundary of the traversable area; Acquire a first trajectory point that satisfies a first condition with the current position of the moving body from the trajectory point set, and adjust the first trajectory point according to the boundary of the passable area to obtain a target trajectory point; Calculate the optimal path from the current position to the target trajectory point as a candidate trajectory; The current position, the target trajectory point, the candidate trajectory and the traversable area boundary are added to the hypergraph, and the target constraint is established using the traversable area boundary constraint, dynamic constraint and kinematic constraint, and the optimized trajectory from the current position to the target trajectory point is optimized.

2. The method according to claim 1, characterized in that Extract the boundaries of the traversable area, including: Traversing the grid map, if a pixel point is identified as representing an obstacle and is located in a traversable area, the pixel point is added to the first grid point set; If the pixel point is identified as representing a passable area, and at least one of the other four pixels around the pixel point is identified as representing a non-passable area, then the pixel point is added to the first grid point set; Clustering the first grid point set to obtain a number of clusters, where each pixel point in the same cluster represents a point belonging to the same polygon after clustering; Extract the contour of each cluster, and perform polygonal approximation on each contour to fit the vertices of the polygon. All contours are fitted with polygons to obtain the initial passable area boundary; The initial passable area boundary is converted from the pixel coordinate system to the world coordinate system to obtain the final extracted passable area boundary.

3. The method according to claim 1, characterized in that Also includes: Use the global path planning algorithm to plan the global path and generate the trajectory point set, including: On the global map, given a global starting point and a global end point, a global path from the global starting point to the global end point is planned using the global path planning algorithm using the road network information of the global map, and the global starting point and the global end point are added to the trajectory point set; Taking the first preset distance as a reference and the global starting point as an initial reference point for traversal, if the distance between the current traversal point and the initial reference point is greater than or equal to the first preset distance, then adding the current traversal point to the trajectory point set, and setting the current traversal point as a new reference point; Continue traversing until all points in the global path are traversed to obtain a complete set of trajectory points.

4. The method according to claim 1, characterized in that: The first condition is: Obtaining the trajectory point closest to the current position of the moving body from the trajectory point set; Judging whether the closest trajectory point is in front of the moving body according to the steering angle of the moving body and the angle between the current position of the moving body and the closest trajectory point, comprises: If the difference between the steering angle and the included angle is less than π / 2, the closest trajectory point is in front of the moving body, and the closest trajectory point is updated to the first trajectory point; If the difference between the steering angle and the included angle is greater than or equal to π / 2, the closest trajectory point is not in front of the moving body, and the next trajectory point of the closest trajectory point is updated to the first trajectory point.

5. The method according to claim 1, characterized in that According to the boundary of the passable area, the first trajectory point is adjusted to obtain the target trajectory point, including: Convert the current position and the first track point from the world coordinate system to the pixel coordinate system to obtain the current position and the first track point in the pixel coordinate system; In the pixel coordinate system, the distance between the first trajectory point and the edge formed by every two vertices of the polygonal vertices of the boundary of the passable area is calculated to determine the edge with the closest distance; Extending a line between the first trajectory point and the first intersection point of the edge closest to the distance to obtain a second intersection point with the polygon, and calculating a first distance between a midpoint of the line between the first intersection point and the second intersection point and the first trajectory point; Comparing the first distance with a second preset distance, if the first distance is less than or equal to the second preset distance, adjusting the first trajectory point to the midpoint; If the first distance is greater than the second preset distance, a circle is drawn with the first trajectory point as the center and the second preset distance as the radius to obtain a third intersection point between the midpoint of the line connecting the first intersection point and the second intersection point and the line connecting the first trajectory point and the circle, and the first trajectory point is adjusted to the third intersection point; The pixel coordinate system is converted into a world coordinate system, and the adjusted first track point is determined as a target track point in the world coordinate system.

6. The method according to claim 5, characterized in that The first distances obtained in each iteration process are summarized to obtain a first distance mean, and the second preset distance is updated according to the first distance mean.

7. The method according to claim 1, characterized in that A global path planning algorithm is used to calculate the optimal path from the current position to the target trajectory point to obtain the candidate trajectory.

8. The method according to claim 1 or 2, characterized in that: Also includes: According to the current position and the passable area boundary, the passable area boundary constraint is established, including: The distance between the candidate trajectory and each edge of the traversable area boundary is calculated in turn as the second distance. If the second distance is greater than or equal to the third preset distance, the traversable area boundary constraint is determined according to the speed of the moving body, the second distance, and the third preset distance.

9. The method according to claim 2, characterized in that: A spatial clustering algorithm is used to cluster the first grid point set according to the compactness of the samples.

10. The method according to claim 2, characterized in that A polygon fitting algorithm is used to perform polygonal approximation on each contour.

11. The method according to claim 1, characterized in that The unstructured environment includes different environments such as land, water and air, and the corresponding moving bodies include vehicles or wheeled mobile robots used in land environments, ships or underwater robots used in water environments, and airplanes or flying robots used in air environments.

12. A path planning device for a traversable area in an unstructured environment, characterized in that: The device contains: A passable area extraction optimization module is used to input a two-dimensional grid map of the passable area, where the grid map represents the passable area and obstacles in pixels, and extract the boundary of the passable area; A global target trajectory point dynamic adjustment module is used to obtain a first trajectory point that satisfies a first condition with the current position of the moving body from the trajectory point set, and adjust the first trajectory point according to the passable area boundary to obtain a target trajectory point; A hypergraph optimization module is used to calculate the optimal path from the current position to the target trajectory point as a candidate trajectory; The current position, the target trajectory point, the candidate trajectory and the traversable area boundary are added to the hypergraph, and the target constraint is established using the traversable area boundary constraint, dynamic constraint and kinematic constraint, and the optimized trajectory from the current position to the target trajectory point is optimized.

13. A computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 11 are implemented.

14. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method described in any one of claims 1 to 11 are implemented.

Images