CN108444482B - A method and system for autonomous pathfinding and obstacle avoidance of unmanned aerial vehicle - Google Patents

Abstract

Translated fromChinese

本发明提供一种无人机自主寻路避障方法及系统。本发明方法，包括：采集障碍物的位置信息；通过栅格法进行三维环境建模，并将其分割成若干个栅格，分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理，通过过起始点及目标终点的判断平面分割栅格，得到二维栅格模型；基于A*算法在所述二维栅格模型上做出全局静态路径规划；在全局静态路径规划后，进行局部动态路径规划；通过贝塞尔曲线对规划出的线路点进行轨迹跟踪，完成对路径轨迹的平滑处理；本发明通过全局静态路径规划和局部动态路径规划，使得无人机可以及时发现环境的动态变化带来的障碍物位置的改变，计算过程简便快捷，解决了传统飞行线路容易碰障碍物的问题。

The present invention provides a method and system for autonomous path finding and obstacle avoidance of an unmanned aerial vehicle. The method of the invention includes: collecting the position information of obstacles; carrying out three-dimensional environment modeling by the grid method, dividing it into several grids, and performing the steps on the grids containing obstacles and the grids not containing obstacles respectively. In the processing of different colors, the grid is divided by the judgment plane of the starting point and the target end point to obtain a two-dimensional grid model; based on the A* algorithm, a global static path planning is made on the two-dimensional grid model; on the global static path After planning, local dynamic path planning is performed; the planned route points are tracked through Bezier curves to complete the smooth processing of the path trajectory; the present invention enables the UAV to The change of the position of the obstacle caused by the dynamic change of the environment is detected in time, and the calculation process is simple and fast, which solves the problem that the traditional flight line is easy to hit the obstacle.

Description

Unmanned aerial vehicle autonomous road finding and obstacle avoiding method and system

Technical Field

The invention relates to the field of unmanned aerial vehicle path planning and trajectory tracking, in particular to an unmanned aerial vehicle autonomous path finding and obstacle avoidance method and system.

Background

Unmanned aerial vehicle can carry out the task of various demands under the complex environment, because the complicated variability of environment leads to unmanned aerial vehicle to meet various problems when the executive task. The most important problem is that the obstacles around the unmanned aerial vehicle are increased due to the changeable environment, so that the unmanned aerial vehicle cannot correctly bypass the obstacles to perform tasks. Direct straight-line around obstacles may also cause accidents that hit the paddles. Because of the complex and changeability of the environment, the static obstacle avoidance and road searching cannot meet the complex environment changing constantly, so a dynamic planning method for monitoring the environment change in real time and changing the path in real time is required.

In the field of path planning and trajectory tracking, various algorithms come up endlessly. Most of the existing related algorithms process static maps and environments, and although the existing related algorithms have excellent performance in the static environment, the existing related algorithms have defects in the dynamic environment field.

Disclosure of Invention

According to the technical problem, the unmanned aerial vehicle autonomous path finding and obstacle avoiding method and the system are provided, wherein after global path planning, local dynamic planning is carried out, and environment changes are better processed. The technical means adopted by the invention are as follows:

an unmanned aerial vehicle autonomous road-finding obstacle-avoiding method comprises the following steps:

s1, collecting position information of the obstacles;

s2, modeling a three-dimensional environment through a grid method, dividing the three-dimensional environment into a plurality of grids, respectively processing grids containing obstacles and grids not containing obstacles in different colors, and dividing the grids through a judgment plane passing through a starting point and a target end point to obtain a two-dimensional grid model;

s3, making a global static path plan on the two-dimensional grid model based on an A-x algorithm, and searching an optimal global reference path between a starting point and a final point;

s4, after the global static path is planned, local dynamic path planning is carried out for returning to the originally planned path as soon as possible after the sudden dynamic environment change is processed;

further, after the local dynamic planning, the method includes: and S5, tracking the planned line points through the Bezier curve, and finishing the smoothing processing of the path track.

Further, the step S1 specifically includes:

s101, acquiring position information of an obstacle in a sensor coordinate system through a sensor;

s102, converting the position information under the sensor coordinate system to an unmanned aerial vehicle coordinate system through coordinate transformation;

and S103, importing the position information in the unmanned aerial vehicle coordinate system into an inertial coordinate system through navigation calculation to obtain dynamic obstacle position information.

Further, the obtaining of the two-dimensional grid model by plane segmentation of the grid through the judgment of the starting point and the target end point specifically includes:

s201, generating a judgment plane perpendicular to a yz plane by passing through a starting point and a target end point;

s202, mapping the two-dimensional grid of the xy plane to the judgment plane,

when the level of the starting point is higher than the level of the target end point:

when the level of the starting point is lower than the level of the target end point:

when the horizontal height of the starting point is higher than the horizontal height of the target end point and is consistent, the judgment plane is parallel to the xy plane,

P_z＝E_z＝S_z (3)

wherein, P₁(P_y,P_z) A projection point S representing the coordinate of any point P on the judgment plane on the yz plane₁(S_y,S_z) Representing the projection of the starting point S on the yz plane, E₁(E_y,E_z) Represents the projection point of the target end point S on the yz plane,

combining the formulas (1), (2) and (3) to obtain the coordinate of any point in the judgment plane,

s203, setting the coordinate P of a certain point in the judgment plane_x＝B_x，P_y＝B_yJudging an obstacle grid and a free grid in the two-dimensional grid through a formula (4), wherein:

if P_z＞B_zThe point P (P) on the plane F_x,P_y,P_z) The grid that is the center is a free grid,

if P_z＜B_zThe point P (P) on the plane F_x,P_y,P_z) The grid that is the center is the barrier grid,

B(B_x,B_y,B_z) Representing three-dimensional obstacle grid coordinates.

Further, the step S3 specifically includes the following steps:

s301, mapping the established two-dimensional grid model into a two-dimensional array, assigning the corresponding element of the barrier grid in the array to be 1, and assigning the corresponding element of the free grid in the array to be 0;

s302, adding the starting point into the OpenList, assigning the address of the starting point information structure body to the parent pointer of the current node,

the starting set is used for storing the information of the grid nodes to be selected;

s303, putting the current node into a closed set CloseList, wherein the closed set is used for storing the grid node information which is searched and selected and cannot be searched again;

s304, searching all free grids in the neighborhood of the current node in the two-dimensional array, calculating the value of a cost function of each node, comparing the value with the nodes in the opening set, if the node exists in the opening set, comparing the respective values of g (k), if the value of g (k) of the node is smaller, updating g (k) and f (k) in the opening set, pointing the parent pointer of the node stored in the opening set to the current node, if the node does not exist in the opening set, adding the node into the opening set, if a target end point is added into the opening set, executing a step S306, otherwise executing a step S305,

g (k), the path cost from the starting point to k of the node to be selected; (k) the total cost value of the node k to be selected;

the cost function f (n) is expressed by the following expression:

f(n)＝g(n)+h(n)，

g (n) represents the path cost from the starting point to the node n, g (n) is calculated using a formula based on minkowski distance, the cost for each direction is calculated to be different so that the path cost is not the same, h (n) represents the heuristic function from the node to the target point; h (n) denotes a Manhattan distance | x₁-x₂|+|y₁-y₂|；

S305, selecting a node with the minimum f (k) from the starting set, pointing a current node pointer to a current node, and executing S303;

and S306, starting from the target node, determining a final path by tracing the parent node pointer, finishing the global path planning, and outputting a path result.

Further, the step S4 specifically includes the following steps:

s401, reading the path point and judging whether the path point reaches a target end point, if so, ending the process, otherwise, executing S402;

s402, judging whether the environment in the current node field changes, if so, performing local dynamic path planning, executing S403, if not, returning to S401, continuing path point tracking,

wherein the dynamic path planning is shown by the following algorithm:

f(n)＝g(n)+h(n)+x(n)

g (n) represents the path cost from the starting point to the node n, g (n) is calculated using a formula based on minkowski distance, the cost for each direction is calculated to be different so that the path cost is not the same, h (n) represents the heuristic function from the node to the target point; h (n) denotes a Manhattan distance | x₁-x₂|+|y₁-y₂L, |; x (n) represents the newly planned node to the original static stateEuclidean distance of the planned path;

s403, after dynamic path planning, judging whether a global path is returned, if so, returning to S401, continuing path point tracking, and if not, continuing dynamic path planning until the global path is returned;

and after the path points are judged point by point, the target end point is reached, and the local dynamic path planning in the global path is completed.

An unmanned aerial vehicle is independently sought way and is kept away barrier system includes:

the acquisition unit is used for acquiring the position information of the path barrier;

the preprocessing unit is used for carrying out three-dimensional environment modeling on the acquired information and reducing the dimension of the acquired information into a two-dimensional grid model; a path planning unit comprising

The global planning module generates an optimal global reference path between a starting point and a final point in the two-dimensional grid by adopting an A-x algorithm;

the local planning module adopts an improved A-x algorithm with memoryless regression to perform local path optimal planning;

and the post-processing module tracks the planned line points by adopting a Bezier curve to ensure that the track is smooth.

The invention carries out three-dimensional environment modeling on the environment of the unmanned aerial vehicle, lowers the model through the judgment plane to obtain a two-dimensional grid model, carries out global path planning on the two-dimensional grid model based on the A-x algorithm, replans sudden dynamic environment change through local dynamic planning, and carries out smooth planning on planned line points through a Bessel curve, so that the unmanned aerial vehicle can find the change of the position of an obstacle caused by the dynamic change of the environment in time, the local dynamic planning has high response speed, good real-time performance and simple, convenient and quick calculation process, solves the problem that the traditional flight line is easy to touch the obstacle, and can be widely popularized in the fields of unmanned aerial vehicle path planning and track tracking based on the reasons.

Drawings

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

FIG. 1 is a flow chart of an autonomous path-finding and obstacle-avoiding method for an unmanned aerial vehicle according to the present invention;

FIG. 2 is a diagram of the autonomous path-finding and obstacle-avoiding device of the unmanned aerial vehicle of the invention;

FIG. 3 is a schematic diagram of the three-dimensional modeling of the present invention, wherein (a) is a schematic diagram of the case where the starting point is higher than the target end point, and (b) is a schematic diagram of the case where the starting point is lower than the target end point;

FIG. 4 is a schematic diagram of determining a planar cutting grid according to the present invention, wherein (a) is a schematic diagram of a case where a start point is higher than a target end point, and (b) is a schematic diagram of a case where a start point is lower than a target end point;

FIG. 5 is a schematic diagram of a two-dimensional grid model of the present invention, wherein (a) is a schematic diagram of a situation where a starting point is higher than a target end point, and (b) is a schematic diagram of a situation where a starting point is lower than a target end point;

FIG. 6 is a schematic diagram of a path search process for global static path planning in accordance with the present invention;

FIG. 7 is a flow chart of the path planning for local dynamic path planning in accordance with the present invention;

fig. 8 is a schematic diagram of the smooth planning of the route points by means of bezier curves according to the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, 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 some, but not all, embodiments of the present invention. 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.

As shown in fig. 1, an autonomous route finding and obstacle avoidance method for an unmanned aerial vehicle includes the following steps:

s2, modeling a three-dimensional environment through a grid method, dividing the three-dimensional environment into a plurality of grids, respectively processing grids containing obstacles and grids not containing obstacles in different colors, and dividing the grids through a judgment plane passing through a starting point and a target end point to obtain a two-dimensional grid model;

s3, making a global static path plan on the two-dimensional grid model based on an A-x algorithm, and searching an optimal global reference path between a starting point and a final point;

s4, after the global static path is planned, local dynamic path planning is carried out for returning to the originally planned path as soon as possible after the sudden dynamic environment change is processed;

and S5, tracking the planned line points through the Bezier curve, and finishing the smoothing processing of the path track.

As shown in fig. 2, an autonomous path-finding and obstacle-avoiding system for an unmanned aerial vehicle includes:

the acquisition unit is used for acquiring the position information of the path barrier;

the preprocessing unit is used for carrying out three-dimensional environment modeling on the acquired information and reducing the dimension of the acquired information into a two-dimensional grid model;

a path planning unit comprising

The global planning module generates an optimal global reference path between a starting point and a final point in the two-dimensional grid by adopting an A-x algorithm;

the local planning module adopts an improved A-x algorithm with memoryless regression to perform local path optimal planning;

and the post-processing module tracks the planned line points by adopting a Bezier curve to ensure that the track is smooth.

Embodiment 1, wherein step S1 includes: the position information of the obstacle under the sensor coordinate system is obtained through the laser and the ultrasonic sensor, the obstacle is converted into the unmanned aerial vehicle coordinate system through coordinate change, and the position of the obstacle under the inertial coordinate system is represented through the navigation system, so that the position information of the dynamic obstacle is obtained.

A planning space is divided into a plurality of basic units with regular shapes by adopting a unit decomposition modeling method, each basic unit is divided into nodes containing obstacles and nodes not containing obstacles, the nodes can visually describe environmental information and are convenient for storage of a computer, and meanwhile, each node is convenient to split and is convenient for processing and operation of a planning algorithm.

The grid method is used for modeling a three-dimensional environment, a three-dimensional planning space with a limited range is divided into equal-sized units, as shown in (a) and (b) of fig. 3, the three-dimensional space is divided into n × n × n grids, in the embodiment, 30 × 30 × 30 grids are adopted, each grid is a cube, the center of each grid is used as a calculation unit of a planning algorithm, an obstacle grid containing obstacles is assigned as 1, the grid is processed by dark color, a free grid not containing obstacles is assigned as 0, and the center of each unit grid is used as the coordinate of the grid by light color processing.

As shown in fig. 4(a) (b), a judgment plane F perpendicular to the yz plane is generated by passing through the starting point and the target end point, the judgment plane F includes the starting point S and the target end point E, and cuts through an obstacle grid higher than the judgment plane F;

as shown in fig. 5(a) (b), a two-dimensional grid of the xy plane is mapped to the judgment plane F, and the unit grid size of the judgment plane F is determined by the angle between the judgment plane F and the xy plane and the unit grid size of the xy plane. The figure includes an acute angle alpha formed by a judgment plane F and an xy plane, and an intersection C (C) of an extension line of a connecting line of a starting point and a target end point and a y axis_y，0)，

As shown in (a) of fig. 3, 4, and 5, when the level of the start point is higher than the level of the target end point:

as shown in (b) of fig. 3, 4, and 5, when the level of the start point is lower than the level of the target end point:

when the horizontal height of the starting point is higher than the horizontal height of the target end point and is consistent, the judgment plane is parallel to the xy plane,

P_z＝E_z＝S_z (3)

wherein, P₁(P_y,P_z) A projection point S representing the coordinate of any point P on the judgment plane on the yz plane₁(S_y,S_z) Representing the projection of the starting point S on the yz plane, E₁(E_y,E_z) Represents the projection point of the target end point S on the yz plane,

combining the formulas (1), (2) and (3) to obtain the coordinate of any point in the judgment plane,

s203, setting the coordinate P of a certain point in the judgment plane_x＝B_x，P_y＝B_yJudging an obstacle grid and a free grid in the two-dimensional grid through a formula (4), wherein:

if P_z＞B_zThe point P (P) on the plane F_x,P_y,P_z) The grid that is the center is a free grid,

if P_z＜B_zThe point P (P) on the plane F_x,P_y,P_z) The grid that is the center is the barrier grid,

B(B_x,B_y,B_z) Representing three-dimensional obstacle grid coordinates.

The essence of the A-algorithm is the combination of the greedy algorithm and the heuristic search algorithm, so the A-algorithm combines the advantages of the greedy algorithm and the heuristic search algorithm and inherits the characteristics of the greedy algorithm and the heuristic search algorithm. Compared with the greedy algorithm, the A algorithm has the advantages that the search efficiency is remarkably improved under the condition that the state space scale is large, compared with the optimal priority search algorithm, the A algorithm gradually tends to optimally search due to the combination of the 'optimization' property of the greedy algorithm, the defect that the optimal priority search algorithm is single in heuristic information is overcome, and theoretically, the optimal solution can be searched in the state space.

The core of the a-algorithm lies in the design of the cost function, and the general expression of the cost function is shown in formula (5).

f(n)＝g(n)+h(n) (5)

Wherein (n) is a cost function of each node, g (n) represents the cost from the starting point to the node n, h (n) represents a heuristic function from the node to the target point, and the heuristic function h (n) is designed with emphasis on different situations. When h (n) is designed to be less than or equal to the real minimum cost from the current node to the target end point, the A-algorithm can theoretically find the optimal path in a planning space, the real minimum cost from the current node to the target end point cannot be estimated and calculated in advance, and h (n) is required to be close to the real cost to the maximum extent, so that the accuracy of path finding of the A-algorithm is ensured, and h (n) is required to be elaborately designed by combining with the actual problem.

As shown in fig. 6, in the process of path planning by using the a-algorithm, it is assumed that the environment model is built by a grid method, and the a-algorithm calculates a cost value of a node to be selected in a state space of the grid model through a cost function, and stores the cost value in the open set. And after the A-algorithm searches the end point and stores the end point into the closing set, the parent node pointer of each node is used for backtracking, and the path searching process of the A-algorithm is completed.

h (n) selecting a manhattan distance having:

h(n)＝D*(|n.x-goal.x|+|n.y-goal.y|)

in the process of global path planning, the algorithm actually checks the information of the two-dimensional grid nodes after dimension reduction and calculates the information as a cost value, and in the process of software simulation, the information contained in each grid node is stored in the form of a structural body, so that a program can be conveniently accessed and called. The structure is shown in the following code.

And member variables g, f and h in the structure respectively represent g (), f (), h (), and farthenot is a node father pointer and points to the structure address of the father node of the current node.

As shown in fig. 7, the global path planning process on the dimension reduction model based on the a-x algorithm is as follows:

s301, mapping the established two-dimensional grid model into a two-dimensional array, assigning the corresponding element of the barrier grid in the array to be 1, and assigning the corresponding element of the free grid in the array to be 0;

s302, adding the starting point into the OpenList, assigning the address of the starting point information structure body to the parent pointer of the current node,

the starting set is used for storing the information of the grid nodes to be selected;

s303, putting the current node into a closed set CloseList, wherein the closed set is used for storing the grid node information which is searched and selected and cannot be searched again;

s304, searching all free grids in the neighborhood of the current node in the two-dimensional array, calculating the value of a cost function of each node, comparing the value with the nodes in the opening set, if the node exists in the opening set, comparing the respective values of g (k), if the value of g (k) of the node is smaller, updating g (k) and f (k) in the opening set, pointing the parent pointer of the node stored in the opening set to the current node, if the node does not exist in the opening set, adding the node into the opening set, if a target end point is added into the opening set, executing a step S306, otherwise executing a step S305,

g (k), the path cost from the starting point to k of the node to be selected; (k) the total cost value of the node k to be selected;

the cost function f (n) is expressed by the following expression:

f(n)＝g(n)+h(n)，

g (n) represents the path cost from the starting point to the node n, g (n) is calculated using a formula based on minkowski distance, the cost for each direction is calculated to be different so that the path cost is not the same, h (n) represents the heuristic function from the node to the target point; h (n) denotes a Manhattan distance | x₁-x₂|+|y₁-y₂|；

S305, selecting a node with the minimum f (k) from the starting set, pointing a current node pointer to a current node, and executing S303;

and S306, starting from the target node, determining a final path by tracing the parent node pointer, finishing the global path planning, and outputting a path result.

As shown in fig. 7, the dynamic path planning process,

s401, reading the path point and judging whether the path point reaches a target end point, if so, ending the process, otherwise, executing S402;

s402, judging whether the environment in the current node field changes, if so, performing local dynamic path planning, executing S403, if not, returning to S401, continuing path point tracking,

wherein the dynamic path planning is shown by the following algorithm:

f(n)＝g(n)+h(n)+x(n)

g (n) represents the path cost from the starting point to the node n, g (n) is calculated using a formula based on minkowski distance, the cost for each direction is calculated to be different so that the path cost is not the same, h (n) represents the heuristic function from the node to the target point; h (n) denotes a Manhattan distance | x₁-x₂|+|y₁-y₂L, |; x (n) tableShowing the Euclidean distance from the newly planned node to the originally statically planned path;

s403, after dynamic path planning, judging whether a global path is returned, if so, returning to S401, continuing path point tracking, and if not, continuing dynamic path planning until the global path is returned;

and after the path points are judged point by point, the target end point is reached, and the local dynamic path planning in the global path is completed.

Because the traditional obstacle avoidance method directly avoids obstacles along a straight line after detecting the obstacles, and is easy to collide with an unmanned aerial vehicle to cause task failure, the method adopts a track planning method based on a Bezier curve to track the track bypassing the obstacles.

Second order Bezier curve, as shown in FIG. 8, by bypassing P₀，P₁，P₂A bezier curve composed of these three points:

B(t)＝(1-t)²P₀+2t(1-t)P₁+t²P₂,t∈[0,1] (6)

general formula for the third order bezier curve:

the method carries out trajectory tracking on the line points planned in the front in a Bezier curve mode, so that the trajectory is smoother and more controllable, and the method has excellent applicability.

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 (6)

Translated fromChinese

1.一种无人机自主寻路避障方法，其特征在于，包括：如下步骤：1. an unmanned aerial vehicle autonomous way-finding and obstacle-avoiding method, is characterized in that, comprises: the following steps:S1、采集障碍物的位置信息；S1. Collect location information of obstacles;S2、通过栅格法进行三维环境建模，并将其分割成若干个栅格，分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理，通过过起始点及目标终点的判断平面分割栅格，得到二维栅格模型；S2. Use the grid method to model the 3D environment, and divide it into several grids. The grids containing obstacles and the grids that do not contain obstacles are processed in different colors. By passing the starting point and the target The judgment plane of the end point divides the grid to obtain a two-dimensional grid model;S3、基于A*算法在所述二维栅格模型上做出全局静态路径规划，用于寻找起始点与最终点之间的最优的全局参考路径；S3, based on the A* algorithm, make a global static path planning on the two-dimensional grid model, for finding the optimal global reference path between the starting point and the final point;S4、在全局静态路径规划后，进行局部动态路径规划，用于在处理完突发的动态环境变化后，尽快回归至原先规划好的路径；S4. After the global static path planning, perform local dynamic path planning, which is used to return to the originally planned path as soon as possible after processing the sudden dynamic environment change;S5、通过贝塞尔曲线对规划出的线路点进行轨迹跟踪，完成对路径轨迹的平滑处理；S5. Track the planned route points through the Bezier curve to complete the smoothing of the path trajectory;动态路径规划如下算法所示：Dynamic path planning is shown in the following algorithm:f(n)＝g(n)+h(n)+x(n)f(n)=g(n)+h(n)+x(n)g(n)表示起始点到节点n的路径代价，g(n)采用一种基于闵可夫斯基距离的公式进行计算，计算各个方向的代价不同从而使得路径代价不一样，h(n)表示从节点到目标点的启发函数；h(n)表示的是曼哈顿距离|x₁-x₂|+|y₁-y₂|；g(n) represents the path cost from the starting point to node n, and g(n) is calculated using a formula based on Minkowski distance. The heuristic function from the node to the target point; h(n) represents the Manhattan distance |x₁ -x₂ |+|y₁ -y₂ |;x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离。x(n) represents the Euclidean distance from the newly planned node to the original statically planned path.2.根据权利要求1所述的方法，其特征在于，所述步骤S1具体包括：2. The method according to claim 1, wherein the step S1 specifically comprises:S101、通过传感器获取障碍物在传感器坐标系下的位置信息；S101, obtaining the position information of the obstacle in the sensor coordinate system through the sensor;S102、通过坐标变换将所述传感器坐标系下的位置信息转换到无人机坐标系下；S102, converting the position information in the sensor coordinate system to the UAV coordinate system through coordinate transformation;S103、通过导航解算将无人机坐标系下的位置信息导入惯性坐标系下，获得动态的障碍物位置信息。S103 , importing the position information in the UAV coordinate system into the inertial coordinate system through the navigation solution to obtain dynamic obstacle position information.3.根据权利要求2所述的方法，其特征在于，所述通过过起始点及目标终点的判断平面分割栅格，得到二维栅格模型具体包括：3. The method according to claim 2, wherein the grid is divided into the grid by the judgment plane passing through the starting point and the target end point, and the obtained two-dimensional grid model specifically comprises:S201、过起始点及目标终点，生成垂直于yz平面的判断平面；S201, generating a judgment plane perpendicular to the yz plane through the starting point and the target end point;S202、将xy平面的二维栅格映射至所述判断平面，S202, map the two-dimensional grid of the xy plane to the judgment plane,当起始点的水平高度高于目标终点的水平高度时：When the horizontal height of the starting point is higher than the horizontal height of the target end point:

当起始点的水平高度低于目标终点的水平高度时：When the horizontal height of the starting point is lower than the horizontal height of the target end point:

当起始点的水平高度高于目标终点的水平高度一致时，判断平面平行于xy平面，When the horizontal height of the starting point is higher than the horizontal height of the target end point, the judgment plane is parallel to the xy plane,P_z＝E_z＝S_z (3)P_z =E_z =S_z (3)其中，P₁(P_y,P_z)表示判断平面上任一点P的坐标在yz平面的投影点，S₁(S_y,S_z)表示起始点S在yz平面的投影点，E₁(E_y,E_z)表示目标终点S在yz平面的投影点，Among them, P₁ (P_y , P_z ) represents the projection point of the coordinates of any point P on the judgment plane on the yz plane, S₁ (S_y , S_z ) represents the projection point of the starting point S on the yz plane, E₁ (E_y , E_z ) represents the projection point of the target end point S on the yz plane,结合公式(1)(2)(3)得到判断平面中任一点的坐标，Combining formulas (1) (2) (3) to obtain the coordinates of any point in the judgment plane,

S203、设判断平面中某点坐标P_x＝B_x，P_y＝B_y，通过公式(4)判断二维栅格中的障碍物栅格和自由栅格，其中：S203, set the coordinates of a certain point in the judgment plane P_x =B_x , P_y =B_y , judge the obstacle grid and the free grid in the two-dimensional grid by formula (4), where:若P_z＞B_z，则平面F上以点P(P_x,P_y,P_z)为中心的栅格为自由栅格，If P_z >B_z , the grid on the plane F with the point P (P_x , P_y , P_z ) as the center is a free grid,若P_z＜B_z，则平面F上以点P(P_x,P_y,P_z)为中心的栅格为障碍物栅格，If P_z <B_z , the grid on the plane F with the point P (P_x , P_y , P_z ) as the center is the obstacle grid,B(B_x,B_y,B_z)表示三维障碍物栅格坐标。B(B_x , By_y , B_z ) represents the three-dimensional obstacle grid coordinates.4.根据权利要求3所述的方法，其特征在于，所述步骤S3具体包括如下步骤：4. The method according to claim 3, wherein the step S3 specifically comprises the following steps:S301、将建立的二维栅格模型映射到二维数组中，将所述障碍物栅格在该数组中对应的元素被赋值为1，自由栅格在数组中的对应元素被赋值为0；S301, mapping the established two-dimensional grid model to a two-dimensional array, assigning the corresponding element of the obstacle grid in the array to 1, and assigning the corresponding element of the free grid to 0 in the array;S302、将起始点加入开启集OpenList中，并将起始点信息结构体的地址赋值给当前节点父指针，S302, adding the starting point to the open set OpenList, and assigning the address of the starting point information structure to the parent pointer of the current node,其中，所述开启集用于存储待选栅格节点信息；Wherein, the open set is used to store the grid node information to be selected;S303、将当前节点放入关闭集CloseList中，所述关闭集用于存储已经搜索选择过的、不能被再次搜索的栅格节点信息；S303, put the current node into the closed set CloseList, and the closed set is used to store the grid node information that has been searched and selected and cannot be searched again;S304、在二维数组中搜索当前节点邻域内的所有自由栅格计算各节点的代价函数的值，并与开启集中的节点进行比较，若开启集已经存在该节点，则比较各自的g(k)值，如果该节点的g(k)值更小，则将开启集中的g(k)与f(k)更新，并将开启集中所存储该节点的父指针指向当前节点，若开启集中没有该节点，则将该节点加入开启集，如果目标终点被加入开启集中，则执行步骤S306，否则执行步骤S305，S304. Search all free grids in the neighborhood of the current node in the two-dimensional array, calculate the value of the cost function of each node, and compare it with the nodes in the open set. If the node already exists in the open set, compare the respective g(k ) value, if the value of g(k) of the node is smaller, the g(k) and f(k) in the open set will be updated, and the parent pointer of the node stored in the open set will point to the current node. the node, add the node to the open set, if the target end point is added to the open set, go to step S306, otherwise go to step S305,其中，g(k)，起始点到待选节点的k的路径代价；f(k)，待选节点k的总代价值；Among them, g(k) is the path cost from the starting point to the node to be selected k; f(k) is the total cost of the node to be selected k;所述代价函数f(n)通过如下表达式表述：The cost function f(n) is expressed by the following expression:f(n)＝g(n)+h(n)，f(n)=g(n)+h(n),g(n)表示起始点到节点n的路径代价，g(n)采用一种基于闵可夫斯基距离的公式进行计算，计算各个方向的代价不同从而使得路径代价不一样，h(n)表示从节点到目标点的启发函数；h(n)表示的是曼哈顿距离|x₁-x₂|+|y₁-y₂|；g(n) represents the path cost from the starting point to node n, and g(n) is calculated using a formula based on Minkowski distance. The heuristic function from the node to the target point; h(n) represents the Manhattan distance |x₁ -x₂ |+|y₁ -y₂ |;S305、从开启集中选取具有最小f(k)的节点，将当前节点指针指向当前节点，执行S303；S305, select the node with the smallest f(k) from the open set, point the current node pointer to the current node, and execute S303;S306、从目标节点开始，通过追溯父节点指针确定最终路径，全局路径规划完成，输出路径结果。S306. Starting from the target node, determine the final path by tracing back the pointer of the parent node, complete the global path planning, and output the path result.5.根据权利要求4所述的方法，其特征在于，所述步骤S4具体包括如下步骤：5. The method according to claim 4, wherein the step S4 specifically comprises the following steps:S401、读取路径点并判断是否到达目标终点，若是，则结束流程，若否，则执行S402；S401, read the waypoint and judge whether the target end point is reached, if so, end the process, if not, execute S402;S402、判断当前节点领域内环境是否改变，若是，则进行局部动态路径规划，执行S403，若否，则返回S401，继续进行路径点跟踪，S402, determine whether the environment in the current node field has changed, if so, perform local dynamic path planning, and execute S403, if not, return to S401, and continue to perform path point tracking,S403、动态路径规划后，判断是否回归全局路径，若是，则返回S401，继续进行路径点跟踪，若否，则继续进行动态路径规划，直到回归到全局路径中；S403, after the dynamic path planning, determine whether to return to the global path, and if so, return to S401, and continue to perform path point tracking, if not, continue to perform dynamic path planning until returning to the global path;路径点逐点判断后，到达目标终点，完成全局路径中局部动态路径规划。After the path point is judged point by point, the target end point is reached, and the local dynamic path planning in the global path is completed.6.一种权利要求1所述无人机自主寻路避障方法的系统，包括：6. A system of the described unmanned aerial vehicle autonomous path-finding and obstacle-avoiding method of claim 1, comprising:采集单元，用于采集路径障碍物的位置信息；a collection unit for collecting the position information of obstacles in the path;预处理单元，用于对采集的信息进行三维环境建模并降维成二维栅格模型；The preprocessing unit is used to model the collected information in 3D environment and reduce the dimension into a 2D grid model;路径规划单元，包括Path planning unit, including其中，全局规划模块，采用A*算法在二维栅格生成起始点与最终点之间的最优的全局参考路径；Among them, the global planning module uses the A* algorithm to generate the optimal global reference path between the starting point and the final point in a two-dimensional grid;局部规划模块，采用改进并具有无记忆回归的A*算法进行局部路径最优规划；The local planning module adopts the improved A* algorithm with memoryless regression for local path optimal planning;后期处理模块，采用贝塞尔曲线对规划出的线路点进行轨迹跟踪，使轨迹平滑；The post-processing module uses the Bezier curve to track the planned route points to make the trajectory smooth;无记忆回归的A*算法如下所示：The A* algorithm for memoryless regression is as follows:f(n)＝g(n)+h(n)+x(n)f(n)=g(n)+h(n)+x(n)g(n)表示起始点到节点n的路径代价，g(n)采用一种基于闵可夫斯基距离的公式进行计算，计算各个方向的代价不同从而使得路径代价不一样，h(n)表示从节点到目标点的启发函数；h(n)表示的是曼哈顿距离|x₁-x₂|+|y₁-y₂|；g(n) represents the path cost from the starting point to node n, and g(n) is calculated using a formula based on Minkowski distance. The heuristic function from the node to the target point; h(n) represents the Manhattan distance |x₁ -x₂ |+|y₁ -y₂ |;x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离。x(n) represents the Euclidean distance from the newly planned node to the original statically planned path.

Images