一种基于无迹卡尔曼滤波的偏振光SLAM方法技术

本发明专利技术公开了一种基于无迹卡尔曼滤波的偏振光SLAM方法，属于无人机自主导航的范畴。该方法结合无人机的状态模型和基于激光雷达传感器、偏振光传感器的量测模型，通过无迹卡尔曼滤波，即UKF算法，实现无人机位置的确定和周围环境地图的构建，利用偏振光信息和激光雷达信息匹配互补、不受其他外界干扰的特性，提高了无人机SLAM系统的稳定性和精度。

【技术实现步骤摘要】
一种基于无迹卡尔曼滤波的偏振光SLAM方法
本专利技术涉及无人机同时定位与构图(SLAM)属于无人机自主导航的范畴，具体涉及一种基于无迹卡尔曼滤波的偏振光SLAM方法，对无人机如何确定自身位置及感知外界环境的问题，SLAM系统旨在通过无人机系统模型，结合相应的滤波方法完成无人机的定位与周围环境的绘制。
技术介绍
SLAM是SimultaneousLocalizationandMapping缩写，意为“同时定位与建图”。它是指运动体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，SLAM技术已经被运用于无人机、无人驾驶、机器人、AR、智能家居等领域。SLAM研究侧重于使用滤波器理论，最小化运动体位姿和地图的路标点的噪声，一般采用里程计的输入作为预测过程的输入，激光传感器的输出作为更新过程的输入。扩展卡尔曼滤波(ExtendedKalmanFilter,EKF)滤波算法是目前大多数SLAM采用的滤波算法，缺点是线性化处理时需要用雅克比(Jacobian)矩阵，其繁琐的计算过程导致该方法实现相对困难，而UKF使用无迹(UT)变换来处理均值和协方差的非线性传递，不需要用求解雅克比矩阵，精度较高。近年来无人机的发展非常迅速，而将SLAM导航用于无人机可以解决无人机自身位置不确定，航迹推算中位置误差累积的情况，利用自身携带的传感器，反复探测环境中的特征，从而完成自身位置及特征位置的校正，同时构建环境地图，无需预知地图信息或依靠外部辅助设备，即可完成无人机位置信息和周围环境地图的构建，但是这种SLAM方案的定位精度和构图精确度都不高，鲁棒性很差。近年来对偏振光的研究越来越多，1871年英国著名物理学家瑞利提出了瑞利散射定律，揭示了光线散射特性，随后人们基于瑞利散射定律获得了全天域大气偏振分布模式。大气偏振分布模式相对稳定，其中蕴涵着丰富的导航信息，自然界中很多生物都能够利用天空偏振光进行导航或辅助导航。偏振光导航机制是一种非常有效的导航手段，具有无源、无辐射、隐蔽性好等特点,能够为复杂环境下的导航任务提供新的解决途径。将偏振光用于无人机SLAM上可以提高无人确定自身位置的和构建周围环境地图的精确度，解决鲁棒性不高的问题。
技术实现思路
本专利技术主要解决的问题是：将自然界中广泛存在的偏振光信息应用到无人机SLAM中以解决无人机同时定位与构图中存在的自身位置确定困难、环境适应性差、构图不够精确的问题。本专利技术采用的技术方案为：一种基于无迹卡尔曼滤波的偏振光SLAM方法，包括以下步骤：(1)选取无人机的姿态、速度、位置和路标点的位置为系统状态，建立无人机的动力学模型；(2)建立激光雷达的量测模型；(3)建立偏振光传感器的量测模型；(4)系统初始化、地图初始化；(5)路标点匹配；(6)利用路标点的激光雷达数据和偏振传感器数据，设计UKF滤波器，估计无人机位置、速度、姿态和路标点的位置；(7)地图更新；其中，所述步骤(1)选取无人机的姿态、速度、位置和路标点为系统状态，建立无人机的动力学模型；以无人机起始位置为原点的世界坐标系，即w系，以正北方向为世界坐标系的x轴的正方向，以正西方向为世界坐标系的y轴的正方向，根据右手准则确定世界坐标系的z轴正方向；建立以无人机机体中心为原点的机体坐标系，即b系，以平行于机身纵轴指向机头方向为机体坐标系x轴的正方向，以平行于机身横轴指向左方为机体坐标系y轴的正方向，根据右手准则确定机体坐标系z轴正方向；选取无人机的位置、速度、姿态和路标点的位置作为系统状态；则系统状态转移方程如下：其中表示k-1时刻的系统状态，表示k-1时刻无人机在世界坐标系下的坐标，分别表示k-1时刻无人机在世界坐标系下x,y,z三个轴方向的坐标，表示k-1时刻无人机在世界坐标系下的速度，分别表示k-1时刻无人机在世界坐标系下指向x,y,z三个轴方向的速度，表示无人机的姿态，以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角，表示k-1时刻世界坐标系下的m个路标点的坐标，表示第i,i＝1,2,...,m个路标点在世界坐标系下的坐标，为k-1时刻机体坐标系下的比力加速度，为k-1时刻机体坐标系下的角速度，为k-1时刻世界坐标下的重加速度，△t为采样时间间隔，为k-1时刻机体坐标系到世界坐标系的变换矩阵，nk-1为系统噪声，噪声设定为高斯白噪声，Qk-1为系统噪声协方差矩阵；其中，所述步骤(2)建立激光雷达的量测模型；选取激光雷达测量的距离、角度作为测量值，针对激光雷达量测的数据给出路标点的量测模型：其中表示第k时刻对第i个路标点的量测方程中的非线性函数，yk,i,lidar＝[dk,iθk,iλk,i]T表示k时刻系统的量测值，dk,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的距离，θk,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的距离俯仰角，λk,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的方位角，uk,lidar表示激光雷达量测噪声，噪声设定为高斯白噪声，Rk,lidar为量测噪声协方差矩阵；其中，所述步骤(3)建立偏振光传感器的量测模型：选取偏振光传感器量测的机体纵轴与太阳子午线的夹角量作为测值，针对偏振光给出的量测数据的观测模型如下：其中为偏振光传感器获取的机体长轴与太阳子午线的夹角，为太阳方位角，为太阳高度角，为太阳赤纬，为观测点的纬度，t为太阳时角，uk,polar表示偏振光传感器量测噪声，噪声设定为高斯白噪声，Rk,polar为量测噪声协方差矩阵；其中，所述步骤(4)系统初始化；将无人机坐标系转换到世界坐标系并将无人机，给定初始时刻的系统状态为初始协方差矩阵P0，并建立以无人机起始位置为原点的全局地图；其中，所述步骤(5)路标点匹配；对k时刻激光雷达量测到的数据，和在全局地图中存储的路标点利用迭代最近点(ICP)算法进行匹配，对于匹配成功的路标点，则将该路标点送入相应的滤波器进行后续的运算；其中，所述步骤(6)利用路标点的激光雷达数据和偏振光传感器数据，设计UKF滤波器,估计无人机的位置、速度、姿态和路标点的位置；首先将k-1时刻的估计状态和协方差矩阵，结合k-1时刻的IMU的输出数据，进行一步递推得到由IMU和状态转移矩阵推算出的机器人位置，然后根据预测值，加入激光传感器信息和偏振光信息进行量测更新的计算，输出无人机的位置、速度、姿态和路标点的位置，并同时输出相应的协方差矩阵，具体步骤如下：时间更新其中为状态向量维度，Pk-1|k-1为k-1时刻的误协方差矩阵，为尺度参数，计算方法为该式中ε为一小量取值范围为[10-4,1]，常量可取0或者3-n；量测更新Kk＝Pxy,k|k-1Pyy,k|k-1-1Pk|k＝Pk|k-1-KkPyy,k|k-1KkT其中Pyy,k|k-1为自协方差矩阵，Pxy,k|k-1为互协方差矩阵；其中，所述步骤(7)地图、状态更新；对于全局地图中已存在的路标点，将对应的滤波后的路标点作为最新路标点，对步骤(5)中未匹配成功的路标点，直接将其加入到全局地图中。本专利技术与现有技术相比的优点在于：本专利技术公开了一种基于无迹卡尔曼滤波的偏振光SLAM方法，属于无人机自主导航的范畴。该方法结合无人机的状态模型和基于激光雷达传感器、偏振光传感器的量测模本文档来自技高网...

【技术保护点】
1.一种基于无迹卡尔曼滤波的偏振光SLAM方法，其特征在于：包括以下步骤：(1)选取无人机的姿态、速度、位置和路标点的位置为系统状态，建立无人机的动力学模型；(2)建立激光雷达的量测模型；(3)建立偏振光传感器的量测模型；(4)系统初始化、地图初始化；(5)路标点匹配；(6)利用路标点的激光雷达数据和偏振传感器数据，设计UKF滤波器，估计无人机位置、速度、姿态和路标点的位置；(7)地图更新。

【技术特征摘要】
1.一种基于无迹卡尔曼滤波的偏振光SLAM方法，其特征在于：包括以下步骤：(1)选取无人机的姿态、速度、位置和路标点的位置为系统状态，建立无人机的动力学模型；(2)建立激光雷达的量测模型；(3)建立偏振光传感器的量测模型；(4)系统初始化、地图初始化；(5)路标点匹配；(6)利用路标点的激光雷达数据和偏振传感器数据，设计UKF滤波器，估计无人机位置、速度、姿态和路标点的位置；(7)地图更新。2.根据权利要求1所述的一种基于无迹卡尔曼滤波的偏振光SLAM方法，其特征在于：所述步骤(1)选取无人机的姿态、速度、位置和路标点为系统状态，建立无人机的动力学模型；以无人机起始位置为原点的世界坐标系，即w系，以正北方向为世界坐标系的x轴的正方向，以正西方向为世界坐标系的y轴的正方向，根据右手准则确定世界坐标系的z轴正方向；建立以无人机机体中心为原点的机体坐标系，即b系，以平行于机身纵轴指向机头方向为机体坐标系x轴的正方向，以平行于机身横轴指向左方为机体坐标系y轴的正方向，根据右手准则确定机体坐标系z轴正方向；选取无人机的位置、速度、姿态和路标点的位置作为系统状态；则系统状态转移方程如下：其中表示k-1时刻的系统状态，表示k-1时刻无人机在世界坐标系下的坐标，分别表示k-1时刻无人机在世界坐标系下x,y,z三个轴方向的坐标，表示k-1时刻无人机在世界坐标系下的速度，分别表示k-1时刻无人机在世界坐标系下指向x,y,z三个轴方向的速度，表示无人机的姿态，以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角，表示k-1时刻世界坐标系下的m个路标点的坐标，表示第i,i＝1,2,...,m个路标点在世界坐标系下的坐标，为k-1时刻机体坐标系下的比力加速度，为k-1时刻机体坐标系下的角速度，为k-1时刻世界坐标下的重加速度，△t为采样时间间隔，为k-1时刻机体坐标系到世界坐标系的变换矩阵，nk-1为系统噪声，噪声设定为高斯白噪声，Qk-1为系统噪声协方差矩阵。3.根据权利要求1所述的一种基于无迹卡尔曼滤波的偏振光SLAM方法，其特征在于：所述步骤(2)建立激光雷达的量测模型；选取激光雷达测量的距离、角度作为测量值，针对激光雷达量测的数据给出路标点的量测模型：其中i＝1,2,...,m表示第k时刻对第i个路标点的量测方程中的非线性函数，yk,i,lidar＝[dk,iθk,iλk,i]T表示k时刻系统的量测值，dk,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的距离，θk,i表示第k时刻激光雷达测量...

【专利技术属性】
技术研发人员：杜涛，白鹏飞，郭雷，王华锋，刘万泉，王月海，
申请(专利权)人：北方工业大学，北京航空航天大学，
类型：发明
国别省市：北京,11