基于两阶段稀疏舒尔补的面向视觉惯性定位方法

专利主权项内容

1.基于两阶段稀疏舒尔补的面向视觉惯性定位方法，其特征在于，包括下列步骤：步骤S1，输入待定位目标的初始位姿；步骤S2，基于置于待定位目标上或待定位目标携带的视觉传感器和惯性传感器，分别获取视觉数据和惯性数据；步骤S3，确定待定位目标的当前位姿：若当前为首次定位处理，则以输入的初始位姿作为待定位目标的当前位姿；否则，以上一时刻更新后的位姿得到待定位目标的当前位姿；步骤S4，基于当前位姿、当前获取的视觉数据和惯性数据构建海森矩阵及其标准向量；步骤S5，对海森矩阵和标准向量进行两阶段稀疏舒尔补处理，得到最终的面向视觉惯性同步定位的待求解方程组；步骤S6，对步骤S5得到的待求解方程组进行求解，得到视觉变量；再并行求解惯性变量和路标点变量；步骤S7，基于当前求解得到的视觉变量、惯性变量和路标点变量更新待定位目标在当前时刻的位姿；步骤S8，检测是否收到同步定位结束指令，若否，则返回步骤；若是，则输出所有时刻的位姿和路标点变量，基于所有时刻的位姿构成待定位目标的轨迹图，基于所有时刻的路标点变量构成待定位目标的稀疏的点云地图。