[发明专利]一种基于无迹卡尔曼滤波的偏振光SLAM方法

权利要求书

1.一种基于无迹卡尔曼滤波的偏振光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时刻机体坐标系到世界坐标系的变换矩阵，n_k-1为系统噪声，噪声设定为高斯白噪声，Q_k-1为系统噪声协方差矩阵；

所述步骤(2)建立激光雷达的量测模型；选取激光雷达测量的距离、角度作为测量值，针对激光雷达量测的数据给出路标点的量测模型：

其中

表示第k时刻对第i个路标点的量测方程中的非线性函数，y_k,i,lidar＝[d_k,i θ_k,i λ_k,i]^T表示k时刻系统的量测值，d_k,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的距离，θ_k,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的距离俯仰角，λ_k,i表示第k时刻激光雷达测量的无人机质心与第i个路标点的方位角，u_k,lidar表示激光雷达量测噪声，噪声设定为高斯白噪声，R_k,lidar为量测噪声协方差矩阵；

所述步骤(3)建立偏振光传感器的量测模型：选取偏振光传感器量测的机体纵轴与太阳子午线的夹角量作为测值，针对偏振光给出的量测数据的观测模型如下：

其中为偏振光传感器获取的机体长轴与太阳子午线的夹角，为太阳方位角，为太阳高度角，为太阳赤纬，为观测点的纬度，为太阳时角，u_k,polar表示偏振光传感器量测噪声，噪声设定为高斯白噪声，R_k,polar为量测噪声协方差矩阵；

所述步骤(4)系统初始化；将无人机坐标系转换到世界坐标系并将无人机，给定初始时刻的系统状态为初始协方差矩阵P₀，并建立以无人机起始位置为原点的全局地图；

所述步骤(5)路标点匹配；对k时刻激光雷达量测到的数据，和在全局地图中存储的路标点利用迭代最近点(ICP)算法进行匹配，对于匹配成功的路标点，则将该路标点送入相应的滤波器进行后续的运算；

所述步骤(6)利用路标点的激光雷达数据和偏振光传感器数据，设计UKF滤波器，估计无人机的位置、速度、姿态和路标点的位置；首先将k-1时刻的估计状态和协方差矩阵，结合k-1时刻的IMU的输出数据，进行一步递推得到由IMU和状态转移矩阵推算出的机器人位置，然后根据预测值，加入激光传感器信息和偏振光信息进行量测更新的计算，输出无人机的位置、速度、姿态和路标点的位置，并同时输出相应的协方差矩阵，具体步骤如下：

时间更新：

其中为状态向量维度，P_k-1|k-1为k-1时刻的误协方差矩阵，为尺度参数，计算方法为该式中ε为一小量取值范围为[10^-4,1]，常量可取0或者3-n；

量测更新：

P_k|k＝P_k|k-1-K_kP_yy,k|k-1K_k^T

其中P_yy,k|k-1为自协方差矩阵，P_xy,k|k-1为互协方差矩阵；

所述步骤(7)地图、状态更新；对于全局地图中已存在的路标点，将对应的滤波后的路标点作为最新路标点，对步骤(5)中未匹配成功的路标点，直接将其加入到全局地图中。