CN113253761B

CN113253761B - Real-time path planning method and device, movable carrier and storage medium

Info

Publication number: CN113253761B
Application number: CN202110634444.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: CN113253761A

Abstract

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

Description

Real-time 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 real-time path planning method, device, mobile carrier, and 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 traveling route area in real time, the collision between the air route and the information in the actual task area is easily caused, and finally the collision accident is 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 carrier path planning algorithm can assist the mobile carrier to execute the route of travel, and a safe travel route is planned in the process that the mobile carrier accesses the local travel 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 for dynamically planning a travel route according to a collision risk of the travel route in real time, so as to make a mobile vehicle safely and efficiently complete the travel route.

Disclosure of Invention

An object of the embodiments of the present invention is to provide a real-time path planning method, an apparatus, a mobile carrier, and a storage medium, which are capable of determining safety of a traveling route of the mobile carrier in real time according to global DEM data and dynamically planning the traveling route of the mobile carrier, so as to effectively avoid a possibility that the mobile carrier enters a dangerous area.

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

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

judging whether the current traveling route is safe or not according to the current traveling data, the global DEM data and the traveling road condition information;

if not, planning a local traveling route according to the current traveling data of the movable carrier and the dynamic constraint;

and controlling the movable vehicle to travel according to the local travel route.

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

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

the judging module is used for judging whether the current traveling route is safe or not according to the current traveling data, the global DEM data and the traveling road condition information;

the determining module is used for planning a local traveling route according to the current traveling data of the movable carrier and dynamic constraints when the current traveling route is unsafe;

and the control module is used for controlling the movable carrier to travel according to the local travel route.

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

the real-time 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 movable carrier can move according to the planned path determined by the real-time 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 real-time path planning method described above.

According to the real-time path planning method, the device, the mobile carrier and the computer-readable storage medium provided by the embodiment of the invention, the current traveling data, the traveling road condition information and the global DEM data of the mobile carrier are obtained; judging whether the current traveling route is safe or not according to the current traveling data, the global DEM data and the traveling road condition information; if not, planning a local traveling route according to the current traveling data of the movable carrier and the dynamic constraint; the movable carrier is controlled to travel according to the local traveling route, and the local traveling route can be efficiently, stably and dynamically planned, so that the movable carrier is accurately controlled, and the movable carrier is prevented 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 method of real-time path planning 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 real-time 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, the real-time path planning method, the real-time path planning apparatus, the mobile carrier and the computer-readable storage medium according to the embodiments of the present invention are described in detail below with reference to fig. 1 to 11. 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 real-time 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 speed regulator 111 is configured to receive the driving signal generated by the real-time 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 real-time path planner 120 may include a control module 121 and a navigation module 122. The navigation module 122 stores global DEM data therein, and communicates with the control module 121 to obtain the current flight state of the aircraft 100, and determine whether the current traveling route of the aircraft 100 has a collision risk in real time, and make an auxiliary path planning strategy. The control module 121 is configured to control the flight of the aircraft 100, obtain the current flight state of the aircraft 100 in real time, 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 current traveling route and the planned path by the navigation module 122.

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 real-time 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 real-time path planning apparatus 120 shown in fig. 1. As shown in fig. 2, the method may specifically include:

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

Optionally, in an embodiment of the present invention, acquiring current travel data of the mobile vehicle may include: the method comprises the steps of obtaining the current position, the current traveling posture, the current traveling speed, the current traveling mode, the starting point and the target point of a current traveling route of the movable carrier. The current position may include longitude lon, latitude lat and altitude high coordinates under the geographic coordinate system; the current heading attitude may include a pitch angle pitch, a roll angle roll, and a yaw angle yaw; the current travel speed may include a northbound speed Vn, an eastern speed Ve, and a ground speed Vd; the current travel 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 in the auxiliary path planning mode, the movable carrier is in auxiliary path planning, and in the task advancing mode, the movable carrier can advance according to a task route without auxiliary path planning; the starting point and the target point of the current travel route can be the starting point and the target point of the whole travel route, and can also be two adjacent waypoints of the flight route.

Optionally, the traveling road condition information may include: the information of the no-fly zone in a preset range around the starting 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 that may be 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 current traveling route is safe or not according to the current traveling data, the global DEM data and the traveling road condition information.

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 current advancing route of the aircraft has risks such as mountain collision and the like can be judged according to the current advancing data, the global DEM data and the advancing road condition information, and whether the current advancing route is safe or not can be judged.

Step S3: if not, planning a local travel route according to the current travel data of the mobile vehicle and the dynamic constraint.

In the embodiment of the invention, if the current traveling route is unsafe, the local traveling route can be planned in real time through a path planning algorithm according to the current traveling data and the dynamic constraint of the movable carrier, and the safe traveling route is planned.

In the embodiment of the invention, when the current traveling route is determined to be unsafe, the movable carrier may already move according to the current traveling route, or the movable carrier has moving speed and certain time delay in path planning, so that the current traveling route can be locally planned, and the local traveling route can be obtained.

According to the embodiment of the invention, whether the local travel route is planned or not can be determined according to the current travel mode of the movable carrier, and if the current travel mode is a non-auxiliary path planning mode such as a manual mode or a landing mode and a task travel mode, the local travel route is not planned. If the current traveling mode is the auxiliary path planning mode, the local traveling route is planned according to other current traveling data of the mobile carrier and dynamic constraints. And if the current traveling mode is the task traveling mode, controlling the movable carrier to enter an auxiliary path planning mode, and planning a local traveling route according to other current traveling data and dynamic constraints of the movable carrier.

In the embodiment of the present invention, a node list for path planning may be generated according to the current travel data and the dynamic constraint, and then a path planning list may be established according to the node list, so as to generate the local travel route according to the path planning list.

Step S4: and controlling the movable vehicle to travel according to the local travel route.

In the embodiment of the invention, after the movable carrier taking the original current traveling route as the task traveling mode travels according to the local route, the auxiliary path planning mode is exited, and the task traveling mode is continued by taking the local traveling route as the traveling route. Or after the movable carrier finishes the local travelling route, the movable carrier exits the auxiliary path planning mode and continues the task travelling mode.

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

According to the real-time path planning method provided by the embodiment of the invention, the flight data and the traveling road condition information of the aircraft 100 can be monitored in real time, whether the current traveling route of the aircraft 100 is safe or not can be judged through the traveling road condition information and the global DEM data, if the current traveling route of the aircraft 100 is not safe, the local traveling route is planned according to the current traveling data and the dynamic constraint of the aircraft 100 in the flight process, the traveling route of the aircraft 100 can be regulated and controlled in real time, and the safety of the aircraft 100 in the flight process is ensured. The method has the advantages that the local traveling route is planned in real time, so that the traveling route of the aircraft 100 can be adjusted in a self-adaptive mode according to the actual specific conditions of the traveling area, the flexibility is high, and the safety is high.

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 current travel 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 current travel route may be determined according to a start point and a target point of the current travel route, where if the current travel route spans multiple blocks, the current travel route corresponds to multiple blocks, and if the start point and the target point of the current travel route are located in the same block, the current travel route corresponds to one 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 current traveling 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 current travelling route according to the resolution of the local DEM data, and determining a plurality of sampling points. Specifically, on a current travel route formed by the starting point and the target point, sampling is performed according to the resolution (generally 30M × 30M) of the local DEM data itself, so as to obtain a plurality of 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 current travel route 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 current traveling 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. Any one of the sampling points is an unsafe point, the travel route is considered as unsafe, and when the starting point, the target point and all the sampling points are safe points, the travel route is considered 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 current traveling data and the dynamic constraint.

Specifically, the current travel data of the aircraft 100 is acquired in real time according to the control module 121, and the predicted position at the next moment may be acquired based on the current travel data, and the predicted position, the target point in the current travel data, and the search node obtained based on the dynamics constraint may be added to the node list for path planning, so as to generate the node list. It should be noted that the node list includes at least 2 nodes: the predicted position and the target point.

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

In the embodiment of the invention, the path node can be generated based on the node in the node list, and the path planning list can be generated based on the path node. 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 local traveling route according to the path planning list.

In the embodiment of the invention, all the path nodes can be connected according to the sequence of all the path nodes in the path planning list to generate the local advancing 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 local travel route.

According to the real-time path planning method provided by the embodiment of the invention, the current traveling data of the aircraft 100 is acquired in real time, the path planning list is established by combining with the dynamics constraint, the nodes which do not accord with the dynamics constraint are discarded, the effective data can be acquired in real time, and the method is high in accuracy and stronger in timeliness.

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

step S311: and determining the predicted position of the movable carrier at the next moment according to the rolling time domain control strategy and the current traveling data.

Specifically, according to a rolling time domain control strategy, geographic coordinates of a current position when a path planning task enters each time are determined, a horizontal coordinate system is established by taking the current position as an origin, and a predicted position at the next moment is determined according to the horizontal coordinates of the current position and the current traveling speed.

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

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

Local_position.y = (Local_coord.lat - Now_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 current position as the origin, Local _ coord is the geographical coordinates of the certain point, and Now _ coord is the geographical coordinates of the current position.

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.

Determining the horizontal coordinate of the predicted position at the next time instant according to the following formula:

Next_position.x = Now_positon.x + Ve * dt

Next_position.y = Now_positon.y + Vn * dt （2）

Next_position.z = Now_positon.z + Vd * dt

wherein, Now _ positon is the geographical coordinate Now _ coord of the current position and translates into the coordinate under the horizontal coordinate system, because the current position is the origin of the horizontal coordinate system, so:

Now_positon.x = 0

Now_positon.y = 0 （3）

Now_positon.z = Now_coord.high

where dt is the time period of the fixed frequency communication between the control module 121 and the navigation module 122, and Ve, Vn and Vd are the current east, north and ground speeds of the aircraft 100, respectively.

By means of the rolling time domain control strategy, the current traveling speed of the aircraft 100 can be determined in real time, and the position of the aircraft 100 at the next moment can be predicted by means of the current traveling speed, so that the real-time path planning of the aircraft 100 is realized.

It should be noted that, in the embodiment of the present invention, the nodes and the path nodes in the subsequent path planning are calculated by using the coordinates in the horizontal coordinate system established with the geographic coordinates of the current position as the origin. The coordinates of the target point in the current travel data are geographical coordinates, which can be converted into horizontal coordinates with the current position as the origin by the above equation (1).

Step S312: the predicted location is stored in a node list.

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

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

Specifically, after the predicted position 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 S314: 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 S315: 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 generated any more 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 predicted position of the mobile carrier at the next time is determined according to the rolling time domain control strategy and the current traveling data; storing the predicted location 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 the path planning is realized in real time according to the current traveling 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 the path planning, so that the efficiency and the success rate of the path planning are improved.

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

step S3131: and determining a horizontal sampling area according to the horizontal coordinate of the current position and the horizontal coordinate of the target point.

It should be noted that, since the horizontal coordinate of the current position and the horizontal coordinate of the target point are both horizontal coordinate systems established with the current position as the origin, the horizontal coordinate of the current position is (0, 0), and the coordinates of the target point in the current travel data, which are geographic coordinates, can be converted into horizontal coordinates with the current position as the origin according to the above formula (1).

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 current position Now _ position and the target point Goal _ position as diagonal angles.

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 current position Now _ 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 current position Now _ position and the target point Goal _ position is a rectangle in fig. 7, and the coordinates of the 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 S3132: and generating random sampling points in the horizontal sampling region according to a horizontal sampling region and a 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 （4）

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 real-time 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 S3133: and 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.

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 S3134: 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 S3135: if not, determining the vertical 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 current position, 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 current position 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 Now _ 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, Now _ position.z and the target point vertical height Goal _ position.z, a height tolerance threshold value 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 value 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 real-time path planning method, 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 S3135, the horizontal coordinate of the random sampling point may be determined according to the methods of steps S3133 to S3134.

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

step S3141: 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 predicted position 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 S313.

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 the node list only includes the predicted position node, it is determined whether the predicted position node and the random sampling point satisfy the dynamics constraint, if so, the predicted position node is a constraint node of the random sampling point, if not, the random sampling point is discarded, and a new random sampling point is generated again according to the method in step S313.

Optionally, when only the predicted position is included in the node list, it is determined whether the predicted position and the random sampling point satisfy the dynamic constraint, if yes, the predicted position is a 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 S313. According to the real-time 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 S3142: 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) （5）

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) （6）

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 S3143: 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 S3144: 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 （7）

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 S315 may include, but is not limited to, the following steps:

step S3151: 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) （8）

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 （9）

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 predicted position may be preset to have 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 predicted position) to the euclidean distance between the predicted position and the second node.

Step S3152: and judging whether the path between the search node and the corresponding father node is safe or not.

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, executing S3153, otherwise executing S3154.

Step S3153: and storing the search node into the 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 there is a parent node corresponding to other nodes in the node list except the initial node, i.e., the predicted location, 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 S3154: 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 methods from the step S313 to the step S314, 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] （10）

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 （11）

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 dynamic constraint is satisfied between a node and its parent node, the current position may be used as the parent node of the initial node, and if the minimum yaw angle yaw constraint, the maximum speed direction change constraint, and the maximum pitch angle θ constraint can be determined, at least one of these constraints is determined, otherwise, the constraint that can be performed is determined. For example, for an initial node in the node list, i.e., the predicted position, it may be determined whether the maximum pitch angle θ constraint in the dynamics constraint is satisfied between the two nodes with the current position as its parent node, and for a node in the node list with the predicted position as its parent node, it may be determined whether the maximum pitch angle θ constraint and the minimum yaw angle constraint in the dynamics constraint are satisfied between the two nodes.

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 S3151, 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 real-time path planning method provided by the embodiment of the invention, interpolation is carried out on the path nodes, and the greedy pruning algorithm is used for optimizing and generating the final path, so that the smoothness of the algorithm is effectively improved, and the tracking flight of the aircraft 100 to the traveling route is facilitated.

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 geographical coordinates of each path node in the path planning list into horizontal coordinates with the current position as the origin is as follows:

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

Path_coord.lon = (Path _postion.x / LonToM) + Now_coord.lon （12）

Path_coord.high = Path_positon.z

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

2. And generating an output result of the local advancing 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. N is a positive integer greater than 0, for example, N may be 3, and the specific value of N may be set according to actual conditions. 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.

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 travel route is safe, whether an auxiliary path planning mode needs to be entered, the number of generated path nodes, and the coordinates of the geographical coordinate system of each path node. Specifically, if the current travel route 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 a cut-off condition for finishing the real-time path planning method, the problem that a proper air route cannot be generated in a limited communication period can be solved.

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

According to the real-time path planning method provided by the embodiment of the invention, the aircraft 100 can judge whether the current traveling route is safe or not in real time and plan and update the local traveling route in real time, when the aircraft 100 judges that the current traveling route is unsafe, the auxiliary path planning mode is entered, and meanwhile, the controller adjusts the traveling direction in real time according to the planned local traveling route, so that the occurrence of collision accidents is avoided.

According to the embodiment of the invention, the position of the next moment can be predicted in real time according to the speed of the movable carrier through a rolling time domain control strategy, so that the real-time planning of the path is realized. And a target deviation probability sampling strategy is adopted for node sampling, so that the convergence rate of the algorithm is improved, the kinematic constraints of the movable carrier are considered, the sampling range is limited, the traceability of a 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 travelling route cannot be generated in a limited communication period is solved.

According to the real-time path planning method provided by the embodiment of the invention, whether the traveling route of the movable carrier is safe or not can be judged in real time, and when the current traveling route has collision risk, the traveling route can be efficiently and accurately planned and laid out, so that the collision risk of the movable carrier is effectively avoided, and the traveling safety is greatly improved.

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

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

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

the judging module 220 is configured to judge whether the current traveling route is safe according to the current traveling data, the global DEM data, and the traveling road condition information;

a determining module 230, configured to plan a local travel route according to current travel data of the mobile vehicle and dynamic constraints when the current travel route is unsafe;

and the control module 240 is used for controlling the movable vehicle to travel according to the local travel route.

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: the method comprises the steps of obtaining the current position, the current traveling posture, the current traveling speed, the current traveling mode, the starting point and the target point of a current traveling route of the movable carrier.

The information on the traveling road condition may specifically include: the information of the no-fly zone in a preset range around the starting 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 current traveling route in the global DEM data according to a starting point and a target point, respectively; and determining local DEM data according to at least one block, and judging whether the current travelling route is safe or not 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 current travel route according to the 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 current traveling 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 current traveling data and the dynamic constraint; establishing a path planning list according to the node list; and generating the local travel route according to the path planning list.

Optionally, in an embodiment of the present invention, the determining module 230 is further configured to: determining the predicted position of the movable carrier at the next moment according to the rolling time domain control strategy and the current traveling data; storing the predicted position in 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 current position 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 current position, 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 sampling point Euclidean distances between the at least one constraint node and the random sampling point; selecting the constraint node corresponding to the Euclidean distance of the minimum sampling point 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 local advancing 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 current travel route is safe; and whether the auxiliary path planning mode needs to be entered; 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 real-time path planning device 310 and a power device 320. The real-time path planning device 310 may be the real-time path planning device described in the above embodiments. The power plant 320 may be the power plant 110 shown in fig. 1. The real-time 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 real-time path planning device 310, so as to achieve safe driving.

According to the real-time path planning method, the device and the movable carrier, whether the current traveling distance has collision risks or not can be judged in real time according to DEM data in a global range. When collision risks exist, a new travelling 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 real-time path planning method of the embodiment of the invention obtains the current traveling data, traveling road condition information and global DEM data of the movable carrier; judging whether the current traveling route is safe or not according to the current traveling data, the global DEM data and the traveling road condition information; if not, planning a local traveling route according to the current traveling data of the movable carrier and the dynamic constraint; the movable carrier is controlled to travel according to the local traveling route, and the local traveling route can be efficiently, stably and dynamically planned, so that the movable carrier is accurately controlled, and the movable carrier is prevented from entering a dangerous area.

According to the real-time path planning method, the device and the mobile carrier, whether the traveling route of the mobile carrier is safe or not can be judged in real time, and when the current traveling route has collision risks, the traveling route can be efficiently and accurately planned and laid out, 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 real-time path planning method, apparatus, and mobile vehicle according to an 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 real-time 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 real-time 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 real-time path planning method for a mobile vehicle, the method comprising:

controlling the mobile vehicle to travel according to the local travel route;

wherein said planning a local travel route based on current travel data and dynamic constraints of said mobile vehicle comprises: generating a node list of path planning according to the current traveling data and the dynamic constraint;

generating a node list of a path plan according to the current travel data and the dynamic constraints, wherein the node list comprises: determining a predicted position of the mobile carrier at a next moment according to a rolling time domain control strategy and the current traveling data; storing the predicted location 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; 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 real-time path planning method of claim 1, wherein said obtaining current travel data of the mobile vehicle comprises: acquiring the current position, the current traveling posture, the current traveling speed, the current traveling mode, the starting point and the target point of the current traveling route of the movable carrier;

the traveling road condition information includes: the information of the no-fly zone in a preset range around the starting point and the boundary coordinates of the electronic fence.

3. The real-time path planning method according to claim 2, wherein the determining whether the current travel route is safe according to the current travel data, the global DEM data, and the travel road condition information includes:

determining at least one block corresponding to the current travelling 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 current traveling route is safe or not according to the local DEM data.

4. The real-time path planning method according to claim 3, wherein the determining whether the current travel route is safe according to the local DEM data specifically includes:

sampling on the current travelling route 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 current traveling route as a safety path.

5. The real-time path planning method of claim 3, wherein the planning of the local travel route based on the current travel data of the mobile vehicle and dynamic constraints further comprises:

establishing a path planning list according to the node list;

and generating the local traveling route according to the path planning list.

6. The real-time path planning method of claim 5, wherein generating random sampling points in a sampling area comprises:

determining a horizontal sampling area according to the horizontal coordinate of the current position 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 real-time path planning method of 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 current position 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 real-time 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 real-time 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 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 real-time path planning method according to any one of claims 8-10, wherein storing the searched nodes 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 real-time 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 real-time path planning method of claim 12, wherein building a path planning list according to the node list further comprises:

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. The real-time path planning method according to claim 12 or 13, wherein the generating the local travel route from 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 local advancing 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 real-time path planning method according to claim 14, wherein the outputting the result comprises:

whether the current travel route is safe; and

whether an auxiliary path planning mode needs to be entered; and

the number of the path nodes generated; and

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

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

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

a determination module for planning a local travel route according to current travel data of the mobile vehicle and dynamic constraints when the current travel route is unsafe;

a control module for controlling the mobile vehicle to travel according to the local travel route;

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

17. A mobile vehicle, comprising:

the real-time path planning device of claim 16, wherein the real-time path planning device is configured to send a command to the power device, and the power device is configured to generate power according to the command, so that the mobile vehicle travels along the planned path determined by the real-time path planning device.

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 the real-time path planning method according to any of claims 1-15.