[发明专利]一种基于深度相机和可见光通信的信标地图构建方法

权利要求书

1.一种基于深度相机和可见光通信的信标地图构建方法，其特征在于包括以下步骤：

步骤一、调制LED灯光，并在调制过程中对每个LED灯体分配一个ID编码；

步骤二、调整CCD工业相机，利用工业相机捕捉调制后的LED灯光，获得LED灯体的ID消息以及灯体中心的像素坐标，并通过里程计获得偏航角度；

步骤三、对t时刻的终端航向角α*、工业相机捕获的LED灯光图片以及LED灯体的ID消息经特征化算法得到每一个位姿下的可见光定位的特征；

步骤四、利用深度相机捕获周围环境的RGBD图像，利用基于ORB-SLAM2算法的LoopClosing线程得到t时刻的深度相机位姿，再经过TF变换转换成LED灯光捕获装置的位姿；

步骤五、对利用深度相机得到的LED灯光捕获装置的位姿及每个位姿下的可见光定位特征进行时间戳上的匹配，得到一个初始可见光定位特征的地图；

步骤六、对初始可见光定位特征的地图进行基于ID上的分类，同时对处于观测到同一个ID特征下预估对应ID的灯具中心的初始世界坐标；

步骤七、利用得到的初始灯具中心的世界坐标及t时刻下的位姿，采用观测函数预估t时刻下的可见光定位的特征，优化灯具中心的世界坐标与工业相机位姿；

步骤八、对经过能观测到之前相同ID的有效范围内的一段位姿通过ICP执行闭环相似检测，减少位姿的误差。

2.根据权利要求1所述的一种基于深度相机和可见光通信的信标地图构建方法，其特征在于：所述步骤一中对LED灯光的调制是使LED灯光的发光芯片按照调制的频率进行闪烁，闪烁频率对应于调制的信号。

3.根据权利要求1所述的一种基于深度相机和可见光通信的信标地图构建方法，其特征在于：所述步骤二中调整CCD工业相机时使工业相机的相机坐标系的y_c轴与turtlebot2的前行方向一致，所述步骤二中具体包括

1)获得灯体的ID消息，将CCD工业相机水平安装在捕获装置上，并调整捕获装置使其捕捉到不多于一个LED灯体的LED灯光，捕获LED灯光后根据LED灯光中调制的内容解析出LED灯体的ID；

2)获得LED灯体的灯具中心，先将图像转换为灰度图，利用高斯模糊处理减少高频噪声并对图像执行二值化，检测到灯具中心区域图像的轮廓后计算轮廓区域的图像矩，获取轮廓中心；

3)获取偏航角，通过turtlebot2订阅navigation功能包中的odometry消息，得到偏航角信息。

4.根据权利要求1所述的一种基于深度相机和可见光通信的信标地图构建方法，其特征在于：所述步骤七中的观测函数是利用了当前工业相机的位姿以及灯具中心的世界坐标对某一位置的可见光定位的观测特征予以估计。

5.根据权利要求1所述的一种基于深度相机和可见光通信的信标地图构建方法，其特征在于：所述步骤七中用观测函数预估t时刻下的可见光定位的特征与实际观测的可见光定位的特征的误差构造成最小二乘问题，通过依次求解得到误差最小时的灯具中心的世界坐标以及LED灯光捕获装置的位姿。

6.根据权利要求5所述的一种基于深度相机和可见光通信的信标地图构建方法，其特征在于：所述最小二乘问题求解采用高斯牛顿迭代法，利用能观测到同一灯具的所有位姿对该灯具中心的世界坐标做出优化，再利用某一时刻下的位姿观测得到的灯具中心的世界坐标对该位姿进行优化。

7.根据权利要求1所述的一种基于深度相机和可见光通信的信标地图构建方法，其特征在于：所述步骤八中通过ICP执行的闭环相似检测包括计算两段轨迹之间的欧式变换矩阵；求出经过变换矩阵之后的一段位姿序列与另一段位姿序列之间匹配的欧式距离，阈值小于误差则完成闭环检测，并将其中一段位姿序列代替检测到闭环位姿序列。