[发明专利]一种单面结构环境下的动力学模型/激光雷达组合导航方法

权利要求书

1.一种单面结构环境下的动力学模型/激光雷达组合导航方法，其特征在于，包括如下步骤：

(1)周期读取k时刻四旋翼飞行器机载传感器信息，包括激光雷达点云信息S(k)，陀螺仪信息加速度计信息气压高度计信息h_b(k)；

(2)利用S(k)提取k时刻单面结构环境的线特征；具体包括如下步骤：

(21)计算S(k)中激光点投影至飞行器所在水平面的二维坐标：

记p_i为S(k)的第i个激光点，i＝1,2,…,N₀，N₀为S(k)中激光点的数量，和为p_i在机体系中的坐标，记θ(k-1)和φ(k-1)分别为飞行器在k-1时刻的俯仰角和横滚角，p_i投影至飞行器所在水平面的二维坐标和根据下式计算：

(22)检测S(k)中的撕裂点：

计算S(k)相邻序号激光点之间的距离若大于阈值E_t，则p_i和p_i+1为撕裂点，记撕裂点集合为N_t为撕裂点的数量；

(23)检测S(k)中的角点，将点云数据分群：

若dc大于阈值E_c则p_i为角点，且撕裂点不是角点，即dc的计算公式如下：

记角点集合为N_c为角点的数量，将角点和撕裂点作为分群的标志，根据激光点在S(k)中的序号依次从p₁遍历到将点云数据分群，记为s_g，1≤g≤N_g，N_g为点群的数量；

(24)筛选计算单面结构环境的线特征的点云数据：

根据激光点在S(k)的序号，计算每个点群中序号最小的点与序号最大的点之间的距离筛选出最大的点群用于计算单面结构环境的线特征，该点群记为

(25)利用点群拟合单面结构环境的直线方程；

记单面结构环境的直线方程为x＝a₁y+a₂，记q_i为的第i个激光点(i＝1,2,…,N₁)，N₁为中激光点的数量，和为q_i投影至飞行器所在水平面的二维坐标，根据下式计算直线方程的参数a₁和a₂：

(3)计算k时刻四旋翼飞行器相对于单面结构环境的相对距离、相对偏航角；具体包括如下步骤：

(31)若a₁＝0，则四旋翼飞行器相对于单面结构环境的距离d(k)为a₂，偏航角ψ_l(k)为：

(32)若a₁≠0，则四旋翼飞行器相对于单面结构环境的距离偏航角ψ_l(k)为：

其中，四旋翼飞行器机头方向垂直指向于单面结构时的偏航角为0，顺时针旋转为正；

(4)预测k时刻四旋翼飞行器的姿态四元数、速度、位置；

(5)通过卡尔曼滤波器，结合四旋翼飞行器的阻力特性，对k时刻的姿态四元数、速度、位置进行校正；其中，机体系的X、Y、Z轴分别为飞行器的机头方向、右向、下向，激光雷达与机体系相固连，导航系的Z轴竖直向下，X轴垂直指向单面结构，Y轴根据右手定则确定，导航系原点为初始时刻机体系原点的位置。