本发明公开了一种自动防撞地航迹规划方法及系统，该方法包括以下步骤：步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；步骤5，规划完整规避航迹。本发明提供的方法计算量小，指导飞机实现实时多维防撞地航迹规划，既能满足复杂地形低空飞行快速反应需求，又能充分利用飞机机动性能。
1.一种自动防撞地航迹规划方法，其特征在于，包括以下步骤：步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参考面内垂直于XOY平面，指向飞机下方；
步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探测能力有关；
步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，并满足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)；
步骤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，根据运动力学原理，计算本机在虚拟作用力下的运动参数；
步骤41，计算本机机体坐标系虚拟合力分量(Fx，Fy，Fz)，其计算方法为：步骤42，计算本机在虚拟合力作用下的过载分量(ax，ay，az)，其计算方法为：其中m为本机质量；
步骤43，过载取值约束，若 则修正过载分量，修正方法为：首先计算修正过载参数 然后(ax，ay，az)＝(ax′，ay′，az′)；
步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx，vy，vz)，其更新方法为：
首先计算 然后(vx，vy，vz)＝(vx′，vy′，vz′)，其中T为本机状态监控系统更新时间周期；
2.根据权利要求1所述的一种自动防撞地航迹规划方法，其特征在于，半径R、半径r和长度L的计算方法分别为：
R＝amax×g×Δt /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和步骤
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)，其计算方法为：其中m为本机质量；
步骤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×Δt /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和步骤
7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至3中任一项所述的方法的步骤。
一种自动防撞地航迹规划方法及系统
技术领域
[0001] 本发明涉及航空安全技术领域，尤其涉及一种自动防撞地航迹规划方法及系统。
背景技术
[0002] 自飞机诞生以来，可控飞行撞地一直都是航空事故主要元凶，即在飞机未发生故障或发动机失效的情况下，由于机组人员未能及时地觉察地形障碍物的危险接近，或者机
组人员操控失误，致使飞机撞山、撞地或掉入水中。尤其当高机动性飞机飞行在高过载或复
杂地面环境条件下，由于飞行员意识丧失或对空间位置及姿态的误判导致飞机撞地的风险
更大。
[0003] 目前大多数飞机使用语音、视觉或者两者兼而有之的方式提前发出撞地告警，提示飞行员手动操控飞行实现防撞规避。自动对地防撞系统可以在手动防撞机动措施失败的
情况下，在碰撞地面之前的最后瞬间由机载系统根据当前态势自动调整飞行状态，最终实
现自动防撞。自动对地防撞技术是避免飞机撞地事故的发生，确保飞机安全的最后保障。
[0004] 现有大多数飞机采用近地告警系统，该系统使用警告方式来让飞行员手动恢复飞机正常飞行。地形提示与告警系统在保持原有的近地告警系统能力基础上，增加了前视地
形警戒和地形显示功能。上述两种防撞地系统只具备告警功能而没有自动防撞地航迹规划
功能，在飞行员误操作或者意识丧失的情况下无法保证飞行安全。目前，仅美国研制出自动
防撞地系统，使用GPS和数字地形高程数据地图结合实现自动防撞地规避，该系统计算量巨
大，数据存储空间要求高，在规避航迹规划方面，主要采用在本机飞行垂直平面内爬升方法
实现规避。
发明内容
[0005] 本发明所要解决的技术问题是：针对现有技术存在的问题，本发明提出了一种自动防撞地航迹规划方法及系统，通过建立基于本机机体坐标系的前向柱状防撞地探测空域
和冲突空域、障碍物最近邻冲突面坐标变换规则，实现对撞地风险的探测与计算，该方法不
依赖数字地形高程数据地图，也不需要地形扫描计算，计算量小，存储空间要求低，并计算
障碍物对本机产生的虚拟作用力，该虚拟作用力引导本机自动调整飞行参数，实现完整防
撞地规避航迹规划。
[0006] 本发明提供的一种自动防撞地航迹规划方法，包括以下步骤：
[0007] 步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
[0008] 步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
[0009] 步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
[0010] 步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；
[0011] 步骤5，规划完整规避航迹。
[0012] 进一步，所述步骤1具体包括：
[0013] 步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参
考面内垂直于XOY平面，指向飞机下方；
[0014] 步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探
测能力有关；
[0015] 步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，
并满足r≤R。
[0016] 进一步，半径R、半径r和长度L的计算方法分别为：R＝amax×g×△t2/2+△R；r＝lw/2+△r；L＝vmax×△t，其中，amax为本机最大过载，g为本地重力加速度，△t为规定最后碰撞
时刻，△R为预留参数，lw为本机机翼长，△r为预留参数，vmax为本机最大极限速度。
[0017] 进一步，所述步骤2具体包括：
[0018] 步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
[0019] 步骤22，选取首要障碍所在冲突面与X轴的交点(xc,0,0)为最近邻正向冲突点；
[0020] 步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0,ycl,zcl)和(0,ycr,zcr)，其中ycl≤
ycr，将上述两个点分别转换为最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)。
[0021] 进一步，所述步骤3具体包括：
[0022] 步骤31，获取最近邻正向冲突点(xc,0,0)与本机质心的距离d；
[0023] 步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L‑d)，其中kc＝avmax×g×m/R，avmax为本机最大爬升过载，m为本机质量；
[0024] 步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π‑arctg(R/d)，Fc与Y轴夹角为π/2；
[0025] 步骤34，获取最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)对本机质心的距离dl和dr；
[0026] 步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L‑dl)，Fsr＝ks(L‑dr)，其中ks＝ahmax×g×m/R，ahmax为本机水平最大过载，m为本机质
量；
[0027] 步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsl和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π‑arccos(ycl/dl)和αr＝π‑arccos(ycr/dr)。
[0028] 进一步，所述步骤4具体包括：
[0029] 步骤41，计算本机机体坐标系虚拟合力分量(Fx,Fy,Fz)，其计算方法为：
[0030]
[0031] 步骤42，计算本机在虚拟合力作用下的过载分量(ax,ay,az)，其计算方法为：
[0032] 其中m为本机质量；
[0033] 步骤43，过载取值约束，若 则修正过载分量，修正方法为：
[0034] 首先计算修正过载参数 然后(ax,ay,az)＝(ax′,ay′,az′)；
[0035] 步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx,vy,vz)，其更新方法为：
[0036] 首先计算 然后(vx,vy,vz)＝(vx′,vy′,vz′)，其中T为本机状态监控系统更新时间周期。
[0037] 进一步，所述步骤5具体包括：
[0038] 步骤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；
[0039] 步骤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时刻的位置
为，
[0040]
[0041] 其中(vx,vy,vz)和(ax,ay,az)分别为本机在tk时的速度和过载；
[0042] 障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
[0043]
[0044] 其中，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)]′，
和
[0045]
[0046] 步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4
和步骤5，否则，完成航迹规划。
[0047] 本发明的另一方面提供的一种自动防撞地航迹规划系统，该系统包括：
[0048] 防撞地安全空域构建模块，用于建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
[0049] 障碍物坐标点转换模块，用于对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
[0050] 虚拟作用力计算模块，用于根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
[0051] 运动参数计算模块，用于根据运动力学原理，计算本机在虚拟作用力下的运动参数；
[0052] 航迹规划模块，用于规划完整规避航迹。
[0053] 进一步，所述防撞地安全空域构建模块建立探测空域和冲突空域的方法包括：
[0054] 步骤11，建立本机机体坐标系，以飞机的质心为坐标原点O，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴在参
考面内垂直于XOY平面，指向飞机下方；
[0055] 步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，半径R和长度L的选择与本机机动能力以及机载探
测能力有关；
[0056] 步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，半径r的选择与本机几何外形以及机动极限有关，
并满足r≤R。
[0057] 进一步，半径R、半径r和长度L的计算方法分别为：R＝amax×g×△t2/2+△R；r＝lw/2+△r；L＝vmax×△t，其中，amax为本机最大过载，g为本地重力加速度，△t为规定最后碰撞
时刻，△R为预留参数，lw为本机机翼长，△r为预留参数，vmax为本机最大极限速度。
[0058] 进一步，所述障碍物坐标点转换模块对首要障碍进行坐标点转换的方法包括：
[0059] 步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
[0060] 步骤22，选取首要障碍所在冲突面与X轴的交点(xc,0,0)为最近邻正向冲突点；
[0061] 步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0,ycl,zcl)和(0,ycr,zcr)，其中ycl≤
ycr，将上述两个点分别转换为最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)。
[0062] 进一步，所述虚拟作用力计算模块计算虚拟作用力的方法包括：
[0063] 步骤31，获取最近邻正向冲突点(xc,0,0)与本机质心的距离d；
[0064] 步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L‑d)，其中kc＝avmax×g×m/R，avmax为本机最大爬升过载，m为本机质量；
[0065] 步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π‑arctg(R/d)，Fc与Y轴夹角为π/2；
[0066] 步骤34，获取最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)对本机质心的距离dl和dr；
[0067] 步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L‑dl)，Fsr＝ks(L‑dr)，其中ks＝ahmax×g×m/R，ahmax为本机水平最大过载，m为本机质
量；
[0068] 步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsl和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π‑arccos(ycl/dl)和αr＝π‑arccos(ycr/dr)。
[0069] 进一步，所述运动参数计算模块计算运动参数的方法包括：
[0070] 步骤41，计算本机机体坐标系虚拟合力分量(Fx,Fy,Fz)，其计算方法为：
[0071]
[0072] 步骤42，计算本机在虚拟合力作用下的过载分量(ax,ay,az)，其计算方法为：
[0073] 其中m为本机质量；
[0074] 步骤43，过载取值约束，若 则修正过载分量，修正方法为：
[0075] 首先计算修正过载参数 然后(ax,ay,az)＝(ax′,ay′,az′)；
[0076] 步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx,vy,vz)，其更新方法为：
[0077] 首先计算 然后(vx,vy,vz)＝(vx′,vy′,vz′)，其中T为本机状态监控系统更新时间周期。
[0078] 进一步，所述航迹规划模块规划航迹的方法包括：
[0079] 步骤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；
[0080] 步骤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时刻的位置
为，
[0081]
[0082] 其中(vx,vy,vz)和(ax,ay,az)分别为本机在tk时的速度和过载；
[0083] 障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
[0084]
[0085] 其中，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)]′，
和
[0086]
[0087] 步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4
和步骤5，否则，完成航迹规划。
[0088] 本发明的另一方面提供的一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上所述的方法的步骤。
[0089] 本发明提供的自动防撞航迹规划方法及系统，通过建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域，降低了系统地形扫描计算量和数据存储量；通过对最
近邻冲突面进行坐标点转换，将面目标转换成三个能够引导本机实施规避的点目标，降低
了系统计算量；通过建立虚拟力场，引导本机按自动产生最优规避机动参数。
[0090] 相比于现有自动防撞地方法，本发明提供的方法计算量小，占用存储空间少，指导飞机实现实时多维防撞地航迹规划，既能满足复杂地形低空飞行快速反应需求，又能充分
利用飞机机动性能。
附图说明
[0091] 本发明将通过例子并参照附图的方式说明，其中：
[0092] 图1为本发明的本机机体坐标系示意图；
[0093] 图2为与本发明的实施例一致的前向柱状自动防撞地模型示意图；
[0094] 图3为与本发明的实施例一致的防撞地航迹规划示意图。
具体实施方式
[0095] 本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
[0096] 本说明书中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子
而已。
[0097] 本发明的自动防撞地航迹规划方法，包括前向柱状防撞地探测空域和冲突空域创建、障碍物最近邻冲突面坐标变换、虚拟作用力形成、本机运动参数和完整规避航迹规划。
[0098] 在一些实施例中，自动防撞地航迹规划方法具体包括以下步骤：
[0099] 步骤1，建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域，在一些实施例中，本机机体坐标系以本机质心位置为起点，本机飞行方向为正方向；
[0100] 步骤2，对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
[0101] 步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
[0102] 步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；
[0103] 步骤5，规划完整规避航迹。
[0104] 在一些实施例中，该方法还包括：
[0105] 步骤6，本机按步骤5所规划的航迹飞行；
[0106] 步骤7，依次执行步骤2至步骤6。
[0107] 在一些实施例中，所述步骤1具体包括：
[0108] 步骤11，建立本机机体坐标系，其中坐标轴原点O为飞机的质心，OX轴位于飞机参考平面内平行于机身轴线并指向飞机前方，OY轴垂直于飞机参考面并指向飞机右方，OZ轴
在参考面内垂直于XOY平面，指向飞机下方，如图1所示。
[0109] 步骤12，建立探测空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为R，长度为L的圆柱体，如图2所示。半径R和长度L的选择与本机机动能力
以及机载探测能力有关。在一些实施例中，半径R和长度L的计算方法分别为：R＝amax×g×
2
△t/2+△R和L＝vmax×△t，其中，amax为本机最大过载；g为本地重力加速度；△t为规定最后
碰撞时刻；△R为预留参数，在一些实施例中，△R不小于机身长度的一半；vmax为本机最大极
限速度。
[0110] 步骤13，建立冲突空域，以当前本机质心位置为起点，本机转滚轴正向为圆柱体轴，建立截面半径为r，长度为L的圆柱体，如图2所示。半径r的选择与本机几何外形以及紧
急机动极限有关，并满足r≤R。在一些实施例中，半径r的计算方法为：r＝lw/2+△r，其中，lw
为本机机翼长；△r为预留参数，在一些实施例中，△r不小于机舱横切面高度。
[0111] 在一些实施例中，所述步骤2具体包括：
[0112] 步骤21，将障碍物离散化为与冲突域截面平行的冲突面，按照冲突面中心与本机质心径向距离大小进行冲突排序，距离最小的冲突面所对应的障碍物标记为首要障碍；
[0113] 步骤22，选取首要障碍所在冲突面与X轴的交点(xc,0,0)为最近邻正向冲突点；
[0114] 步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0,ycl,zcl)和(0,ycr,zcr)，其中ycl≤
ycr，将上述两个点分别转换为最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)，如
图2所示。
[0115] 在一些实施例中，所述步骤3具体包括：
[0116] 步骤31，获取最近邻正向冲突点(xc,0,0)与本机质心的距离d。
[0117] 步骤32，计算最近邻正向冲突点对本机的虚拟作用力Fc：Fc＝kc(L‑d)，其中kc的选vmax
择与本机爬升机动能力有关。在一些实施例中，kc的计算方法为：kc＝a ×g×m/R，其中，
avmax为本机最大爬升过载，m为本机质量。
[0118] 步骤33，计算虚拟作用力Fc对本机的作用力方向，Fc与X轴夹角为αc＝π‑arctg(R/d)，Fc与Y轴夹角为π/2。
[0119] 步骤34，获取最近邻左冲突点(xc,ycl,0)和最近邻右冲突点(xc,ycr,0)对本机质心的距离dl和dr。
[0120] 步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力Fsl和Fsr：Fsl＝ks(L‑dl)，Fsr＝ks(L‑dr)，其中ks的选择与本机转向机动能力有关。在一些实施例中，ks的
计算方法为：ks＝ahmax×g×m/R，其中，ahmax为本机水平最大过载，m为本机质量。
[0121] 步骤36，计算虚拟作用力Fsl和Fsr对本机的作用力方向，Fsl和Fsr与X轴形成的夹角为αc，与Y轴形成的夹角分别为αl＝π‑arccos(ycl/dl)和αr＝π‑arccos(ycr/dr)。
[0122] 在一些实施例中，所述步骤4具体包括：
[0123] 步骤41，计算本机机体坐标系虚拟合力分量(Fx,Fy,Fz)，其计算方法为：
[0124]
[0125] 步骤42，计算本机在虚拟合力作用下的过载分量(ax,ay,az)，其计算方法为：
[0126] 其中m为本机质量。
[0127] 步骤43，过载取值约束，若 则修正过载分量。在一些实施例中，修正的方法为：
[0128] 首先计算修正过载参数 然后(ax,ay,az)＝(ax′,ay′,az′)。
[0129] 步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(vx,vy,vz)，其更新方法为：
[0130] 首先计算 然后(vx,vy,vz)＝(vx′,vy′,vz′)，其中T为本机状态监控系统更新时间周期。
[0131] 在一些实施例中，所述步骤5具体包括：
[0132] 步骤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；。
[0133] 步骤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时刻的位置
为，
[0134]
[0135] 其中(vx,vy,vz)和(ax,ay,az)分别为本机在tk时的速度和过载；
[0136] 障碍物关键点tk+1时刻在新本机机体坐标系中的位置为，
[0137]
[0138] 其中，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)]′，
和
[0139]
[0140] 步骤53，建立以tk时刻本机机体坐标系为基础的前向柱状防撞地探测空域和冲突空域，判断首要障碍是否在冲突空域内，若在冲突空域内，则依次执行步骤2、步骤3、步骤4
和步骤5，否则，完成航迹规划，如图3所示。
[0141] 在一些实施例中，所述步骤6具体包括：
[0142] 步骤61，将本机的当前位置与步骤5规划的航迹点转换为地理坐标；
[0143] 步骤62，以本机的当前位置为起点，若当前时刻位置不是规划航迹结束点，则以下一时刻航迹规划点为目标位置，调整飞行控制参数，否则，结束对当前首要障碍防撞规避状
态。
[0144] 在一些实施例中，本发明公开了一种自动防撞地航迹规划系统，该系统包括：
[0145] 防撞地安全空域构建模块，用于建立基于本机机体坐标系的前向柱状防撞地探测空域和冲突空域；
[0146] 障碍物坐标点转换模块，用于对地形进行防撞地冲突探测，若冲突空域内存在障碍物，则判定存在撞地冲突，选取最近邻冲突面为首要障碍，对首要障碍进行坐标点转换；
[0147] 虚拟作用力计算模块，用于根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；
[0148] 运动参数计算模块，用于根据运动力学原理，计算本机在虚拟作用力下的运动参数；
[0149] 航迹规划模块，用于规划完整规避航迹。
[0150] 本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。
