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

说明书

本发明公开了一种单面结构环境下的动力学模型/激光雷达组合导航方法，包括如下步骤：(1)周期读取k时刻四旋翼飞行器机载传感器信息，包括激光雷达点云信息S(k)，陀螺仪信息加速度计信息气压高度计信息h_b(k)；(2)利用S(k)提取k时刻单面结构环境的线特征；(3)计算k时刻四旋翼飞行器相对于单面结构环境的相对距离、相对偏航角；(4)预测k时刻四旋翼飞行器的姿态四元数、速度、位置；(5)通过卡尔曼滤波器，结合四旋翼飞行器的阻力特性，对k时刻的姿态四元数、速度、位置进行校正。本发明能够在单面结构环境下，完成基于激光雷达的四旋翼飞行器姿态、速度、位置的解算。

技术领域

本发明涉及组合导航领域，尤其是一种单面结构环境下的动力学模型/激光雷达组合导航方法。

背景技术

四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点，近年来在军用、民用领域得到了广泛的应用。导航系统为四旋翼飞行器提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。目前，卫星导航与视觉导航是四旋翼飞行器常用的导航方法。然而，卫星导航信号易受干扰，视觉导航易受光线干扰。当卫星导航、视觉导航不可用时，激光雷达是四旋翼飞行器常用的导航方式。

激光雷达是以发射激光束探测目标的位置、速度等特征量的雷达系统。其工作原理是向目标发射探测信号，然后将接收到的从目标反射回来的信号与发射信号进行比较，作适当处理后，就可获得目标的有关信息。当激光雷达用于导航时，普遍采用同时定位与地图构建方法(Simultaneous Localization And Mapping，简称SLAM)，在载体上通过对各种传感器数据进行采集和计算，实现对其自身位置、姿态的定位以及对场景地图的构建。

在采用激光雷达SLAM方法时，要求周围环境具有较为明显的结构特征，当特征稀疏时会影响SLAM效果。本发明中涉及的单面结构环境，指的是仅有光滑的一个墙面的环境，属于特征稀疏环境。四旋翼飞行器在执行贴墙飞行任务时，激光雷达所探测的环境有时会出现单面结构环境，此时通过传统的激光雷达SLAM方法得到的导航精度较差。

发明内容

本发明所要解决的技术问题在于，提供一种单面结构环境下的动力学模型/激光雷达组合导航方法，能够在单面结构环境下，完成基于激光雷达的四旋翼飞行器姿态、速度、位置的解算。

为解决上述技术问题，本发明提供一种单面结构环境下的动力学模型/激光雷达组合导航方法，包括如下步骤：

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

(2)利用S(k)提取k时刻单面结构环境的线特征；

(3)计算k时刻四旋翼飞行器相对于单面结构环境的相对距离、相对偏航角；

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

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

优选的，步骤(2)中，提取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投影至飞行器所在水平面的二维坐标和根据下式计算：