[发明专利]一种跟踪定位和标志点定位混合的定位方法和装置

权利要求书

1.一种跟踪定位和标志点定位混合的定位方法，其特征在于，包括：

球扫在扫描时，若判断获知开始进入遮挡区域，则采用至Handy模式，若判断获知进入未遮挡区域，则采用切换Tracker模式。

2.根据权利要求1所述的跟踪定位和标志点定位混合的定位方法，其特征在于，若判断获知开始进入遮挡区域，则采用至Handy模式，若判断获知进入未遮挡区域，则采用切换Tracker模式，具体包括：

S101，若判断获知开始扫描时采用Tracker模式，则在开始进入遮挡区域的时候切换至Handy模式，出遮挡区域后再切换Tracker模式；

S102，若判断获知开始扫描时采用Handy模式，则在开始进入未遮挡区域的时候切换至Tracker模式，等再次进入遮挡区域后再切换回Handy模式。

3.根据权利要求2所述的跟踪定位和标志点定位混合的定位方法，其特征在于，还包括：

若判断获知进入过渡区域，在过渡区域内球扫在Track的视野内，且球扫扫描的区域内贴有标志点，则在过渡区域获取标志点坐标系m1和Track坐标系t1的转换关系。

4.根据权利要求3所述的跟踪定位和标志点定位混合的定位方法，其特征在于，则在过渡区域获取标志点坐标系m1和Track坐标系t1的转换关系，具体包括：

标志点坐标系m1到Track坐标系转换t1，在某一时刻当前帧在球扫坐标系下激光线三维数据坐标P_c，则存在以下转换关系：

令：

其中，激光线三维坐标点由标志点坐标系m1转换到Track坐标系t1的旋转矩阵为R_{m1_t1}，平移向量为T_{m1_t1}；R_{c_m1}和T_{c_m1}分别表示为当前帧球扫坐标系与标志点坐标系m1之间的旋转矩阵和平移向量；R_{c_t1}和T_{c_t1}分别表示为当前帧球扫坐标系与Track坐标系t1之间的旋转矩阵和平移向量；当前帧激光线在球扫坐标系下坐标表示为P_c，在标志点坐标系m1下表示为P_m1，在Track坐标系t1下表示为P_t1；

Track坐标系t1到标志点坐标系m1转换，在某一时刻当前帧在球扫坐标系下数据计算的激光线三维数据坐标P_c，则存在以下转换关系：

令：

其中，激光线三维坐标点由Track坐标系t1转换到标志点坐标系m1的旋转矩阵为R_{t1_m1}，平移向量为T_{t1_m1}；R_{c_m1}和T_{c_m1}分别表示为当前帧球扫坐标系与标志点坐标系m1之间的旋转矩阵和平移向量；R_{c_t1}和T_{c_t1}分别表示为当前帧球扫坐标系与Track坐标系t1之间的旋转矩阵和平移向量；当前帧计算的激光线在球扫坐标系下坐标表示为P_c，在标志点坐标系m1下表示为P_m1，在Track坐标系t1下表示为P_t1。