[发明专利]一种基于因子图优化的仿生偏振同步定位与构图的方法

权利要求书

1.一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于，所述方法包括以下步骤：

步骤1：选取载体姿态、速度、位置作为组合导航系统状态；

步骤2：建立雷达里程计量测模型、建立偏振光相机量测模型和建立惯性导航系统运动模型；

步骤3：对由雷达里程计、偏振光相机和惯性导航系统组成的组合导航系统初始化；

步骤4：获得当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束；

步骤5：根据当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束建立组合导航系统状态和量测节点的误差函数，获得待优化因子图，并对待优化因子图进行优化，获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；

获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下：

步骤(5a)：构建载体(k-m)～k时刻m为正整数的状态节点误差函数：

步骤(5b)：根据状态节点位置约束，建立雷达里程计量测的误差函数为：

其中e_l,k为雷达里程计量测误差，∑_l,k为雷达里程计节点协方差，表示k时刻雷达里程计量测值，其为雷达里程计量测的x,y,z三个轴方向的坐标值；h_l(x_k)为雷达里程计量测函数；

步骤(5c)：根据状态节点航向约束，建立偏振光相机量测的误差函数为：

其中e_p,k为偏振光相机量测误差，∑_p,k为偏振光相机节点协方差，为偏振光相机测量值，为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；h_p(x_k)为偏振光相机测量函数；

步骤(5d)：根据相邻状态节点位姿变换约束，建立惯性导航系统量测的误差函数为：

其中e_i,k为惯性导航系统量测误差，∑_i,k为惯性导航系统运动模型节点协方差，为惯性导航系统在k时刻的量测值；h_i(x_k-1,u_k-1)为惯性导航系统的量测函数；

步骤(5e)：建立因子图优化问题，表示如下：

其中X＝[x_k-m...x_k]^T，为X的最优估计；

步骤(5f)：使用高斯-牛顿法对因子图优化问题进行求解，获得误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值；

对所述误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值更新是对的状态节点的更新，并将对应的更新后的雷达里程计的扫描点注册到全局地图中，再对全局地图进行更新，并对全局地图中重复的雷达里程计扫描点进行剔除；

步骤6：对误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值和地图更新，同步实现载体的定位和构图。

2.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于：通过步骤1所述的组合导航系统状态构建k时刻的组合导航系统的状态节点表示如下：

其中：

x_k为k时刻的组合导航系统的状态节点，表示k时刻载体世界坐标系下的三维位置信息，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；

表示k时刻载体世界坐标系下的三维速度，分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；

表示载体姿态，载体姿态以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角。