本发明涉及无人机同时定位与构图(slam)属于无人机自主导航的范畴，具体涉及一种基于扩展卡尔曼滤波的偏振光slam方法，对无人机如何确定自身位置及感知外界环境的问题，slam系统旨在通过无人机系统模型，结合相应的滤波方法完成无人机的定位与周围环境的绘制。
背景技术：
slam是的simultaneouslocalizationandmapping缩写，意为“同时定位与建图”。它是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，slam技术已经被运用于无人机、无人驾驶、机器人、ar、智能家居等领域。
slam研究侧重于使用滤波器理论，最小化运动体位姿和地图的路标点的噪声，一般采用里程计的输入作为预测过程的输入，激光传感器的输出作为更新过程的输入。扩展卡尔曼滤波(extendedkalmanfilter,ekf)滤波算法是目前大多数slam方法采用的滤波算法。近年来无人机的发展非常迅速，而将slam用于无人机可以解决无人机自身位置不确定，航迹推算中位置误差累积的情况，利用自身携带的传感器，反复探测环境中的特征，从而完成自身位置及特征位置的校正，同时构建环境地图，无需预知地图信息或依靠外部辅助设备，即可完成无人机位置信息和周围环境地图的构建，但是这种slam方案的定位精度和构图精确度都不高，鲁棒性很差。
近年来对偏振光的研究越来越多，1871年英国著名物理学家瑞利提出了瑞利散射定律，揭示了光线散射特性，随后人们基于瑞利散射定律获得了全天域大气偏振分布模式。大气偏振分布模式相对稳定，其中蕴涵着丰富的导航信息，自然界中很多生物都能够利用天空偏振光进行导航或辅助导航。偏振导航机制是一种非常有效的导航手段，具有无源、无辐射、隐蔽性好等特点,能够为复杂环境下的导航任务提供新的解决途径。
将偏振光用于无人机slam上可以提高无人确定自身位置的和构建周围环境地图的精确度，解决鲁棒性不高的问题。
技术实现要素：
本发明主要解决的问题是：如何将偏振信息应用到无人机slam中以解决无人机同时定位与构图中存在的自身位置确定困难、环境适应性差、构图不够精确的问题。
本发明采用的技术方案为：一种基于扩展卡尔曼滤波的偏振光slam方法，包括以下步骤：
(1)选取无人机的姿态、速度、位置和路标点为系统状态，建立无人机的动力学模型；
(2)建立激光雷达的量测模型；
(3)建立偏振光传感器的量测模型；
(4)系统初始化、地图初始化；
(5)路标点匹配；
(6)利用路标点的激光雷达数据和偏振光传感器数据，通过分布式扩展卡尔曼滤波器，估计无人机位置、速度、姿态和路标点的位置；
(7)地图更新；
其中，所述步骤(1)选取无人机的姿态、速度、位置和路标点为系统状态，建立无人机的动力学模型；以无人机起始位置为原点的世界坐标系，即w系，以正北方向为世界坐标系的x轴的正方向，以正西方向为世界坐标系的y轴的正方向，根据右手准则确定世界坐标系的z轴正方向；建立以无人机机体中心为原点的机体坐标系，即b系，以平行于机身纵轴指向机头方向为机体坐标系x轴的正方向，以平行于机身横轴指向左方为机体坐标系y轴的正方向，根据右手准则确定机体坐标系z轴正方向；选取无人机的位置、速度、姿态和路标点的位置作为系统状态；则系统状态转移方程如下：
其中
为k-1时刻机体坐标系到世界坐标系的变换矩阵，nk-1为系统噪声，噪声设定为高斯白噪声，qk-1为系统噪声协方差矩阵；
其中，所述步骤(2)建立激光雷达的量测模型；选取激光雷达测量的距离、角度作为测量值，针对激光雷达的数据给出路标点的量测模型：
其中
其中，所述步骤(3)建立偏振光传感器的量测模型：选取偏振光传感器量测的机体纵轴与太阳子午线的夹角量作为测值，针对偏振光给出的量测数据的观测模型如下：
其中
其中，所述步骤(4)系统初始化；将无人机坐标系转换到世界坐标系并将无人机，给定初始时刻的系统状态为
其中，所述步骤(5)路标点匹配；对k时刻激光雷达量测到的数据，和在全局地图中存储的路标点利用迭代最近点(icp)算法进行匹配，对于匹配成功的路标点，则将该路标点送入相应的子滤波器进行后续的运算，对于匹配失败的路标点，则建立相应的子滤波器，为进行下次迭代做准备，对于未能观测到的路标点删除其相应的子滤波器；
其中，所述步骤(6)利用路标点的激光雷达数据和偏振光传感器数据，通过分布式扩展卡尔曼滤波器，估计无人机的位置、速度、姿态和路标点的位置；首先将k-1时刻的每个子滤波器的估计状态和协方差矩阵
时间更新：
量测更新：
主滤波器加权平均：
其中
其中，所述步骤(7)地图更新；对于全局地图中已存在的路标点，将对应的滤波后的路标点作为最新路标点，对步骤(5)中未匹配成功的路标点，直接将其加入到全局地图中；
本发明与现有技术相比的优点为：
本发明公开了一种基于扩展卡尔曼滤波的偏振光slam方法，属于无人机自主导航的范畴。该方法通过建立无人机的状态模型和基于激光雷达传感器、偏振光传感器的量测模型，利用分布式ekf算法实现无人机自身位置的确定和周围环境地图的构建，有效的利用了偏振信息和激光雷达信息互补、不受其他外界干扰的特性，提高了无人机位置估计的精度。在保持激光雷达量测信息不变的情况下，利用偏振光量测误差不随时间累积的优点，降低了长时间情况下偏航角的误差的累积、提高了全局位姿的准确性。使无人机在位置环境中的定位与构图精度更高，对未知环境的适应能力增强。
附图说明
图1为一种基于扩展卡尔曼滤波的偏振光slam方法流程图；
图2为一种基于扩展卡尔曼滤波的偏振光slam方法仿真实验结果图。
具体实施方式
下面结合附图以及具体实施方式进一步说明本发明。
如图1所示，本发明一种基于扩展卡尔曼滤波的偏振光slam方法，包括以下步骤：
(1)选取无人机的姿态、速度、位置和路标点为系统状态，建立无人机的动力学模型；
所述步骤(1)选取无人机的姿态、速度、位置和路标点为系统状态，建立无人机的动力学模型；以无人机起始位置为原点的世界坐标系，即w系，以正北方向为世界坐标系的x轴的正方向，以正西方向为世界坐标系的y轴的正方向，根据右手准则确定世界坐标系的z轴正方向；建立以无人机机体中心为原点的机体坐标系，即b系，以平行于机身纵轴指向机头方向为机体坐标系x轴的正方向，以平行于机身横轴指向左方为机体坐标系y轴的正方向，根据右手准则确定机体坐标系z轴正方向；选取无人机的位置、速度、姿态和路标点的位置作为系统状态；则系统状态转移方程如下：
其中
(2)建立激光雷达的量测模型；
所述步骤(2)建立激光雷达的量测模型；选取激光雷达测量的距离、角度作为测量值，针对激光雷达的数据给出路标点的量测模型：
其中
(3)建立偏振光传感器的量测模型；
所述步骤(3)建立偏振光传感器的量测模型：选取偏振光传感器量测的机体纵轴与太阳子午线的夹角量作为测值，针对偏振光给出的量测数据的观测模型如下：
其中
(4)系统初始化、地图初始化；
所述步骤(4)系统初始化；将无人机坐标系转换到世界坐标系并将无人机，给定初始时刻的系统状态为
(5)路标点匹配；
所述步骤(5)路标点匹配；对k时刻激光雷达量测到的数据，和在全局地图中存储的路标点利用迭代最近点(icp)算法进行匹配，对于匹配成功的路标点，则将该路标点送入相应的子滤波器进行后续的运算，对于匹配失败的路标点，则建立相应的子滤波器，为进行下次迭代做准备，对于未能观测到的路标点删除其相应的子滤波器；
(6)利用路标点的激光雷达数据和偏振光传感器数据，通过分布式扩展卡尔曼滤波器，估计无人机位置、速度、姿态和路标点的位置；
所述步骤(6)利用路标点的激光雷达数据和偏振光传感器数据，通过分布式扩展卡尔曼滤波器，估计无人机的位置、速度、姿态和路标点的位置；首先将k-1时刻的每个子滤波器的估计状态和协方差矩阵
时间更新：
量测更新：
主滤波器加权平均：
其中
(7)地图更新；
所述步骤(7)地图更新；对于全局地图中已存在的路标点，将对应的滤波后的路标点作为最新路标点，对步骤(5)中未匹配成功的路标点，直接将其加入到全局地图中；
如图2所示为仿真实验结果，*点为真实的无人机位姿，三角点为所提出方法估计的无人机位姿，五星点为设定的路标点。经过多次试验，无人机在x轴的平均误差为0.1812米，y轴平均误差为0.2081米，z轴平均误差为0.2532米，位置平均误差为0.4323米。