System.ArgumentOutOfRangeException: 索引和长度必须引用该字符串内的位置。 参数名: length 在 System.String.Substring(Int32 startIndex, Int32 length) 在 zhuanliShow.Bind()
【技术实现步骤摘要】
本专利技术涉及机器人导航定位领域,具体涉及一种基于二维码和激光的融合定位方法及介质。
技术介绍
1、在机器人定位中,为了应对高动态环境下,激光定位失效,使用二维码辅助机器人定位是一种常见的做法,但现有方式存在部署要求高、鲁棒性低等缺点。常用的方式是二维码结合轮式里程计,这种方式当轮子出现打滑时会导致精度下滑,甚至出现无法沿路径扫描到下一个二维码,导致定位丢失的问题。有些方法也结合了激光导航,但现有融合激光导航的方法更多用在二次定位,使用激光导航到站点附近,扫描二维码后开始局部定位,从而实现高精度的站点取货,而激光导航在环境变化的情况下很容易出现失效的情况,导致激光导航无法到达对应的站点。也有方法实现了激光和二维码的融合定位,但其基本思想都是使用高精度的二维码定位给定初始位姿、在此基础上进行后续的导航,该方式只用到了二维码定位的少量信息,导致定位精度不会很高,当地图变化较大,激光导航失效时仍存在定位丢失的可能。
技术实现思路
1、本申请要解决的技术问题是提供一种基于二维码和激光的融合定位方法及介质,具有定位精度更高的特点。
2、第一方面,一种实施例中提供一种基于二维码和激光的融合定位方法,该定位方法基于在全局环境地图对应的路径上布置好的二维码实现,应用于对机器人小车定位装置;定位方法包括:
3、在机器人的行进路线上扫描任意一张二维码,基于该任意一张二维码的标定结果,得到任意时刻机器人的定位坐标作为第一定位坐标;所述标定结果的获取方法包括:扫描每一张二维码,对于
4、在行进路线上扫描任意一张二维码时,将获得的所有激光帧作为激光关键帧,并标记为二维码激光关键帧,对于任意一个二维码激光关键帧,选取该二维码激光关键帧所对应的多个二维码数据帧,对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化,得到优化后的多个第一定位坐标的集合及优化后的机器人的速度;
5、对于任意一个二维码激光关键帧,基于其所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度,采用时间速度和时间差计算出坐标作为该任意一个二维码激光关键帧的坐标;
6、基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标。
7、一种实施例中,所述的基于所有二维码的第一坐标和对应的第二坐标,对任意一张二维码的坐标进行非线性优化,得到所有二维码的坐标,包括:
8、,
9、其中,表示二维码j所对应的在i时刻的第一坐标,表示二维码j所对应的在i时刻的第二坐标,表示欧式距离计算,i表示时刻的索引,1≤i≤i,i表示时刻的总数,j表示二维码的索引,1≤j≤j,j表示二维码的总数;表示二维码j的坐标,随着优化而改变;表示待优化的所有二维码的坐标集合。
10、一种实施例中,所述的在机器人的行进路线上扫描任意一张二维码,基于该任意一张二维码的标定结果,得到任意时刻机器人的定位坐标作为第一定位坐标,包括:
11、,
12、其中,表示i时刻的第一定位坐标,表示二维码j所对应的在i时刻的第一坐标,i表示时刻的索引,1≤i≤i,i表示时刻的总数,j表示二维码的索引,1≤j≤j,j表示二维码的总数;表示二维码j的坐标,随着优化而改变。
13、一种实施例中,所述的对于任意一个二维码激光关键帧,选取该二维码激光关键帧所对应的多个二维码数据帧,对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化,得到优化后的多个第一定位坐标的集合及优化后的机器人的速度,包括:
14、,
15、其中,表示该多个二维码数据帧所对应的机器人的速度,,表示线速度,表示角速度;表示所述多个第一定位坐标的集合,j表示二维码的索引,1≤j≤j,j表示二维码的总数;f为代价函数,,表示欧式距离计算,i表示时刻的索引,1≤i≤i,i表示时刻的总数,表示机器人观测的协方差的倒数,表示机器人预测的协方差的倒数,基于实际的机器人模型以及二维码传感器的精度确定;表示i-1时刻到i时刻机器人的位移,,表示i-1时刻到i时刻的时间增量,表示i-1时刻机器人的yaw角,表示x方向的位移,表示y方向的位移,表示机器人的yaw角的增量。
16、一种实施例中,所述的对于任意一个二维码激光关键帧,基于其所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度,采用时间速度和时间差计算出坐标作为该任意一个激光关键帧的坐标,包括:
17、,
18、其中,表示任意一个二维码激光关键帧的坐标,表示该任意一个二维码激光关键帧所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标,表示该任意一个二维码激光关键帧所对应的第一时刻的二维码数据帧所对应的机器人的yaw角,表示扫描到二维码的时刻到二维码激光关键帧的时刻的时间差。
19、一种实施例中,所述的对于任意一个二维码激光关键帧,选取该二维码激光关键帧所对应的多个二维码数据帧,包括:对于二维码时刻,将任意一个二维码激光关键帧之前一个时刻的二维码数据帧和之后两个时刻的二维码数据帧作为任意一个激光关键帧所对应的二维码数据帧。
20、一种实施例中,所述的基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标,包括:
21、将该任意一个二维码激光关键帧的坐标作为机器人的坐标。
22、一种实施例中,所述的基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标,包括:
23、将所述任意一个二维码激光关键帧的坐标作为残差加入非线性优化函数进行优化,将优化后的该任意一个二维码激光关键帧的坐标作为机器人的坐标。
24、一种实施例中,所述的将所述任意一个二维码激光关键帧的坐标作为残差加入非线性优化函数进行优化,包括:
25、,
26、其中,表示优化后的所述任意一个二维码激光关键帧的坐标,表示所述任意一个二维码激光关键帧的代价函数,,表示所述任意一个二维码激光关键帧扫描的激光点云,k表示激光点云中点的索引,k表示激光点云中点的总数,表示二维码观测的权重,表示栅格地图双三次插值函数。
27、第二方面,一种实施例中提供一种计算机可读存储介质,存储介质上存储有程序,存储的程序包括能够被处理器加载并处理上述任意一实施例中的基于二维码和激光的融合定位方法。
28、本专利技术的有益效果是:
29、定位方法中,通过激光的坐标估计得到所有二维码的坐标,从而实现对所有二维码的标定。基于每张二维码的标定结本文档来自技高网...
【技术保护点】
1.一种基于二维码和激光的融合定位方法,其特征在于,该定位方法基于在全局环境地图对应的路径上布置好的二维码实现,应用于对机器人小车定位装置;定位方法包括:
2.如权利要求1所述的基于二维码和激光的融合定位方法,其特征在于,所述的基于所有二维码的第一坐标和对应的第二坐标,对任意一张二维码的坐标进行非线性优化,得到所有二维码的坐标,包括:
3.如权利要求1所述的基于二维码和激光的融合定位方法,其特征在于,所述的在机器人的行进路线上扫描任意一张二维码,基于该任意一张二维码的标定结果,得到任意时刻机器人的定位坐标作为第一定位坐标,包括:
4.如权利要求1所述的基于二维码和激光的融合定位方法,其特征在于,所述的对于任意一个二维码激光关键帧,选取该二维码激光关键帧所对应的多个二维码数据帧,对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化,得到优化后的多个第一定位坐标的集合及优化后的机器人的速度,包括:
5.如权利要求4所述的基于二维码和激光的融合定位方法,其特征在于,所述的对于任意一个二维码激光关键帧,基于其所对应的
6.如权利要求1到5中任意一项所述的基于二维码和激光的融合定位方法,其特征在于,所述的对于任意一个二维码激光关键帧,选取该二维码激光关键帧所对应的多个二维码数据帧,包括:对于二维码时刻,将任意一个二维码激光关键帧之前一个时刻的二维码数据帧和之后两个时刻的二维码数据帧作为任意一个激光关键帧所对应的二维码数据帧。
7.如权利要求1到5中任意一项所述的基于二维码和激光的融合定位方法,其特征在于,所述的基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标,包括:
8.如权利要求1到5中任意一项所述的基于二维码和激光的融合定位方法,其特征在于,所述的基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标,包括:
9.如权利要求8所述的基于二维码和激光的融合定位方法,其特征在于,所述的将所述任意一个二维码激光关键帧的坐标作为残差加入非线性优化函数进行优化,包括:
10.一种计算机可读存储介质,其特征在于,所述存储介质上存储有程序,存储的程序包括能够被处理器加载并处理如权利要求1到9中任意一项所述的基于二维码和激光的融合定位方法。
...【技术特征摘要】
1.一种基于二维码和激光的融合定位方法,其特征在于,该定位方法基于在全局环境地图对应的路径上布置好的二维码实现,应用于对机器人小车定位装置;定位方法包括:
2.如权利要求1所述的基于二维码和激光的融合定位方法,其特征在于,所述的基于所有二维码的第一坐标和对应的第二坐标,对任意一张二维码的坐标进行非线性优化,得到所有二维码的坐标,包括:
3.如权利要求1所述的基于二维码和激光的融合定位方法,其特征在于,所述的在机器人的行进路线上扫描任意一张二维码,基于该任意一张二维码的标定结果,得到任意时刻机器人的定位坐标作为第一定位坐标,包括:
4.如权利要求1所述的基于二维码和激光的融合定位方法,其特征在于,所述的对于任意一个二维码激光关键帧,选取该二维码激光关键帧所对应的多个二维码数据帧,对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化,得到优化后的多个第一定位坐标的集合及优化后的机器人的速度,包括:
5.如权利要求4所述的基于二维码和激光的融合定位方法,其特征在于,所述的对于任意一个二维码激光关键帧,基于其所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度,...
【专利技术属性】
技术研发人员:周军,陈航,黄佳遥,龙羽,徐菱,
申请(专利权)人:成都睿芯行科技有限公司,
类型:发明
国别省市:
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。