CN108549408B - 一种自动防撞地航迹规划方法及系统 - Google Patents
一种自动防撞地航迹规划方法及系统 Download PDFInfo
- Publication number
- CN108549408B CN108549408B CN201810526389.1A CN201810526389A CN108549408B CN 108549408 B CN108549408 B CN 108549408B CN 201810526389 A CN201810526389 A CN 201810526389A CN 108549408 B CN108549408 B CN 108549408B
- Authority
- CN
- China
- Prior art keywords
- machine
- collision
- conflict
- local
- calculating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种自动防撞地航迹规划方法及系统，该方法包括以下步骤：步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；步骤5，规划完整规避航迹。本发明提供的方法计算量小，指导飞机实现实时多维防撞地航迹规划，既能满足复杂地形低空飞行快速反应需求，又能充分利用飞机机动性能。
Description
技术领域
本发明涉及航空安全技术领域，尤其涉及一种自动防撞地航迹规划方法及系统。
背景技术
自飞机诞生以来，可控飞行撞地一直都是航空事故主要元凶，即在飞机未发生故障或发动机失效的情况下，由于机组人员未能及时地觉察地形障碍物的危险接近，或者机组人员操控失误，致使飞机撞山、撞地或掉入水中。尤其当高机动性飞机飞行在高过载或复杂地面环境条件下，由于飞行员意识丧失或对空间位置及姿态的误判导致飞机撞地的风险更大。
目前大多数飞机使用语音、视觉或者两者兼而有之的方式提前发出撞地告警，提示飞行员手动操控飞行实现防撞规避。自动对地防撞系统可以在手动防撞机动措施失败的情况下，在碰撞地面之前的最后瞬间由机载系统根据当前态势自动调整飞行状态，最终实现自动防撞。自动对地防撞技术是避免飞机撞地事故的发生，确保飞机安全的最后保障。
现有大多数飞机采用近地告警系统，该系统使用警告方式来让飞行员手动恢复飞机正常飞行。地形提示与告警系统在保持原有的近地告警系统能力基础上，增加了前视地形警戒和地形显示功能。上述两种防撞地系统只具备告警功能而没有自动防撞地航迹规划功能，在飞行员误操作或者意识丧失的情况下无法保证飞行安全。目前，仅美国研制出自动防撞地系统，使用GPS和数字地形高程数据地图结合实现自动防撞地规避，该系统计算量巨大，数据存储空间要求高，在规避航迹规划方面，主要采用在本机飞行垂直平面内爬升方法实现规避。
发明内容
本发明所要解决的技术问题是：针对现有技术存在的问题，本发明提出了一种自动防撞地航迹规划方法及系统，通过建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域、障碍物最近邻冲突面坐标变换规则，实现对撞地风险的探测与计算，该方法不依赖数字地形高程数据地图，也不需要地形扫描计算，计算量小，存储空间要求低，并计算障碍物对本机产生的虚拟作用力，该虚拟作用力引导本机自动调整飞行参数，实现完整防撞地规避航迹规划。
本发明提供的一种自动防撞地航迹规划方法，包括以下步骤：
步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；
步骤5，规划完整规避航迹。
进一步，所述步骤1具体包括：
步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参考面内垂直于XOY平面，指向飞机下方；
步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探测能力有关；
步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，并满足r≤R。
进一步，半径R、半径r和长度L的计算方法分别为：R＝amax×g×△t2/2+△R；r＝lw/2+△r；L＝vmax×△t，其中，amax为本机最大过载，g为本地重力加速度，△t为规定最后碰撞时刻，△R为预留参数，lw为本机机翼长，△r为预留参数，vmax为本机最大极限速度。
进一步，所述步骤2具体包括：
步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
步骤22，选取首要障碍所在冲突面与X轴的交点(xc,0,0)为最近邻正向冲突点；
步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0,ycl,zcl)和(0,ycr,zcr)，其中ycl≤ycr，将上述两个点分别转换为最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)。
进一步，所述步骤3具体包括：
步骤31，获取最近邻正向冲突点(xc,0,0)与本机质心的距离d；
步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L-d)，其中kc＝avmax×g×m/R，avmax为本机最大爬升过载，m为本机质量；
步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π-arctg(R/d)，Fc与Y轴夹角为π/2；
步骤34，获取最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)对本机质心的距离dl和dr；
步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L-dl)，Fsr＝ks(L-dr)，其中ks＝ahmax×g×m/R，ahmax为本机水平最大过载，m为本机质量；
步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsl和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π-arccos(ycl/dl)和αr＝π-arccos(ycr/dr)。
进一步，所述步骤4具体包括：
步骤41，计算本机机体坐标系虚拟合力分量(Fx,Fy,Fz)，其计算方法为：
步骤42，计算本机在虚拟合力作用下的过载分量(ax,ay,az)，其计算方法为：
步骤43，过载取值约束，若
则修正过载分量，修正方法为：
首先计算修正过载参数
然后(ax,ay,az)＝(ax′,ay′,az′)；
步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx,vy,vz)，其更新方法为：
首先计算
然后(vx,vy,vz)＝(vx′,vy′,vz′)，其中T为本机状态监控系统更新时间周期。
进一步，所述步骤5具体包括：
步骤51，根据步骤4预测本机在未来一段时间内每个更新时刻tk时的运动速度(vx,vy,vz)，计算本机偏航角改变量△θ＝θk-θk-1，俯仰角改变量
滚转角改变量△γ＝γk-γk-1，其中，tk＝t0+k×T，k＝1,2,3,…，t0为防撞地航迹规划启动时刻，θk、
γk分别为tk时刻时的偏航角、俯仰角、滚转角，t0时△θ＝0、
△γ＝0；
步骤52，预测本机在tk+1时刻的位置(x0(tk+1),y0(tk+1),z0(tk+1))，以及障碍物在tk+1时刻在本机机体坐标系中的位置(xc(tk+1),ycl(tk+1),zcl(tk+1))，本机在tk+1时刻的位置为，
其中(vx,vy,vz)和(ax,ay,az)分别为本机在tk时的速度和过载；
障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
其中，XL＝[xc(tk+1),ycl(tk+1),zcl(tk+1)]′，XR＝[xc(tk+1),ycr(tk+1),zcr(tk+1)]′，X0＝[x0(tk),y0(tk),z0(tk)]′，XL0＝[xc(tk),ycl(tk),zcl(tk)]′，XR0＝[xc(tk),ycr(tk),zcr(tk)]′，
和
步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4和步骤5，否则，完成航迹规划。
本发明的另一方面提供的一种自动防撞地航迹规划系统，该系统包括：
防撞地安全空域构建模块，用于建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
障碍物坐标点转换模块，用于对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
虚拟作用力计算模块，用于根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
运动参数计算模块，用于根据运动力学原理，计算本机在虚拟作用力下的运动参数；
航迹规划模块，用于规划完整规避航迹。
进一步，所述防撞地安全空域构建模块建立探测空域和冲突空域的方法包括：
步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参考面内垂直于XOY平面，指向飞机下方；
步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探测能力有关；
步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，并满足r≤R。
进一步，半径R、半径r和长度L的计算方法分别为：R＝amax×g×△t2/2+△R；r＝lw/2+△r；L＝vmax×△t，其中，amax为本机最大过载，g为本地重力加速度，△t为规定最后碰撞时刻，△R为预留参数，lw为本机机翼长，△r为预留参数，vmax为本机最大极限速度。
进一步，所述障碍物坐标点转换模块对首要障碍进行坐标点转换的方法包括：
步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
步骤22，选取首要障碍所在冲突面与X轴的交点(xc,0,0)为最近邻正向冲突点；
步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0,ycl,zcl)和(0,ycr,zcr)，其中ycl≤ycr，将上述两个点分别转换为最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)。
进一步，所述虚拟作用力计算模块计算虚拟作用力的方法包括：
步骤31，获取最近邻正向冲突点(xc,0,0)与本机质心的距离d；
步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L-d)，其中kc＝avmax×g×m/R，avmax为本机最大爬升过载，m为本机质量；
步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π-arctg(R/d)，Fc与Y轴夹角为π/2；
步骤34，获取最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)对本机质心的距离dl和dr；
步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L-dl)，Fsr＝ks(L-dr)，其中ks＝ahmax×g×m/R，ahmax为本机水平最大过载，m为本机质量；
步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsl和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π-arccos(ycl/dl)和αr＝π-arccos(ycr/dr)。
进一步，所述运动参数计算模块计算运动参数的方法包括：
步骤41，计算本机机体坐标系虚拟合力分量(Fx,Fy,Fz)，其计算方法为：
步骤42，计算本机在虚拟合力作用下的过载分量(ax,ay,az)，其计算方法为：
步骤43，过载取值约束，若
则修正过载分量，修正方法为：
首先计算修正过载参数
然后(ax,ay,az)＝(ax′,ay′,az′)；
步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx,vy,vz)，其更新方法为：
首先计算
然后(vx,vy,vz)＝(vx′,vy′,vz′)，其中T为本机状态监控系统更新时间周期。
进一步，所述航迹规划模块规划航迹的方法包括：
步骤51，根据步骤4预测本机在未来一段时间内每个更新时刻tk时的运动速度(vx,vy,vz)，计算本机偏航角改变量△θ＝θk-θk-1，俯仰角改变量
滚转角改变量△γ＝γk-γk-1，其中，tk＝t0+k×T，k＝1,2,3,L，t0为防撞地航迹规划启动时刻，θk、
γk分别为tk时刻时的偏航角、俯仰角、滚转角，t0时△θ＝0、
△γ＝0；
步骤52，预测本机在tk+1时刻的位置(x0(tk+1),y0(tk+1),z0(tk+1))，以及障碍物在tk+1时刻在本机机体坐标系中的位置(xc(tk+1),ycl(tk+1),zcl(tk+1))，，本机在tk+1时刻的位置为，
其中(vx,vy,vz)和(ax,ay,az)分别为本机在tk时的速度和过载；
障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
其中，XL＝[xc(tk+1),ycl(tk+1),zcl(tk+1)]′，XR＝[xc(tk+1),ycr(tk+1),zcr(tk+1)]′，X0＝[x0(tk),y0(tk),z0(tk)]′，XL0＝[xc(tk),ycl(tk),zcl(tk)]′，XR0＝[xc(tk),ycr(tk),zcr(tk)]′，
和
步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4和步骤5，否则，完成航迹规划。
本发明的另一方面提供的一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上所述的方法的步骤。
本发明提供的自动防撞航迹规划方法及系统，通过建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域，降低了系统地形扫描计算量和数据存储量；通过对最近邻冲突面进行坐标点转换，将面目标转换成三个能够引导本机实施规避的点目标，降低了系统计算量；通过建立虚拟力场，引导本机按自动产生最优规避机动参数。
相比于现有自动防撞地方法，本发明提供的方法计算量小，占用存储空间少，指导飞机实现实时多维防撞地航迹规划，既能满足复杂地形低空飞行快速反应需求，又能充分利用飞机机动性能。
附图说明
本发明将通过例子并参照附图的方式说明，其中：
图1为本发明的本机机体坐标系示意图；
图2为与本发明的实施例一致的前向柱状自动防撞地模型示意图；
图3为与本发明的实施例一致的防撞地航迹规划示意图。
具体实施方式
本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
本说明书中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子而已。
本发明的自动防撞地航迹规划方法，包括前向柱状防撞地探测空域和冲突空域创建、障碍物最近邻冲突面坐标变换、虚拟作用力形成、本机运动参数和完整规避航迹规划。
在一些实施例中，自动防撞地航迹规划方法具体包括以下步骤：
步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域，在一些实施例中，本机机体坐标系以本机质心位置为起点，本机飞行方向为正方向；
步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；
步骤5，规划完整规避航迹。
在一些实施例中，该方法还包括：
步骤6，本机按步骤5所规划的航迹飞行；
步骤7，依次执行步骤2至步骤6。
在一些实施例中，所述步骤1具体包括：
步骤11，建立本机机体坐标系，其中坐标轴原点O为飞机的质心，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参考面内垂直于XOY平面，指向飞机下方，如图1所示。
步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，如图2所示。半径R和长度L的选择与本机机动能力以及机载探测能力有关。在一些实施例中，半径R和长度L的计算方法分别为：R＝amax×g×△t2/2+△R和L＝vmax×△t，其中，amax为本机最大过载；g为本地重力加速度；△t为规定最后碰撞时刻；△R为预留参数，在一些实施例中，△R不小于机身长度的一半；vmax为本机最大极限速度。
步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，如图2所示。半径r的选择与本机几何外形以及紧急机动极限有关，并满足r≤R。在一些实施例中，半径r的计算方法为：r＝lw/2+△r，其中，lw为本机机翼长；△r为预留参数，在一些实施例中，△r不小于机舱横切面高度。
在一些实施例中，所述步骤2具体包括：
步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
步骤22，选取首要障碍所在冲突面与X轴的交点(xc,0,0)为最近邻正向冲突点；
步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0,ycl,zcl)和(0,ycr,zcr)，其中ycl≤ycr，将上述两个点分别转换为最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)，如图2所示。
在一些实施例中，所述步骤3具体包括：
步骤31，获取最近邻正向冲突点(xc,0,0)与本机质心的距离d。
步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L-d)，其中kc的选择与本机爬升机动能力有关。在一些实施例中，kc的计算方法为：kc＝avmax×g×m/R，其中，avmax为本机最大爬升过载，m为本机质量。
步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π-arctg(R/d)，Fc与Y轴夹角为π/2。
步骤34，获取最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)对本机质心的距离dl和dr。
步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L-dl)，Fsr＝ks(L-dr)，其中ks的选择与本机转向机动能力有关。在一些实施例中，ks的计算方法为：ks＝ahmax×g×m/R，其中，ahmax为本机水平最大过载，m为本机质量。
步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsl和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π-arccos(ycl/dl)和αr＝π-arccos(ycr/dr)。
在一些实施例中，所述步骤4具体包括：
步骤41，计算本机机体坐标系虚拟合力分量(Fx,Fy,Fz)，其计算方法为：
步骤42，计算本机在虚拟合力作用下的过载分量(ax,ay,az)，其计算方法为：
步骤43，过载取值约束，若
则修正过载分量。在一些实施例中，修正的方法为：
首先计算修正过载参数
然后(ax,ay,az)＝(ax′,ay′,az′)。
步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx,vy,vz)，其更新方法为：
首先计算
然后(vx,vy,vz)＝(vx′,vy′,vz′)，其中T为本机状态监控系统更新时间周期。
在一些实施例中，所述步骤5具体包括：
步骤51，根据步骤4预测本机在未来一段时间内每个更新时刻tk时的运动速度(vx,vy,vz)，计算本机偏航角改变量△θ＝θk-θk-1，俯仰角改变量
滚转角改变量△γ＝γk-γk-1，其中，tk＝t0+k×T，k＝1,2,3,…，t0为防撞地航迹规划启动时刻，θk、
γk分别为tk时刻时的偏航角、俯仰角、滚转角，t0时△θ＝0、
△γ＝0；。
步骤52，预测本机在tk+1时刻的位置(x0(tk+1),y0(tk+1),z0(tk+1))，以及障碍物在tk+1时刻在本机机体坐标系中的位置(xc(tk+1),ycl(tk+1),zcl(tk+1))，本机在tk+1时刻的位置为，
其中(vx,vy,vz)和(ax,ay,az)分别为本机在tk时的速度和过载；
障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
其中，XL＝[xc(tk+1),ycl(tk+1),zcl(tk+1)]′，XR＝[xc(tk+1),ycr(tk+1),zcr(tk+1)]′，X0＝[x0(tk),y0(tk),z0(tk)]′，XL0＝[xc(tk),ycl(tk),zcl(tk)]′，XR0＝[xc(tk),ycr(tk),zcr(tk)]′，
和
步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4和步骤5，否则，完成航迹规划，如图3所示。
在一些实施例中，所述步骤6具体包括：
步骤61，将本机的当前位置与步骤5规划的航迹点转换为地理坐标；
步骤62，以本机的当前位置为起点，若当前时刻位置不是规划航迹结束点，则以下一时刻航迹规划点为目标位置，调整飞行控制参数，否则，结束对当前首要障碍防撞规避状态。
在一些实施例中，本发明公开了一种自动防撞地航迹规划系统，该系统包括：
防撞地安全空域构建模块，用于建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
障碍物坐标点转换模块，用于对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
虚拟作用力计算模块，用于根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
运动参数计算模块，用于根据运动力学原理，计算本机在虚拟作用力下的运动参数；
航迹规划模块，用于规划完整规避航迹。
本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。
Claims (7)
1.一种自动防撞地航迹规划方法，其特征在于，包括以下步骤：
步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
所述步骤1具体包括：
步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参考面内垂直于XOY平面，指向飞机下方；
步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探测能力有关；
步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，并满足r≤R；
步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
所述步骤2具体包括：
步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
步骤22，选取首要障碍所在冲突面与X轴的交点(xc，0，0)为最近邻正向冲突点；
步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0，ycl，zcl)和(0，ycr，zcr)，其中ycl≤ycr，将上述两个点分别转换为最近邻左冲突点(xc，ycl，0)和最近邻右冲突点(xc，ycr，0)；
步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
所述步骤3具体包括：
步骤31，获取最近邻正向冲突点(xc，0，0)与本机质心的距离d；
步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L-d)，其中kc＝avmax×g×m/R，avmax为本机最大爬升过载，m为本机质量；
步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π-arctg(R/d)，Fc与Y轴夹角为π/2；
步骤34，获取最近邻左冲突点(xc，ycl，0)和最近邻右冲突点(xc，ycr，0)对本机质心的距离dl和dr；
步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L-dl)，Fsr＝ks(L-dr)，其中ks＝ahmax×g×m/R，ahmax为本机水平最大过载，m为本机质量；
步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsr和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π-arccos(ycl/dl)和αr＝π-arccos(ycr/dr)；
步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；
所述步骤4具体包括：
步骤41，计算本机机体坐标系虚拟合力分量(Fx，Fy，Fz)，其计算方法为：
步骤42，计算本机在虚拟合力作用下的过载分量(ax，ay，az)，其计算方法为：
步骤43，过载取值约束，若
则修正过载分量，修正方法为：
首先计算修正过载参数
然后(ax，ay，az)＝(ax′，ay′，az′)；
步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx，vy，vz)，其更新方法为：
首先计算
然后(vx，vy，vz)＝(vx′，vy′，vz′)，其中T为本机状态监控系统更新时间周期；
步骤5，规划完整规避航迹。
2.根据权利要求1所述的一种自动防撞地航迹规划方法，其特征在于，半径R、半径r和长度L的计算方法分别为：
R＝amax×g×Δt2/2+ΔR；r＝lw/2+Δr；L＝vmax×Δt，其中，amax为本机最大过载，g为本地重力加速度，△t为规定最后碰撞时刻，△R为预留参数，lw为本机机翼长，△r为预留参数，vmax为本机最大极限速度。
3.根据权利要求1所述的一种自动防撞地航迹规划方法，其特征在于，所述步骤5具体包括：
步骤51，根据步骤4预测本机在未来一段时间内每个更新时刻tk时的运动速度(vx，vy，vz)，计算本机偏航角改变量Δθ＝θk-θk-1，俯仰角改变量
滚转角改变量Δγ＝γk-γk-1，其中，tk＝t0+k×T，k＝1，2，3，…，t0为防撞地航迹规划启动时刻，θk、
γk分别为tk时刻时的偏航角、俯仰角、滚转角，t0时Δθ＝0、
Δγ＝0；
步骤52，预测本机在tk+1时刻的位置(x0(tk+1)，y0(tk+1)，z0(tk+1))，以及障碍物在tk+1时刻在本机机体坐标系中的位置(xc(tk+1)，ycl(tk+1)，zcl(tk+1))，本机在tk+1时刻的位置为，
其中(vx，vy，vz)和(ax，ay，az)分别为本机在tk时的速度和过载；
障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
其中，XL＝[xc(tk+1)，ycl(tk+1)，zcl(tk+1)]，XR＝[xc(tk+1)，ycr(tk+1)，zcr(tk+1)]′，X0＝[x0(tk)，y0(tk)，z0(tk)]′，XL0＝[xc(tk)，ycl(tk)，zcl(tk)]′，XR0＝[xc(tk)，ycr(tk)，zcr(tk)]′，
和
步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4和步骤5，否则，完成航迹规划。
4.一种自动防撞地航迹规划系统，其特征在于，该系统包括：
防撞地安全空域构建模块，用于建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
所述防撞地安全空域构建模块建立探测空域和冲突空域的方法包括：
步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参考面内垂直于XOY平面，指向飞机下方；
步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探测能力有关；
步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，并满足r≤R；
障碍物坐标点转换模块，用于对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
所述障碍物坐标点转换模块对首要障碍进行坐标点转换的方法包括：
步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
步骤22，选取首要障碍所在冲突面与X轴的交点(xc，0，0)为最近邻正向冲突点；
步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0，ycl，zcl)和(0，ycr，zcr)，其中ycl≤ycr，将上述两个点分别转换为最近邻左冲突点(xc，ycl，0)和最近邻右冲突点(xc，ycr，0)；
虚拟作用力计算模块，用于根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
所述虚拟作用力计算模块计算虚拟作用力的方法包括：
步骤31，获取最近邻正向冲突点(xc，0，0)与本机质心的距离d；
步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L-d)，其中kc＝avmax×g×m/R，avmax为本机最大爬升过载，m为本机质量；
步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π-arctg(R/d)，Fc与Y轴夹角为π/2；
步骤34，获取最近邻左冲突点(xc，ycl，0)和最近邻右冲突点(xc，ycr，0)对本机质心的距离dl和dr；
步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L-dl)，Fsr＝ks(L-dr)，其中ks＝ahmax×g×m/R，ahmax为本机水平最大过载，m为本机质量；
步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsr和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π-arccos(ycl/dl)和αr＝π-arccos(ycr/dr)；
运动参数计算模块，用于根据运动力学原理，计算本机在虚拟作用力下的运动参数；
所述运动参数计算模块计算运动参数的方法包括：
步骤41，计算本机机体坐标系虚拟合力分量(Fx，Fy，Fz)，其计算方法为：
步骤42，计算本机在虚拟合力作用下的过载分量(ax，ay，az)，其计算方法为：
步骤43，过载取值约束，若
则修正过载分量，修正方法为：
首先计算修正过载参数
然后(ax，ay，az)＝(ax′，ay′，az′)；
步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx，vy，vz)，其更新方法为：
首先计算
然后(vx，vy，vz)＝(vx′，vy′，vz′)，其中T为本机状态监控系统更新时间周期；
航迹规划模块，用于规划完整规避航迹。
5.根据权利要求4所述的一种自动防撞地航迹规划系统，其特征在于，半径R、半径r和长度L的计算方法分别为：
R＝amax×g×Δt2/2+ΔR；r＝lw/2+Δr；L＝vmax×Δt，其中，amax为本机最大过载，g为本地重力加速度，△t为规定最后碰撞时刻，△R为预留参数，lw为本机机翼长，△r为预留参数，vmax为本机最大极限速度。
6.根据权利要求4所述的一种自动防撞地航迹规划系统，其特征在于，所述航迹规划模块规划航迹的方法包括：
步骤51，根据步骤4预测本机在未来一段时间内每个更新时刻tk时的运动速度(vx，vy，vz)，计算本机偏航角改变量Δθ＝θk-θk-1，俯仰角改变量
滚转角改变量Δγ＝γk-γk-1，其中，tk＝t0+k×T，k ＝1，2，3，…，t0为防撞地航迹规划启动时刻，θk、
γk分别为tk时刻时的偏航角、俯仰角、滚转角，t0时Δθ＝0、
Δγ＝0；
步骤52，预测本机在tk+1时刻的位置(x0(tk+1)，y0(tk+1)，z0(tk+1))，以及障碍物在tk+1时刻在本机机体坐标系中的位置(xc(tk+1)，ycl(tk+1)，zcl(tk+1))，本机在tk+1时刻的位置为，
其中(vx，vy，vz)和(ax，ay，az)分别为本机在tk时的速度和过载；
障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
其中，XL＝[xc(tk+1)，ycl(tk+1)，zcl(tk+1)]′，XR＝[xc(tk+1)，ycr(tk+1)，zcr(tk+1)]′X0＝[x0(tk)，y0(tk)，z0(tk)]′，XL0＝[xc(tk)，ycl(tk)，zd(tk)]′，XR0＝[xc(tk)，ycr(tk)，zcr(tk)]′，
和
步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4和步骤5，否则，完成航迹规划。
7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至3中任一项所述的方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810526389.1A CN108549408B (zh) | 2018-05-29 | 2018-05-29 | 一种自动防撞地航迹规划方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810526389.1A CN108549408B (zh) | 2018-05-29 | 2018-05-29 | 一种自动防撞地航迹规划方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108549408A CN108549408A (zh) | 2018-09-18 |
| CN108549408B true CN108549408B (zh) | 2021-03-30 |
Family
ID=63511374
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810526389.1A Active CN108549408B (zh) | 2018-05-29 | 2018-05-29 | 一种自动防撞地航迹规划方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108549408B (zh) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR3025618B1 (fr) * | 2014-09-08 | 2016-09-16 | Airbus Operations Sas | Procede et dispositif de protection automatique d'un aeronef contre un risque de collision avec le sol. |
| CN111290426B (zh) * | 2018-12-07 | 2023-09-15 | 上海航空电器有限公司 | 一种航空器自动规避逃逸路径的预测控制方法 |
| CN111368383B (zh) * | 2018-12-07 | 2023-08-08 | 上海航空电器有限公司 | 深度强化学习的战斗机自动近地防撞系统碰撞评估方法 |
| CN109979247A (zh) * | 2019-04-18 | 2019-07-05 | 西安希德雷达科技有限公司 | 一种基于飞机飞行性能的近地告警方法 |
| CN110517538A (zh) * | 2019-08-06 | 2019-11-29 | 电子科技大学 | 飞行器主动发现与协同防碰撞方法与系统 |
| CN110673631B (zh) * | 2019-09-26 | 2022-05-03 | 深圳市道通智能航空技术股份有限公司 | 一种无人机飞行方法、装置和无人机 |
| CN112578817A (zh) * | 2021-02-26 | 2021-03-30 | 北京三快在线科技有限公司 | 无人机的控制方法、装置、存储介质和电子设备 |
| CN114115312B (zh) * | 2021-11-25 | 2023-11-10 | 北京航空航天大学 | 一种实时机载自动防撞地告警及回避决策方法和系统 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105786019A (zh) * | 2016-04-27 | 2016-07-20 | 广州极飞电子科技有限公司 | 一种载机飞行控制方法和系统 |
| CN106997693A (zh) * | 2017-04-14 | 2017-08-01 | 四川九洲空管科技有限责任公司 | 一种飞行器防撞算法验证测试方法及系统 |
| CN107015571A (zh) * | 2017-05-12 | 2017-08-04 | 南京航空航天大学 | 一种编队无人机追踪与规避移动目标的算法 |
| CN107170298A (zh) * | 2016-03-07 | 2017-09-15 | 群光电子股份有限公司 | 无人飞行器的防碰撞系统及其方法 |
| CN107380162A (zh) * | 2017-06-08 | 2017-11-24 | 南京航空航天大学 | 基于功能分配与多目标模糊决策的协同避撞方法 |
| CN107885227A (zh) * | 2017-11-30 | 2018-04-06 | 广州市华科尔科技股份有限公司 | 一种无人机自动避障方法 |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104155998B (zh) * | 2014-08-27 | 2017-08-25 | 电子科技大学 | 一种基于势场法的航迹规划方法 |
| US9806846B2 (en) * | 2015-07-24 | 2017-10-31 | Ziva Corp. | Wireless sensing with time reversal |
| CN105353771B (zh) * | 2015-12-11 | 2017-12-01 | 谭圆圆 | 无人飞行器控制方法及控制装置 |
| CN105717942B (zh) * | 2016-01-31 | 2018-07-06 | 中国人民解放军海军航空大学 | 一种无人飞行器空间避障方法及相关路径在线规划方法 |
| US10053218B2 (en) * | 2016-03-31 | 2018-08-21 | General Electric Company | System and method for positioning an unmanned aerial vehicle |
| KR20170115350A (ko) * | 2016-04-07 | 2017-10-17 | 삼성전자주식회사 | 전자 장치 및 그의 동작 방법 |
| CN105955304A (zh) * | 2016-07-06 | 2016-09-21 | 零度智控（北京）智能科技有限公司 | 一种避障方法、避障装置及无人飞行器 |
| CN108268048A (zh) * | 2016-12-30 | 2018-07-10 | 昊翔电能运动科技(昆山)有限公司 | 无人机训飞操控方法和无人机训飞操控装置 |
| CN107831777B (zh) * | 2017-09-26 | 2020-04-10 | 中国科学院长春光学精密机械与物理研究所 | 一种飞行器自主避障系统、方法及飞行器 |
| CN108062109B (zh) * | 2017-12-13 | 2020-09-11 | 天津萨瑞德科技有限公司 | 无人机避障方法 |
| CN107909856B (zh) * | 2017-12-19 | 2019-11-01 | 四川九洲空管科技有限责任公司 | 一种碰撞冲突探测方法及系统 |
-
2018
- 2018-05-29 CN CN201810526389.1A patent/CN108549408B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107170298A (zh) * | 2016-03-07 | 2017-09-15 | 群光电子股份有限公司 | 无人飞行器的防碰撞系统及其方法 |
| CN105786019A (zh) * | 2016-04-27 | 2016-07-20 | 广州极飞电子科技有限公司 | 一种载机飞行控制方法和系统 |
| CN106997693A (zh) * | 2017-04-14 | 2017-08-01 | 四川九洲空管科技有限责任公司 | 一种飞行器防撞算法验证测试方法及系统 |
| CN107015571A (zh) * | 2017-05-12 | 2017-08-04 | 南京航空航天大学 | 一种编队无人机追踪与规避移动目标的算法 |
| CN107380162A (zh) * | 2017-06-08 | 2017-11-24 | 南京航空航天大学 | 基于功能分配与多目标模糊决策的协同避撞方法 |
| CN107885227A (zh) * | 2017-11-30 | 2018-04-06 | 广州市华科尔科技股份有限公司 | 一种无人机自动避障方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108549408A (zh) | 2018-09-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108549408B (zh) | 一种自动防撞地航迹规划方法及系统 | |
| EP2182419B1 (en) | Avoidance manoeuvre generator for an aircraft | |
| US8527118B2 (en) | Automated safe flight vehicle | |
| CN110262492B (zh) | 一种无人艇的实时避碰和目标跟踪方法 | |
| CN106530840B (zh) | 一种基于飞机实时性能的飞行撞地威胁规避方法 | |
| US11900824B2 (en) | Predictive aircraft flight envelope protection system | |
| CN109917812B (zh) | 高空高速无人机着陆接地状态控制方法 | |
| US20200393852A1 (en) | Three dimensional aircraft autonomous navigation under constraints | |
| US10062293B2 (en) | Safety system, a helicopter fitted with such a system, and a safety method seeking to avoid an undesirable event | |
| US8547252B2 (en) | Adapting selective terrain warnings as a function of the instantaneous maneuverability of a rotorcraft | |
| CN101176133A (zh) | 飞机的地形回避方法及系统 | |
| CN111045450B (zh) | 固定翼无人机双机编队组队过程制导方法 | |
| JP2020520501A (ja) | 航空機の外部物体を検知し回避するためのシステムおよび方法 | |
| CN105844972A (zh) | 一种直升机前视地形告警方法 | |
| US20210375147A1 (en) | Method and system for detecting and avoiding obstacles with several detection spaces for aircraft | |
| CN114675663A (zh) | 一种侧风环境下无人机着陆的规划及控制方法 | |
| CN111752287A (zh) | 无人航空载具场外着陆系统 | |
| CN111290426B (zh) | 一种航空器自动规避逃逸路径的预测控制方法 | |
| US10809743B1 (en) | System and method for recovery from wake vortex encounters | |
| CN108154715B (zh) | 一种侧向碰撞监测方法 | |
| JP2020529349A (ja) | 検知された空気の動きに基づいて航空機を制御するためのシステムおよび方法 | |
| CA3109865C (en) | Time available before aircraft auto-recovery begins | |
| US11508244B2 (en) | Method, computer program product, system and craft for collision avoidance | |
| WO2022264813A1 (ja) | 運行管制システムの経路計画装置 | |
| CN115577486A (zh) | 一种基于飞机爬升性能的极限告警曲线计算方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 255 No. 621000 Mianyang city of Sichuan Province Branch Chong Park Jiuzhou Avenue Applicant after: SICHUAN JIUZHOU ATC TECHNOLOGY Co.,Ltd. Address before: 621000 Jiuzhou Avenue, 255 chuanchuang Park, Anzhou District, Mianyang, Sichuan Applicant before: SICHUAN JIUZHOU ATC TECHNOLOGY Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |