一种室内视觉定位系统及方法
技术领域
本发明属于视觉定位技术领域,尤其涉及一种室内视觉定位系统及方法。
背景技术
传统定位技术通常有全球定位系统(GPS)和航位推算方法。目前,运用机器视觉技术实现对运动目标定位或移动载体姿态估计已经成为上述定位技术的有效补充,受到国内外学者的广泛关注。根据使用摄像机方式的不同,基于视觉的定位方法主要分为3类。一是立体视觉定位,这类方法能获得较多的信息,如文[Jirawinut R,Prakoonwit S,CeceljaF,etal visual odometer for pedestrian navigation.IEEE Transactions onInstrumentation and Measurement,2004,52(4):1166-1173]提出了一个应用于室内环境的视觉导航系统,但是算法复杂,计算成本也较高。二是全维视觉定位,这类方法可以获得大范围的信息,但图像畸变较大,如文[王景顺,陈卫东,曹其新.基于全景视觉与里程计的机器人自定位方法研究.机器人,2005,27(1):41-45.]介绍了一种应用于足球机器人的自定位方法。三是单目视觉定位,这类方法一般用于能获得较多先验信息的环境,如文[Dellaertt F,Thrun S,Thorpe C.Mosaicing a Large Number of Widely Dispersed,Noisy,and Distorted Images a Bayesian Approach[R].USA:Camegie MellonUniversity,1999]介绍了一使用了最小二乘互相关算法和贝叶斯算法的自定位方法。
在现有技术中,提出了多种借助于视觉系统解决移动载体定位问题的技术方法,弥补了传统定位方法在室内环境下的不足。中国专利CN 101441769A,公开日2009年5月27日,申请号200810204397.0,单目摄像机实时视觉定位方法。本发明只通过单个摄像机就能实现自定位、导航作用,从而降低系统复杂度和成本。但该发明需要先建立目标图像特征数据库并进行实时训练,实现相机姿态估计,环境适应能力不足,定位精度受限。
中国专利CN101033958A,公开日2007年9月12日,申请号200710051446.7一种机器视觉定位方法。采用两个普通摄像机共同完成检测任务,包括用于摄取目标物体的全局图像的远距离摄像机和用于摄取感兴趣部位的近距离图像的近距离摄像机。该发明利用两个普通摄像机实现测量的高精度要求,但是其配准过程复杂,自动化程度不高。
单一采用单目视觉定位环境适应能力不足、定位精度受限于先验知识准确度,而单一采用双目视觉定位方法配准过程复杂、定位速度慢、灵活性差。
因此,设计一个兼顾定位精度、系统复杂度、自动化程度等因素的视觉定位系统及方法是一个值得探索的问题。
发明内容
为了解决现上述问题,本发明提供了一种室内视觉定位系统,定位快速、系统简单、使用灵活。
本发明所要解决的技术问题是通过以下技术方案实现的:
一种室内视觉定位系统,包括固定摄像机与移动摄像机,所述固定摄像机悬挂于室内墙壁上,移动摄像机安装于移动载体上,所述固定摄像机与移动摄像机上均设有前置面板、电源模块、传感器组、通信模块以及信息处理模块,所述前置面板上还设有定位灯组。
进一步的,所述传感器组包括CCD传感器、倾角测量传感器、方向角传感器。
进一步的,所述定位灯组包括6个颜色各不相同的定位灯,在前置面板上呈正六边形排布,移动摄像机与固定摄像机的镜头均设置在正六边形的中心位置。
一种室内视觉定位方法,包括以下步骤:
1)系统自检;
2)固定摄像机与移动摄像机进行无线握手通信,启动双目视觉工作模式;
3)固定摄像机与移动摄像机互以对方前置面板的定位灯组为参照,标定双目视觉系统;
4)固定摄像机对固定摄像机与移动摄像机各自拍摄的场景图像进行处理,然后构建初始环境地图并将该地图发送给移动摄像机;
5)固定摄像机与移动摄像机均进入单目视觉工作模式;
6)固定摄像机跟踪移动摄像机定位灯组,计算移动摄像机的空间坐标,并将计算结果发送给移动摄像机;
7)移动摄像机以环境地图的点、线距离知识为基础获取新的点、线距离特征,并将其发送给固定摄像机,若固定摄像机无法对新的点、线距离特征进行匹配,则再切换到双目视觉工作模式,进行地图更新。
进一步的,所述双目视觉工作模式下移动摄像机的工作流程包括以下步骤:
5A、控制云台旋转,拍摄场景图像PT,并记录姿态信息;
5B、从图像PT中搜索识别固定摄像机的面板上的特征点,若识别成功则继续步骤5C,否则返回到步骤5A;
5C、停止云台旋转,记录移动摄像机当前的姿态信息;
5D、向固定摄像机发送双目配准请求,若收到固定摄像机的同步命令则执行步骤5E,否则返回步骤5A;
5E、移动摄像机以固定摄像机发送的同步命令中的同步时间戳为准,拍摄场景图像PM;
5F、向固定摄像机发送包含图像PM及移动摄像机姿态的报文;
5G、移动摄像机接收固定摄像机发送来的移动摄像机当前位置坐标及环境地图信息。
进一步的,所述双目视觉工作模式下固定摄像机的工作流程包括以下步骤:
6A、接收移动摄像机发送的双目配准请求;
6B、向移动摄像机发送双目配准同步时间戳;
6C、以步骤6B中的时间戳为准,拍摄场景图像PW;
6D、接收移动摄像机发送的包含图像PM及移动摄像机姿态的报文;
6E、解析步骤6D中的报文,得到图像PM以及移动摄像机的姿态参数;
6F、分析图像PW、PM确定两幅图像中的特征点位置;
6G、分别以图像PW以及PM上特征点为参照来确定固定摄像机与移动摄像机的位姿;
6H、统一以固定摄像机坐标系作为两个摄像机的世界坐标系,匹配位姿数据,若所得的位姿数据一致则执行步骤6I,否则执行步骤6B;
6I、根据视觉以及传感器所得位姿数据计算固定摄像机与移动摄像机在世界坐标系中的位姿;
6J、检测并匹配图像PW、PM中的角点、边缘特征;
6K、根据视觉三角测量原理计算角点、直线的空间坐标;
6L、绘制初始地图并将地图发送给移动摄像机。
进一步的,所述单目视觉工作模式下固定摄像机的工作流程包括以下步骤:
7A、固定摄像机定时采集场景图像PX;
7B、检测图像PX中是否包含移动摄像机面板上的所有特征点信息,若检测到所有特征点信息则执行步骤7C,否则跳到步骤7F;
7C、计算移动摄像头的空间坐标;
7D、将移动摄像机的空间坐标发送给移动摄像机;
7E、结束;
7F、向移动摄像机发送超范围指令;
7G、接收移动摄像机的地图更新请求,在图像PX中检测识别需要更新的点线特征,若检测到这些点、线特征则执行步骤7H,否则执行步骤7I;
7H、向移动摄像机发送地图更新确认信息,跳到步骤7E;
7I、向移动摄像机发送地图更新失败信息;
7J、启动双目视觉工作模式,配准地图,跳到步骤7E。
进一步的,所述单目视觉工作模式下移动摄像机的工作流程包括以下步骤:
8A、接收固定摄像机发送的定位信息;
8B、拍摄场景图像PY;
8C、检测图像PY中角点、直线特征信息;
8D、利用图中已知的点、线、距离知识计算新特征的空间坐标;
8E、将新特征补充到环境地图;
8F、将环境地图发送到固定摄像机确认,若确认成功则执行步骤8G,否则执行步骤8H;
8G、删除旧环境地图,以新环境地图指导后续运动,跳到步骤8I;
8H、用新环境地图指导移动摄像头后期运动,启动双目视觉配准模式;
8I、结束。
本发明所达到的有益效果是:(1)本发明的室内视觉定位系统及方法,在摄像机的前置面板上配置了6个位置精确固定、相对位置关系已知、颜色不同的LED定位指示灯,提供定位参考信息,使系统工作于双目视觉模式时,能够自标定,不需要系统之外的额外靶标,系统工作于单目视觉模式时,能够快速定位跟踪。
(2)本发明的室内视觉定位系统及方法,配置了无线通信模块,摄像机间能够实时通信,使系统的部署更加灵活,使用更加方便。
(3)本发明的室内视觉定位系统及方法,在构建地图时采用双目视觉工作模式,测距更准确,而移动载体运动中,系统可工作于单目视觉模式,定位更快速。
附图说明
图1为本发明中固定摄像机与移动摄像机的结构示意图;
图2为本发明中固定摄像机与移动摄像机的位置部署示意图;
图3为本发明中固定摄像机与移动摄像机的前置面板结构示意图;
图4为本发明中固定摄像机与移动摄像机功能框架示意图;
图5为本发明的工作流程图;
图6为本发明中移动摄像机的初始化流程图;
图7为本发明中固定摄像机的初始化流程图;
图8为本发明的室内视觉定位系统在双目视觉工作模式下移动摄像机的工作流程图;
图9为本发明的室内视觉定位系统在双目视觉工作模式下固定摄像机的工作流程图;
图10为本发明的室内视觉定位系统在单目视觉工作模式下固定摄像机的工作流程图;
图11为实施例提供的室内视觉定位系统在单目视觉工作模式下移动摄像机工作流程图。
具体实施方式
为了进一步描述本发明的技术特点和效果,以下结合附图和具体实施方式对本发明做进一步描述。
参照图1-11所示,一种室内视觉定位系统,采用两个同构摄像机共同完成定位监测任务,其中,一个为位置固定的摄像机,称为固定摄像机CW,可以悬挂于室内墙壁上,用于检测环境全局信息及指导另一摄像机的移动;另一个摄像机安装于移动载体上,称为移动摄像机CM,用于检测环境局部信息及配合完成双目视觉任务。所述固定摄像机CW和移动摄像机CM的硬件结构是一样的,包括前置面板20、电源模块10、传感器组30、通信模块40、信息处理模块50。所述的前置面板20上装有定位灯组21,定位灯组21由6个位置精确固定、相对位置关系已知、颜色不同的LED灯组成,分别是红色指示灯211、黄色指示灯212、绿色指示灯213、紫色指示灯214、蓝色指示灯215、白色指示灯216,6个LED灯在呈正六边形排布,边长为4cm,将这6个LED灯作为特征点。用作双目视觉工作模式的自标定以及单目视觉工作模式的运动目标跟踪标志,两个摄像机的镜头311均位于六边形的中心位置。所述的电源模块10包括锂电池11和充放电保护模块12两部分,锂电池11为摄像机系统提供5V工作电压,充放电保护模块12负责在电池充电时将220V电压变换成充电电压,在电池组放电时提供放电保护及实时告警,必要时断掉电源。所述传感器组由CCD传感器31、倾角测量传感器32、方向角传感器33组成,倾角测量传感器32负责测量摄像机与铅垂线的倾角,用作位姿配准数据(即摄像机坐标系与世界坐标系之间的关系)方向角传感器33用于测量摄像机相对于正南方向的方向角,CCD传感器31用于采集场景图像。所述通信模块40具有无线发送模块41与无线接收模块42,是摄像机间通信的接口,用于发送/接收控制命令、场景图像、地图数据等信息。所述信息处理模块50由信息采集子模块51、信息处理子模块52、控制输出子模块53组成。信息采集子模块51负责采集来自传感器组的数据,这些数据传递给信息处理子模块52。信息处理子模块52对传感器组的数据以及通信模块的数据进行融合处理和判断决策。控制输出子模块53根据信息处理模块的判断决策向移动载体输出控制信号。
本发明的定位主要是针对移动摄像机的定位,移动摄像机CM安装在移动载体上,固定摄像机CW安装在墙壁上,两摄像机都有根据对方面板上的定位灯组21为参考计算自身位置的能力,最后通过固定摄像机CW通过坐标变换将位置坐标统一到同一个世界坐标系中。
本发明的室内视觉定位系统可以灵活工作在双目视觉模式和单目视觉模式下,充分发挥双目视觉定位的准确性以及单目视觉定位的高效率。
本发明的室内视觉定位方法总体工作流程如图5所示:第1步进行系统自检;第2步,固定摄像机CW与移动摄像机CM无线握手通信,启动双目视觉工作模式;第3步,固定摄像机CW与移动摄像机CM,互以对方前置面板的6个LED灯为参照,标定双目视觉系统(即标定自身在对方定位灯组坐标下的坐标);第4步,固定摄像机CW对两摄像机各自拍摄的场景图像进行处理,处理内容包括预处理、特征提取、特征匹配,构建初始环境地图,发送给移动摄像机CM;第5步,两摄像机CW、CM各自进入单目视觉工作模式;第6步,固定摄像机CW跟踪移动摄像机CM的6个LED灯,计算移动摄像机CM的空间坐标,并无线发送给移动摄像机CM;第7步,移动摄像机CM在固定摄像机CW的指令下,以环境地图的点、线、面、距离知识为基础,获取新特征,发送给固定摄像机CW,若固定摄像机CW无法对新的点、线距离特征进行匹配,则再切换到双目视觉工作模式,进行地图更新。
其中步骤3中的标定双目视觉系统采用P6P方法,所述P6P方法说明如下:
摄像机的成像过程可以用针孔模型描述,通常建立世界坐标系OwXwYwZw、摄像机坐标系OcXcYcZc、图像坐标系OUV三个坐标系描述空间点与像点之间的关系。若空间点在世界坐标系下的齐次坐标系记为(xw,yw,zw,1),该点在摄像机坐标系下的坐标记为(xc,yc,zc,1),其对应像点在图像坐标系下记为(u,v,1),则图像坐标系与摄像机坐标系的关系可以描述为:
其中,Min是摄像机的内参数,其中,fu、fv分别为图像平面上u轴和v轴上的尺度因子,u0、v0为主点坐标,是图像平面与光轴的交点。本发明所用摄像机已标定,即Min已知。
摄像机的外参数cMw,是世界坐标系在摄像机坐标中的描述。R、T分别表示旋转矩阵和平移向量。R=[n o a]T,T=[t1,t2,t3]T。向量n、o、a分别表示世界坐标系坐标轴Xw轴、Yw轴、Zw轴在摄像机坐标系OcXcYcZc中的方向向量,而向量[t1,t2,t3]T表示OwXwYwZw的坐标原点在摄像机坐标系OcXcYcZc中的坐标。
摄像机的位姿,是指摄像机坐标系在世界坐标系中的描述,即因此,求得摄像机的外参数,随即可得摄像机的位姿。
由式(1)、(2)知,每一个图像点可以建立两个关于R、T的方程,cMw中共有12个未知数,因此,已知6个空间点的世界坐标(即6个LED定位灯),即可线性求解摄像机的位姿。
本发明的系统自检过程主要完成摄像机的状态检测,使其进入合适的工作状态。系统中的两个摄像机各自完成自己的自检过程。如图6所示,移动摄像机CM的自检过程包括如下步骤:第1步,进行基本输入输出检测;第2步,进行惯性传感器模块检测;第3步,检测云台控制模块;第4步,检测倾斜角;第5步,检测视觉系统各模块是否工作正常。摄像机CW的自检过程和摄像机CM的略有不同,如图7所示,摄像机CW不需要检测云台控制模块。
本发明的室内视觉定位系统完成自检后,进入双目视觉工作模式,完成场景地图构建,为单目视觉工作模式提供先验知识。双目视觉模式首先要标定外参数,本发明使用两摄像机的前置面板上6个相对位置关系确定的指示灯为参照完成标定。双目视觉工作模式下,摄像机CM的工作流程如图8所示:第1步,控制云台旋转,拍摄场景图片PT,并记录方向角及倾斜角等姿态信息;第2步,分析图片PT,搜索识别相机CW的面板6特征点;若识别成功(即准确标定出6个LED的位置),则执行第3步,否则执行第1步;第3步,停止云台旋转,记录移动摄像机CM的当前姿态信息(姿态信息包括倾角、方向角状态);第4步,以无线通信方式,向固定摄像机CW发送双目配准请求;若收到CW的同步命令,同步命令中包含同步时间戳,则执行第5步,否则,执行第1步;第5步,以固定摄像机CW发送的同步时间戳为准,拍摄场景图像PM;第6步,向固定摄像机CW发送包含图片PM及移动摄像机CM姿态的报文;第7步,接收固定摄像机CW发送来的当前位置坐标及环境地图信息。
同样地,双目视觉工作模式下,固定摄像机CW的工作流程如图9所示:第1步,接收移动摄像机CM发送的双目配准请求;第2步,向移动摄像机CM发送双目配准同步时间戳;第3步,以此时间戳为准,拍摄场景图片PW;第4步,接收移动摄像机CM发送的图片及位姿报文,报文中包含拍摄时间、倾斜角、方位角及以固定摄像机CW面板LED指示灯为参照的空间三维坐标;第5步,解析报文,得到图片PM及移动摄像头CM的位姿参数;第6步,分析图片PW、PM,检测两幅图像中的6特征点;第7步,以对方面板的6特征点为参照,运用P6P方法,确定固定摄像机CW与移动摄像机CM的位姿;第8步,统一以CW相机坐标系作为两相机的世界坐标系,匹配位姿数据;若所得位姿数据一致,则执行第9步,否则,执行第2步;第9步,运用视觉所得位姿数据及其他传感器得到的位姿数据,计算双目视觉系统的外参数,即两摄像机在世界坐标系中的位姿;第10步,检测图片PW、PM中的角点、边缘特征,匹配两幅图像中的角点、边缘特征;第11步,根据立体视觉三角测量原理,计算角点、直线的空间坐标;第12步,绘制初始地图(初始地图是移动摄像机CM移动前的地图),并将该地图发送给移动摄像机CM。
本发明通过双目视觉模式构建了蕴含丰富先验知识的环境地图后,进入快速便捷的单目视觉工作模式。单目视觉工作模式下,固定摄像机CW的工作流程如图10所示:第1步,固定摄像机CW定时(每隔3秒)采集场景图像PX;第2步,检测图像PX中是否包含移动摄像机CM面板的6个特征点信息,若检测到6个特征点信息,则执行第3步,否则,执行第6步;第3步,计算移动摄像机CM的空间坐标;第4步,将该坐标以无线通信方式发送给移动摄像机CM;第5步,结束;第6步,向CM发送超范围指令;第7步,接收移动摄像机CM地图更新请求,在PX图像中检测识别需要更新的点、线特征,若检测到这些点、线特征,则执行第8步,否则执行第9步;第8步,向CM发送地图更新确认信息,执行第5步;第9步,发送地图更新失败信息;第10步,启动双目视觉工作模式,配准地图,执行第5步。
同样地,单目视觉工作模式,摄像机CM的工作流程如图11所示:第1步,接收固定摄像机CW发送的定位信息;第2步,拍摄场景图像PY;第3步,检测角点、直线等特征信息;第4步,利用地图中已知的点、线、面、距离等知识,计算新特征的空间坐标;第5步,将新特征补充到环境地图;第6步,将新环境地图发送到固定摄像机CW,等待确认;若确认成功(确认主要包括更新的点、线位置坐标及时间戳),则执行第7步,否则,执行第8步;第7步,以新地图指导后期运动,删除旧地图,执行第9步;第8步,新地图临时指导后期运动,启动双目视觉配准模式;第9步,结束。
上述实施例不以任何形式限定本发明,凡采取等同替换或等效变换的形式所获得的技术方案,均落在本发明的保护范围之内。