[发明专利]一种基于SLAM的故障卫星相对导航方法

权利要求书

1.一种基于SLAM的故障卫星相对导航方法，其特征在于，具体包括以下步骤：

步骤一，首先将传感器获取的故障卫星的深度信息转化为三维点云；

步骤二，采用随机采样一致性算法(RANSAC,Random Sample Consensus)提取出目标点云中的平面数据；

步骤三，基于平面几何尺寸的平面特征识别，所述平面特征包括矩形面特征识别和圆特征识别；

步骤四，基于已识别的圆特征和矩形面特征，建立组合特征本体坐标系，计算其与传感器测量坐标系之间的相对位姿参数；

步骤五，将步骤四中的相对位姿参数作为观测数据，利用EKF-SLAM算法估计故障卫星与服务星的相对位置、故障卫星姿态，以及组合特征在故障卫星中的位姿参数。

2.根据权利要求1所述的一种基于SLAM的故障卫星相对导航方法，其特征在于，根据RANSAC算法，从点云数据中提取平面的具体步骤如下：

步骤2.1，在点云数据集中随机选择三个点P₁(x₁,y₁,z₁)、P₂(x₂,y₂,z₂)、P₃(x₃,y₃,z₃)；

步骤2.2，由P₁、P₂、P₃三点确定一个平面S；

步骤2.3，统计平面S上点的数量，设平面厚度为2ε，即阈值为ε，计算点云中任意一点P_i(x_i,y_i,z_i)到平面的距离，有：

其中，d_i为P_i到平面的距离；

步骤2.4，统计符合d_i＜ε条件的点的个数；

步骤2.5，重复步骤2.1～步骤2.4K次，选择出满足d_i＜ε的数量最多的平面，其中K的计算公式为：

其中，τ为位于平面外点所占比例的值，Φ为经过K次采样后最优平面被选中的概率。

3.根据权利要求1所述的一种基于SLAM的故障卫星相对导航方法，其特征在于，所述步骤三中，在矩形面特征识别时，通过点云分布矩阵和特征质心在传感器坐标系下的位置计算特征本体坐标系与传感器测量坐标系之间的姿态旋转矩阵和平移矩阵；并据此将点云坐标转化到点云平面点云本体坐标系下，以准确计算平面几何尺寸。