一种基于ph曲线的无人机路径规划方法
【专利摘要】本发明公开了一种基于PH曲线的无人机路径规划方法，包括：(1)初始化无人机运动条件和任务地图；(2)根据初始条件设定多个路径控制点，并根据控制点形成PH曲线，并将其表示为贝塞尔n阶多项式；(3)将贝塞尔n阶多项式限定为五阶多项式，并据此对其进行求导运算和两点间插值运算，确定出无人机路径曲线形状，并计算出相应控制点位置参数；(4)调整回归系数以改变切矢量的长度，以满足PH曲线约束条件和无人机约束条件，从而实现对控制点的修正，获得可规避障碍物的无人机路径。本发明的方法可以解决现有技术中存在的无法规避路径中障碍物、路径规划精确度低、路径不满足无人机运动学约束的问题。
【专利说明】
一种基于PH曲线的无人机路径规划方法
技术领域
[0001] 本发明属于无人机路径规划技术领域，尤其是涉及一种无人机避障飞行路径规划 方法。
【背景技术】
[0002] 无人机(Unmanned Aerial Vehicle)，通常是指利用无线电遥控设备和自备的程 序控制装置操纵的不载人飞机。与载人飞机相比，它具有体积小、造价低、使用方便、对操作 环境要求低、生存能力较强等特性，这些特有的优越性能使其在应用方面具有很大的发展 潜力。军事上可应用于情报获取、地面战场侦察和监视、近距离空中支持和禁飞巡逻、电子 战、通信中继等方面，在民用方面一样大有可为，如重大自然灾害之后的搜索与救援、巡逻 监视和目标跟踪、辑毒和反走私、高压线/大桥/水坝和地震后路段的检查、航拍和成图等方 面。
[0003] 无人机路径规划的目的是在满足特定的约束条件下，寻找初始位置和目的地之间 的一条最优或接近最优的飞行路径。根据无人机对环境信息掌握的程度不同，路径规划可 以分为基于模型的全局路径规划和基于传感器的局部路径规划。其中，全局规划方法主要 包括构型空间法、拓扑法、栅格解耦法、自由空间法、神经网络法等。
[0004] 专利文献CN 104808688A公开了一种无人机曲率连续可调路径规划方法，包括以 下步骤:第一步，路径曲率连续化，利用参数化的CatmulΙ-Rom曲线连接各路标点，通过优化 插值方法保证连结点处曲率连续;第二步，计算路径曲线各点曲率值，查找并标注曲率值超 过规定阈值的曲线起点和终点；第三步，采用最小曲率圆转移法，利用曲率值单调变化的 Bezier曲线连接起点和终点。与现有技术相比，提出了一种经过所有路标点的平滑路径规 划方法，采用参数化的Catmul Ι-Rom曲线，能够确保路径经过所有路标点，同时通过优化插 值算法，保证路径各点曲率连续，而且利用曲率单调平滑连接算法，控制路径曲率极值范 围，使路径在满足无人机运动学条件前提下，具有精确性和可行性。
[0005] 但是上述路径规划方法对路径中障碍物的规避问题涉及不多，而且其规划精度仍 有提尚空间。
【发明内容】
[0006] 本发明的目的是提供一种无人机避障飞行路径规划方法，其利用PH曲线实现对路 径的优化，解决现有技术中存在的无法规避路径中障碍物、路径规划精确度低、路径不满足 无人机运动学约束的问题。
[0007] 为实现上述目的，按照本发明的一个方面，提供一种基于PH曲线的无人机路径规 划方法，包括：
[0008] (1)初始化无人机运动条件和任务地图，包括设定无人机路径的起始点、终点以及 沿途中需要经过的目标点，标注各障碍物的位置坐标和威胁半径，设定无人机的最大飞行 速度、最大飞行加速度以及最大飞行曲率；
[0009] (2)根据初始条件设定多个路径控制点，并根据所述控制点形成PH曲线，并将其表 示为贝塞尔η阶多项式；
[0010] ⑶根据最低阶含有拐点的ΡΗ曲线为五阶ΡΗ曲线，将所述贝塞尔η阶多项式限定为 五阶多项式，并据此对其进行求导运算和两点间插值运算，确定出无人机路径曲线形状，并 计算出相应控制点位置参数；
[0011] (4)调整回归系数以改变切矢量的长度，以满足ΡΗ曲线约束条件和无人机约束条 件，实现对控制点的修正，从而获得可规避障碍物的无人机路径。
[0012] 作为本发明的进一步优选，所述贝塞尔η阶多项式为：
[0014]式中，r(q) |q=o和r(q) |q=1分别表示路径的起点和终点，q是被正规化后的路径参 数
[0015] 作为本发明的进一步优选，所述步骤(3)中，对应所述五阶PH曲线的控制点为六 个，包括路径起点PQ、终点P5以及位于起点与终点之间从该起点至终点依次排布的四个控制 点P1，P2，P3和P4，该步骤(3 )具体过程如下：
[0016] (3.1)将无人机路径起点和终点位置切线方向的正切向量Ι?^ΓΙ和IFKI表示成切 矢量形式，切矢量分别用回归系数cdP C1修正：
[0019] 其中，C0和C1为回归系数，COe [ 1，00 ]，Cle [ 1，00 ]，钱；.和齡分别为初始位置和终止 位置的切向角，山和也分别为路径起点和终点位置切线方向的切矢量。
[0020] (3.2)将q = 0和q=l时得到的初始位置和目标位置坐标带入所述五阶多项式并进 行一阶求导，得到控制点P#PP4的位置参数：
[0023]作为本发明的进一步优选，所述步骤(4)中，对控制点的修正包括对控制点P2和P3 的修正，其中，所述控制点p#Pp3的数学表达式为：
[0031]作为本发明的进一步优选，控制点P1，P4，P5的位置参数的数学表达式如下；
[0035] 总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效 果:本发明中通过PH曲线具体为五阶PH曲线进行无人机路径规划，并通过控制点的求导和 插值运算，确定路径曲线形状，同时结合调整回归系数以改变切矢量的长度，以满足PH曲线 约束条件和无人机约束条件，从而实现对控制点的修正，获得可规避障碍物的无人机路径， 具有精度高，能够规避障碍物的优势。
【附图说明】
[0036] 图1:为本发明实施例的方法流程图；
[0037]图2:为本发明实施例的贝塞尔曲线图；
[0038] 图3:为本发明实施例的和声算法流程图；
[0039] 图4:为本发明实施例的碰撞条件判据图；
[0040] 图5:为本发明实施例具体实施例示意图；
【具体实施方式】
[0041] 为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发 明作进一步的详细描述，应当理解，此处所描述的实施示例仅限于说明和解释本发明，并不 用于限定本发明。
[0042] 如图1，本发明实施例所提供的一种无人机基于PH曲线和和声搜索算法进行规避 障碍物的路径规划方法，包括以下步骤：
[0043] (1)初始化无人机运动条件和任务地图，标注各障碍物的位置坐标和威胁半径，设 定无人机的最大飞行速度、最大飞行加速度、最大飞行曲率；
[0044] 该步骤中，具体包括初始化地图，设定无人机在该次任务中的起始点、终点以及沿 途中需要经过的目标点，根据无人机运动学约束，设定无人机飞行的高度，最大飞行速度， 最大飞行加速度，最大飞行曲率等。
[0045] 设定飞行过程中无人机在指定高度匀速飞行。
[0046] (2)根据初始条件给定多个控制点po，P1，…，pn，根据相应控制点形成对应的PH曲 线，并表示成B6zier形式下的η阶多项式。
[0047] 本步骤中，根据给定的飞行任务，飞行地图和运动学约束条件设定控制点，并根据 控制点写出Bezier样条曲线，如图2,其具体实现包括以下步骤：
[0048] (2 · 1)根据无人机飞行条件设定控制点Pk = (Xk，yk)，k = 0，，1，2，…，η，Xk，yk分别表 示控制点二维坐标。控制点用PH曲线表示，并表达为Β?ζ ier形式下的n阶多项式：
[0050]式中：r(q) |q=o和r(q) |q=1分别表示路径的起点和终点，q是被正规化后的路径参 数。
[0052] (2.2)利用求导公式求得η阶Bgzier曲线的第r阶导数，可以写成如下形式：
[0054] 其中：
[0055] Δ rPj =pj+1-Pj，Δ 〇pj =Pj。
[0056] (2.3)在路径规划中，最低阶含有拐点的PH曲线是五次PH曲线，即η = 5。取6个控制 点：PQ(xo，y())，pi(xi，yi)，P2(X2，y2)，p3(X3，y3)，p4(X4，y4)，p5(X5，y5)，当n = 5时等式（1)变为 如下形式：
[0058] (2.4)将五阶Β?ζ i er曲线表示成由控制点组成时的形式，控制点之间的数学表达 式如下；
[0060] 其中，pk= (xk，yk)，k = 0,，1，2，···，n，xk，yk分别表示控制点二维坐标，uq，ui，U2，vo， VI，V2为互素多项式，用这些多项式来构造 PH曲线。
[0061] (2.5)根据等式(3)求出五阶PH路径的一阶导数，用于之后的插值运算。路径r(q) 的一阶导数为：
[0063] (3)对5阶PH多项式作求导运算和两点间插值运算，确定连接曲线形状，计算得出 控制点PQ、PI、P5、P4位置参数。
[0064] 见图3,依托只能算法的优势，利用和声搜索算法进行反复迭代搜索，直到找到最 优的位置参数。和声搜索算法是业内成熟的算法，在此不再赘述。
[0065] (3.1)将无人机路径起点和终点位置切线方向的正切向量|兩瓦|和1??]表示成切 矢量形式，切矢量分别用回归系数CQ和C1修正，c〇e [1，00]，ClE [1，00];
[0067] (3.2)将q = 0和q=l时得到的初始位置和目标位置坐标带入到等式(4)和(6)求路 径的一阶导数，得到控制点P1和P4，计算公式如下所示：
[0069] (4)调整曲线参数增加切矢量丨^?和Ι?1的长度，修正控制点pdPp3进行规避 障碍物，并同时满足PH曲线约束条件和无人机飞行约束条件。
[0070] 当无人机路径上的点到障碍物中心的距离小于威胁半径时，这判断无人机将于障 碍物碰撞，反之则证明无人机路径安全，如图4所示。
[0071] (4.1)结合等式(5)和等式⑶计算得到(UQ，VQ)和(U2，V2)，计算结果如下式所示；
[0074] (4.2)将(110，￥0)和(112，￥2)带入到公式(5)可以求得(111，￥1)的表达式 ；
[0077] (4.3)将(110，￥0)、（111，￥1)和(112，￥2)带入到公式(5)求得控制点口2和口3;
[0078] (4.4)将得到的6个控制点代入PH曲线的表达式，即下面公式（11)，调整回归系数 CdPC1的大小，改变起始点和终止点的切矢量长度，使路径规避障碍物，并得到最终规划好 的无人机可飞行路径，完成路径规划。
[0080] 如图5,在一个具体实施例中，无人机任务的起始点坐标和终点目标分别为(0,0)， (100，100 )，测试地图大小120* 120m。障碍物1的位置中心坐标为(35，40 )，威胁半径大小为 l〇m，障碍物2的位置中心坐标为(35,40)，威胁半径大小为20m。图中点画线和虚线为路径规 划调整过程中的路径，都不能规避障碍物，只有实线满足无人机可飞行条件，并能规避障碍 物。
[0081] 应当理解，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明 专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要 求所保护的范围情况下，还可以做出替换或者变形，均落入本发明的保护范围之内，本发明 的请求保护范围应所附权利要求为准。
【主权项】
1. 一种基于PH曲线的无人机路径规划方法，包括： (1) 初始化无人机运动条件和任务地图，包括设定无人机路径的起始点、终点以及沿途 中需要经过的目标点，标注各障碍物的位置坐标和威胁半径，设定无人机的最大飞行速度、 最大飞行加速度以及最大飞行曲率； (2) 根据初始条件设定多个路径控制点，并根据所述控制点形成PH曲线，并将其表示为 贝塞尔η阶多项式； (3) 根据最低阶含有拐点的ΡΗ曲线为五阶ΡΗ曲线，将所述贝塞尔η阶多项式限定为五阶 多项式，并据此对其进行求导运算和两点间插值运算，确定出无人机路径曲线形状，并计算 出相应控制点位置参数； (4) 调整回归系数以改变切矢量的长度，以满足ΡΗ曲线约束条件和无人机约束条件，实 现对控制点的修正，从而获得可规避障碍物的无人机路径。2. 根据权利要求1所述的一种基于ΡΗ曲线的无人机路径规划方法，其中，所述贝塞尔η 阶多项式为：式中，r(q)|q=o和分别表示路径的起点和终点，q是被正规化后的路径参数，3. 根据权利要求1或2所述的一种基于PH曲线的无人机路径规划方法，其中，所述步骤 (3)中，对应所述五阶PH曲线的控制点为六个，包括路径起点po、终点p 5以及位于起点与终点 之间从该起点至终点依次排布的四个中间控制点pi，P2，P3和P4，该步骤⑶具体过程如下： (3.1) 将无人机路径起点和终点位置切线方向的正切向量丨和|p4Psl表示成切矢量 形式，切矢量分别用回归系数cdPC1修正：其中，CQ和C1为回归系数，c〇e [1，〇〇]，ci￡ [1，〇〇]，05和叫分别为初始位置和终止位置 的切向角，山和也分别为路径起点和终点位置切线方向的切矢量； (3.2) 将q = 0和q=l时得到的初始位置和目标位置坐标带入所述五阶多项式并进行一 阶求导，得到中间控制点P#PP4的位置参数：4. 根据权利要求3所述的一种基于PH曲线的无人机路径规划方法，其中，中间控制点p2 和P3的数学表达式为：其中，a，b，c为中间参数，且5.根据权利要求1或2所述的一种基于PH曲线的无人机路径规划方法，其中，其中，中间 控制点pi，P4，P5的位置参数的数学表达式如下；
【文档编号】G05D1/10GK105867421SQ201610352890
【公开日】2016年8月17日
【申请日】2016年5月25日
【发明人】高亮, 易进, 邬嘉扬, 徐奥, 李新宇
【申请人】华中科技大学