CN119164403A

CN119164403A - Path planning method and device for traversable areas in unstructured environments

Info

Publication number: CN119164403A
Application number: CN202411400588.XA
Authority: CN
Inventors: 郗浩宇; 李玮; 赵方舟; 陈亮; 胡瑜
Original assignee: Institute of Computing Technology of CAS
Current assignee: Institute of Computing Technology of CAS
Priority date: 2024-10-09
Filing date: 2024-10-09
Publication date: 2024-12-20
Anticipated expiration: 2044-10-09
Also published as: CN119164403B

Abstract

The invention provides a path planning method and device for a passable area facing an unstructured environment, the method comprises the steps of inputting a two-dimensional grid map of the passable area, wherein the grid map represents the passable area and an obstacle in a pixel mode, extracting a passable area boundary, acquiring a first track point meeting a first condition with the current position of a moving body from a track point set, 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 track point as a candidate track, adding a hypergraph into the current position, the target track point, the candidate track and the passable area boundary, establishing a target constraint by utilizing the passable area boundary constraint, the dynamic constraint and the kinematic constraint, and optimizing to obtain an optimized track from the current position to the target track point. The method improves the safety and the real-time performance of path planning in the unstructured environment scene, and further improves the accuracy of path planning.

Description

Path planning method and device for passable area of unstructured environment

Technical Field

The invention belongs to the field of path planning in an unstructured environment, and particularly relates to a path planning method and device for a passable area of an unstructured environment.

Background

Path planning in unstructured environments such as rural roads and rough terrain is a very challenging task, where clear driving boundaries or lane lines are lacking, and passable areas are often difficult to identify. In addition, vehicles or mobile robots often encounter irregularly shaped obstacles, such as shrubs, trees, and hills, which can present a potential hazard to navigation.

In the prior art, the global path planning method can plan a complete track from a starting point to an end point at one time, however, when the method faces towards an unstructured environment, due to the lack of incompleteness of a map and environment information, the method cannot process obstacles which are not on the map, and cannot receive observation of a real-time sensor. Methods for observing the neural network based on the sensor include a reinforcement learning method and a simulation learning method. The reinforcement learning method relies on interaction with the environment, but training directly in a real world environment can be dangerous, while training in a simulation environment can lead to simulation to real problems. The simulation learning method requires a large amount of training data, and labeling such data requires a great deal of cost. In the local path planning method, dynamic window method (Dynamic WindowApproach, DWA) is a sampling-based method that samples from the motion space and obeys the kinematic constraint, but DWA is not prospective. Therefore, the prior art does not have a method for accurately realizing path planning in a non-structural environment scene.

Disclosure of Invention

Aiming at the defects of the prior art, the invention provides a path planning method and device for a passable area of an unstructured environment, which improve the safety and instantaneity of path planning in an unstructured environment scene and further improve the accuracy of path planning.

In order to achieve the above objective, an aspect of the present invention discloses a path planning method for an unstructured environment passable area, which includes:

Inputting a two-dimensional grid map of the passable area, wherein the grid map represents the passable area and the obstacle by 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 a track point set, and adjusting the first track point according to the boundary of the passable area to obtain a target track point;

calculating an optimal path from the current position to the target track point as a candidate track;

Adding the current position, the target track point, the candidate track and the passable region boundary into a hypergraph, establishing target constraint by using passable region boundary constraint, dynamic constraint and kinematic constraint, and optimizing to obtain an optimized track from the current position to the target track point.

In some embodiments, extracting passable region boundaries includes:

Traversing the grid map, and adding the pixel point to a first grid point set if the pixel point is identified to represent an obstacle and is positioned in a passable area;

If the identified pixel points represent passable areas, and at least one pixel point of the other four surrounding pixel points taking the pixel point as a center is identified as representing non-passable areas, adding the pixel point into a first grid point set;

Clustering the first grid point set to obtain a plurality of clusters, wherein each pixel point in the same cluster represents a point belonging to the same polygon after clustering;

Extracting contours from each cluster, performing polygon approximation on each contour, fitting out vertexes of polygons, and fitting all the contours through the polygons to obtain an initial passable region boundary;

and converting the initial passable region boundary from a pixel coordinate system to a world coordinate system to obtain the finally extracted passable region boundary.

In some embodiments, the method further comprises:

Planning a global path by using a global path planning algorithm to generate the track point set, wherein the track point set comprises:

On a global map, giving a global starting point and a global end point, using road network information of the global map, using global path planning rules to draw a global path from the global starting point to the global end point, and adding the global starting point and the global end point into a track point set;

traversing by taking a first preset distance as a reference and taking the global starting point as an initial reference point, adding the current traversing point into a track point set and setting the current traversing point as a new reference point if the distance between the current traversing point and the initial reference point is greater than or equal to the first preset distance;

and continuing traversing until all points in the global path are traversed, and obtaining the complete track point set.

In some embodiments, the first condition is:

Acquiring a track point closest to the current position of the moving body from the track point set;

judging whether the nearest track point is in front of the moving body according to the steering angle of the moving body and the included angle between the current position of the moving body and the nearest track point, comprising:

if the difference between the steering angle and the included angle is smaller than pi/2, the track point closest to the steering angle is in front of the moving body, and the track point closest to the steering angle is updated to be the first track point;

if the difference between the steering angle and the included angle is greater than or equal to pi/2, the track point closest to the steering angle is not in front of the moving body, and the next track point of the track point closest to the steering angle is updated to be the first track point.

In some embodiments, adjusting the first track point according to the passable region boundary to obtain a target track point includes:

Converting the current position and the first track point from a world coordinate system into a pixel coordinate system to obtain the current position and the first track point under the pixel coordinate system;

calculating the distance between the first track point and the edge formed by every two vertexes in the polygon vertexes of the passable area boundary under a pixel coordinate system, and determining the edge closest to the first track point;

extending a connecting line between the first track point and a first intersection point of the edge closest to the first track point to obtain a second intersection point of the polygon, and calculating a first distance between a midpoint of the connecting line between the first intersection point and the second intersection point and the first track point;

comparing the first distance with a second preset distance, and if the first distance is smaller than or equal to the second preset distance, adjusting the first track point to be the midpoint;

if the first distance is greater than the second preset distance, the first track point is used as a circle center, the second preset distance is used as a radius to make a circle, a third intersection point of a connecting line between the midpoint of the connecting line of the first intersection point and the second intersection point and the connecting line of the first track point and the circle is obtained, and the first track point is adjusted to be 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.

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

In some embodiments, the candidate trajectory is obtained by calculating an optimal path from the current location to the target trajectory point using a global path planning algorithm.

In some embodiments, the method further comprises:

Establishing the passable zone boundary constraint according to the current position and the passable zone boundary, including:

and sequentially calculating the distance between the candidate track and each side of the passable region boundary, and determining the passable region boundary constraint according to the speed of the moving body, the second distance and the third preset distance if the second distance is larger than or equal to the third preset distance.

In some embodiments, the first set of grid points is clustered according to the degree of compactness of the sample using a spatial clustering algorithm.

In some embodiments, a polygon-fitting algorithm is employed to perform a polygon approximation for each contour.

In some embodiments, the unstructured environment comprises a land, water, or air environment, and the corresponding moving body comprises a vehicle or a wheeled mobile robot applied to the land environment, a ship or an underwater robot applied to the water environment, and an airplane or a flying robot applied to the air environment.

In another aspect, the present invention further discloses a path planning device facing an unstructured environment passable area, which adopts the path planning method facing the unstructured environment passable area, and the device at least comprises:

the passable area extraction optimization module is used for inputting a two-dimensional grid map of the passable area, wherein the grid map represents the passable area and the obstacle by pixels, and extracting the boundary of the passable area;

The global target track point dynamic adjustment module is used for 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;

And adding the current position, the target track point, the candidate track and the passable region boundary into a hypergraph, establishing target constraint by using passable region boundary constraint, dynamic constraint and kinematic constraint, and optimizing to obtain an optimized track from the current position to the target track point.

In yet another aspect, the invention also discloses a computer readable storage medium, on which a computer program is stored which, when being executed by a processor, implements the steps of the above method.

In yet another aspect, the invention also discloses a computer program product comprising a computer program which, when executed by a processor, implements the steps of the above method.

The advantages of the invention are as follows:

The path planning method for the passable area of the unstructured environment adds global target point adjustment, adjusts the track point to the middle of the passable area as much as possible, generates the optimal candidate track by using the adjusted target point, adds jerk constraint, and adopts a clustering and polygon approximation method to represent the obstacle as a polygon by using the two-dimensional grid passable area, thereby reducing the calculation cost. And G2O hypergraph optimization is utilized, the generated track is prevented from being trapped into a local minimum value as far as possible by optimizing the optimal candidate track, and the passable region boundary constraint is used for replacing the barrier constraint, so that the hypergraph optimization time is reduced, and the instantaneity is further improved.

Drawings

FIG. 1 is a general flow chart of a method for path planning for an unstructured environment-oriented navigable area according to an embodiment of the invention;

FIG. 2 illustrates a passable region extraction optimization schematic;

FIG. 3 shows a global target point dynamic adjustment schematic procedure;

FIG. 4 shows a block diagram of a path planning apparatus;

FIG. 5 is a schematic view of an application environment of a first electronic device according to the present invention;

fig. 6 is a schematic structural diagram of a second electronic device according to the present invention.

Reference numerals:

200, a path planning device facing to a passable area of an unstructured environment;

a passable region extraction optimization module 210;

220, a global target track point dynamic adjustment module;

230, a hypergraph optimization module;

300, a first electronic device;

310, data acquisition equipment;

320 an information display device;

400 a second electronic device;

a calculation unit 410;

420:ROM;

430:RAM;

440, an input unit;

450 an output unit;

460 a communication unit;

470 storage medium.

Detailed Description

It is noted that in the present application, relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.

Without further limitation, an element defined by the phrase "comprising one does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises an element.

Time Elastic Band (TEB) is a multi-objective path optimization algorithm that can incorporate different constraints into the path planning problem. However, the existing TEB method has disadvantages in terms of security and real-time when facing unstructured environments. However, if the initial trajectory intersects with an obstacle, a local minimum is involved, and trajectory optimization becomes unsafe. The track points are planned on a given map by global path planning, and because of errors between the map and the actual positions, the track points have larger errors (generally average 5-6 m), and in extreme cases, the track points are outside the road. Aiming at the real-time problem, when the TEB processes the obstacle, the whole cost map needs to be traversed, and pixel points representing the obstacle in the cost map are added into the algorithm in the form of point-shaped obstacles, so that the algorithm considers all points in the optimization, the original shape of the obstacle is ignored, and the calculated amount is greatly increased. This will result in too long decision time of the algorithm, resulting in that the vehicle or the mobile robot cannot acquire the change information of the surrounding environment in time, thereby increasing the frequency of manual intervention and resulting in reduced path planning accuracy.

When the inventor researches a TEB planning method in an unstructured environment, the defects in the prior art are found to be caused by two aspects, namely, the position of a track point deviated from a road influences the effect of a local path planning algorithm, and the problem of high time overhead for extracting obstacles aiming at irregular boundaries. For the first defect, the reason is mainly that when the global map information is not accurate enough, a large deviation exists in the track planned by the global path on the offline map, so that the global target point needs to be dynamically adjusted in combination with the real-time road passable area information. For the second defect, in the unstructured scene, the road boundary and the shape of the obstacle are irregular, and in the existing method for approximating the obstacle by adopting a polygon, the error is larger by adopting a convex polygon approximation method, and the time cost is large by adopting a concave polygon approximation method. Based on the method, the embodiment of the invention improves the existing TEB path planning method, provides a passable area extraction optimization module, efficiently uses polygons to represent passable area boundaries in a two-dimensional grid map by using a clustering and polygon approximation method, provides a global target point dynamic adjustment module, adaptively corrects future track points and obtains optimal candidate tracks, so that a moving body can better run along the middle of a road, the possibility of sinking into local minima is reduced, original obstacle constraints in the TEB are replaced by using passable area boundary constraints, and acceleration constraint is added, G2O optimization time is reduced, and the moving body can safely run. The method for planning the path of the passable area of the unstructured environment is described in detail below.

Referring to fig. 1, fig. 1 is a schematic general flow diagram of a path planning method for an unstructured environment-oriented passable area according to an embodiment of the present invention;

A path planning method facing to a passable area of an unstructured environment comprises the following steps:

s1, inputting a two-dimensional grid map of the passable area, wherein the grid map represents the passable area and the obstacle by pixels, and extracting the boundary of the passable area.

In this embodiment, the purpose of solving the path planning problem in the unstructured environment is not limited to the land environment, but may be different environments such as water area and air in practice, the corresponding moving body is not limited to vehicles, wheeled mobile robots and the like in the land environment, vessels, underwater robots and the like in the water area environment, and airplanes, flying robots and the like in the air environment.

In this embodiment, a two-dimensional grid map of the passable area is input, wherein the grid map represents information of the passable area and the obstacle in pixels. If the identified pixel point represents the passable area, and at least one pixel point of the other four surrounding pixels taking the pixel point as the center is identified as representing the non-passable area, wherein the non-passable area comprises the obstacle and other parts not belonging to the passable area.

The pixel is also added to the first set of grid points. The first set of grid points is then clustered, in one embodiment, using a DBSCAN density-based spatial clustering algorithm, based on the degree of compactness of the samples. And clustering the first grid point set to obtain a plurality of clusters, wherein each pixel point in the same cluster represents a point belonging to the same polygon after clustering. And further extracting edges of each cluster by adopting an edge detection algorithm to obtain a contour. And performing polygon approximation on each contour, fitting out the vertexes of the polygons, and obtaining an initial passable area boundary b through polygon fitting of all the contours. In one embodiment, a Douglas-Peucker algorithm is used to perform a polygonal approximation for each contour. The algorithm can well fit the passable area boundary, the fitting accuracy can be adjusted, and the algorithm does not need to increase extra time cost.

Finally, the initial passable region boundary b is further converted from a pixel coordinate system to a world coordinate system, and the finally extracted passable region boundary b _w is obtained. Specifically, the initial passable region boundary b is converted from (X _g,Y_g) in the pixel coordinate system to a point (X _w,Y_w) in the world coordinate system, the coordinate p _w of the current position of the moving body in the world coordinate system is known to be (X _w,y_w), and the pixel coordinate system is known to be (X _g,y_g), and the calculation formula is as follows:

wherein P represents a rotation matrix, which is calculated by x _w,y_w and the current steering angle beta _w of the moving body, and Res represents the resolution of the two-dimensional grid map. And converting all points in the initial passable region boundary into a world coordinate system to obtain a passable region boundary b _w extracted under the world coordinate system.

The above-described process of extracting the passable region boundary may be actually implemented according to the program flow shown in fig. 2.

Furthermore, in some embodiments, the passable region boundary constraint f _b is established further in accordance with the current position p _w of the moving body, the passable region boundary b _w. Passable zone boundary constraints are designed to better utilize passable zone boundary information. This constraint takes into account both the speed v of the moving body and the distance from the border of the passable area, since the faster the speed, the greater the risk of the moving body deviating from the road. Thus, it can prevent the moving body from exiting the boundary and keep it within the passable area. Specifically, a second distance d _b between the candidate track and each side of the passable region boundary is calculated, if the second distance d _b is greater than or equal to a third preset distance d _minb, the passable region boundary constraint f _b＝v*e(-d_b,-d_minb, epsilon, S, N is determined according to the speed v of the moving body, the second distance d _b and the third preset distance d _minb, wherein e represents a penalty function, epsilon is an offset, S is a scaling factor, and N is a polynomial order.

The passable region boundary constraint can be used in the hypergraph optimization process in step S4, specifically see the corresponding content of step S4, which is not described herein too much.

And S2, 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 region boundary to obtain a target track point.

In this embodiment, the set of track points is generated by planning a global path using a global path planning algorithm. Specifically, on a global map, a global starting point and a global end point are given, a global path W _global from the global starting point to the global end point is drawn by using a rule of A by utilizing road network information of the global map, the global starting point and the global end point are added into a track point set g, and the two points of the global starting point and the global end point are marked as traversed. Then, traversing by taking the first preset distance dis _global as a reference and taking the global starting point as an initial reference point, adding the current traversing point into the track point set g, updating the track point set and setting the current traversing point as a new reference point if the distance between the current traversing point and the initial reference point is greater than or equal to the first preset distance dis _global. Each time a point is traversed, the point is marked as traversed, the traversing is continued until all points in the global path are traversed, and a complete track point set g is obtained, wherein the points in the g are coordinates corresponding to an x axis and a y axis in a world coordinate system.

After the track point set is obtained, a first track point meeting a first condition with the current position of the moving body is further obtained from the track point set. Here, the first condition is specifically set to determine whether or not the closest trajectory point is in front of the moving body based on the steering angle of the moving body and the angle between the current position of the moving body and the closest trajectory point. Specifically, the distance between each track point in the track point set g and the current position p _w of the moving body is calculated to obtain a track point p _close closest to the current position of the moving body. If the difference between the steering angle β _w and the included angle p _wp_close is smaller than pi/2, it indicates that the closest trajectory point is in front of the moving body, and updates the closest trajectory point p _close to the first trajectory point p. If the difference between the steering angle β _w and the included angle p _wp_close is greater than or equal to pi/2, the closest trajectory point p _close is not in front of the moving body, and the next trajectory point in the trajectory point set of the closest trajectory point is updated to the first trajectory point p. A third distance dis _c between the first trajectory point p and the current position p _w at this time is recorded.

In this embodiment, after the first track point is obtained, the position of the first track point near the center of the passable area is further adjusted according to the border of the passable area, so as to obtain the target track point. Specifically, the world coordinate system is first converted into a pixel coordinate system, and the current position p _w and the first track point in the world coordinate system are respectively represented as p _{w_grid} and p _grid in the pixel coordinate system. At this time, in the pixel coordinate system, the distance between the first trajectory point p _grid and the edge formed by every two vertices in the polygon vertices of the passable region boundary is calculated, and the edge closest to the edge is determined. Extending the line between the first locus point p _grid and the first intersection point p ₁ of the nearest side to obtain a second intersection point p ₂ of the polygon, and calculating a first distance dis between the midpoint p _tmp of the line p ₁p₂ between the first intersection point and the second intersection point and the first locus point p _grid. The second preset distance dis _threshold is used to limit the distance that the first track point is adjusted to, which is initially set to a value. If the first trajectory point p _grid is simply placed at the midpoint of the passable area, the global point may shake too much, and the large shake may affect the processing of the sensor information, and such a deviation may affect the segmentation of the road ahead, resulting in an increasingly larger error. Thus, 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, the first trajectory point p _grid is adjusted to be the midpoint p _tmp, which means that the first trajectory point p _grid is updated to be closer to the midpoint p _tmp from the center of the passable area. If the first distance is greater than the second preset distance, which means that the distance between the midpoint p _tmp closer to the center of the passable area and the first track point p _grid is too far, and exceeds the threshold value, the distance needs to be limited and adjusted, at this time, the first track point p _grid is used as the center, the second preset distance dis _threshold is used as the radius to make a circle, and a third intersection point p ₃ of the circle and a connection line p _tmpp_grid between the midpoint p _tmp and the first track point is obtained, and the first track point p _grid is adjusted to be the third intersection point p ₃. at this time, after the adjustment of the first trajectory point is completed, the pixel coordinate system is reconverted to the world coordinate system, and the adjusted first trajectory point is determined as the target trajectory point p' in the world coordinate system.

In addition, in this embodiment, the second preset distance dis _threshold is set to a value after the initialization, and the value is adaptively adjusted according to the calculated history distance dis. Specifically, the first distances obtained in each iteration process are summarized, for example, 10 times, a first distance average value is obtained, and the second preset distance is updated according to the first distance average value.

The above adjustment process for the first track point is repeated until the number of times num of adjustment is greater than the threshold maxnum of times num of adjustment or the third distance dis _c between the recorded current position and the first track point is smaller than dis _min, and the adjustment of the track point is stopped. 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 points in certain circumstances. Each time one track point in the track point set g stops adjusting, the next track point is switched to, and the process is repeated.

The above-described dynamic adjustment process of the target track point may be actually implemented according to the program flow shown in fig. 3.

And S3, calculating an optimal path from the current position to the target track point as a candidate track.

In some embodiments, the candidate trajectory is obtained by calculating an optimal path from the current location to the target trajectory point using a Theta algorithm.

And S4, adding the current position, the target track point, the candidate track and the passable region boundary into a hypergraph, establishing target constraint by using passable region boundary constraint, dynamic constraint and kinematic constraint, and optimizing to obtain a safe, comfortable and dynamically feasible optimized track from the current position to the target track point.

In the step S4, adding the current position, the target track point, the candidate track and the passable region boundary into a hypergraph, and obtaining an optimized track from the current position to the target track point by using hypergraph optimization. Specifically, passable region boundary constraints, dynamic constraints and kinematic constraints are added to the target constraint conditions to establish the target constraint.

In the hypergraph optimization process, a passable region boundary b, a target track point p' and a candidate track Traj are utilized, and a G2O framework is used for solving a sparse optimization problem. Using the state s of the moving body to represent, the i-th time planning trajectory may be represented by the state s _i of the moving body:

s_i＝[x_i,y_i,β_i]^T

Wherein x _i,y_i,β_i represents the coordinate and steering angle of the moving body at the time i, respectively, wherein the x-axis represents the moving body orientation and the y-axis represents the moving body left side. The time interval between two adjacent states s is denoted by Δt _i and the total time T can be expressed by the following formula:

The optimization objective is to minimize the total travel time on the premise of meeting the kinematic and dynamic constraints of the moving body, and the optimization problem can be expressed by the following formula:

B:={s₁,ΔT₁,s₂,ΔT₂,……,s_n-1,ΔT_n-1,s_n}

Wherein B represents a parameter to be optimized, f (B) is an objective function, and B\ { s ₁,s_n } represents that the starting point and the end point are fixed and not optimized. Punishment function e is used in the optimization process to punish the behavior violating the constraint condition:

Where z is related to the state of each constraint, z _r is the reference, e is the offset, S is the scaling factor, and N is the polynomial order.

Constraints need to be added to the G2O hypergraph, including passable region boundary constraints, kinematic constraints, and kinetic constraints. Wherein the kinematic constraint comprises a non-complete kinematic constraint, and the kinematic constraint comprises a minimum turning radius constraint, a speed constraint, an acceleration constraint, a time constraint, a jerk constraint, and the like. Maximum and minimum speeds and accelerations of the AUV are considered in path planning so that the planned path will be kinetically viable. Furthermore, path planning may create sharp corners, which are not kinetically feasible. Thus, the path needs to be smoothed to ensure that the turning radius meets the minimum turning radius requirement of the AUV. Combining path planning with time ensures that the dynamics constraints are met at every point in time across the path. However, the division is not absolute, and there is a certain flexibility in the specific classification, which depends on the application scenario and the focus of attention.

Wherein, each constraint condition is expressed as follows:

Incomplete kinematics constraint f _h: incomplete kinematics h _i(s_i+1,s_i) requires that two adjacent configurations need to lie on a common arc of constant curvature, i.e. the angle θ _i between the current state s _i and the direction d _i must be equal to the corresponding angle θ _i+1 at the next time state s _i+1, the incomplete kinematics constraint f _h is formulated as follows:

f_h＝e(h_i(s_i+1,s_i),0,∈,S,N)

The minimum turning radius constraint f _r for a moving body, its movement is also limited by the minimum turning radius between two successive states. f _r denotes a minimum turning radius constraint, d _r denotes a turning radius, and d _minr denotes a minimum turning radius.

f_r＝e(-d_r,-d_minr,∈,S,N)

Speed constraint f _v speed vector v _i includes a linear speed v and an angular speed w, which can be calculated from two states in succession:

wherein, I represent a second norm.

Acceleration constraint f _a acceleration vector a _i includes linear acceleration a _v,i and angular acceleration a _w,i, which can be calculated from two consecutive velocity vectors:

Time constraint f _time to ensure that the time of the whole track is as short as possible, the time constraint minimizes the square of the sum of all time differences:

Jerk constraint f _jerk to prevent the motion body from generating a series of unstable phenomena such as jerk and brake, in one embodiment, jerk constraint is further introduced. jerk _max jerk maximum reference vector, jerk jerk _i may be expressed as:

the corresponding penalty function can be expressed as:

f_jerk＝e(jerk,jerk_max,∈,S,N)

passable area boundary constraint f _b in this embodiment, the passable area boundary constraint is used to replace the original barrier constraint, so that passable area boundary information can be better utilized, a moving body is constrained to run in a passable area as much as possible, optimization time can be reduced by reducing the number of edges to be optimized in a G2O hypergraph, and instantaneity is improved.

f_b＝v*e(-d_b,-d_minb,∈,S,N)

Where d _b represents the distance of p _w from each side of the polygon and d _minb represents the threshold.

After defining the above target constraint, an objective function f (B) is constructed that contains various constraints, and the objective function can be written as a weighted sum of different terms:

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

where W is the weight of each constraint and f is the penalty function corresponding to the constraint.

Furthermore, it should be noted that the kinematic and dynamic constraints may also be specifically different in different application scenarios, for example, the underwater environment often varies, and contains many unknown or unpredictable elements, such as dynamic currents, moving obstacles (e.g. other underwater robots or marine organisms), and complex terrain variations, etc., and the stability and capacity of the AUV may be affected by currents, vortices and other hydrodynamic effects. At this time, the dynamic constraint and the kinematic constraint of the underwater robot may be different from the mobile robot on the land, for example, the water flow changes, and the method is not limited to factors such as incomplete kinematics, minimum turning radius, speed, acceleration, time and jerk.

In the process of hypergraph optimization, initialization is performed first, and then hypergraphs are constructed. In this process, an equally spaced trajectory is generated using the target point trajectory p' and the candidate trajectory Traj as the vertex of the hypergraph, and the state s of the hypergraph contains both the pose and the motion time information. Constraint information such as passable region boundaries, speed acceleration and the like is then added by using the state s to serve as the edges of the hypergraph. After the hypergraph is constructed, optimization of the hypergraph is started. The start state s ₁ and end state s _n are set to be non-optimizable, and the number of iterations can be set during the optimization process, with the goal of dynamically adding new points, deleting previous points, or optimizing the values of existing points in each iteration, in order to minimize the overall objective function. While each point sets a tolerable error range over the time interval to avoid oscillations. And obtaining an optimized track from the current position to the target track point after hypergraph optimization.

Finally, after the final optimized track is obtained, a track point at a certain moment can be selected as a control command according to the optimized track and the current position of the moving body, and the generated control command is sent to the controller through a proper communication protocol (such as Wi-Fi, bluetooth, wired connection and the like). The stability and security of the communication link is ensured to prevent data loss or tampering. Meanwhile, after receiving the control command, the controller adjusts the motion state of the moving body according to the command. The controller monitors the actual movement condition of the moving body at the same time, collects relevant state data (such as position, speed, acceleration and the like) and sends the state data to the cloud or a remote upper computer through a communication network (such as 4G/5G, wi-Fi, satellite communication and the like). After the cloud or remote upper computer receives the data, the data are stored, processed and analyzed so as to monitor the state of the moving body in real time and make necessary adjustment. The controller further corrects and optimizes the track of the moving body in real time according to the feedback data so as to improve the control precision and efficiency. In addition, a user interface is developed on the cloud or remote upper computer and used for displaying information such as real-time position, track, state and the like of the moving body. In addition, in the whole process, fault early warning processing is carried out so as to quickly identify, isolate and restore the system when faults occur.

In summary, the path planning method for the passable area of the unstructured environment adds global target point adjustment, adjusts the track point to the middle of the passable area as much as possible, generates an optimal candidate track by using the adjusted target point, adds jerk constraint, and adopts a clustering and polygon approximation method to represent the obstacle as a polygon by using the two-dimensional grid passable area, thereby reducing the calculation cost. And G2O hypergraph optimization is utilized, the generated track is prevented from being trapped into a local minimum value as far as possible by optimizing the optimal candidate track, and the passable region boundary constraint is used for replacing the barrier constraint, so that the hypergraph optimization time is reduced, and the instantaneity is further improved.

In addition, it should be noted that the path planning method disclosed in the present invention is not limited to the execution in the order of steps S1-S4, and the steps may be reordered, added or deleted, so long as the desired result of the technical solution of the present invention can be achieved, and the present invention is not limited herein.

In addition, the invention also provides an embodiment of the path planning device facing the passable area of the unstructured environment, which corresponds to the embodiment of the path planning method.

Referring to fig. 4, fig. 4 shows a schematic structural diagram of a path planning device facing an unstructured environment passable area. The path planning device 200 facing the passable area of the unstructured environment at least comprises:

The passable region extraction optimization module 210 is configured to input a two-dimensional grid map of the passable region, where the grid map represents the passable region and the obstacle in pixels, and extract a border of the passable region.

The global target track point dynamic adjustment module 220 is configured to obtain a first track point satisfying a first condition with a current position of the moving body from the track point set, and adjust the first track point according to the passable region boundary to obtain a target track point.

A hypergraph optimization module 230 for calculating the optimal path from the current position to the target track point as a candidate track, and

It will be clear to those skilled in the art that, for convenience and brevity of description, specific working procedures of each module of the described apparatus may refer to corresponding procedures in the foregoing method embodiments, which are not described herein again.

As shown in fig. 5, the present invention further provides a first electronic device 300 according to another embodiment, as shown in fig. 5, the first electronic device 300 may be further connected to a data acquisition device 310 and an information display device 320 through a wired or wireless information transmission scheme, where the data acquisition device 310 is configured to acquire real-time information of a moving body, including information such as a position, and a passable area, and the information display device 320 is configured to display a track result obtained by the analysis of the present invention.

The information display device 320 may sort the data output by the first electronic device 300 based on the information display mechanism, so as to improve the readability of the data output by the first electronic device a. The information display mechanism may be preset manually, for example, the data output by the first electronic device a may be displayed visually, and the display parameter may be, for example, a display data range according to a display parameter and/or an attribute set by a user, and the display attribute may be, for example, a display font, a color, whether to scroll and play, etc. The appointed key information, such as specific positions, running tracks and the like, is presented to the user, and the user can know the information more timely without visiting a secondary page or scrolling the page, so that the operation of the user is saved. Or the information display mechanism can be an artificial intelligent AI display model, which can learn the key attention information of the user according to the previous use habits of the user, such as watching time length, clicking times, editing times and the like, so that the automatic user presents rich and necessary key information.

The present invention also provides a computer program product comprising a computer program storable on a readable storage medium which, when executed by a processor, is capable of performing the path planning method provided by the methods described above.

The invention in another embodiment also proposes a storage medium for storing a computer program for performing the path planning method. It should be understood that the storage medium in embodiments of the present invention may be volatile memory or nonvolatile memory, or may include both volatile and nonvolatile memory. The nonvolatile memory may be a read-only memory (ROM), a Programmable ROM (PROM), an erasable programmable ROM (erasable PROM), an electrically erasable programmable EPROM (EEPROM), or a flash memory. The volatile memory may be random access memory (random access memory, RAM) which acts as external cache memory. By way of example, and not limitation, many forms of random access memory (random accessmemory, RAM) are available, such as static random access memory (STATIC RAM, SRAM), dynamic Random Access Memory (DRAM), synchronous Dynamic Random Access Memory (SDRAM), double data rate synchronous dynamic random access memory (doubledatarate SDRAM, DDR SDRAM), enhanced synchronous dynamic random access memory (ENHANCED SDRAM, ESDRAM), synchronous link dynamic random access memory (SYNCHLINK DRAM, SLDRAM), and direct memory bus random access memory (directrambus RAM, DR RAM).

Fig. 6 shows a schematic block diagram of a second electronic device 400 that may be used to implement an embodiment of the invention. Second electronic device 400 the electronic device is intended to represent various forms of digital computers, such as laptops, desktops, workstations, personal digital assistants, servers, blade servers, mainframes, and other appropriate computers. The second electronic device 400 may also represent various forms of mobile devices, such as personal digital processing, cellular telephones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions, are meant to be exemplary only, and are not meant to limit implementations of the inventions described and/or claimed herein. The second electronic device 400 may be the same as or different from the first electronic device 300.

The second electronic device 400 comprises a computing unit 410 that may perform various suitable 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 RAM430, various programs and data required for the operation of the device 1000 may also be stored. The computing unit 410, ROM 420, and RAM430 are connected to each other by a bus. An input/output (I/O) interface is also connected to the bus.

The components in the second electronic device 400 are connected to I/O interfaces, including an input unit 440, such as a keyboard, a mouse, etc., an output unit 450, such as various types of displays, speakers, etc., storage media, such as magnetic disks, optical disks, 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.

The computing unit 410 may be a variety of general and/or special purpose processing components having processing and computing capabilities. Some examples of computing unit 410 include, but are not limited to, a Central Processing Unit (CPU), a Graphics Processing Unit (GPU), various specialized Artificial Intelligence (AI) computing chips, various computing units running machine learning model algorithms, a Digital Signal Processor (DSP), and any suitable processor, controller, microcontroller, 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 tangibly embodied on 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 onto device 400 via ROM 420 and/or communication unit 460. When the computer program is loaded into RAM 430 and executed by computing unit 410, one or more steps of the methods described above may be performed. Alternatively, in other embodiments, the computing unit 410 may be configured to perform the method by any other suitable means (e.g., by means of firmware).

Although embodiments of the present invention have been disclosed above, it is not limited to the details and embodiments shown and described, it is well suited to various fields of use for which the invention would be readily apparent to those skilled in the art, and accordingly, the invention is not limited to the specific details and illustrations shown and described herein, without departing from the general concepts defined in the claims and their equivalents.

Claims

1. A method for path planning for a passable area of an unstructured environment, comprising:

2. The method of claim 1, wherein extracting passable region boundaries comprises:

3. The method as recited in claim 1, further comprising:

4. The method of claim 1, wherein the first condition is:

5. The method of claim 1, wherein adjusting the first trajectory point to obtain a target trajectory point based on the passable region boundary comprises:

6. The method of claim 5, wherein the step of determining the position of the probe is performed,

Summarizing the first distance obtained in each iteration process to obtain a first distance average value, and updating the second preset distance according to the first distance average value.

7. The method of claim 1, wherein the step of determining the position of the substrate comprises,

And calculating an optimal path from the current position to the target track point by using a global path planning algorithm to obtain the candidate track.

8. The method according to claim 1 or 2, further comprising:

9. The method of claim 2, wherein the step of determining the position of the substrate comprises,

And clustering the first grid point set according to the tightness degree of the sample by adopting a spatial clustering algorithm.

10. The method of claim 2, wherein the step of determining the position of the substrate comprises,

And adopting a polygon fitting algorithm to carry out polygon approximation on each contour.

11. The method of claim 1, wherein the unstructured environment comprises a different environment of land, water, air, and the corresponding moving body comprises a vehicle or a wheeled mobile robot applied to the land environment, a ship or an underwater robot applied to the water environment, and an airplane or a flying robot applied to the air environment.

12. A path planning device for an unstructured navigable area, the device comprising:

The hypergraph optimizing module is used for calculating an optimal path from the current position to the target track point to serve as a candidate track;

13. A computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the method of any of claims 1-11.

14. A computer program product comprising a computer program, characterized in that the computer program, when executed by a processor, implements the steps of the method of any of claims 1-11.