[发明专利]一种全局无漂移的自主机器人同时定位与地图构建方法

权利要求书

1.一种全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

将局部定位和全局感知定位融合，实现精确的全局感知定位，

所述局部定位包括采用相机、惯性测量单元、激光雷达进行状态估计；

所述全局感知定位包括全球导航卫星系统、磁力计、气压计进行状态估计。

2.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

将视觉惯性与全球导航卫星系统进行多源融合，所述视觉惯性包括惯性测量单元和相机。

3.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

该方法包括以下步骤：

S1、前端点线特征的跟踪；

S2、后端视觉惯性SLAM位姿态联合优化；

S3、与GNSS位姿融合获得全局位姿。

4.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

在步骤S1中，采用线性误差将线性特征表示为直线端点上的线性约束，从而将线特征自然地整合到基于特征点算法的线性表示中。

5.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

步骤S1包含以下子步骤：

S11、建立点线特征表示观测模型；

S12、建立IMU预积分观测模型。

6.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

在步骤S1中，由相机的光心和p_i,k及q_i,k构成的直线可以确定一个平面，其单位法向量l_i,k表示为：

其中，l₁、l₂、l₃为参数系数，为待求解值，i表示相机不同帧图像，在一帧图像中通常具有多个点特征，观测到的第j个特征点的3D坐标表示为X_w,j∈R³，第k条线段端点为P_w,k、Q_w,k∈R³，其在图像平面的投影坐标为对应的齐次坐标为R³表示三维坐标、R²表示二维坐标；

相机的投影模型表示为π，齐次坐标表示为π^h：

其中，R_w以及t_w分别表示T_iw的旋转和平移，T_iw表示相机第i帧的位姿；w表示世界坐标系；P_w∈R³表示世界坐标系下的点坐标，O是其对应的相机坐标系下的点坐标，f_x、f_y和c_x、c_y分别为相机的焦距和主点；

所述点线特征表示观测模型通过点特征的重投影误差和线特征的重投影误差表示，分别为：

其中，上标p表示点，上标l表示线，x_i，j，y_i，j表示第i帧匹配得到的第j个特征点的像素坐标，和分别为p_i,k和q_i,k到投影直线的距离。

7.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，

在步骤S12中，离散时间为k和k+1之间的IMU预积分观测模型为：

其中，b_m表示当前帧，b_m+1表示下一帧；

表示离散形式的第m个IMU时刻到第m+1个IMU时刻积分得到的的位置、速度和旋转的增量；表示上一时刻前后两帧之间经过中值积分得到的IMU增量信息；表示加速度，表示角速度。