[发明专利]一种自动防撞地航迹规划方法及系统

权利要求书

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轴的交点(x_c，0，0)为最近邻正向冲突点；

步骤23，将首要障碍所在冲突面在YOZ平面上投影，选取投影面落在冲突空域内的部分Y坐标值最小和Y坐标值最大的两个点，分别记为(0，y_cl，z_cl)和(0，y_cr，z_cr)，其中y_cl≤y_cr，将上述两个点分别转换为最近邻左冲突点(x_c，y_cl，0)和最近邻右冲突点(x_c，y_cr，0)；

步骤3，根据首要障碍位置计算等效冲突点径向距离、等效冲突角，形成冲突面对本机的虚拟作用力与作用方向角；

所述步骤3具体包括：

步骤31，获取最近邻正向冲突点(x_c，0，0)与本机质心的距离d；

步骤32，计算最近邻正向冲突点对本机的虚拟作用力F_c：F_c＝k_c(L-d)，其中k_c＝a_vmax×g×m/R，a_vmax为本机最大爬升过载，m为本机质量；

步骤33，计算虚拟作用力F_c对本机的作用力方向，Fc与X轴夹角为α_c＝π-arctg(R/d)，F_c与Y轴夹角为π/2；

步骤34，获取最近邻左冲突点(x_c，y_cl，0)和最近邻右冲突点(x_c，y_cr，0)对本机质心的距离d_l和d_r；

步骤35，计算最近邻左冲突点和最近邻右冲突点对本机的虚拟作用力F_sl和F_sr：F_sl＝k_s(L-d_l)，F_sr＝k_s(L-d_r)，其中k_s＝a_hmax×g×m/R，a_hmax为本机水平最大过载，m为本机质量；

步骤36，计算虚拟作用力F_sl和F_sr对本机的作用力方向，F_sr和F_sr与X轴形成的夹角为α_c，与Y轴形成的夹角分别为α_l＝π-arccos(y_cl/d_l)和α_r＝π-arccos(y_cr/d_r)；

步骤4，根据运动力学原理，计算本机在虚拟作用力下的运动参数；

所述步骤4具体包括：

步骤41，计算本机机体坐标系虚拟合力分量(F_x，F_y，F_z)，其计算方法为：

步骤42，计算本机在虚拟合力作用下的过载分量(a_x，a_y，a_z)，其计算方法为：

其中m为本机质量；

步骤43，过载取值约束，若则修正过载分量，修正方法为：

首先计算修正过载参数然后(a_x，a_y，a_z)＝(a_x′，a_y′，a_z′)；

步骤44，根据修正后的过载分量更新计算本机防撞地规避运动速度(v_x，v_y，v_z)，其更新方法为：

首先计算然后(v_x，v_y，v_z)＝(v_x′，v_y′，v_z′)，其中T为本机状态监控系统更新时间周期；

步骤5，规划完整规避航迹。