CN113253760B

CN113253760B - Path planning method and device, movable carrier and storage medium

Info

Publication number: CN113253760B
Application number: CN202110634437.0A
Authority: CN
Inventors: 张新璐
Original assignee: Beijing Yuandu Internet Technology Co ltd
Current assignee: Beijing Yuandu Internet Technology Co ltd
Priority date: 2021-06-08
Filing date: 2021-06-08
Publication date: 2021-11-09
Anticipated expiration: 2041-06-08
Also published as: CN113253760A

Abstract

The embodiment of the invention provides a path planning method, a path planning device, a movable carrier and a storage medium, wherein the method is used for the movable carrier and comprises the following steps: acquiring course data, course road condition information and global DEM data of the movable carrier; judging whether the air route is safe or not according to the air route data of the movable carrier, the air route road condition information and the global DEM data; if not safe, the air route is re-planned according to the air route data and the dynamic constraint of the movable carrier, and the air route can be planned efficiently and stably, so that the movable carrier is accurately controlled, and the movable carrier is prevented from entering a dangerous area.

Description

Path planning method and device, movable carrier and storage medium

Technical Field

The present invention relates to the field of motion control, and in particular, to a path planning method, a path planning apparatus, a mobile carrier, and a computer-readable storage medium.

Background

The movable carrier, such as an unmanned aerial vehicle, a robot, a mobile car, a mobile ship or underwater mobile equipment, plays an important role in many fields of agriculture, industry, movie and television, search and rescue, police, military and the like, and can adapt to complex environments.

Conventional mobile vehicles, such as drones, require that the flight path be designed first by the operator before work can be performed. The course may include a plurality of local travel segments made up of a sequence of coordinate points that are sequentially visited by the mobile vehicle. Because the operator can not know the condition information in the air line area in real time, the air line is easy to collide with the information in the actual task area, and finally collision accidents are caused.

The path planning of the movable vehicle refers to that a path from an initial position to a target position is generated under the constraint of optimal specific evaluation indexes, such as path length, path search time, motion energy loss and the like, and on the premise of meeting the condition that an obstacle does not collide, and the path planning is an important process for autonomous cruising of the movable vehicle and improvement of traveling efficiency and safety. The mobile load path planning algorithm can assist the mobile load to execute the route, and a safe traveling route is planned in the process that the mobile load accesses the local traveling line segment which conflicts with the actual task area information.

At present, the common movable auxiliary path planning, taking the auxiliary path planning of the unmanned aerial vehicle as an example, mainly includes: low latitude many rotor unmanned aerial vehicle relies on airborne perception sensor like: and sensing environments and maps by using a laser radar, an ultrasonic sensor, a monocular camera and the like, and planning a path in real time.

The method is only suitable for the category of low-altitude multi-rotor unmanned aerial vehicles, the search range of airborne sensors of the method is limited, and the method is not suitable for the current operating environment with higher flying altitude. In order to solve the above problems, it is necessary to provide a method capable of determining collision risk of the flight path and effectively planning the flight path, so as to make the mobile carrier safely and efficiently complete the flight path.

Disclosure of Invention

An object of the embodiments of the present invention is to provide a path planning method, an apparatus, a mobile carrier, and a storage medium, which can pre-determine safety of a flight path of the mobile carrier according to global DEM data, and re-plan the flight path of the mobile carrier if the flight path of the mobile carrier conflicts with the global DEM data, thereby effectively avoiding a possibility that the mobile carrier enters a dangerous area.

In a first aspect, an embodiment of the present invention provides a path planning method for a mobile vehicle, including:

acquiring course data, course road condition information and global DEM data of the movable carrier;

judging whether the air route is safe or not according to the air route data of the movable carrier, the air route road condition information and the global DEM data;

if not, replanning the air route according to the air route data of the movable carrier and the dynamic constraint.

In a second aspect, an embodiment of the present invention provides a path planning apparatus for a mobile vehicle, including:

the acquisition module is used for acquiring the route data, route road condition information and global DEM data of the movable carrier;

the judging module is used for judging whether the air route is safe or not according to the air route data of the movable carrier, the air route road condition information and the global DEM data;

and the determining module is used for re-planning the air route according to the air route data of the movable carrier and the dynamic constraint when the air route is unsafe.

In a third aspect, an embodiment of the present invention provides a mobile carrier, including:

the mobile vehicle navigation system comprises a power device and the path planning device, wherein the path planning device is used for sending instructions to the power device, and the power device is used for generating power according to the instructions so that the mobile vehicle can travel according to the air route determined by the path planning device.

In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, where the computer-readable storage medium includes instructions, which when executed on a computer, cause the computer to execute the path planning method described above.

According to the path planning method, the path planning device, the movable carrier and the computer readable storage medium, the route data, the route road condition information and the global DEM data of the movable carrier are obtained; judging whether the current route is safe or not according to the route data, the route road condition information and the global DEM data; if not, planning the route according to the route data and the dynamic constraint of the movable carrier, judging whether the preset route of the movable carrier has risk, and if so, re-planning the route efficiently and stably by the path planning method, thereby realizing the accurate control of the movable carrier and avoiding the movable carrier from entering a dangerous area.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.

FIG. 1 is a schematic architectural diagram of an aircraft according to an embodiment of the invention;

FIG. 2 is a schematic flow chart diagram of a path planning method according to an embodiment of the present invention;

FIG. 3 is a schematic flow chart of step S2 in FIG. 2;

FIG. 4 is a schematic flow chart of step S3 in FIG. 2;

FIG. 5 is a schematic flow chart of step S31 in FIG. 4;

fig. 6 is a schematic flowchart of step S313 in fig. 5;

FIG. 7 is a schematic diagram of a rectangular area provided by the practice of the present invention;

fig. 8 is a schematic flowchart of step S314 in fig. 5;

FIG. 9 is a schematic flowchart of step S315 of FIG. 5;

FIG. 10 is a node location relationship diagram provided by embodiments of the present invention;

fig. 11 is a schematic structural diagram of a path planning apparatus 200 according to an embodiment of the present invention;

fig. 12 is a schematic structural diagram of a mobile carrier 300 according to an embodiment of the present invention;

FIG. 13 is a schematic block diagram of a computer device according to an embodiment of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

For better understanding of the present invention, a path planning method, an apparatus, a mobile carrier and a computer-readable storage medium according to embodiments of the present invention are described in detail below with reference to fig. 1 to 13. It should be noted that these examples are not intended to limit the scope of the present disclosure. As shown in fig. 1, in one embodiment of the present invention, the movable vehicle is an aircraft, such as an unmanned aerial vehicle, and in other possible embodiments, the movable vehicle may also be a manned aircraft, a robot, an automobile, a ship, a model airplane, an unmanned airship, a fixed-wing aircraft, an unmanned hot air balloon, and the like.

As shown in fig. 1, the aircraft 100 may include a power plant 110, a path planner 120, and a frame 130. The power plant 110 may include an electronic governor (abbreviated as an electric governor) 111, one or more propellers 112, and one or more motors 113 corresponding to the one or more propellers 112, wherein the motors 113 are connected between the electronic governor 111 and the propellers 112, and the motors 113 and the propellers 112 are disposed on corresponding arms; the electronic governor 111 is configured to receive a driving signal generated by the path planning device 120 and provide a driving current to the motor 113 according to the driving signal to control the rotation speed of the motor 113. The motor 113 is used to drive the propeller 112 to rotate, thereby powering the aircraft 100.

The path planner 120 may include a control module 121 and a navigation module 122. The navigation module 122 stores global DEM data therein, acquires course data of the aircraft 100 through communication with the control module 121, judges whether a collision risk exists on a course of the aircraft 100, and makes a path planning strategy for an unsafe course. The control module 121 is configured to control the flight of the aircraft 100, obtain flight path data and a real-time flight status of the aircraft 100, and communicate with the navigation module in real time. The control module 121 controls the unmanned aerial vehicle to fly according to the planned path according to the risk judgment of the navigation module 122 on the air route and the planned path.

It should be understood that the above-described nomenclature for the various components of aircraft 100 is for identification purposes only, and should not be construed as limiting embodiments of the present invention.

Fig. 2 is a flowchart of a path planning method according to an embodiment of the present invention, which may be applied to various types of mobile vehicles, such as the aircraft 100 shown in fig. 1, and which may be executed by the path planning apparatus 120 shown in fig. 1, for example. As shown in fig. 2, the method may specifically include:

step S1: and acquiring the route data, route road condition information and global DEM data of the movable carrier.

Optionally, in an embodiment of the present invention, acquiring the course data of the mobile vehicle may include: and acquiring a starting point and a target point of the course of the movable carrier. The starting point and the target point of the route can be the starting point and the target point of the whole route, and can also be any two adjacent route points in the whole route; the coordinates of the starting point and the target point may include longitude lon, latitude lat, and altitude high coordinates under the geographic coordinate system. Optionally, the airline data may further include a preset starting traveling speed, a preset starting traveling attitude, a preset traveling speed and a preset traveling attitude at each position on the airline, and a traveling position and a preset traveling mode at each time after takeoff according to a preset time, where the preset starting traveling speed and the preset traveling speed at each position on the airline may include a northbound speed Vn, an easbound speed Ve, and a ground-bound speed Vd; the preset starting advancing attitude and the preset advancing attitude at each position on the flight path can include a pitch angle pitch, a roll angle roll and a yaw angle yaw; the preset traveling mode may include any one of the following modes: the system comprises an auxiliary path planning mode, a task advancing mode, a landing mode and a manual control mode, wherein the movable carrier can carry out auxiliary path planning in the auxiliary path planning mode, and can advance according to a preset task route in the task advancing mode without carrying out auxiliary path planning.

Optionally, the route road condition information may include: the information of the start point, the no-fly zone in the preset range around the target point and the boundary coordinates of the electronic fence.

In the embodiment of the present invention, the control module 121 of the aircraft 100 includes a GPS positioning device, and after the control module 121 is initialized, information of a no-fly zone within a certain range around its own positioning position is loaded. Optionally, the information of the no-fly zones in the surrounding 50kM may be loaded, and the total number of the no-fly zones selected to be loaded may be 10. After the no-fly zone is loaded, the information of the electronic fence for limiting the flight range of the aircraft 100 is loaded. After the control module 121 is loaded, the information of the no-fly zone and the boundary coordinates of the electronic fence are sent to the navigation module 122.

Step S2: and judging whether the air route is safe or not according to the air route data of the movable carrier, the air route road condition information and the global DEM data.

In the embodiment of the invention, the DEM (Digital Elevation Model) is a solid ground Model for representing the ground Elevation in the form of a group of ordered numerical arrays.

In the embodiment of the invention, whether the preset air route of the aircraft has risks such as mountain collision and the like can be judged according to the air route data of the movable carrier, the air route road condition information and the global DEM data.

Step S3: if not, replanning the air route according to the air route data of the movable carrier and the dynamic constraint.

In the embodiment of the invention, if the preset air route is unsafe and the movable carrier is not started, a new safe air route can be re-planned between the starting point and the target point of the original air route through a path planning algorithm according to the preset air route data and the dynamic constraint of the movable carrier.

According to the embodiment of the invention, whether a new air route is planned or not can be determined according to the preset traveling mode in the air route data of the movable carrier, and if the preset traveling mode is a non-auxiliary path planning mode such as a manual mode or a landing mode and a task traveling mode, the new air route is not planned. If the preset traveling mode is the auxiliary path planning mode, when the preset air route is judged to be unsafe, the movable carrier is controlled to enter the auxiliary path planning mode, and a new safe air route is planned again according to the air route data and the dynamic constraint of the movable carrier.

It should be noted that, when the safety of the preset air route is judged, the traveling mode and the starting and stopping state of the movable carrier are not limited, the movable carrier can perform safety judgment on the preset air route before taking off, and update the traveling air route in advance when the air route has risks, and the movable carrier can also perform safety judgment on the preset air route while taking off or after taking off, and update the rest air routes which are not traveled into new safe air routes in time when the air route has risks.

In the embodiment of the invention, a node list of path planning can be generated according to the route data and the dynamic constraint, and then a path planning list is established according to the node list, so that the route is generated according to the path planning list.

In the embodiment of the invention, after the movable carrier with the original traveling mode as the task traveling mode travels according to the re-planned route, the movable carrier exits the auxiliary path planning mode, and continues the task traveling mode by taking the re-planned route as the current route. Or after the movable carrier finishes the local air route, the movable carrier exits the auxiliary path planning mode and continues the task traveling mode.

In an embodiment of the present invention, if it is determined that the predetermined route is unsafe, a new route may be re-planned according to the route data and the dynamic constraints of the aircraft 100. If the preset route is safe, the vehicle can travel according to the current route, and when the period that the navigation module 122 acquires the current flight state of the aircraft 100 is reached, the steps of S1-S3 are executed again.

According to the path planning method provided by the embodiment of the invention, whether the preset air route of the aircraft 100 is safe or not can be judged according to the preset flight data, the air route road condition information and the global DEM data of the aircraft 100, if the preset air route of the aircraft 100 is not safe, the air route is re-planned according to the air route data and the dynamic constraint of the aircraft 100, and the safety of the aircraft 100 in the flight process is ensured. According to the method, the preset air route is subjected to safety prejudgment, so that the air route of the aircraft 100 can be subjected to self-adaptive route adjustment according to the global DEM data, and the method is high in flexibility and high in safety.

Optionally, in an embodiment of the present invention, as shown in fig. 3, step S2 may specifically include:

step S21: and determining at least one block corresponding to the route in the global DEM data according to the starting point and the target point respectively.

It should be noted that the DEM data is composed of blocks with preset sizes, and a block corresponding to the lane line can be determined according to a starting point and a target point of the lane line, where the lane line corresponds to multiple blocks if the lane line spans multiple blocks, and corresponds to one block if the starting point and the target point of the lane line are located in the same block.

Step S22: local DEM data is determined based on the at least one block. Specifically, if the number of the corresponding blocks is determined to be multiple, the multiple blocks are merged, and the merged blocks are determined local DEM data. And if the corresponding block is determined to be one, determining the block to be the determined local DEM data. And the local DEM data are all in the boundary coordinate range of the electronic fence.

Step S23: and judging whether the air route is safe or not according to the local DEM data.

Optionally, in an embodiment of the present invention, step S23 may specifically include:

and sampling on the aerial route according to the resolution of the local DEM data, and determining a plurality of sampling points. Specifically, the starting point and the target point of the whole route section can be determined according to the route data, the route can be divided into multiple route sections according to preset route points, every two adjacent route points are determined to be the starting point and the target point of the corresponding route section respectively, and sampling is performed on the route section formed by the starting point and the target point of each route section according to the resolution (generally 30M × 30M) of the local DEM data to obtain multiple sampling points. It should be noted that, according to the longitude and latitude coordinates and the altitude of the starting point and the target point, the longitude and latitude coordinates and the altitude of any point on the corresponding route segment can be obtained, and after sampling, the longitude and latitude coordinates and the altitude of each sampling point can be determined.

After the plurality of sampling points are determined, the earth surface heights in the local DEM data corresponding to the same horizontal coordinates of the starting point, the target point and the plurality of sampling points are respectively determined, the altitudes of the starting point, the target point and the plurality of sampling points are respectively compared with the corresponding earth surface heights, the starting point is determined, the altitudes of the target point and the plurality of sampling points are higher than the corresponding earth surface heights, and the points with the altitude difference not less than the safety threshold value are safety points. Wherein the safety threshold may be 20 m. And if the starting point, the target point and the plurality of sampling points are all determined as safe points, determining the section of the route as a safe path.

Specifically, for the starting point, the target point and all the sampling points, the altitude of the starting point, the target point and all the sampling points are respectively compared with the ground surface height in the corresponding local DEM data under the horizontal coordinate, if the altitude is higher than the ground surface height and the height difference is not less than a certain safety threshold (the safety threshold may be 20 m), the starting point, the target point and all the sampling points are regarded as a safety point, otherwise, the starting point, the target point and all the sampling points are regarded as an unsafe point. And when the starting point, the target point and all the sampling points are safe points, the section of the route is regarded as a safe path.

Optionally, in an embodiment of the present invention, as shown in fig. 4, step S3 may specifically include:

step S31: and generating a node list of the path plan according to the route data and the dynamic constraint.

Specifically, according to the route data of the aircraft 100 preset by the control module 121, the starting point, the target point, and the search node obtained based on the dynamic constraint in the route data are added to the node list of the path planning, so as to generate the node list. The node list can be generated corresponding to the starting point and the target point of the whole route, and the node list can also be generated by respectively taking two adjacent route points on the whole route as the starting point and the target point. It should be noted that the node list includes at least 2 nodes: a starting point and a target point.

Step S32: and establishing a path planning list according to the node list.

In the embodiment of the invention, the path nodes can be generated based on the nodes in the node list, and the path planning list can be generated based on the path nodes, wherein the path planning list can be generated for the node list corresponding to the starting point and the target point of the whole airline, and the corresponding path planning lists can also be generated respectively by taking two adjacent waypoints on the airline as the node lists corresponding to the starting point and the target point respectively. It should be noted that the number of nodes in the node list may be greater than or equal to the number of path nodes in the path planning list.

Step S33: and generating the air route according to the path planning list.

In the embodiment of the invention, each path node can be connected according to the sequence of each path node in the path planning list to plan a new route, if one or more waypoints are preset on the whole route, a plurality of new route segments are planned according to the correspondingly generated path planning list respectively, and the planned new route segments are connected into a new whole route.

For example, if the path node in the path planning list is A, B, D, G, the 4 path nodes are sequentially connected in series to generate a new route segment, and a plurality of route segments are sequentially connected to generate a new route.

According to the path planning method provided by the embodiment of the invention, the path planning list is established by combining the preset air route data of the aircraft 100 with the dynamic constraint, the nodes which do not conform to the dynamic constraint are discarded, the effective data can be efficiently obtained, and the accuracy is high.

Optionally, in an embodiment of the present invention, as shown in fig. 5, step S31 may specifically include:

step S311: and storing the starting point into the node list.

It should be noted that the nodes in the node list are arranged according to the adding order, and the starting point is the first node added to the node list and is the initial point in the node list.

Step S312: random sampling points are generated in the sampling region.

Specifically, after the starting point is stored in the node list, a random sampling point is generated in a preset sampling area through a sampling strategy. It should be noted that, subsequently, the random sampling point may be regenerated by determining whether the random sampling point is in the no-fly area, whether the path between the search node and the parent node is safe, and whether the condition for finishing storing the search node into the node list is reached.

Step S313: and determining a search node according to the dynamic constraint and the random sampling point.

Specifically, whether the determined random sampling point and the node in the node list meet corresponding dynamic constraints or not is calculated, and the search node is determined.

Step S314: and storing the search node into the node list.

In the embodiment of the invention, after each searching node is determined, the searching node is added into the node list.

In the embodiment of the invention, when the search node is stored in the node list each time, the iteration number is added by 1, then whether the condition of finishing the storage of the search node in the node list is met is judged, if so, the target point is stored in the node list, the search node is not stored in the node list, namely, the random sampling point is not produced to determine a new search node; otherwise, storing the search node into a node list, regenerating a random sampling point in a sampling area to determine a new search node, and storing the new search node into the node list until the condition of finishing storing the search node into the node list is met.

It should be noted that the end of the search node is stored in the node list, that is, the end of re-determining a new search node is finished, that is, the end of re-generating the random sampling point is finished, and the end of the cycle of determining the search node is determined.

The condition for finishing the storage of the search node into the node list comprises at least one of the following conditions:

1) the number of iterations is greater than the maximum number of iterations.

In the embodiment of the present invention, the maximum iteration number may be set in a user-defined manner, and the set maximum iteration number may be a plurality of different values for a node list generated between two different adjacent waypoints.

2) And the Euclidean distance between the search node and the target point is less than the search cut-off distance.

It should be noted that, after each time a search node is added to the node list, the euclidean distance between the newly added search node and the target point needs to be calculated to determine whether it is smaller than the search cutoff distance, for example, after each search node new _ pt is stored in the node list, it is determined whether the euclidean distance between the new _ pt and the target point is smaller than the search cutoff distance.

The search cutoff distance may be preset to indicate a minimum distance for path planning, and when the distance between two points is smaller than the minimum distance, it indicates that the distance between the two points is small, and it is not necessary to plan nodes again.

3) The path between the search node and the target point is a safe path, and the kinetic constraint is satisfied between the search node and the target point.

In the embodiment of the invention, after the search node is added into the node list each time, whether a path between the search node and a target point is a safe path and whether the dynamic constraint is satisfied between the search node and the target point is calculated, if the path is the safe path and the dynamic constraint is satisfied, path planning is not required between the two points, otherwise, at least one of the safe path and the dynamic constraint is not satisfied, a random sampling point is generated again in a sampling area to determine a new search node, and the new search node is stored into the node list until the condition of finishing storing the search node into the node list is satisfied.

In the above embodiment of the present invention, the starting point is first stored in the node list; generating random sampling points in a sampling area; determining a search node according to the dynamic constraint and the random sampling point; the search nodes are stored in the node list, so that path planning is realized according to the route data and the dynamic constraint, and the nodes in the node list are automatically generated by finishing the condition of storing the search nodes in the node list during path planning, so that the efficiency and the success rate of path planning are improved.

In one embodiment of the present invention, as shown in fig. 6, step S312 may include, but is not limited to, the following steps:

step S3121: and determining a horizontal sampling area according to the horizontal coordinate of the starting point and the horizontal coordinate of the target point.

It should be noted that the horizontal coordinate of the start point and the horizontal coordinate of the target point are both horizontal coordinate systems established with the start point as the origin, and therefore, the horizontal coordinate of the start point is (0, 0), and the coordinates of the target point in the course data can be converted into the horizontal coordinate with the start point as the origin as the geographic coordinates according to the following formula (1).

Optionally, a horizontal coordinate system is established with the geographical coordinates of the starting point as an origin. The geographic coordinates of any point can be converted to horizontal coordinates with the origin at the starting point by the following formula:

Local_position.x = (Local_coord.lon- Start_coord.lon) * LonToM

Local_position.y = (Local_coord.lat - Start_coord.lat) * R2 （1）

Local_position.z = Local_coord.high

where R2 is the polar radius, Local _ position is the coordinates of a certain point in a horizontal coordinate system with the starting point as the origin, Local _ color is the geographical coordinates of the certain point, and Start _ color is the geographical coordinates of the starting point.

Where LonToM is a conversion coefficient, LonToM = (R1 × pi/180) × (Local _ coord. lat × pi/180), R1 is the earth equatorial radius, and pi is the circumferential ratio.

It should be noted that, in the embodiment of the present invention, for one or more waypoints preset on a route, every two adjacent waypoints may be regarded as a start point and a target point of a corresponding route segment, when a node list between every two waypoints is determined, a horizontal coordinate system established with the start point of each route segment as an origin is respectively determined, a corresponding horizontal sampling area of the route segment is determined according to the horizontal coordinate of the start point of each route segment and the horizontal coordinate of the target point, and a node and a path node in subsequent path planning are calculated by using coordinates under the horizontal coordinate system established with the geographical coordinate of the start point of the route segment as the origin.

In the embodiment of the present invention, in order to avoid planning failure due to a sampling region that is too long and narrow or has a too small area, the horizontal sampling region may be uniformly processed as a square region, and the specific calculation manner is as follows:

1. a horizontal rectangular region is found with the Start point Start _ position and the target point Goal _ position as diagonal corners.

Fig. 7 is a schematic diagram of a rectangular area provided by the implementation of the present invention, as shown in fig. 7, the horizontal coordinate of the Start point Start _ position is (0, 0), and the horizontal coordinate of the target point Goal _ position is (x 2, y 2). The horizontal rectangular region R diagonal to the Start point Start _ position and the target point Goal _ position is a rectangle in fig. 7, and the coordinates of 4 vertices thereof are (0, y 2), (x 2, y 2), (x 2, 0), and (0, 0), respectively.

2. The geometric center point coordinates of the rectangular region are (x 1, y 1), and the horizontal sampling region of the square is arranged at the center point.

3. Defining half of the longer side length of the rectangular region R as m, the side length of the square as L = 2 × m × R, where R is an expansion coefficient, which may be preset, in the embodiment of the present invention, an empirical value is provided: r may be 1.5. The coordinates of the center point and the side length of the square are known, and the coordinates of the four vertices of the square can be obtained, and the coordinates of the four vertices of the square are (x 1-r m, y1-r m), (x 1-r m, y1+ r m), (x 1+ r m, y1+ r m), (x 1+ r m, and y1-r m) according to the description.

As shown in fig. 7, when the square is expanded, assuming that the longer side length is the side between the two vertices (0, y 2) and (x 2, y 2), the longer side length I = x2, i.e., half of the longer side length m = I/2= x2/2= x1, and the resulting square has 4 vertices as shown in fig. 7.

It should be noted that if the rectangle itself is a square, the square area can still be expanded according to the method.

After expanding the square region, since the actually selected sampling point may exceed the region, the region is expanded according to the expansion coefficient.

Step S3122: and generating random sampling points in the horizontal sampling region according to the horizontal sampling region and the random sampling strategy, and determining the horizontal coordinates of the random sampling points.

In the embodiment of the invention, the random sampling strategy can be a target deviation probability threshold sampling strategy. Generating random sampling points and determining the horizontal coordinates Rand _ position (Rand _ x, Rand _ y) of the random sampling points are calculated as follows:

if rand _ p > rand _ bias

rand_x = Goal_position.x （2）

rand_y = Goal_position.y

The rand _ p is a random probability of the generated random sampling point, optionally, a value is arbitrarily selected in (0, 1) through a random algorithm to generate the rand _ p, the rand _ bias is a target deviation probability threshold, the target deviation probability threshold can be used for representing that the generated random sampling point is a threshold of a target point, the target deviation probability threshold can be preset and set to be 0.5, 0.7 and the like, the random probability of the generated random sampling point can be limited in a range through setting the target deviation probability threshold, and when the generated random probability exceeds the range, the random sampling point is the target point, so that the randomness of the sampling point is reduced, and the convergence speed of the planning algorithm is improved.

(Goal _ position. y) is the target point horizontal coordinate, rand _ x represents the x-axis coordinate of the generated random sampling point, and rand _ y represents the y-axis coordinate of the generated random sampling point. And when the random probability of the generated random sampling point is greater than the target deviation probability threshold value, the random sampling point is the target point.

In the embodiment, the algorithm convergence speed of the path planning method provided by the embodiment of the invention can be effectively improved by adopting a target deviation probability sampling strategy and simultaneously limiting a reasonable sampling range.

Step S3123: and determining whether the random sampling point is in the no-fly area or not according to the local DEM data and the horizontal coordinate of the random sampling point.

In the embodiment of the present invention, the no-fly area may be a closed area formed by using a plurality of horizontal coordinate points as boundaries, and after determining horizontal coordinates (rand _ x, rand _ y) of a random sampling point, whether the random sampling point is in the no-fly area is determined by determining whether the horizontal coordinate of the random sampling point is in the closed area.

Step S3124: and if so, regenerating the random sampling points according to the horizontal sampling area and the random sampling strategy until the horizontal coordinates of the regenerated random sampling points are not in the no-fly zone.

Specifically, whether the random sampling point is in the no-fly area or not is judged by inquiring the determined local DEM data, and if the random sampling point falls in the no-fly area, the sampling point is determined again. By inquiring the local DEM data, the effectiveness of random sampling points can be effectively ensured.

Step S3125: if not, determining the height coordinate of the random sampling point.

In the embodiment of the present invention, the height coordinate of the random sampling point may be determined according to the horizontal coordinate of the random sampling point, the highest altitude value in the local DEM data, the height coordinate of the starting point, and the height coordinate of the target point.

When the height coordinate of the random sampling point is determined, data which are the same as the horizontal coordinate of the random sampling point can be found in the local DEM data, so that the DEM altitude corresponding to the horizontal coordinate of the random sampling point under the DEM data is inquired, the maximum height coordinate of the random sampling point is determined according to the vertical height of the random sampling point, the highest altitude value in the local DEM data, the height coordinate of the starting point and the height coordinate of the target point, and then the height coordinate of the random sampling point is determined according to the vertical height and the maximum height coordinate of the random sampling point.

When determining the maximum height coordinate of the random sampling point, the method may be one of the following ways:

1) and taking the vertical height of the random sampling point as min _ z, determining the highest altitude value as high _ max in the local DEM data, selecting the maximum value from the high _ max, the Start _ position.z and the target point vertical height Goal _ position.z, and determining the maximum altitude coordinate max _ z of the random sampling point.

It should be noted that this manner of determining the maximum height coordinate is generally used in the case where the maximum height coordinate is the vertical height of the non-random sampling point.

2) After determining the maximum value among high _ max, Start _ position.z and target point vertical height Goal _ position.z, a height tolerance threshold is added on the basis of the maximum value to determine the maximum height coordinate max _ z of the random sampling point, wherein the height tolerance threshold can be 50 m.

After determining the maximum height coordinate max _ z of the random sampling point, a random height rand _ z is generated within the range of the vertical height and the maximum height coordinate (min _ z, max _ z) of the random sampling point, thereby generating the horizontal coordinate and the height coordinate (rand _ x, rand _ y, rand _ z) of the random sampling point rand _ pt.

It should be noted that, when the maximum height coordinate of the random sampling point is determined by the above-mentioned manner 1), there is a case where the vertical height of the random sampling point is the maximum height coordinate, and at this time, the range of the random height is min _ z.

According to the path planning method provided by the embodiment of the invention, the random sampling points in the horizontal plane are respectively determined, the range of the points in the vertical plane is selected according to the determined local DEM data, and the points meeting the constraint are searched in the range, so that the success rate of determining the search nodes is improved, and a large amount of search resources are prevented from being wasted on the calculation constraint.

It should be noted that, in the embodiment of the present invention, after the height of the random sampling point is determined according to step S3125, the horizontal coordinate of the random sampling point may be determined according to the methods in steps S3123 to S3124.

In one embodiment of the present invention, as shown in fig. 8, step S313 may include, but is not limited to, the following steps:

step S3131: at least one constraint node satisfying the dynamic constraint with the random sampling point is determined in the node list.

In the embodiment of the invention, after the starting point is stored in the node list and the random sampling point is generated in the sampling area, all constraint nodes meeting the dynamic constraint with the random sampling point are determined in the node list, and the number of the constraint nodes can be at least one.

It should be noted that, if there is no node in the node list that satisfies the dynamic constraint with the random sampling point, the search node cannot be determined, the random sampling point is discarded, and a new random sampling point is generated again according to the method in step S312.

In the embodiment of the invention, dynamic constraint check can be respectively carried out on any node in the node list and the random sampling node, and if any point and the random sampling node meet the dynamic constraint, the any point is a constraint node. According to the method, all nodes in the node list are polled to determine at least one constrained node in the node list that satisfies the dynamic constraint with the random sampling point.

Optionally, when only the starting point is included in the node list, it is determined whether the starting point and the random sampling point satisfy the dynamic constraint, if so, the starting point is the constraint node of the random sampling point, and if not, the random sampling point is discarded, and a new random sampling point is generated again according to the method in step S312.

According to the path planning method provided by the embodiment of the invention, the reliability and the safety of the path planning effect are effectively ensured by adopting the dynamic constraint as the constraint condition of the path planning.

Step S3132: and respectively determining Euclidean distances between at least one constraint node and the random sampling points.

In the embodiment of the invention, the calculation mode of the Euclidean distance between any constraint node and the random sampling point is as follows:

Dist = sqrt(Dx * Dx + Dy * Dy + Dz * Dz) （3）

the method comprises the following steps that Dist represents the Euclidean distance between any one constraint node and a random sampling point, sqrt () is a square root function, Dx represents the relative distance between the random sampling point and the any one constraint node on an x axis, Dy represents the relative distance between the random sampling point and the constraint node on a y axis, and Dz represents the distance between the random sampling point and the constraint node in the vertical direction.

For example, if the random sampling point is rand _ pt and any point in the node list is nodes _ pt, Dx, Dy, and Dz between the two nodes can be calculated by the following formula:

Dx = fabs(rand_pt.x–nodes_pt.x)

Dy = fabs(rand_pt.y–nodes_pt.y) （4）

Dz = fabs(rand_pt.z–nodes_pt.z)

the fabs () is an absolute value taking function, the random sampling point is rand _ pt, and any point in the node list is nodes _ pt.

Step S3133: and selecting the constraint node corresponding to the minimum Euclidean distance as the minimum constraint node. Namely, the constraint node with the minimum Euclidean distance with the random sampling node is selected as the minimum constraint node.

In the embodiment of the invention, after the Euclidean distance between each constraint node and a random sampling point is determined, the constraint node corresponding to the minimum Euclidean distance from the sampling point to the minimum value of the Euclidean distances is selected, the constraint node is recorded as a minimum constraint node near _ pt, and the Euclidean distance corresponding to the minimum constraint node is called as a minimum Euclidean distance which is recorded as min _ dist.

Step S3134: and determining a searching node in a connecting line of the random sampling point and the minimum constraint node by a connecting step length.

In the embodiment of the invention, in the connection line of the random sampling point rand _ pt and the minimum constraint node near _ pt, the connection step length is intercepted from the direction from the minimum constraint node to the random sampling point to determine the search node new _ pt. Wherein, the step value can be adjusted according to different scenes.

In the embodiment of the invention, if the minimum Euclidean distance is greater than the connection step length, the coordinate of the search node is determined according to the coordinate of the random sampling point, the coordinate of the minimum constraint node and the minimum Euclidean distance, and if the minimum Euclidean distance is less than or equal to the connection step length, the random sampling point is the search node.

For example, the search node may be determined by:

if min _ dist > step,

new_pt.x=near_pt.x+((rand_pt.x-near_pt.x)*step)/ min_dist

new_pt.y=near_pt.y+((rand_pt.y-near_pt.y)*step)/ min_dist （5）

new_pt.z=near_pt.z+((rand_pt.z-near_pt.z)*step)/ min_dist

otherwise new _ pt = rand _ pt.

Wherein, new _ pt is a search node, and step is a connection step length for intercepting a connection line between near _ pt and rand _ pt.

In the embodiment of the invention, the search node is determined by the method, so that the efficiency and the accuracy of generating the search node are improved, the search node which does not meet the dynamic constraint is prevented from being generated, and the overall efficiency of path planning is improved.

In one embodiment of the present invention, as shown in fig. 9, step S314 may include, but is not limited to, the following steps:

step S3141: and determining a parent node corresponding to the search node in the node list.

In the embodiment of the present invention, the parent node of a node means: when a certain node N has several alternative options of father nodes, the nodes can be respectively used as temporary father nodes of the node N, and the temporary father node with the minimum cost value of the node N is called as the father node of the node N, that is, the true father node of the node.

In the embodiment of the present invention, a minimum constraint node may be used as a search constraint node, at least one search constraint node that satisfies the dynamic constraint with the search node and has an euclidean distance with the search node smaller than a reconnection distance is determined in the node list, the cost value of the search node is determined when each search constraint node is used as a temporary parent node of the search node, and the search constraint node corresponding to the minimum cost value of the search node is selected as a parent node of the search node, that is, a true parent node of the search node.

It should be noted that, in the search constraint node that is determined in the node list to satisfy the dynamic constraint with the search node and whose euclidean distance from the search node is smaller than the reconnection distance, a minimum constraint node may not be included, and if the minimum constraint node is included, since the minimum constraint node is already used as the search constraint node, it is not necessary to use the minimum constraint node as the search constraint node again.

In the embodiment of the invention, when determining the search constraint node which meets the dynamic constraint with the search node and has the Euclidean distance with the search node smaller than the reconnection distance in the node list, whether any one point in the node list and the search node meet the dynamic constraint can be calculated, for the nodes meeting the dynamic constraint, the Euclidean distance of the search point of each node and the search node is respectively calculated, the nodes with the Euclidean distance with the search node smaller than the reconnection distance are determined, and the nodes and the minimum constraint node form the search constraint node.

In the embodiment of the present invention, it is not limited to first determine a node satisfying the dynamic constraint with the search node, and then determine a node having a euclidean distance with the search node smaller than the reconnection distance, or first determine a node having a euclidean distance with the search node smaller than the reconnection distance from a node list, and then determine a node satisfying the dynamic constraint with the search node.

It should be noted that, if the other nodes in the node list except the minimum constraint node and the search node do not satisfy the dynamics constraint, the minimum constraint node is directly used as the parent node of the search node.

In the embodiment of the invention, the calculation method of the Euclidean distance between the nodes and the search node can refer to the calculation method of the Euclidean distance of the sampling point. The calculation formula of the reconnection distance recannect _ dis is as follows:

reconnect_dis = search_radius * sqrt(ln(nodes_num) / nodes_num) （6）

the search _ radius is an adjustable constant, can also be expressed as an adjustment coefficient, and can be preset. node _ num is the number of nodes in the node list.

After determining search constraint nodes (including a minimum constraint node and nodes which satisfy dynamic constraints with the search node and have Euclidean distances smaller than reconnection distances from the search node) from a node list, respectively determining the cost value of each search constraint node as a temporary father node of the search node, and selecting the corresponding search constraint node with the minimum cost value of the search node as the father node of the search node. For example, when each search constraint node is calculated to be a temporary parent node of the search node, the cost value new _ pt.

In the embodiment of the invention, the cost value of each search node can be determined according to the Euclidean distance between each search constraint node and the search node and the cost value of each search constraint node.

The cost value calculation method of a certain node is as follows:

n_value = n_dist + np_value （7）

wherein n _ value is a parent node of a certain node, n _ dist is the Euclidean distance between the node and the parent node, and np _ value is a cost value of the parent node of the node.

It should be noted that any node in the node list has its own cost value, and for the first node added to the node list: the starting point may preset a cost value of 0, and for the second node added to the node list, the cost value may be obtained by adding 0 (i.e., the cost value of the starting point) to the euclidean distance between the starting point and the second node.

Step S3142: and judging whether the path between the search node and the corresponding parent node is safe.

It should be noted that, when determining whether the path between the search node and the corresponding parent node is safe after determining the parent node of the search node, reference may be made to the method in step S23 to determine whether the path between the search node and the corresponding parent node is a safe path.

If so, go to S3143, otherwise go to S3144.

Step S3143: and storing the search nodes into a node list.

In the embodiment of the invention, when the search nodes are stored in the node list, all the nodes can be sequentially arranged according to the storage sequence of all the nodes.

It should be noted that, for other nodes in the node list except the initial node, i.e. the starting point, there are corresponding parent nodes, and there may be no corresponding child nodes for these nodes. There may be multiple child nodes for any one node in the node list.

Step S3144: and abandoning the search node, and regenerating a random sampling point in the sampling area to determine a new search node.

If the path between the search node and the corresponding father node is not safe, the search node is abandoned, a new random sampling point is generated again according to the method from the step S312 to the step S313, and a new search node is determined.

In the embodiment of the invention, the father node of the search node is determined by the method, so that the success rate and the efficiency of path planning are improved.

Fig. 10 is a node location relationship diagram according to an embodiment of the present invention. The random sampling point is rand _ pt, any point in the node list is node _ pt, the node _ pt _ p in the node list is a parent node of the node _ pt, the node _ pt _ pp in the node list is a parent node of the node _ pt _ p, the tangential speed of an inscribed circle of an included angle formed by the node _ pt _ pp, the node _ pt _ p and the node _ pt is Vector1, and the tangential speed of an inscribed circle of an included angle formed by the node _ pt _ p, the near _ pt and the rand _ pt is Vector 2. Wherein:

Vector1 = [nodes_pt.x - nodes_pt _p.x, nodes_pt.y - nodes_pt _p.y]

Vector2 = [rand_pt.x - nodes_pt.x, rand_pt.y - nodes_pt.y] （8）

the dynamic constraints include, but are not limited to, the following:

1. minimum yaw angle raw constraint: the included angle formed by the nodes _ pt, nodes _ pt _ p and nodes _ pt _ pp is greater than or equal to a certain preset angle, for example: 120 degrees.

2. Maximum speed direction change constraint: it is meant that the angular change in direction of the above-mentioned

vectors

1 and 2 is less than a preset maximum speed direction change value, for example, 30 °.

3. And (4) constraining the maximum pitch angle theta: it means that the pitch angle θ determined by the coordinates of the two nodes is less than or equal to a preset angle, for example, 10 °. Wherein the content of the first and second substances,

θ = arctan(fabs(dz) / sqrt(dx * dx + dy * dy)

wherein dz = rand _ pt.z-nodes _ pt.z

dx = rand_pt.x - nodes_pt.x

dy = rand_pt.y - nodes_pt.y （9）

The fabs () is an absolute value taking function, and the sqrt () is a square root taking function. If rand _ pt and nodes _ pt satisfy the maximum pitch angle constraint, nodes _ pt is the constraint node of rand _ pt.

It should be noted that, when determining whether a node and its parent node satisfy the dynamic constraint, for the first node added to the node list, i.e. the starting point, there is no corresponding parent node, so that the starting point is not determined by the dynamic constraint; for the node which is added into the father node in the node list and is used as the starting point, the maximum pitch angle constraint can be judged; for nodes which are added into the node list and have father nodes and father nodes of the father nodes, the minimum yaw angle constraint and the maximum pitch angle constraint can be judged.

And when the constraint node of the random sampling point is determined, determining the constraint node of the random sampling point according to each node in the dynamic constraint polling node list. A method of determining at least one search constraint node satisfying the kinetic constraint with the search node in the node list, and determining a parent node of the target point in the node list may refer to this method.

In the embodiment of the invention, the search node is determined through the dynamic constraint, so that the generation of invalid search nodes is avoided, and the efficiency of path planning is improved.

Optionally, in an embodiment of the present invention, step S32 may specifically include:

and determining a father node of the target point in the node list, sequentially extracting nodes in the node list from the target point according to the relation sequence of the nodes and the father node, and establishing the path planning list by taking the nodes as path nodes.

Determining the father node of the target point according to the method in step S3141, that is, respectively calculating target constraint nodes satisfying dynamic constraints between each node in the node list and the target point, then respectively determining the euclidean distance between each target constraint node and the target point, selecting the corresponding target constraint node when the cost value of the target point is minimum as the father node of the target point, and determining the father node of the target point by referring to the method for determining the father node of the search node.

Alternatively, the parent node of the target point may be directly determined. When the condition for ending the search of the node list includes: the Euclidean distance between a search node and the target point is smaller than a search cutoff distance, and/or a path between the search node and the target point is a safe path, and the search node and the target point meet the dynamics constraint, that is, if a certain search node is stored in a node list, the Euclidean distance between the search node and the target point is smaller than the search cutoff distance, and/or the path between the search node and the target point is a safe path, and the dynamics constraint is met, the search node can be directly determined to be a father node of the target point.

After determining the father node of the target point, sequentially extracting the nodes in the node list from the target point according to the relation sequence of the nodes and the father node, and establishing a path planning list by taking the extracted nodes as path nodes. The sequence of each path node in the path planning list is as follows: and taking the target point as the last bit, and sequentially extracting the father nodes corresponding to the path nodes forwards. It is noted that the nodes in the node list may be more than or equal to the path nodes in the path plan list.

For example, the node in the node list is A, B, C, D, G, where G is the target point, D is the parent node of G, B is the parent node of C and D, and a is the parent node of B, then starting from the target point G, the nodes extracted in sequence according to the relationship between the nodes and the parent node are G-D-B-a, a path planning list is established with G, D, B, A as path nodes, the order of each path node in the established path planning list is A, B, D, G, the target point is the last path node, and C in the node list is not extracted into the path planning list.

In the embodiment of the invention, the father node of the target point is determined in the node list, the nodes in the node list are sequentially extracted from the target point according to the relation sequence of the nodes and the father node, and the path planning list is established by taking the nodes as the path nodes.

Optionally, in an embodiment of the present invention, step S32 may further include:

equally dividing and sampling between two adjacent path nodes of the path planning list to generate a plurality of equally divided and sampled nodes, respectively inserting the equally divided and sampled nodes between two corresponding adjacent path nodes, generating a path planning list comprising the path nodes and the arrangement sequence of the equally divided and sampled nodes, and executing the following steps by taking the target point in the path planning list as the current node:

step A: searching path nodes and the equal sampling nodes according to the arrangement sequence, and taking the path node or the equal sampling node which meets the dynamic constraint with the current node and is farthest away as a new father node of the current node;

and B: judging whether the new father node is the last path node of the path planning list or not;

and C: if not, taking the new father node as the current node, and continuing to execute the step A;

step D: and if so, updating the path planning list according to the relationship sequence of the nodes and the father nodes by the target point and the determined multiple new father nodes.

Specifically, for the generated path planning list, a line segment formed by every two adjacent path nodes in the list is subjected to equal sampling, and the sample _ num of each segment may be preset, for example, the sample _ num may be 5. And inserting the equant sampling points generated in each segment of the line segment between two corresponding adjacent path nodes, and generating a path planning list comprising each path node and the arrangement sequence of the equant sampling points to obtain a new path node list after the new path node is interpolated. For each path node in the new path node list, only its neighboring path nodes can be connected. And (3) using a greedy pruning algorithm, starting searching by taking the last path node wp _ last (which can be a target point) of the new path node list as a current node, finding a farthest path node wp _ last _ p meeting the dynamic constraint with the current node, taking the point as a new parent node of wp _ last, and discarding nodes between the wp _ last node and wp _ last _ p node. And then, taking wp _ last _ p as the current node, continuing to search forwards, repeating the operation, and finding the farthest path node meeting the dynamic constraint as a new parent node of wp _ last _ p. And repeating the steps until the last node in the new path node list is found, finishing pruning, and sequentially placing all path nodes determined in the new path node list into a path planning list according to the relationship sequence of the nodes and the father node.

It should be noted that, in the updated path planning list, the sequence of each path node is that the target point is the last bit, and the parent nodes corresponding to each path node are sequentially extracted forward. It should be noted that the number of path nodes in the interpolated path planning list may be greater than or equal to the number of path nodes in the pruned path planning list.

According to the path planning method provided by the embodiment of the invention, interpolation is carried out on the path nodes, and the final path is generated by optimizing the greedy pruning algorithm, so that the smoothness of the algorithm is effectively improved, and the method is more beneficial to the tracking flight of the aircraft 100 to the planned route.

Optionally, in an embodiment of the present invention, step S33 may specifically include:

1. and converting the coordinates of the horizontal coordinate system of the path nodes in the path planning list into the coordinates of the geographic coordinate system.

Specifically, after the path planning list is established, a calculation formula for converting the horizontal coordinate of each path node in the path planning list into the geographic coordinate is as follows:

Path_coord.lat = (Path_position.y / R2) + Start_coord.lat

Path_coord.lon = (Path _postion.x / LonToM) + Start_coord.lon （10）

Path_coord.high = Path_positon.z

where R2 is a polar radius, Path _ position is coordinates of a Path node in a horizontal coordinate system with a Start point as an origin, Path _ coord is a geographical coordinate of the Path node, and Start _ coord is a geographical coordinate of the Start point.

2. And generating an output result of the air route according to the path planning list.

And determining the number of the path nodes in the path planning list, and if the path nodes are determined to exceed the path node threshold value N, taking the coordinates of the first N path nodes as output results for planning the route. Wherein, N is a positive integer greater than 0, and the specific numerical value of N can be set according to the actual situation. And if the number of the path nodes is determined to be less than or equal to N, taking all the path node coordinates as output results for planning the route.

Optionally, if one or more waypoints are preset in the whole route, a path planning list between adjacent waypoints can be determined sequentially or simultaneously from the starting point to the target point of the whole route, and path node coordinates are output for planning the route.

By setting the path node threshold, the first N path nodes in the generated path Planning list are used as output results Planning _ result in each Planning period, so that the data receiving success rate as high as possible is ensured.

Optionally, the outputting the result may include: whether the current preset air route is safe, whether an auxiliary path planning mode needs to be entered, the number of generated path nodes and the coordinates of the geographic coordinate system of each path node. Specifically, if the preset airline is unsafe, the auxiliary path planning mode needs to be entered, and the coordinates of the geographic coordinate system may include latitude, longitude, and altitude.

3. And waiting for the time stamp to be updated to the sending time, and sending an output result. By adopting the planning time as the ending condition of the path planning method in the embodiment of the invention, the problem that a proper air route cannot be generated in a limited communication period can be avoided.

It is noted that the navigation module 122 may send the output result to the control module 121.

According to the path planning method provided by the embodiment of the invention, the aircraft 100 can judge whether the set air route is safe in advance, and plan and update a safer air route according to the actual DEM data, when the aircraft 100 judges that the air route with the preset value is not safe, the aircraft enters an auxiliary path planning mode, and meanwhile, the controller advances according to the planned new air route, so that the occurrence of collision accidents is avoided.

According to the embodiment of the invention, the node sampling is carried out by adopting a target deviation probability sampling strategy, so that the convergence rate of the algorithm is improved, the kinematic constraints of the movable carrier, including the minimum yaw angle, the maximum pitch angle, the maximum speed direction change rate and the like, are considered, the sampling range is limited, the traceability of the final generated path is improved, the search success rate is greatly improved, and compared with the traditional path planning algorithm, the method avoids wasting a large amount of search resources on the calculation constraints. After the planned path is generated, the finally generated path is optimized by using a greedy pruning algorithm of path node interpolation, so that the smoothness of the algorithm is improved, and the movable carrier can track and advance more conveniently. Meanwhile, the planning time stamp is used as a cut-off condition for finishing the algorithm, so that the problem that a proper air route cannot be generated in a limited communication period is avoided.

According to the path planning method provided by the embodiment of the invention, whether the preset air route of the movable carrier is safe or not can be judged, and when the air route has a collision risk, a new air route can be efficiently and accurately planned, so that the collision risk of the movable carrier is effectively avoided, and the traveling safety is greatly improved.

The path planning method according to the embodiment of the present invention is described above, and the path planning apparatus, the mobile carrier and the computer-readable storage medium according to the embodiment of the present invention are described below with reference to fig. 11 to 13, respectively.

Fig. 11 is a schematic structural diagram of a path planning apparatus 200 according to an embodiment of the present invention. The apparatus 200 may be, for example, the path planning apparatus 120 shown in fig. 1. As shown in fig. 11, the path planning apparatus 200 for a mobile vehicle includes:

the obtaining module 210 is configured to obtain lane line data, lane line road condition information, and global DEM data of the mobile carrier;

the judging module 220 is used for judging whether the air route is safe according to the air route data of the movable carrier, the air route road condition information and the global DEM data;

and a determining module 230 for re-planning the course according to the course data of the mobile vehicle and the dynamic constraints when the course is unsafe.

Alternatively, the obtaining module 210 and the control module 240 may be disposed inside the control module 121 shown in fig. 1, and the determining module 220 and the determining module 230 may be disposed inside the navigation module 122 shown in fig. 1.

Optionally, in an embodiment of the present invention, the obtaining module 210 is specifically configured to: and acquiring a starting point and a target point of the course of the movable carrier.

The route road condition information may specifically include: the information of the start point, the no-fly zone in the preset range around the target point and the boundary coordinates of the electronic fence.

Optionally, in an embodiment of the present invention, the determining module 220 is configured to determine at least one block corresponding to the route in the global DEM data according to the starting point and the target point, respectively; and determining local DEM data according to the at least one block, and judging whether the air route is safe according to the local DEM data, wherein the local DEM data are all in the boundary coordinate range of the electronic fence.

Optionally, in an embodiment of the present invention, the determining module 220 is configured to sample on the route according to a resolution of the local DEM data, and determine a plurality of sampling points; respectively determining the earth surface heights in the local DEM data corresponding to the same horizontal coordinates of the starting point, the target point and the plurality of sampling points; comparing the altitude of the starting point, the altitude of the target point and the altitude of the plurality of sampling points with the corresponding ground surface altitude respectively; determining the starting point, wherein the altitude of the target point and the plurality of sampling points is higher than the corresponding ground surface height, and the points with the altitude difference not less than a safety threshold are safety points; and if the starting point, the target point and the plurality of sampling points are all determined as the safety points, determining the route as a safety path.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to generate a node list of the path plan according to the route data and the dynamic constraint; establishing a path planning list according to the node list; and generating the route according to the path planning list.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: storing the starting point into a node list; generating random sampling points in a sampling area; determining a search node according to the dynamics constraint and the random sampling point; and storing the search node in the node list.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: determining a horizontal sampling area according to the horizontal coordinate of the starting point and the horizontal coordinate of the target point; generating random sampling points in a horizontal sampling region according to the horizontal sampling region and a random sampling strategy, and determining horizontal coordinates of the random sampling points; determining whether the random sampling point is in a no-fly zone or not according to the local DEM data and the horizontal coordinate of the random sampling point; if so, regenerating the random sampling points according to the horizontal sampling area and the random sampling strategy until the horizontal coordinates of the regenerated random sampling points are not in the no-fly area; if not, determining the height coordinate of the random sampling point.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to determine the height coordinate of the random sampling point according to the horizontal coordinate of the random sampling point, the highest altitude value in the local DEM data, the height coordinate of the starting point, and the height coordinate of the target point.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: determining at least one constraint node in a node list that satisfies the kinetic constraint with the random sampling point; respectively determining Euclidean distances between the at least one constraint node and the random sampling points; selecting the constraint node corresponding to the minimum Euclidean distance as a minimum constraint node; and determining a searching node in a connecting line of the random sampling point and the minimum constraint node by a connecting step length.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: determining a parent node corresponding to the search node in the node list; judging whether the path between the search node and the corresponding father node is safe or not; if the node is safe, the search node is stored into the node list; if not, abandoning the search node, and regenerating a random sampling point in the sampling area to determine a new search node.

Optionally, in an embodiment of the invention, the kinetic constraints comprise at least one or more of the following: a minimum yaw angle constraint, a maximum speed direction change constraint, and a maximum pitch angle constraint.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: taking the minimum constraint node as a search constraint node, and determining at least one search constraint node which meets the dynamic constraint with the search node and has a Euclidean distance with the search node smaller than a reconnection distance in the node list; respectively determining a cost value of each search node when each search constraint node is used as a temporary father node of the search node; and selecting the corresponding search constraint node when the cost value of the search node is minimum as a parent node of the search node.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to determine the cost value of each search constraint node according to the euclidean distance between the search constraint node and the search node and the cost value of each search constraint node.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to add 1 to the iteration number; judging whether the condition of storing the node into the node list after the search is finished is met or not; if yes, storing the target point into the node list; otherwise, storing the search node into a node list, regenerating a random sampling point in a sampling area to determine a new search node, and storing the new search node into the node list until the condition of finishing storing the search node into the node list is met;

the condition for finishing the storage of the search node into the node list comprises at least one of the following conditions: the iteration times are greater than the maximum iteration times; the Euclidean distance between the search node and the target point is smaller than the search cut-off distance; the path between the search node and the target point is a safe path, and the kinetic constraint is satisfied between the search node and the target point.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: determining a parent node of the target point in the node list; and sequentially extracting the nodes in the node list from the target point according to the relation sequence of the nodes and the father node, and establishing the path planning list by taking the nodes as path nodes.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: equally sampling between two adjacent path nodes of the path planning list and generating a plurality of equally sampled nodes; respectively inserting a plurality of equally-divided sampling nodes between two corresponding adjacent path nodes to generate a path planning list comprising each path node and the arrangement sequence of the equally-divided sampling nodes; starting with the target point in the path planning list as the current node, executing the following steps:

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: converting the coordinates of the horizontal coordinate system of the path nodes in the path planning list into the coordinates of a geographic coordinate system; generating an output result of the route according to the path planning list; and waiting for the time stamp to be updated to the sending time, and sending the output result.

Optionally, in an embodiment of the present invention, the outputting the result includes: whether the airline is safe; and the number of the path nodes generated; and coordinates of a geographical coordinate system of each of the path nodes.

Optionally, in an embodiment of the present invention, the mobile carrier includes any one of: manned vehicles, unmanned vehicles (unmanned), robots, automobiles, and boats.

Fig. 12 is a schematic structural diagram of a mobile carrier 300 according to an embodiment of the present invention. The mobile carrier 300 may include: a path planner 310 and a power plant 320. The path planning device 310 may be the path planning device described in the above embodiments. The power plant 320 may be the power plant 110 shown in fig. 1. The path planning device 310 is configured to send a command to the power device 320, and the power device 320 is configured to generate power according to the command, so that the mobile vehicle 300 travels according to the planned path determined by the path planning device 310, so as to achieve safe driving.

The path planning method, the device and the movable carrier can judge whether the preset air route has collision risk or not according to DEM data in the global range. When collision risks exist, a new air route is planned by combining dynamic constraints of the movable carrier and a path planning algorithm, an auxiliary driving function is provided for the movable carrier, a planned result is intercepted by a preset number of path nodes, and the data receiving success rate as high as possible is guaranteed.

The path planning method of the embodiment of the invention obtains the route data, route road condition information and global DEM data of the movable carrier; judging whether the air route is safe or not according to the air route data, the air route road condition information and the global DEM data; if not, replanning the air route according to the air route data of the movable carrier and the dynamic constraint; and controlling the movable carrier to move according to the re-planned route, and efficiently and stably planning a safe route, so that the movable carrier is accurately controlled, and the movable carrier is prevented from entering a dangerous area.

The embodiment of the invention adopts a target deviation probability sampling strategy to carry out node sampling, improves the convergence rate of the algorithm, simultaneously considers the kinematic constraints of the movable carrier, including the minimum yaw angle, the maximum pitch angle, the maximum speed direction change rate and the like, limits the sampling range, improves the traceability of the final generated path, greatly improves the search success rate, and avoids wasting a large amount of search resources on the calculation constraints compared with the traditional path planning algorithm. After the planned path is generated, the finally generated path is optimized by using a greedy pruning algorithm of path node interpolation, so that the smoothness of the algorithm is improved, and the movable carrier can track and advance more conveniently. Meanwhile, the planning time stamp is used as a cut-off condition for finishing the algorithm, so that the problem that a proper air route cannot be generated in a limited communication period is avoided.

According to the path planning method, the device and the mobile carrier, whether the preset air route of the mobile carrier is safe or not can be judged, and when the air route has collision risks, the air route can be efficiently and accurately re-planned, so that the collision risks of the mobile carrier are effectively avoided, and the traveling safety is greatly improved.

An embodiment of the present invention further provides a computer-readable storage medium, which is located in the computer device 400 shown in fig. 13, where fig. 13 is a structural diagram illustrating an exemplary hardware architecture of a computing device capable of implementing the path planning method, the apparatus, and the mobile vehicle according to the embodiment of the present invention.

As shown in fig. 13, computing device 400 includes an input device 401, an input interface 402, a central processor 403, a computer-readable storage medium 404, an output interface 405, and an output device 406. The input interface 402, the central processor 403, the computer-readable storage medium 404, and the output interface 405 are connected to each other via a bus 410, and the input device 401 and the output device 406 are connected to the bus 410 via the input interface 402 and the output interface 405, respectively, and further connected to other components of the computing device 400.

Specifically, the input device 401 receives input information from the outside and transmits the input information to the central processor 403 through the input interface 402; the central processor 403 processes the input information based on computer-executable instructions stored in the computer-readable storage medium 404 to generate output information, stores the output information in the computer-readable storage medium 404 temporarily or permanently, and then transmits the output information to the output device 406 through the output interface 405; output device 406 outputs the output information outside of computing device 400 for use by a user.

In one embodiment, the computing device 400 shown in fig. 13 may be implemented as a path planner that may include: a memory configured to store a program; a processor configured to run a program stored in the memory to perform the path planning method described in the above embodiments.

According to an embodiment of the invention, the process described above with reference to the flow chart may be implemented as a computer software program. For example, embodiments of the invention include a computer program product comprising a computer program tangibly embodied on a machine-readable medium, the computer program comprising program code for performing the method illustrated in the flow chart. In such an embodiment, the computer program may be downloaded and installed from a network, and/or installed from a removable storage medium.

In the above embodiments, the implementation may be wholly or partially realized by software, hardware, firmware, or any combination thereof. When implemented in software, may be implemented in whole or in part in the form of a computer program product. The computer program product comprises one or more computer instructions which, when run on a computer, cause the computer to perform the methods described in the various embodiments above. The procedures or functions according to the embodiments of the invention are brought about in whole or in part when the computer program instructions are loaded and executed on a computer. The computer may be a general purpose computer, a special purpose computer, a network of computers, or other programmable device. The computer instructions may be stored in a computer readable storage medium or transmitted from one computer readable storage medium to another, for example, the computer instructions may be transmitted from one website, computer, server, or data center to another website, computer, server, or data center by wire (e.g., coaxial cable, fiber optic, Digital Subscriber Line (DSL)) or wirelessly (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device, such as a server, a data center, etc., that incorporates one or more of the available media. The usable medium may be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid state disk), among others.

The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and the parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.

Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method for a mobile vehicle, the method comprising:

if not, replanning the route according to the route data and the dynamic constraint of the movable carrier, comprising the following steps: generating a node list of path planning according to the route data and the dynamic constraint;

generating a node list of a path plan according to the route data and the dynamic constraints, wherein the node list comprises: storing a starting point into the node list; generating random sampling points in a sampling area; determining a search node according to the dynamic constraint and the random sampling point; storing the search node into the node list;

determining a search node according to the dynamic constraint and the random sampling point, wherein the determining comprises: determining at least one constraint node in a node list that satisfies the kinetic constraint with the random sampling point; respectively determining Euclidean distances between the at least one constraint node and the random sampling points; selecting the constraint node corresponding to the minimum Euclidean distance as a minimum constraint node; and determining a searching node in a connecting line of the random sampling point and the minimum constraint node by a connecting step length.

2. The path planning method of claim 1 wherein said obtaining course data for said mobile vehicle comprises: acquiring a starting point and a target point of a course of the movable carrier;

the route road condition information comprises: the starting point, the information of the no-fly zone in the preset range around the target point and the boundary coordinates of the electronic fence.

3. The path planning method according to claim 2, wherein the determining whether the route is safe according to the route data of the mobile vehicle, the route road condition information, and the global DEM data comprises:

determining at least one block corresponding to the air route in the global DEM data according to the starting point and the target point respectively;

determining local DEM data according to the at least one block;

and judging whether the air route is safe or not according to the local DEM data.

4. The path planning method according to claim 3, wherein the determining whether the route is safe according to the local DEM data comprises:

sampling on the flight path according to the resolution of the local DEM data, and determining a plurality of sampling points;

respectively determining the earth surface heights in the local DEM data corresponding to the same horizontal coordinates of the starting point, the target point and the plurality of sampling points;

comparing the altitude of the starting point, the altitude of the target point and the altitude of the plurality of sampling points with the corresponding ground surface altitude respectively;

determining the starting point, wherein the altitude of the target point and the plurality of sampling points is higher than the corresponding ground surface height, and the points with the altitude difference not less than a safety threshold are safety points;

and if the starting point, the target point and the plurality of sampling points are all determined as the safety points, determining the route as a safety path.

5. The path planning method of claim 3 wherein replanning the route based on route data and dynamic constraints of the mobile vehicle further comprises:

establishing a path planning list according to the node list;

and generating the route according to the path planning list.

6. The path planning method of claim 5, wherein generating random sampling points in a sampling region comprises:

determining a horizontal sampling area according to the horizontal coordinate of the starting point and the horizontal coordinate of the target point;

generating random sampling points in the horizontal sampling region according to the horizontal sampling region and a random sampling strategy, and determining horizontal coordinates of the random sampling points;

determining whether the random sampling point is in a no-fly zone or not according to the local DEM data and the horizontal coordinate of the random sampling point;

if so, regenerating the random sampling points according to the horizontal sampling area and the random sampling strategy until the horizontal coordinates of the regenerated random sampling points are not in the no-fly area;

if not, determining the height coordinate of the random sampling point.

7. The path planning method according to claim 6, wherein said determining the height coordinate of the random sampling point comprises:

and determining the height coordinate of the random sampling point according to the horizontal coordinate of the random sampling point, the highest altitude value in the local DEM data, the height coordinate of the starting point and the height coordinate of the target point.

8. The method of claim 5, wherein storing the search node in the node list comprises:

determining a parent node corresponding to the search node in the node list;

judging whether the path between the search node and the corresponding father node is safe or not;

if the node is safe, the search node is stored into the node list;

if not, abandoning the search node, and regenerating a random sampling point in the sampling area to determine a new search node.

9. The path planning method according to claim 8, wherein the determining a parent node corresponding to the search node in the node list includes:

taking the minimum constraint node as a search constraint node, and determining at least one search constraint node which meets the dynamic constraint with the search node and has a Euclidean distance with the search node smaller than a reconnection distance in the node list;

respectively determining a cost value of each search node when each search constraint node is used as a temporary father node of the search node;

and selecting the corresponding search constraint node when the cost value of the search node is minimum as a parent node of the search node.

10. The path planning method according to claim 9, wherein determining the cost value of each search node when the search constraint node is used as the temporary parent node of the search node respectively comprises:

and determining the cost value of each search node according to the Euclidean distance between each search constraint node and the search node and the cost value of each search constraint node.

11. The path planning method according to any one of claims 8 to 10, wherein storing the search node in the node list comprises:

adding 1 to the iteration number;

judging whether the condition of storing the node into the node list after the search is finished is met or not;

if yes, storing the target point into the node list;

otherwise, storing the search node into a node list, regenerating a random sampling point in a sampling area to determine a new search node, and storing the new search node into the node list until the condition of finishing storing the search node into the node list is met;

the iteration times are greater than the maximum iteration times;

the Euclidean distance between the search node and the target point is smaller than the search cut-off distance;

the path between the search node and the target point is a safe path, and the kinetic constraint is satisfied between the search node and the target point.

12. The path planning method according to claim 11, wherein the building a path planning list according to the node list comprises:

determining a parent node of the target point in the node list;

and sequentially extracting the nodes in the node list from the target point according to the relation sequence of the nodes and the father node, and establishing the path planning list by taking the nodes as path nodes.

13. The path planning method according to claim 12, wherein a path planning list is created according to the node list, further comprising:

equally sampling between two adjacent path nodes of the path planning list and generating a plurality of equally sampled nodes;

respectively inserting a plurality of equally-divided sampling nodes between two corresponding adjacent path nodes to generate a path planning list comprising each path node and the arrangement sequence of the equally-divided sampling nodes;

starting with the target point in the path planning list as the current node, executing the following steps:

14. A path planning method according to claim 12 or 13, wherein the planning of routes according to the path planning list comprises:

converting the coordinates of the horizontal coordinate system of the path nodes in the path planning list into the coordinates of a geographic coordinate system;

generating an output result of the route according to the path planning list;

and waiting for the time stamp to be updated to the sending time, and sending the output result.

15. The path planning method according to claim 14, wherein the outputting the result comprises:

whether the airline is safe; and

the number of the path nodes generated; and

coordinates of a geographical coordinate system of each of the path nodes.

16. A path planning device for a mobile vehicle, comprising:

the determining module is used for re-planning the air route according to the air route data and dynamic constraints of the movable carrier when the air route is unsafe;

wherein the determining module is specifically configured to: generating a node list of path planning according to the route data and the dynamic constraint;

generating a node list of a path plan according to the route data and the dynamic constraints, wherein the node list comprises: storing the starting point into the node list; generating random sampling points in a sampling area; determining a search node according to the dynamic constraint and the random sampling point; storing the search node into the node list;

17. A mobile vehicle, comprising:

the path planner according to claim 16, comprising a power plant and the path planner, wherein the path planner is configured to send instructions to the power plant, and wherein the power plant is configured to generate power in accordance with the instructions such that the mobile vehicle travels along a route determined by the path planner.

18. A computer-readable storage medium, having a computer program stored thereon, comprising instructions which, when run on a computer, cause the computer to perform a path planning method according to any of claims 1-15.