一种固定翼无人机自主避障的航迹规划方法
技术领域
本发明涉及航空科学的技术领域,具体涉及一种基于离线片元航迹库及在线航迹搜索与决策的固定翼无人机自主避障的航迹规划方法。
背景技术
目前,无人机已经成为各国投入大量人力和财力进行研发的作战装备,为了使无人机具有或接近鸟类在城市和山林等障碍环境中快速飞行的能力,无人机自主避障的航迹规划方法已经成为研究热点。
目前,无人机自主避障的航迹规划方法大多集中在旋转翼无人机的航迹规划方法,或者基于已知环境地图情况的固定翼无人机航迹规划方法。旋转翼无人机具有悬停和倒退功能,因此,旋转翼无人机的避障局限于“一停二看三通过”的形式,难以达到城市局部战争中快速突防的要求。进一步地,由于旋转翼无人机的续航时间短,旋转翼无人机难以执行远距离的飞行任务。现有的固定翼无人机航迹规划方法需要基于已知的环境地图信息,一方面:预先构建环境地图信息不仅工作量大,而且在面对快速执行的任务时,难以满足环境地图的构建;另一方面:由于航迹规划方法是基于已知地图进行的离线规划,一旦环境信息发生改变或者失效,整个飞行任务即会失败;再者:固定翼无人机航迹规划方法中无人机仅被当作质点,并没有考虑无人机的动力学因素,使得航迹规划失去意义。
因此,针对现有无人机自主避障的航迹规划方法所存在的问题,并结合固定翼无人机应用前景,本发明实施例中提出一种固定翼无人机自主避障的航迹规划方法,该方法无需基于已知环境地图进行离线规划,只需要根据初始任务航点的信息,在复杂环境下实现看见即躲避的快速自主的避障航迹规划。
发明内容
针对现有无人机的航迹规划方法所存在的问题,并结合固定翼无人机应用前景,本发明实施例中提出一种固定翼无人机的航迹规划方法。本发明实施例所提出的固定翼无人机的航迹规划方法基于离散片元航迹库及在线航迹搜索和决策,能够实现固定翼无人机只需根据初始任务航点的信息,在未建模的复杂环境下,实现看见即躲避的快速自主的避障航迹规划。
该固定翼无人机自主避障的航迹规划方法的具体方案如下:一种固定翼无人机自主避障的航迹规划方法,包括步骤S1:建立固定翼无人机的动力学模型,并分析所述固定翼无人机的相关飞行性能;步骤S2:根据所述动力学模型,建立片元航迹库;步骤S3:构建所述片元航迹的机动自动机;步骤S4:根据航迹规划的信息参数,执行在线的航迹规划搜索算法。
优选地,所述固定翼无人机的相关飞行性能包括最小转弯半径、最大滚转角速率、最大滚转角、最小飞行速度和最大飞行速度。
优选地,所述航迹规划的信息参数包括所述固定翼无人机的初始任务航点信息、所述固定翼无人机的飞行性能、所述固定翼无人机的飞行状态参数和实时感知探测到的障碍物点云信息。
优选地,所述步骤S2具体包括步骤S21:选取所述固定翼无人机的飞行状态,构建标称的片元航迹库;步骤S22:设定所述每条片元航迹包括相应的飞行状态下的飞行状态参数和控制量的开环轨迹状态;步骤S23:根据所述固定翼无人机的飞行速度范围,构建不同速度下的片元航迹库;步骤S24:根据干扰因素和飞行安全参数,在所述标称的片元航迹库上构建航迹通道。
优选地,所述步骤S24包括步骤S241:根据协调转弯公式,计算所述步骤S23中不同速度下和步骤S21中不同滚转角下所对应的标称的片元航迹和转弯半径;步骤S242:根据所述固定翼无人机的翼展和***时间延迟因素,确定航迹片元通道的外圆半径和内圆半径。
优选地,所述步骤S21中的飞行状态包括所述固定翼无人机的平飞状态、左滚状态和右滚状态。
优选地,所述步骤S22中的飞行状态参数包括所述固定翼无人机的位置、姿态角、气流角、速度、角速度、角加速度和飞行高度。
优选地,所述步骤S22中的控制量包括舵面偏转角和油门推力。
优选地,所述步骤S3具体包括步骤S31:判断任意两条片元航迹之间切换是否满足最大滚角速率的限制条件,并选出所有能够满足所述限制条件的切换方式,将所述切换方式构成片元航迹的机动自动机;步骤S32:根据所述固定翼无人机的飞行速度范围,构建不同速度下的多组片元航迹的机动自动机。
优选地,所述步骤S4中的搜索算法具体包括步骤S41:检测所述固定翼无人机是否探测到障碍物;若检测到障碍物,则执行步骤S42;否则,则执行无障碍飞行;步骤S42:确定所述固定翼无人机的当前飞行状态和当前的片元航迹指令;步骤S43:确定片元航迹库的初始搜索方向;步骤S44:确定片元航迹库的搜索顺序;步骤S45:检验所述障碍物的约束条件,确定安全的片元航迹通道;步骤S46:将搜索出的安全的片元航迹通道的标称片元航迹作为期望航迹指令发送给飞行控制***,由飞行控制***完成一个控制周期的航迹闭环跟踪控制;步骤S47:判断所述固定翼无人机是否达到当前的任务航点,若未到达则跳转步骤至S41;否则,则顺序执行步骤S48;步骤S48:判断所到达航点是否为最终任务航点,如果不是最终任务航点,则将下一个任务航点作为当前任务航点,并跳转至步骤S41;如果是最终任务航点,则顺序执行步骤S49;步骤S49:返航结束。
优选地,所述步骤S43的具体过程为:根据所述固定翼无人机的飞行状态和当前任务航点,计算当前任务航点视线角,并规定所述固定翼无人机的飞行速度矢量到所述固定翼无人机的当前位置与当前任务航点连线的顺时针方向为正方向,逆时针方向为负方向;其中,当前任务航点视线角的取值范围为(-180°,180°]。
优选地,所述步骤S43中的初始搜索方向是始终将航点视线角减小到零的方向。
优选地,所述步骤S44的具体过程为:以所述固定翼无人机的当前飞行状态为起点,根据步骤S3中构建的片元航迹的机动自动机原则,按步骤S43所确定的初始方向搜索,直到所述方向上的片元航迹搜索完毕,再转向另一方向进行搜索,直到遍历所有片元航迹;在所述搜索过程中,记录每一次检测片元航迹通道的障碍物数量,一旦某条片元航迹通道满足障碍物约束条件,立即停止搜索,并将所述片元航迹作为期望航迹;如果遍历所有航迹均不满足障碍物约束条件,则选取初始方向上最大滚转角速率限制的片元航迹作为期望航迹。
优选地,所述步骤S45的具体过程为:假设片元航迹通道中,标称的片元航迹方程为内圆方程为(x+ai)2+(y+bi)2=(Ri-ΔRi2)2,外圆方程为(x+ai)2+(y+bi)2=(Ri+ΔRi1)2;假设障碍物的点云坐标为(xoj,yoj),如果某个障碍物(xo,yo)在片元航迹通道内,则满足条件:(xo+ai)2+(yo+bi)2>(Ri-ΔRi2)2且(xo+ai)2+(yo+bi)2<(Ri+ΔRi1)2,并记Ωo=1,否则,该障碍物不在该片元航迹通道内,并记Ωo=0;如果所有障碍物在片元航迹通道中检测都满足则所检测的片元航迹通道内没有障碍物,所述片元航迹通道为安全的片元航迹通道,可作为期望航迹通道;否则,所述片元航迹通道不安全。
优选地,所述无障碍飞行的具体过程为:执行步骤S42和S43,在初始搜索方向上,将与当前片元航迹之间的最小机动的片元航迹作为期望片元航迹,再跳转至步骤S46。
从以上技术方案可以看出,本发明实施例具有以下优点:
本发明实施例中所提供一种固定翼无人机自主避障的航迹规划方法,该方法无需预先对环境进行建模,只需要根据最初的任务航点信息,考虑飞行性能约束并结合实时探测到的障碍物的点云信息和无人机飞行状态信息,通过在线航迹规划算法,实现固定翼无人机的快速和/或远距离的自主避障飞行任务。本发明实施例中所提供的一种固定翼无人机自主避障的航迹规划方法具有很强的环境适应性,通用性和战争实用性。
附图说明
图1为本发明实施例中提供的一种固定翼无人机自主避障的航迹规划方法的流程示意图;
图2为图1所示实施例中建立片元航迹库的流程示意图;
图3为本发明实施例中提供的一种片元航迹库的轨迹示意图;
图4为图2所示实施例中构建片元航迹通道的流程示意图;
图5为图1所示实施例中构建片元航迹的机动自动机的流程示意图;
图6为图1所示实施例中在线航迹规划搜索算法的流程示意图;
图7为本发明实施例中提供的一种目标航点视线角的示意图;
图8为本发明实施例中提供的一种片元航迹的机动自动机的示意图;
图9为本发明实施例中提供的一种片元航迹通道的示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”和“第四”等(如果存在)是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
如图1所示,本发明实施例中提供的一种固定翼无人机自主避障的航迹规划方法的流程示意图。固定翼无人机自主避障的航迹规划方法总共包括四个步骤,具体如下:
步骤S1:建立固定翼无人机的动力学模型,并分析所述固定翼无人机的相关飞行性能。在该实施例中,固定翼无人机的相关飞行性能包括最小转弯半径Rmin、最大滚转角速率最大滚转角φmax、最小飞行速度Vmin和最大飞行速度Vmax。
步骤S2:根据所述动力学模型,建立片元航迹库。如图2所示,本发明实施例中提供的一种建立片元航迹库的流程示意图。步骤S2中建立片元航迹库具体包括四个步骤:
步骤S21:选取所述固定翼无人机的飞行状态,构建标称的片元航迹库。在该实施例中,飞行状态具体包括所述固定翼无人机的平飞状态、左滚状态和右滚状态。如图3所示,本发明实施例所提供的片元航迹库的轨迹示意图。如图3所示,固定翼无人机100的飞行状态构建标称的片元航迹库,具体包括F1大机动左转、F2小机动左转、F3平飞、F4小机动右转和F5大机动右转。其中,F1大机动左转和F2小机动左转对应于上文的左滚状态,F4小机动右转和F5大机动右转对应上文的右滚状态。其中,左转状态和右转状态可根据任务的复杂程度和任务的控制精度要求,进一步细化成左滚-10°、-20°、-30°、-40°……和右滚10°、20°、30°、40°……。当然,左滚状态和右滚状态的滚转角的细化角度不仅限于以10°为一个状态间隔的划分,也可以采用其他度数作为状态间隔,如5°、20°、15°等,但不局限于以均匀间隔划分。最大滚转角可由任务要求给定,但不应超过无人机最大滚转角约束。
步骤S22:设定所述每条片元航迹包括相应的飞行状态下的飞行状态参数和控制量的开环轨迹状态。在该实施例中,步骤S22中的飞行状态参数Xprimitive具体包括所述固定翼无人机的位置、姿态角、气流角、速度、角速度、角加速度和飞行高度。具体如,飞行状态参数Xprimitive=[φ θ ψ α β u v w p q r h]T,其中,[φ θ ψ]T为无人机姿态角,无人机姿态角不局限于此处列举欧拉角形式,其也可以是四元素形式;[α β]T为无人机气流角;[u vw]T为无人机机体轴速度分量,且速度不局限于以机体轴分量形式;[p q r]T为机体轴角速度分量,且角速度不局限于以机体轴分量形式;h为无人机飞行高度。步骤S22中的控制量Uprimitive具体包括舵面偏转角和油门推力。具体如,控制量Uprimitive=[δa δe δr δT]T,其中,四个量分别为副翼偏转角、升降舵偏转角、方向舵偏转角和油门推力。
步骤S23:根据所述固定翼无人机的飞行速度范围,构建不同速度下的片元航迹库。在一具体实施例中,速度可每间隔1m/s进行划分。速度的具体数值可根据典型任务的飞行速度要求而确定,速度的具体数值也可每间隔0.5m/s、2m/s等,但不限于以均匀间隔划分。
步骤S24:根据干扰因素和飞行安全参数,在所述标称的片元航迹库上构建航迹通道。如图4所示,本发明实施例中提供的一种构建航迹通道的流程示意图。步骤S24中构建航迹通道具体包括两个步骤:
步骤S241:根据协调转弯公式计算所述步骤S23中不同速度下和步骤S21中不同滚角下所对应的标称的片元航迹Ti和转弯半径Ri。
步骤S242:根据所述固定翼无人机的翼展和***时间延迟因素,确定航迹片元通道的外圆半径Ri+ΔRi1和内圆半径Ri-ΔRi2。
步骤S3:构建所述片元航迹的机动自动机。如图5所示,本发明实施例中提供的一种构建片元航迹的机动自动机的流程示意图。步骤S3具体包括两个步骤:
步骤S31:判断任意两条片元航迹之间切换是否满足最大滚角速率的限制条件,并选出所有能够满足所述限制条件的切换方式,将所述切换方式构成片元航迹的机动自动机。在一具体实施例中,可假设在左滚-40°与右滚40°之间的切换,若不满足最大滚转角速率的限制,则应在片元航迹的库机动自动机中阻断两者之间的切换。
步骤S32:根据所述固定翼无人机的飞行速度范围,构建不同速度下的多组片元航迹的机动自动机。
步骤S4:根据航迹规划的信息参数,执行在线的航迹规划搜索算法。如图6所示,本发明实施例中提供的一种在线航迹规划搜索算法的流程示意图。步骤S4具体包括九个步骤:
步骤S41:检测所述固定翼无人机是否探测到障碍物;若检测到障碍物,则执行步骤S42;否则,则执行无障碍飞行。
步骤S42:确定所述固定翼无人机的当前飞行状态和当前的片元航迹指令。
步骤S43:确定片元航迹库的初始搜索方向。根据固定翼无人机的飞行状态和任务航点的信息,计算出当前任务航点视线角η。如图7所示,本发明实施例中所提供的目标航点视线角示意图。在该实施例中,定义固定翼无人机的速度矢量到固定翼无人机的当前位置与当前任务航点连线的顺时针方向为正方向,逆时针方向为负方向。如图7所示,无人机速度矢量V到无人机当前位置P与当前任务航点Q连线PQ的顺时针方向为正方向,逆时针方向为负方向。因此,当前任务航点视线角η取值范围为(-180°,180°]。初始搜索方向是始终将航点视线角减小到零的方向。在一具体实施例中,若当前片元航迹滚转角指令为-20°,当前任务航点视线角为30°,则初始方向为正方向。
步骤S44:确定片元航迹库的搜索顺序。以固定翼无人机的当前飞行状态为起点,根据步骤S3中构建的片元航迹的机动自动机原则,按步骤S43所确定的初始方向搜索,直到该方向上片元航迹搜索完毕,再转向另一方向进行搜索,直到遍历所有片元航迹。在一具体实施例中,若片元航迹滚转角为-60°,-50°,-40°,-30°,-20°,-10°,0°,10°,20°,30°,40°,50°,60°,无人机当前飞行状态是左滚-20°,航迹片元机动自动机原则中,-20°无法切换到40°、50°和60°,初始搜索方向为负,则搜索顺序为-20°→-30°→-40°→-50°→-60°→-10°→0°→10°→20°→30°。在搜索过程中,记录每一次检测片元航迹通道的障碍物数量,一旦某条片元航迹满足障碍物的约束条件,立即停止搜索,并将该片元航迹作为期望航迹;如果遍历所有航迹均不满足障碍物的约束条件,则选取初始方向上最大滚转角速率限制的片元航迹作为期望航迹,并向地面站发送一次警告。例如,按上述-20°→-30°→-40°→-50°→-60°→-10°→0°→10°→20°→30°的搜索顺序,如果搜索到滚转角为-50°对应片元航迹通道内没有障碍物,则立即停止搜索,将滚转角为-50°的片元航迹作为期望航迹,若遍历所有航迹均不满足障碍物约束条件,则选取-60°作为期望航迹,并向地面站发送一次警告。
步骤S45:检验所述障碍物的约束条件,确定安全的片元航迹通道。如图8所示,本发明实施例所提供的一种片元航迹的机动自动机示意图。在该实施例中,假设片元航迹Ti通道中,标称的片元航迹方程为内圆方程为(x+ai)2+(y+bi)2=(Ri-ΔRi2)2,外圆方程为(x+ai)2+(y+bi)2=(Ri+ΔRi1)2。假设障碍物的点云坐标为(xoj,yoj),如果某个障碍物(xo,yo)在片元航迹通道内,则满足条件:(xo+ai)2+(yo+bi)2>(Ri-ΔRi2)2且(xo+ai)2+(yo+bi)2<(Ri+ΔRi1)2,并记Ωo=1,否则,该障碍物不在该片元航迹通道内,并记Ωo=0。如果所有障碍物在片元航迹通道Ti中检测都满足则所检测的片元航迹通道Ti内没有障碍物,Ti为安全的片元航迹通道,可作为期望航迹通道;否则Ti不安全。如图9所示,本发明实施例所提供的一种片元航迹通道的示意图,具体为不同飞行状态下切换的航道示意。
步骤S46:将搜索出的安全的片元航迹通道的标称片元航迹作为期望航迹指令发送给飞行控制***,由飞行控制***完成一个控制周期的航迹闭环跟踪控制。
步骤S47:判断所述固定翼无人机是否达到当前的任务航点,若未到达则跳转步骤至S41;否则,则顺序执行步骤S48。
步骤S48:判断所到达航点是否为最终任务航点,如果不是最终任务航点,则将下一个任务航点作为当前任务航点,并跳转至步骤S41;如果是最终任务航点,则顺序执行步骤S49。
步骤S49:返航结束。返航程序将设定固定翼无人机爬高至无障碍高度,进行无障碍飞行返航,此时目标航点可设为起飞点。
在该实施例中,步骤S41和步骤S49中所涉及到的无障碍航点飞行方法,具体步骤如下:执行步骤S42和S43,将与当前片元航迹之间的最小的机动片元航迹作为期望片元航迹,再跳转至步骤S46。在一具体实施例中,当固定翼无人机在无障碍飞行时,若当前航迹滚转角指令为-20°,当前任务航点视线角为30°,则初始搜索方向为正方向,并选取机动最小的滚转角为-10°的片元航迹作为期望航迹;如果当前任务航点视线角为0°,则说明无人机处于直线平飞状态,当前航迹滚转角为0°,则依然将当前片元航迹作为期望航迹。
本发明实施例中所提供一种固定翼无人机自主避障的航迹规划方法,该方法无需预先对环境进行建模,只需要根据最初的任务航点信息,考虑飞行性能约束并结合实时探测到的障碍物的点云信息和无人机飞行状态信息,通过在线航迹规划算法,实现固定翼无人机的快速和/或远距离的自主避障飞行任务。本发明实施例中所提供的一种固定翼无人机自主避障的航迹规划方法具有很强的环境适应性,通用性和战争实用性。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。