本发明涉及一种电力巡检机器人位置数据的三维还原算法。
背景技术:
常规电力巡检机器人系统二维平面居多,无法实现电力巡检机器人实时数据的直观显示,特别是机器人的实时位置数据(备注:巡检机器人的位置数据,决定了机器人在场所中的什么位置,更有利于远程监控人员了解机器人在监控场所中的实时位置);目前的二维平面系统,对于远程监控人员不友好,无法了解电力巡检机器人的实时状态信息。
技术实现要素:
本发明的目的在于提供一种电力巡检机器人位置数据的三维还原算法,能够实现巡检机器人实时位置数据的三维还原。
为实现上述目的,本发明的技术方案是:一种电力巡检机器人位置数据的三维还原算法,步骤s1、获取巡检机器人实时位置数据(x,y),获取数据时间点tnow、速度数据v;
步骤s2、根据巡检机器人系统地图和三维场景初始映射关系,将步骤s1获取的实时位置数据(x,y)转换为巡检机器人在三维场景的实时位置数据pos(x,y,z),此时位置记为位置p;
步骤s3、记录巡检机器人在三维场景的上一个位置数据posfront(x,y,z),上一个获取数据时间点tp;
步骤s4、计算机器人行进方向:d(x,y,z)=pos(x,y,z)-posfront(x,y,z),并获取巡检机器人在该行进时间段的理论行进距离:l=(tnow-tp)*v,从而计算得到机器人理论行进位置:pos2=pos+d(x,y,z)*l,此时位置记为位置p’;
步骤s5、根据步骤s4所得位置p’、步骤s2所得位置p及步骤s3中的posfront和对应的机器人位置数据(x,y),通过相似三角形映射,修正巡检机器人系统地图和三维场景映射关系,多次迭代后,实现巡检机器人实时位置数据的三维还原。
相较于现有技术,本发明具有以下有益效果:本发明能够实现巡检机器人实时二维位置数据的三维还原。
附图说明
图1为本发明方法流程图。
具体实施方式
下面结合附图,对本发明的技术方案进行具体说明。
如图1所示,本发明提供了一种电力巡检机器人位置数据的三维还原算法,步骤s1、获取巡检机器人实时位置数据(x,y),获取数据时间点tnow、速度数据v;
步骤s2、根据巡检机器人系统地图和三维场景初始映射关系,将步骤s1获取的实时位置数据(x,y)转换为巡检机器人在三维场景的实时位置数据pos(x,y,z),此时位置记为位置p;
步骤s3、记录巡检机器人在三维场景的上一个位置数据posfront(x,y,z),上一个获取数据时间点tp;
步骤s4、计算机器人行进方向:d(x,y,z)=pos(x,y,z)-posfront(x,y,z),并获取巡检机器人在该行进时间段的理论行进距离:l=(tnow-tp)*v,从而计算得到机器人理论行进位置:pos2=pos+d(x,y,z)*l,此时位置记为位置p’;
步骤s5、根据步骤s4所得位置p’、步骤s2所得位置p及步骤s3中的posfront和对应的机器人位置数据(x,y),通过相似三角形映射,修正巡检机器人系统地图和三维场景映射关系,多次迭代后,实现巡检机器人实时位置数据的三维还原。
以下为本发明的实现代码。
以上是本发明的较佳实施例,凡依本发明技术方案所作的改变,所产生的功能作用未超出本发明技术方案的范围时,均属于本发明的保护范围。