CN105867421A - 一种基于ph曲线的无人机路径规划方法 - Google Patents

Abstract

本发明公开了一种基于PH曲线的无人机路径规划方法，包括：(1)初始化无人机运动条件和任务地图；(2)根据初始条件设定多个路径控制点，并根据控制点形成PH曲线，并将其表示为贝塞尔n阶多项式；(3)将贝塞尔n阶多项式限定为五阶多项式，并据此对其进行求导运算和两点间插值运算，确定出无人机路径曲线形状，并计算出相应控制点位置参数；(4)调整回归系数以改变切矢量的长度，以满足PH曲线约束条件和无人机约束条件，从而实现对控制点的修正，获得可规避障碍物的无人机路径。本发明的方法可以解决现有技术中存在的无法规避路径中障碍物、路径规划精确度低、路径不满足无人机运动学约束的问题。

Description

一种基于PH曲线的无人机路径规划方法

技术领域

本发明属于无人机路径规划技术领域，尤其是涉及一种无人机避障飞行路径规划方法。

背景技术

无人机(Unmanned Aerial Vehicle)，通常是指利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机。与载人飞机相比，它具有体积小、造价低、使用方便、对操作环境要求低、生存能力较强等特性，这些特有的优越性能使其在应用方面具有很大的发展潜力。军事上可应用于情报获取、地面战场侦察和监视、近距离空中支持和禁飞巡逻、电子战、通信中继等方面，在民用方面一样大有可为，如重大自然灾害之后的搜索与救援、巡逻监视和目标跟踪、辑毒和反走私、高压线/大桥/水坝和地震后路段的检查、航拍和成图等方面。

无人机路径规划的目的是在满足特定的约束条件下，寻找初始位置和目的地之间的一条最优或接近最优的飞行路径。根据无人机对环境信息掌握的程度不同，路径规划可以分为基于模型的全局路径规划和基于传感器的局部路径规划。其中，全局规划方法主要包括构型空间法、拓扑法、栅格解耦法、自由空间法、神经网络法等。

专利文献CN 104808688A公开了一种无人机曲率连续可调路径规划方法，包括以下步骤：第一步，路径曲率连续化，利用参数化的Catmull-Rom曲线连接各路标点，通过优化插值方法保证连结点处曲率连续；第二步，计算路径曲线各点曲率值，查找并标注曲率值超过规定阈值的曲线起点和终点；第三步，采用最小曲率圆转移法，利用曲率值单调变化的Bezier曲线连接起点和终点。与现有技术相比，提出了一种经过所有路标点的平滑路径规划方法，采用参数化的Catmull-Rom曲线，能够确保路径经过所有路标点，同时通过优化插值算法，保证路径各点曲率连续，而且利用曲率单调平滑连接算法，控制路径曲率极值范围，使路径在满足无人机运动学条件前提下，具有精确性和可行性。

但是上述路径规划方法对路径中障碍物的规避问题涉及不多，而且其规划精度仍有提高空间。

发明内容

本发明的目的是提供一种无人机避障飞行路径规划方法，其利用PH曲线实现对路径的优化，解决现有技术中存在的无法规避路径中障碍物、路径规划精确度低、路径不满足无人机运动学约束的问题。

为实现上述目的，按照本发明的一个方面，提供一种基于PH曲线的无人机路径规划方法，包括：

(1)初始化无人机运动条件和任务地图，包括设定无人机路径的起始点、终点以及沿途中需要经过的目标点，标注各障碍物的位置坐标和威胁半径，设定无人机的最大飞行速度、最大飞行加速度以及最大飞行曲率；

(2)根据初始条件设定多个路径控制点，并根据所述控制点形成PH曲线，并将其表示为贝塞尔n阶多项式；

(3)根据最低阶含有拐点的PH曲线为五阶PH曲线，将所述贝塞尔n阶多项式限定为五阶多项式，并据此对其进行求导运算和两点间插值运算，确定出无人机路径曲线形状，并计算出相应控制点位置参数；

(4)调整回归系数以改变切矢量的长度，以满足PH曲线约束条件和无人机约束条件，实现对控制点的修正，从而获得可规避障碍物的无人机路径。

作为本发明的进一步优选，所述贝塞尔n阶多项式为：

$r\left(q\right)=\underset{k=0}{\overset{n}{\Σ}}{p}_k\left(\begin{array}{c}n\\ k\end{array}\right)q^k{\left(1-q\right)}^{\left(n-k\right)},q\∈\[0,1\]$

式中，r(q)|_q＝0和r(q)|_q＝1分别表示路径的起点和终点，q是被正规化后的路径参数，k＝0，1，2，…，n。

作为本发明的进一步优选，所述步骤(3)中，对应所述五阶PH曲线的控制点为六个，包括路径起点p₀、终点p₅以及位于起点与终点之间从该起点至终点依次排布的四个控制点p₁，p₂，p₃和p₄，该步骤(3)具体过程如下：

(3.1)将无人机路径起点和终点位置切线方向的正切向量和表示成切矢量形式，切矢量分别用回归系数c₀和c₁修正：

其中，c₀和c₁为回归系数，c₀∈[1，∞]，c₁∈[1，∞]，和分别为初始位置和终止位置的切向角，d₀和d₅分别为路径起点和终点位置切线方向的切矢量。

(3.2)将q＝0和q＝1时得到的初始位置和目标位置坐标带入所述五阶多项式并进行一阶求导，得到控制点p₁和p₄的位置参数：

$p_1=p_0+\frac{1}{5}{d}_0$

$p_4=p_5+\frac{1}{5}{d}_5$

作为本发明的进一步优选，所述步骤(4)中，对控制点的修正包括对控制点p₂和p₃的修正，其中，所述控制点p₂和p₃的数学表达式为：

$p_2=p_1+\frac{1}{5}(u_0{u}_1-v_0{v}_1,u_0{v}_1+u_1{v}_0)$

$p_3=p_2+\frac{2}{15}({u_1}^2-{v_1}^2,2u_1{v}_1)$

而

$\begin{array}{c}(u_1,v_1)=-\frac{3}{4}\left(u_0+u_2,v_0+v_2\right)\\ \±\sqrt{\frac{1}{2}}\left(\sqrt{c+a},sign\left(b\right)\sqrt{c-a}\right)\end{array}$

其中:

作为本发明的进一步优选，控制点p₁，p₄，p₅的位置参数的数学表达式如下；

$p_1=p_0+\frac{1}{5}({u_0}^2-{v_0}^2,2u_0{v}_0)$

$p_4=p_3+\frac{1}{5}(u_1{u}_2-v_1{v}_2,u_1{v}_2+u_2{v}_1)$

$p_5=p_4+\frac{1}{5}({u_2}^2-{v_2}^2,2u_2{v}_2).$

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：本发明中通过PH曲线具体为五阶PH曲线进行无人机路径规划，并通过控制点的求导和插值运算，确定路径曲线形状，同时结合调整回归系数以改变切矢量的长度，以满足PH曲线约束条件和无人机约束条件，从而实现对控制点的修正，获得可规避障碍物的无人机路径，具有精度高，能够规避障碍物的优势。

附图说明

图1：为本发明实施例的方法流程图；

图2：为本发明实施例的贝塞尔曲线图；

图3：为本发明实施例的和声算法流程图；

图4：为本发明实施例的碰撞条件判据图；

图5：为本发明实施例具体实施例示意图；

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅限于说明和解释本发明，并不用于限定本发明。

如图1，本发明实施例所提供的一种无人机基于PH曲线和和声搜索算法进行规避障碍物的路径规划方法，包括以下步骤：

(1)初始化无人机运动条件和任务地图，标注各障碍物的位置坐标和威胁半径，设定无人机的最大飞行速度、最大飞行加速度、最大飞行曲率；

该步骤中，具体包括初始化地图，设定无人机在该次任务中的起始点、终点以及沿途中需要经过的目标点，根据无人机运动学约束，设定无人机飞行的高度，最大飞行速度，最大飞行加速度，最大飞行曲率等。

设定飞行过程中无人机在指定高度匀速飞行。

(2)根据初始条件给定多个控制点p₀,p₁,…,p_n，根据相应控制点形成对应的PH曲线，并表示成Bézier形式下的n阶多项式。

本步骤中，根据给定的飞行任务，飞行地图和运动学约束条件设定控制点，并根据控制点写出Bézier样条曲线，如图2，其具体实现包括以下步骤：

(2.1)根据无人机飞行条件设定控制点p_k＝(x_k，y_k)，k＝0，，1，2，…，n，x_k，y_k分别表示控制点二维坐标。控制点用PH曲线表示，并表达为Bézier形式下的n阶多项式：

$r\left(q\right)=\underset{k=0}{\overset{n}{\Σ}}{p}_k\left(\begin{array}{c}n\\ k\end{array}\right)q^k{\left(1-q\right)}^{\left(n-k\right)},q\∈\[0,1\]---\left(1\right)$

式中：r(q)|_q＝0和r(q)|_q＝1分别表示路径的起点和终点，q是被正规化后的路径参数。

$\left(\begin{array}{c}n\\ k\end{array}\right)=\frac{n!}{k!(n-k)!}---\left(2\right)$

(2.2)利用求导公式求得n阶Bézier曲线的第r阶导数，可以写成如下形式：

$\frac{d^{r}r\left(q\right)}{{\mathrm{dq}}^r}=\frac{n!}{(n-r)!}\underset{j=0}{\overset{n-r}{\Σ}}{\mathrm{\Δ}}^r{p}_j\left(\begin{array}{c}n-r\\ j\end{array}\right)q^j{(1-q)}^{n-r-j}---\left(3\right)$

其中：

Δ^rp_j＝p_j+1-p_j，Δ⁰p_j＝p_j。

(2.3)在路径规划中，最低阶含有拐点的PH曲线是五次PH曲线，即n＝5。取6个控制点：p₀(x₀，y₀)，p₁(x₁，y₁)，p₂(x₂，y₂)，p₃(x₃，y₃)，p₄(x₄，y₄)，p₅(x₅，y₅)，当n＝5时等式(1)变为如下形式：

$r\left(q\right)=\underset{k=0}{\overset{5}{\Σ}}{p}_k\left(\begin{array}{c}5\\ k\end{array}\right)q^k{\left(1-q\right)}^{\left(5-k\right)}---\left(4\right)$

(2.4)将五阶Bézier曲线表示成由控制点组成时的形式，控制点之间的数学表达式如下；

$\begin{array}{c}{p}_1=p_0+\frac{1}{5}\left({u_0}^2-{v_0}^2,2u_0{v}_0\right)\\ p_2=p_1+\frac{1}{5}\left(u_0{u}_1-v_0{v}_1,u_0{v}_1+u_1{v}_0\right)\\ p_3=p_2+\frac{2}{15}\left({u_1}^2-{v_1}^2,2u_1{v}_1\right)\\ +\frac{1}{15}\left(u_0{u}_2-v_0{v}_2,u_0{v}_2+u_2{v}_0\right)\\ p_4=p_3+\frac{1}{5}\left(u_1{u}_2-v_1{v}_2,u_1{v}_2+u_2{v}_1\right)\\ p_5=p_4+\frac{1}{5}({u_2}^2-{v_2}^2,2u_2{v}_2\end{array}---\left(5\right)$

其中，p_k＝(x_k，y_k)，k＝0，，1，2，…，n，x_k，y_k分别表示控制点二维坐标，u₀，u₁，u₂，v₀，v₁，v₂为互素多项式，用这些多项式来构造PH曲线。

(2.5)根据等式(3)求出五阶PH路径的一阶导数，用于之后的插值运算。路径r(q)的一阶导数为：

$\frac{d^{r}r\left(q\right)}{dq}=5\underset{j=0}{\overset{4}{\Σ}}{\mathrm{\Δ}}^1{p}_j{q}^j{(1-q)}^{4-j}---\left(6\right)$

(3)对5阶PH多项式作求导运算和两点间插值运算，确定连接曲线形状，计算得出控制点p₀、p₁、p₅、p₄位置参数。

见图3，依托只能算法的优势，利用和声搜索算法进行反复迭代搜索，直到找到最优的位置参数。和声搜索算法是业内成熟的算法，在此不再赘述。

(3.1)将无人机路径起点和终点位置切线方向的正切向量和表示成切矢量形式，切矢量分别用回归系数c₀和c₁修正，c₀∈[1，∞]，c₁∈[1，∞]；

(3.2)将q＝0和q＝1时得到的初始位置和目标位置坐标带入到等式(4)和(6)求路径的一阶导数，得到控制点p₁和p₄，计算公式如下所示：

$\begin{array}{c}{p}_1=p_0+\frac{1}{5}{d}_0\\ p_4=p_5+\frac{1}{5}{d}_5\end{array}---\left(8\right)$

(4)调整曲线参数增加切矢量和的长度，修正控制点p₂和p₃进行规避障碍物，并同时满足PH曲线约束条件和无人机飞行约束条件。

当无人机路径上的点到障碍物中心的距离小于威胁半径时，这判断无人机将于障碍物碰撞，反之则证明无人机路径安全，如图4所示。

(4.1)结合等式(5)和等式(8)计算得到(u₀，v₀)和(u₂，v₂)，计算结果如下式所示；

(4.2)将(u₀，v₀)和(u₂，v₂)带入到公式(5)可以求得(u₁，v₁)的表达式；

$\begin{array}{c}(u_1,v_1)=-\frac{3}{4}\left(u_0+u_2,v_0+v_2\right)\\ \±\sqrt{\frac{1}{2}}\left(\sqrt{c+a},sign\left(b\right)\sqrt{c-a}\right)\end{array}---\left(10\right)$

其中:

(4.3)将(u₀，v₀)、(u₁，v₁)和(u₂，v₂)带入到公式(5)求得控制点p₂和p₃；

(4.4)将得到的6个控制点代入PH曲线的表达式，即下面公式(11)，调整回归系数c₀和c₁的大小，改变起始点和终止点的切矢量长度，使路径规避障碍物，并得到最终规划好的无人机可飞行路径，完成路径规划。

$\begin{array}{c}\stackrel{`}{x\left(q\right)}=u{\left(q\right)}^2-v{\left(q\right)}^2\\ \stackrel{`}{y\left(q\right)}=2u\left(q\right)v\left(q\right)\end{array}---\left(11\right)$

如图5，在一个具体实施例中，无人机任务的起始点坐标和终点目标分别为(0，0)，(100,100)，测试地图大小120*120m。障碍物1的位置中心坐标为(35,40)，威胁半径大小为10m,障碍物2的位置中心坐标为(35,40)，威胁半径大小为20m。图中点画线和虚线为路径规划调整过程中的路径，都不能规避障碍物，只有实线满足无人机可飞行条件，并能规避障碍物。

应当理解，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或者变形，均落入本发明的保护范围之内，本发明的请求保护范围应所附权利要求为准。

Claims (5)

1.一种基于PH曲线的无人机路径规划方法，包括：

(1)初始化无人机运动条件和任务地图，包括设定无人机路径的起始点、终点以及沿途中需要经过的目标点，标注各障碍物的位置坐标和威胁半径，设定无人机的最大飞行速度、最大飞行加速度以及最大飞行曲率；

(2)根据初始条件设定多个路径控制点，并根据所述控制点形成PH曲线，并将其表示为贝塞尔n阶多项式；

(3)根据最低阶含有拐点的PH曲线为五阶PH曲线，将所述贝塞尔n阶多项式限定为五阶多项式，并据此对其进行求导运算和两点间插值运算，确定出无人机路径曲线形状，并计算出相应控制点位置参数；

(4)调整回归系数以改变切矢量的长度，以满足PH曲线约束条件和无人机约束条件，实现对控制点的修正，从而获得可规避障碍物的无人机路径。

2.根据权利要求1所述的一种基于PH曲线的无人机路径规划方法，其中，所述贝塞尔n阶多项式为：

$r\left(q\right)=\underset{k=0}{\overset{n}{\mathrm{\Σ}}}{p}_k\left(\begin{array}{c}n\\ k\end{array}\right)q^k{\left(1-q\right)}^{\left(n-k\right)},q\∈\[0,1\]$

式中，r(q)|_q＝0和r(q)|_q＝1分别表示路径的起点和终点，q是被正规化后的路径参数，k＝0，1，2，…，n。

3.根据权利要求1或2所述的一种基于PH曲线的无人机路径规划方法，其中，所述步骤(3)中，对应所述五阶PH曲线的控制点为六个，包括路径起点p₀、终点p₅以及位于起点与终点之间从该起点至终点依次排布的四个中间控制点p₁，p₂，p₃和p₄，该步骤(3)具体过程如下：

(3.1)将无人机路径起点和终点位置切线方向的正切向量和表示成切矢量形式，切矢量分别用回归系数c₀和c₁修正：

其中，c₀和c₁为回归系数，c₀∈[1，∞]，c₁∈[1，∞]，和分别为初始位置和终止位置的切向角，d₀和d₅分别为路径起点和终点位置切线方向的切矢量；

(3.2)将q＝0和q＝1时得到的初始位置和目标位置坐标带入所述五阶多项式并进行一阶求导，得到中间控制点p₁和p₄的位置参数：

$p_1=p_0+\frac{1}{5}{d}_0$

$p_4=p_5+\frac{1}{5}{d}_5$

4.根据权利要求3所述的一种基于PH曲线的无人机路径规划方法，其中，中间控制点p₂和p₃的数学表达式为：

$p_2=p_1+\frac{1}{5}(u_0{u}_1-v_0{v}_1,u_0{v}_1+u_1{v}_0)$

$p_3=p_2+\frac{2}{15}({u_1}^2-{v_1}^2,2u_1{v}_1)$

而

$\begin{array}{c}(u_1,v_1)=-\frac{3}{4}\left(u_0+u_2,v_0+v_2\right)\\ \±\sqrt{\frac{1}{2}}(\sqrt{c+a},sign(b)\sqrt{c-a})\end{array}$

其中，a,b,c为中间参数，且

$\left\{\begin{array}{c}a=\frac{9}{16}\left({u_0}^2-{v_0}^2+{u_2}^2-{v_2}^2\right)+\frac{5}{8}\left(u_0{u}_2-v_0{v}_2\right)+\frac{15}{2}\left(x_4-x_1\right)\\ b=\frac{9}{8}\left(u_0{v}_0+u_2{v}_2\right)+\frac{5}{8}\left(u_0{u}_2+v_0{v}_2\right)+\frac{15}{2}\left(y_4-y_1\right)\\ c=\sqrt{a^2+b^2}\end{array}\right.$

5.根据权利要求1或2所述的一种基于PH曲线的无人机路径规划方法，其中，其中，中间控制点p₁，p₄，p₅的位置参数的数学表达式如下；

$p_1=p_0+\frac{1}{5}({u_0}^2-{v_0}^2,2u_0{v}_0)$

$p_4=p_3+\frac{1}{5}(u_1{u}_2-v_1{v}_2,u_1{v}_2+u_2{v}_1)$

$p_5=p_4+\frac{1}{5}({u_2}^2-{v_2}^2,2u_2{v}_2).$