CN109191529A - 一种基于双目技术的室内可见光自定位系统与方法 - Google Patents

Abstract

本发明公开了一种基于双目技术的室内可见光自定位系统与方法，用于通过双目技术实现室内设备的自定位并获得倾角。包括相互协同作业的图像分析系统和自定位系统，图像分析系统包括图像采集模块、摄像头自标定模块、图像处理模块和立体匹配模块，自定位系统包括坐标及倾角计算模块，坐标及倾角计算具体实现过程包括：图像采集、获得摄像机内外参数和匹配点坐标、计算左右摄像头的坐标、计算手持设备的倾角四个步骤。本发明实现了室内设备的高精度实时自定位，在成本、通用性、资源利用率等方面具有优势。

Description

一种基于双目技术的室内可见光自定位系统与方法

技术领域

本发明涉及一种基于双目技术的室内可见光自定位系统与方法，属于图像处理和可见光 定位技术领域。

背景技术

随着各种综合体类建筑(如大型购物中心、大型交通中转站等)的出现，人们活动空间越 来越庞大和复杂，对定位和导航需求日趋强烈，同时精准营销、智能仓储、机器人、疗养护 理等行业也需要利用定位技术获取特定对象的室内位置信息，目前应用最为广泛的室外无线 电定位系统是GPS或北斗全球定位系统，它是通过在移动终端内置卫星的位置信号接收模块， 利用无线电信号实现室外的高精度定位。但在人们活动较为频繁的家庭、办公室、大型商场 等室内环境以及隧道、地下停车场等特殊环境，由于对空间无线电信号的阻挡及屏蔽效应导 致GPS等传统卫星定位系统难以适用，据统计，现代社会人们80％的时间是在室内活动，并 且随着城市化的发展，各类大型建筑物越来越多，因此室内定位需求日益迫切。

现有的针对该种室内定位需求的定位装置或方法有较多公开内容，例如：中国发明专利 “双目非标定空间定位方法”(CN102622767B)是通过背景差分法获取运动目标中心点的位 置数据；通过摄像机成像原理，计算出运动目标在场景中相对于当前分析画面的拍摄光轴的 角度、方向数据，得出运动点与拍摄点的坐标，其不足之处是：使用背景差分法获取运动目 标中心点的位置数据，易受到光线变化、环境变化、背景变化、场景中物体运动的干扰，且 用运动物体相对于拍摄光轴角度、方向数据来计算其坐标，并不能实现对自身的定位；发明 专利“基于FPGA实现的双目视觉定位装置”(CN201310651555)是通过两个相机采集图像 信息，利用两片SDRAM分别对两幅图像进行缓存，最后在LCD上实时显示两幅图像。在 FPGA内部实现两幅图像的并行处理，经过图像滤波、特征提取等数字图像处理方法获得特 定目标图像位置信息后，利用BP神经网络标定方法确定目标物体的笛卡尔空间位置信息， 虽然定位装置利用BP神经网络标定方法实现二维信息到三维信息之间的非线性变换，无需 确定摄像机的内、外参数，同时可降低摄像机镜头各种畸变及外界噪声的影响，但其实现的 仅是对目标物体的定位，并不能对自身进行定位；发明专利“基于logo的双目视觉室内定位 方法”(CN 106228538A)通过建立logo图像采集的视觉地图数据库等五个步骤可实现定位， 但该种方法存在离线阶段需要在室内长走廊中布置较多的Logo图像，并在光线较好时对Logo 进行拍照，因此实施难度大，另一方面：摄像机距logo的距离是3.5m时，定位误差将达到 0.22m，可见其定位误差较大；发明专利“基于双目相机的空间定位方法及装置”(CN107705333A) 只能获得对自身的定位，不能得到其姿态，且该专利未写出详细的定位算法；发明专利“一 种鱼眼双目视觉立体匹配空间定位方法”(CN107909543A)是用鱼眼相机采集图像，并直接 对未经校正的图像进行立体匹配，但由于实现室内定位需要的采集的图像范围不大，因此对 相机的视角要求不高，普通的相机即可满足要求，同样还存在只能对目标物体的定位，而不 是自定位问题；发明专利“一种基于UWB和双目视觉的导航定位方法”(CN108012325A) 通过通过UWB基站及定位标签来确定叉车的位置，所采用的双目技术仅用于确定得到货架 托盘相对叉车的距离信息和姿态信息，用以调整叉车使其正对货架托盘，没有通过固定的LED 和双目技术确定设备的位置；发明专利“一种基于双目视觉的机械臂目标定位抓取方法”(CN 107767423A)是采用双目技术获取目标物体的三维空间坐标及机械臂末端的状态信息，无法 实现对安装有双目相机机械臂的定位，还需要判断目标物体所在象限；发明专利“一种室内 可见光定位系统”(CN107703485A)的核心为通过采集LED的虚拟坐标与其真实坐标值进 行转换，以确定相机位于真实坐标系下的坐标，从而实现相机的真实定位；发明专利“一种 使用相机的室内可见光异步定位方法”(CN105306141A)通过LED异步发送信号，利用智 能终端的相机进行室内可见光定位。

综上所述，现有的室内可见光定位普遍存在如下两方面的问题：

⑴定位效率不高，无法实时更新在室内的定位：现有的可见光系统和算法设计过于复杂， 无法快速地得到设备的定位。

⑵获得信息受限，无法获得设备的姿态：这些室内可见光定位方法只能设备的位置信息， 无法判断设备是否平衡，无法得到设备的状态，获得的信息较单一，对可见光的资源利用率 低。

⑶通用性不强，需要布设多个LED灯：这些基于室内可见光实现的定位方法绝大部分需 要专门的接收模块接收LED发送的号，在具体实施中设备复杂、可拓展性差，商业应用性 不强。

基于此，一种具有节能降耗、健康安全、布设简单、成本低廉、无电磁干扰等突出优点 的室内可见光自定位系统与方法成为必需，它不仅能实现设备的自定位，还能获得设备的实 时姿态，因此在室内无线定位领域具有良好的应用前景。

发明内容

本发明针对背景技术所述问题，设计一种基于双目技术的室内可见光自定位系统与方法， 可实现安装有双目相机的设备在室内、车内等的快速定位，并获得设备的倾角。基于最小均 方算法的定位由于其计算复杂度低，计算速度快，因此可以对设备进行实时定位。本发明所 述基于双目技术的室内可见光定位系统与方法具有成本低、定位精度高、计算规则明确等的 综合优势。

为达到上述目的，本发明采用技术方案：

一种基于双目技术的室内可见光自定位系统，包括相互协同作业的图像分析系统和自定 位系统；所述图像分析系统包括：信标模块、图像采集模块、相机自标定模块、图像处理模 块、立体匹配模块；所述自定位系统包括：坐标及倾角计算模块；其特征在于：

所述信标模块包括三个固定在上方的LED灯，三个LED灯分布在非同一直线上，且各 LED灯在空间参考坐标系的坐标已知，各LED灯分别工作在设定的频率中；

所述图像采集模块包括安装在手持设备上由左右二个相机组成的双目相机，所述双目相 机用于从两个不同角度同时获得同一物体的图像信息；

所述相机自标定模块用于获得双目相机的内外参数，包括：图像中心、焦距f、镜头畸变 和相机图像坐标系的旋转矩阵和平移矩阵；

所述图像处理模块用于降低噪声对图像质量的影响，增加图像对比度以增强边缘特性， 提高后续两幅图像之间立体匹配的精确度；

所述立体匹配模块用于存储从各LED灯的OOK(On-Off Keying)编码到坐标的映射关 系，通过解码图像中各LED灯的编码信息，找到空间内各LED灯在两幅图像中的匹配点，并获得左右两幅图像上匹配点在图像坐标系中的坐标，设定所述坐标分别为(a,b)和(a',b')；

所述坐标及倾角计算模块用于对获得的坐标进行处理，得到左右相机在三维空间坐标系 的坐标，并计算手持设备的倾角；

进一步地，所述双目相机为两个内外参数相同的CMOS相机，双目相机在拍照过程中对 图像像素进行逐行扫描，从而识别并提取图像所中包含的信息。

进一步地，所述坐标及倾角计算模块在工作中具体实现过程包括以下步骤：

第1步，图像采集：

在设定空间内固定安装三个坐标不同且已知具体位置的LED灯，分别为LED₁、LED₂和LED₃，各LED灯在空间直角参考坐标系内的坐标分别为L₁(m₁,n₁,q₁)、L₂(m₂,n₂,q₂)和 L₃(m₃,n₃,q₃)；以OOK编码I₁、I₂、I₃对LED₁、LED₂和LED₃进行调制，再对一定的载波 频率f₀对编码后的LED进行调制，使得LED的发射频率大于1kHz以保证其变化无法被人眼 察觉，并可以有效地保护视力。若CMOS相机的扫描频率为f_s，则LED的载波频率f₀为f_s/n (n为任意正整数)，同时将LED的发射频率与坐标之间的映射关系存储到立体匹配模块中。 在待定位设备上安装两个内外参数相同的相机，使其构成一对双目相机。相机在同一时刻从 不同角度拍摄天花板，得到两张图像分别为左图像和右图像；

第2步，获得相机内外参数和匹配点坐标：

双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d、相机的焦距f，左 右相机的旋转矩阵R、R'，左右相机的平移矩阵T、T'等参数。图像处理模块对图像进行处 理，增加图像的对比度以增强边缘特性。立体匹配模块将两幅图像转化成灰度图并对其模糊 化处理，然后使其通过一个单边OTSU滤波器，找到每个LED灯在图像上的对应子区域并从 中解码得到LED_i(i＝1,2,3)在左右两幅图像中的匹配点的坐标分别为P_i(u_i，v_i)、P_i'(u_i'，v_i')；

第3步，计算左右相机的坐标：

设定：左右相机在世界坐标系的坐标分别为S＝(x，y，z)、S'＝(x'，y'，z')，由抽象出的双 目相机与各LED灯之间的几何关系，列出包含未知左右相机坐标的方程组分别为：

公式(1)和公式(2)中：将方程组分别整理成AX＝B和A'X'＝B'的矩阵形式，则有：

其中：A、B、A'、B'是由实际参数确定的已知向量，X、X'为需要求解的未知向量；

通过最小均方算法(Least Mean Square，LMS)或最小二乘法(Least SquareMethod，LS) 求解该矩阵方程，进而得到左右相机的坐标；

第4步，计算手持设备的倾角。

进一步地，第3步中，用LMS算法求解左相机坐标的具体步骤包括：

S1，X(n)表示n次迭代后的估计系数X；

S2，设置步长μ，μ的取值范围为0<μ<2/λ_max；其中：λ_max是A的自相关矩阵的最大 特征值；

S3，设置初始估计系数X(0)为任意值；

S4，判断是否满足误差项e(n)＝B-AX(n)是否为0；若是，则所得的即为左相机的横纵坐 标，根据已求得的横纵坐标可以推算出左相机的z轴坐标至此， 左相机坐标的计算完成；若否，则执行下一步；

S5，更新系数X(n+1)＝X(n)+μA^He(n)并循环S4，直到满足最小误差条件；所述A^H表示矩阵A的共轭转置。

进一步地，第3步中，用LMS算法求解右相机坐标的具体步骤包括：

U1，X'(n)表示n次迭代后的估计系数X'；

U2，设置步长μ'，μ'的取值范围为0<μ'<2/λ'_max。其中，λ'_max是A'的自相关矩阵的最大特征值；

U3，设置初始估计系数X'(0)为任意值；

U4，判断是否满足误差项e'(n)＝B'-A'X'(n)是否为0；若是，则所得的即为左相机的横纵 坐标，根据已求得的横纵坐标可以推算出右相机的z轴坐标至 此，左相机坐标的计算完成；若否，则执行下一步；

U5，更新系数X'(n+1)＝X'(n)+μ'A'^H e'(n)并循环U5，直到满足最小误差条件；所述 A'^H表示矩阵A'的共轭转置。

进一步地，第3步中，用LS求解左相机坐标的具体步骤包括：

Q1，残差值e＝B-AX，残差的平方f(X)表示为：

f(X)＝e²＝(B-AX)²＝(B-AX)^T(B-AX)；

Q2，对f(X)求导并令其等于零，即

Q3，得到X的解X＝(A^TA)^-1A^TB，根据已求得的横纵坐标可以推算出左相机的z轴坐标至此，左相机坐标的计算完成；

进一步地，第3步中，用LS求解右相机坐标的具体步骤包括：

W1，残差值e'＝B'-A'X',残差的平方：

f(X')＝e'²＝(B'-A'X)²＝(B'-A'X')^T(B'-A'X')；

W2，对f(X')求导并令其等于零，即：

W3，得到X的解X'＝(A'^T A')^-1A'^T B'，根据已求得的横纵坐标可以推算出右相机的z 轴坐标至此，右相机坐标的计算完成；

进一步地，第4步中，计算相机倾角的具体步骤包括：

V1，令n₁＝[0,a,0]，将其转换成世界坐标系的坐标n₁，由[n₂,1]＝[n₁,1]*W计算得到，其 中转换矩阵所述R表示左相机坐标系的旋转矩阵；所述T表示左相机的平移矩 阵。

V2,计算设备的倾角

与现有技术相比，本发明所描述的一种基于双目技术的室内可见光自定位系统与方法， 具有以下优点：

(1)定位效率高，算法简单，可以在室内实现快速实时定位，对设备的硬件要求不高；

(2)通过定位得到的双目相机的坐标计算其倾角，获得设备的姿态信息；

(3)利用双目技术实现高精度的定位，LED的布施简单，仅需要三个LED就可实现自定位。

附图说明

图1显示为本发明中的基于双目技术的室内可见光自定位系统与方法的模块图；

图2显示为本发明中的基于双目技术的室内可见光自定位系统与方法的自定位流程图；

图3显示为本发明中的基于双目技术的室内可见光自定位系统与方法的左相机定位算法 流程图；

图4显示为本发明中的基于双目技术的室内可见光自定位系统与方法的右相机定位算法 流程图；

图5显示为本发明实施例中的室内结构示意图，图中LED₁、LED₂、LED₃分别安装在天花板上的不同位置的LED灯，且各LED灯具体坐标点已预先测得，SMC表示安装有双目相 机的设备。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露 的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加 以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精 神下进行各种修饰或改变。

本发明的一种基于双目技术的室内可见光自定位系统与方法的核心技术思想是基于最小 均方算法实现双目相机的自定位和倾角的计算。为使本发明的技术方案更为清晰，下面将结 合附图进一步的对本发明作详细描述。

图1是本发明中一种基于双目技术的室内可见光自定位系统与方法的模块图，其功能为 通过双目相机采集图像信号，经过相机的自标定、图像处理技术和立体匹配得到匹配点坐标， 计算此双目相机在空间直角参考坐标系的坐标并最终得到设备的定位和倾角。具体地，该基 于双目技术的室内可见光自定位系统与方法包括：图像采集模块、自标定模块、图像处理模 块、立体匹配模块、坐标及倾角计算模块等。

图2是本发明中的一种基于双目技术的室内可见光自定位系统与方法中的自定位流程 图，实现装有双目相机的设备的自定位和倾角计算，具体实现过程包括以下步骤：

第1步，图像采集：

在设定空间内固定安装三个坐标不同且已知具体位置的LED灯，分别为LED₁、LED₂和LED₃，各LED灯在空间直角参考坐标系内的坐标分别为L₁(m₁,n₁,q₁)、L₂(m₂,n₂,q₂)和 L₃(m₃,n₃,q₃)。以OOK(On-Off Keying)编码I₁、I₂、I₃对LED₁、LED₂和LED₃进行调制， 再用一定的载波频率f₀对调制后的LED进行调制，使得LED的发射频率大于1kHz以保证其 变化无法被人眼察觉，并可以有效地保护视力。若CMOS相机的扫描频率为f_s，则LED的 载波频率f₀为f_s/n(n为任意正整数)，同时将LED的OOK编码与坐标之间的映射关系 存储到立体匹配模块中。在待定位设备上安装两个内外参数相同的相机，使其构成一对双目 相机。相机在同一时刻从不同角度拍摄天花板，得到两张图像分别为左图像和右图像；

第2步，获得摄像机内外参数和匹配点坐标：

双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d、相机的焦距f，左 右相机的旋转矩阵R、R'，左右相机的平移矩阵T、T'等参数。图像处理模块对图像进行处 理，增加图像的对比度以增强边缘特性。立体匹配模块将两幅图像转化成灰度图并对其模糊 化处理，然后使其通过一个单边OTSU滤波器，找到每个LED在图像上的对应子区域并从中 解码得到LED_i(i＝1,2,3)在左右两幅图像中的匹配点的坐标分别为P_i(u_i，v_i)、P_i'(u_i'，v_i')；

第3步，计算相机的坐标：

假设左右相机在空间直角参考坐标系的坐标分别为S＝(x，y，z)、S'＝(x'，y'，z')。由抽象 出的双目相机与LED之间的几何关系列出包含未知左、右相机坐标的方程组分别为：

其中将方程组分别整 理成AX＝B和A'X'＝B'的矩阵形式，则有

其中，A、B、A'和B'是由实际参数确定的已知向量，X、X'为需要求解的未知向量。并通过最小均方算法(Least Mean Square，LMS)或最小二乘法(Least Square Method，LS)求解矩阵方程，进而得到左右相机的坐标。

第4步，计算设备的倾角。

进一步地，第3步中，用LMS算法求解左相机坐标的具体步骤包括：

S1，X(n)表示n次迭代后的估计系数X；

S2，设置步长μ，μ的取值范围为0<μ<2/λ_max。其中，λ_max是A的自相关矩阵的最大 特征值；

S3，设置初始估计系数X(0)为任意值；

S4，判断是否满足误差项e(n)＝B-AX(n)是否为0；若是，则所得的即为左相机的横纵坐 标，根据已求得的横纵坐标可以推算出左相机的z轴坐标至此， 左相机坐标的计算完成；若否，则执行下一步；

S5，更新系数X(n+1)＝X(n)+μA^He(n)并循环S4，直到满足最小误差条件，其中A^H表示矩阵A的共轭转置；

进一步地，第3步中，用LMS算法求解右相机坐标的具体步骤包括：

U1，X'(n)表示n次迭代后的估计系数X'；

U2，设置步长μ'，μ'的取值范围为0<μ'<2/λ'_max。其中，λ'_max是A'的自相关矩阵的最大特征值；

U3，设置初始估计系数X'(0)为任意值；

U4，判断是否满足误差项e'(n)＝B'-A'X'(n)是否为0；若是，则所得的即为左相机的横纵 坐标，根据已求得的横纵坐标可以推算出右相机的z轴坐标至 此，左相机坐标的计算完成；若否，则执行下一步；

U5，更新系数X'(n+1)＝X'(n)+μ'A'^H e'(n)并循环U5，直到满足最小误差条件。其中 A'^H表示矩阵A'的共轭转置；

进一步地，第3步中，用LS求解左相机坐标的具体步骤包括：

Q1，残差值e＝B-AX,残差的平方

f(X)＝e²＝(B-AX)²＝(B-AX)^T(B-AX)；

Q2，对f(X)求导并令其等于零，即

Q3，得到X的解X＝(A^TA)^-1A^TB,根据已求得的横纵坐标可以推算出左相机的z轴坐标 至此，左相机坐标的计算完成。

进一步地，第3步中，用LS求解右相机坐标的具体步骤包括：

W1，残差值e'＝B'-A'X',残差的平方

f(X')＝e'²＝(B'-A'X)²＝(B'-A'X')^T(B'-A'X')；

W2，对f(X')求导并令其等于零，即

W3，得到X的解X'＝(A'^T A')^-1A'^TB',根据已求得的横纵坐标可以推算出右相机的z轴 坐标至此，右相机坐标的计算完成。

进一步地，第4步中，计算设备倾角的具体步骤包括：

V1，令n₁＝[0,a,0]，将其转换成世界坐标系的坐标n₁，由[n₂,1]＝[n₁,1]*W计算得到，其 中转换矩阵R表示左相机坐标系的旋转矩阵，T表示左相机的平移矩阵。

V2,设备的倾角

下面通过具体的实施例对本发明的基于双目技术的室内可见光自定位系统与方法进行说 明。

在本实施例中，将用简化的实验案例来验证本发明的定位计算方法及倾角计算方法。在 设备(SMC)上安装两个同型号的相机，其内外参数相同，并构成一对双目相机。图5是本 发明实施例中的室内结构示意图，选取室内一点作为坐标原点，天花板上安装有三个坐标已 知的LED灯分别为LED₁、LED₂和LED₃，各LED灯在空间直角参考坐标系内的坐标分别为L₁(0.21,2.35,2.80)、L₂(2.50,3.75,2.80)和L₃(1.30,1.50,2.80)，分别以OOK(On-OffKeying) 编码0100、1101、1010对LED₁、LED₂和LED₃进行调制。设定CMOS相机的扫描频率f_s＝3 kHz，再用1.5kHz的载波频率对调制后的LED进行调制。并将LED频率与坐标之间的映射关系存储到立体匹配模块中。相机在同一时刻从不同角度拍摄天花板，得到两张图像分别为 左图像和右图像；

将相机光心连线的中点坐标视为设备的坐标。令设备做无规则运动。定位装置获得设备 在第j秒(j＝1,2,3,4,5,6,7,8,9,10)时的定位D_j：D₁＝(0.1700,0.3621,0.3648)，D₂＝(0.1976,0.2815,0.4265)，D₃＝(0.2013,0.3984,0.3940)，D₄＝(0.2322,0.4551,0.4603)，D₅＝(0.3762,0.4876,0.3724)，D₆＝(0.3072,0.5293,0.5351)，D₇＝(0.4054,0.5632,0.6290)， D₈＝(0.4012,0.5868,0.6423)，D₉＝(0.4646,0.6038,0.7100)，D₁₀＝(0.4790,0.6353,0.7182)，同时 双目相机捕获图像；

第2步，获得摄像机内外参数和匹配点坐标：

双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d＝0.1、相机的焦距 f＝0.028,第js的左右相机的旋转矩阵R_j、R_j'和平移矩阵T_j、T_j'等参数，

T₁＝[0.1680 0.3165 0.3565]^T，T₁'＝[-0.1680 -0.4165 -0.3565]^T，

T₂＝[0.2026 0.2338 0.4233]^T，T₂'＝[-0.2026 -0.3338 -0.4233]^T，

T₃＝[-0.1046 0.5376 0.2805]^T,T₃'＝[-0.1046 -0.6376 0.2805]^T，

T₄＝[0.0904 -0.1373 -0.6555]^T,T₄'＝[0.0904 0.2373 -0.6555]^T，

T₅＝[0.5690 0.3063 0.2592]^T,T₅'＝[0.5690 -0.4063 0.2592]^T，

T₆＝[0.2047 0.3134 0.6977]^T,T₆'＝[0.2047 -0.4134 0.6977]^T，

T₇＝[-0.1786 0.8109 0.3226]^T,T₇'＝[-0.1786 -0.9109 0.3226]^T，

T₈＝[-0.1933 0.2484 0.8897]^T,T₈'＝[-0.1933 -0.3484 0.8897]^T，

T₉＝[0.4117 0.9012 0.1016]^T,T₉'＝[0.4117 -1.0012 0.1016]^T，

T₁₀＝[-0.1403 0.5318 0.8892]^T,T₁₀'＝[-0.1403 -0.6318 0.8892]^T，

图像处理模块对图像进行处理，增加图像的对比度以增强边缘特性。立体匹配模块将两 幅图像转化成灰度图并对其模糊化处理，然后使其通过一个单边的OTSU滤波器，找到每个 LED在图像上的对应子区域并从中解码得到LED_i(i＝1,2,3)在左右两幅图像中的匹配点的坐标 分别为P_i(u_i，v_i)、P_i'(u_i'，v_i')；将第j秒(j＝1,2,3,4,5,6,7,8,9,10)时LEDi在左右图像上的 匹配点坐标分别记作P_i,i(u_i，v_i)、P_i,i'(u_i'，v_i')；

P_1,1＝(0.0003,0.0251)，P_1,1'＝(0.0003,-0.0239)，

P_2,1＝(0.0125,0.0142)，P_2,1'＝(0.0125,-0.0130)，

P_3,1＝(0.0275,0.0401)，P_3,1'＝(0.0275,-0.0389)，

P_1,2＝(-0.0008,0.0266)，P_1,2'＝(0.0009,-0.0254)，

P_2,2＝(0.0125,0.0150)，P_2,2'＝(0.0130,-0.0138)，

P_3,2＝(0.0293,0.0410)，P_3,2'＝(0.0293,-0.0398)，

P_1,3＝(-0.0831,0.1891)，P_1,3'＝(-0.0831,-0.1824)，

P_2,3＝(-0.0188,0.0368)，P_2,3'＝(-0.0188,-0.0351)，

P_3,3＝(0.0038,0.0546)，P_3,3'＝(0.0038,-0.0533)，

P_1,4＝(-0.0128,0.0184)，P_1,4'＝(-0.0831,-0.0172)，

P_2,4＝(-0.0117,0.0021)，P_2,4'＝(-0.0117,-0.0010)，

P_3,4＝(0.0023,0.0057)，P_3,4'＝(0.0023,-0.0057)，

P_1,5＝(0.0235,0.0745)，P_1,5'＝(-0.0831,-0.0719)，

P_2,5＝(0.0169,-0.0256)，P_2,5'＝(-0.0117,-0.0010)，

P_3,5＝(0.0654,0.0480)，P_3,5'＝(0.0654,-0.0462)，

P_1,6＝(-0.0028,0.0276)，P_1,6'＝(-0.0028,-0.0263)，

P_2,6＝(-0.0008,0.0084)，P_2,6'＝(-0.0008,-0.0073)，

P_3,6＝(0.0152,0.0142)，P_3,6'＝(0.0152,-0.0135)，

P_1,7＝(0.1579,-0.3009)，P_1,7'＝(-0.0028,-0.0263)，

P_2,7＝(-0.0310,0.05447)，P_2,7'＝(-0.0310,-0.0518)，

P_3,7＝(0.0019,0.0945)，P_3,7'＝(0.0019,-0.0923)，

P_1,8＝(-0.0205,0.02790)，P_1,8'＝(-0.0205,-0.0263)，

P_2,8＝(-0.0153,0.0058)，P_2,8'＝(-0.0153,-0.0045)，

P_3,8＝(0.0004,0.0109)，P_3,8'＝(0.0004,-0.0102)，

P_1,9＝(-0.1656,1.1663)，P_1,9'＝([-0.1656,-1.1239)，

P_2,9＝(0.0199,0.0726)，P_2,9'＝(0.0199,-0.0694)，

P_3,9＝(-0.0931,-0.1781)，P_3,9'＝(-0.0931,0.1734)，

P_1,10＝(-0.0278,0.0526)，P_1,10'＝([-0.1656,-0.0502)，

P_2,10＝(-0.0149,0.0153)，P_2,10'＝(-0.0149,-0.0138)，

P_3,10＝(0.0029,0.0224)，P_3,10'＝(0.0029,-0.0215)，

第3步，计算相机的坐标：

假设第j秒(j＝1,2,3,4,5,6,7,8,9,10)时左右相机的坐标分别为S_Lj＝(xl_j,yl_j,zl_j)、 S_Rj＝(xr_j,yr_j,zr_j)。由抽象出的双目相机与LED之间的几何关系并通过最小均方算法(Least Mean Square，LMS)求解左右相机的坐标。由抽象出的双目相机与LED之间的几何关系列出第j 秒(j＝1,2,3,4,5,6,7,8,9,10)时包含未知左相机坐标的方程组分别为：

K_1,1＝3.1759,K_2,1＝2.9364,K_3,1＝4.8119

K_1,2＝3.1827,K_2,2＝2.9084,K_3,2＝4.8277

K_1,3＝3.0866,K_2,3＝2.8350,K_3,3＝4.7053

K_1,4＝3.0362,K_2,4＝2.7790,K_3,4＝4.6436

K_1,5＝3.1091,K_2,5＝2.8183,K_3,5＝4.6155

K_1,6＝2.9424,K_2,6＝2.6706,K_3,6＝4.5270

K_1,7＝-2.8626,K_2,7＝2.5676,K_3,7＝4.4361

K_1,8＝2.8241,K_2,8＝2.5179,K_3,8＝4.3844

K_1,9＝2.7848,K_2,9＝2.4677,K_3,9＝-4.3345

K_1,10＝2.7503,K_2,10＝2.4204,K_3,10＝4.2877

列出第j秒(j＝1,2,3,4,5,6,7,8,9,10)时包含未知右相机坐标的方程组分别为：

K_1,1'＝3.1111,K_2,1'＝2.8958,K_3,1'＝4.7435

K_1,2'＝3.1146,K_2,2'＝2.8661,K_3,2'＝4.7571

K_1,3'＝3.1101,K_2,3'＝2.8958,K_3,3'＝4.7409

K_1,4＝2.9861,K_2,4'＝2.7739,K_3,4'＝4.6248

K_1,5'＝3.0196,K_2,5'＝2.7578,K_3,5'＝4.5603

K_1,6'＝2.8733,K_2,6'＝2.6436,K_3,6'＝4.487

K_1,7'＝-2.7747,K_2,7'＝2.4890,K_3,7'＝4.3404

K_1,8'＝2.7625,K_2,8'＝2.5020,K_3,8'＝4.3491

K_1,9'＝2.6859,K_2,9'＝2.3777,K_3,9'＝-4.2469

K_1,10'＝2.6709,K_2,10'＝2.3787,K_3,10'＝4.2262

将第j秒时左、右方程组分别整理成A_jX_j＝B_j和A_j'X_j'＝B_j'的矩阵形式，则有

其中，A_j、A_j'、B_j、B_j'是由实际参数确定的已知向量，X_j、X_j'为需要求解的未 知向量。

进一步地，第3步中，用LMS算法求解左相机坐标的具体步骤包括：

S1，X_j(n)表示n次迭代后的估计系数X_j；

S2，设置步长μ_j＝0.1；

S3，设置初始估计系数X_j(j＝1,2,3,4,5,6,7,8,9,10)为[0 1]^T；

S4，判断是否满足误差项e_j(n)＝B_j-A_jX_j(n)是否为0；若是，则所得的即为左相机的横纵 坐标，根据已求得的横纵坐标可以推算出左相机的z轴坐标若否，则执行下一步；

S5，更新系数X_j(n+1)＝X_j(n)+μA_j ^He_j(n)并循环S4，直到满足最小误差条件；

至此，左相机坐标的计算完成：

S₁＝(0.1725，0.3171，0.3603)，S₂＝(0.2010，0.2316，0.4247)，

S₃＝(0.2335，0.3800，0.4239)，S₄＝(0.2676，0.4200，0.4568)，

S₅＝(0.4000，0.4561，0.3417)，S₆＝(0.3359，0.4900，0.5235)，

S₇＝(0.3941，0.5200，0.6065)，S₈＝(0.4300，0.5431，0.6383)，

S₉＝(0.4624，0.5700，0.6732)，S₁₀＝(0.4965，0.5900，0.7061)。

进一步地，第3步中，用LMS算法求解右相机坐标的具体步骤包括：

U1，X_j'(n)表示n次迭代后的估计系数X_j'；

U2，设置步长μ_j'＝0.1(j＝1,2,3,4,5,6,7,8,9,10)；

U3，设置初始估计系数X_j'(0)＝[0 1]^T,(j＝1,2,3,4,5,6,7,8,9,10)；

U4，判断是否满足误差项e_j'(n)＝B_j'-A_j'X_j'(n)，(j＝1,2,3,4,5,6,7,8,9,10)是否为0； 若是，则所得的即为左相机的横纵坐标，根据已经求得的横纵坐标可以推算出右相机的z轴 坐标若否，则执行下一步；

U5，更新系X_j'(n+1)＝X_j'(n)+μ_j'A_j'^He_j'(n)数并循环U4，直到满足最小误差条件；

至此，右相机坐标的计算完成：

S₁′＝(0.1683，0.4120，0.3666)，S₂′＝(0.1942，0.3313，0.4282)，

S₃′＝(0.1691，0.4169，0.3641)，S₄′＝(0.1967，0.4902，0.4638)，

S₅′＝(0.3524，0.5191，0.4030)，S₆′＝(0.2785，0.56860.5466)，

S₇′＝(0.4166，0.6064，0.6514)，S₈′＝(0.3724，0.6275，0.6464)，

S₉′＝(0.4669，0.6376，0.7468)，S₁₀′＝(0.4616，0.6805，0.303)。

由第j秒时左相机的坐标S_j和右相机的坐标S_j'得到设备的坐标h_j:

h₁＝(0.1700,0.3621,0.3648)，h₂＝(0.1976,0.2815,0.4265)，h₃＝(0.2013，0.3984，0.3940)， h₄＝(0.2322，0.4551，0.4603)，h₅＝(0.3762，0.4876，0.3724)，h₆＝(0.3072，0.5293，0.5351)， h₇＝(0.4054，0.5632，0.6290)，h₈＝(0.4012，0.5868，0.6423)，h₉＝(0.4646，0.6038，0.7100)， h₁₀＝(0.4790，0.6353，0.7182)。

进一步地，第3步中，用LS求解左相机坐标的具体步骤包括：

Q1，残差值e＝B-AX,残差的平方

f(X)＝e²＝(B-AX)²＝(B-AX)^T(B-AX)；

Q2，对f(X)求导并令其等于零，即

Q3，得到X的解X＝(A^TA)^-1A^TB,根据已求得的横纵坐标可以推算出左相机的z轴坐标 至此，左相机坐标的计算完成：

S₁＝(0.1725，0.3171，0.3603)，S₂＝(0.2010，0.2316，0.4247)，

S₃＝(0.2335，0.3800，0.4239)，S₄＝(0.2676，0.4200，0.4568)，

S₅＝(0.4000，0.4561，0.3417)，S₆＝(0.3359，0.4900，0.5235)，

S₇＝(0.3941，0.5200，0.6065)，S₈＝(0.4300，0.5431，0.6383)，

S₉＝(0.4624，0.5700，0.6732)，S₁₀＝(0.4965，0.5900，0.7061)。

进一步地，第3步中，用LS求解右相机坐标的具体步骤包括：

W1，残差值e'＝B'-A'X',残差的平方

f(X')＝e'²＝(B'-A'X)²＝(B'-A'X')^T(B'-A'X')；

W2，对f(X')求导并令其等于零，即

W3，得到X的解X'＝(A'^T A')^-1A'^TB',根据已求得的横纵坐标可以推算出右相机的z轴 坐标至此，右相机坐标的计算完成：

S₁′＝(0.1683，0.4120，0.3666)，S₂′＝(0.1942，0.3313，0.4282)，

S₃′＝(0.1691，0.4169，0.3641)，S₄′＝(0.1967，0.4902，0.4638)，

S₅′＝(0.3524，0.5191，0.4030)，S₆′＝(0.2785，0.56860.5466)，

S₇′＝(0.4166，0.6064，0.6514)，S₈′＝(0.3724，0.6275，0.6464)，

S₉′＝(0.4669，0.6376，0.7468)，S₁₀′＝(0.4616，0.6805，0.303)。

此时得到设备的坐标h_j':

h₁'＝(0.1700,0.3621,0.3648)，h₂'＝(0.1976,0.2815,0.4265)，h₃'＝(0.2013，0.3984，0.3940)， h₄'＝(0.2322，0.4551，0.4603)，h₅'＝(0.3762，0.4876，0.3724)，h₆'＝(0.3072，0.5293，0.5351)， h₇'＝(0.4054，0.5632，0.6290)，h₈'＝(0.4012，0.5868，0.6423)，h₉'＝(0.4646，0.6038，0.7100)， h₁₀'＝(0.4790，0.6353，0.7182)。

进一步地，第4步中，计算设备倾角的具体步骤包括：

V1，令t₁＝[0,1,0]，将其分别转换成js时在世界坐标系的坐标g_j，由[g_j,1]＝[t₁,1]*W_j计 算得到，其中转换矩阵

V2，计算设备在第j秒的倾角从而得到θ₁＝2.7783°，θ₂＝4.4085°， θ₃＝25.8477°，θ₄＝45.3953°，θ₅＝50.9556°，θ₆＝38.1990°，θ₇＝30.1831°，θ₈＝35.5312°， θ₉＝47.4994°，θ₁₀＝25.1547°。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的， 不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下 在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权 利要求及其等同限定。

Claims (7)

1.一种基于双目技术的室内可见光自定位系统，包括相互协同作业的图像分析系统和自定位系统；所述图像分析系统包括：信标模块、图像采集模块、相机自标定模块、图像处理模块、立体匹配模块；所述自定位系统包括：坐标及倾角计算模块；其特征在于：

所述信标模块包括三个固定在上方的LED灯，三个LED灯分布在非同一直线上，且各LED灯在空间参考坐标系的坐标已知，各LED灯分别工作在设定的频率中；

所述图像采集模块包括：安装在手持设备上由左右二个相机组成的双目相机，所述双目相机用于从两个不同角度同时获得同一物体的图像信息；

所述相机自标定模块用于获得双目相机的内外参数，包括：图像中心、焦距f、镜头畸变和相机图像坐标系的旋转矩阵和平移矩阵；

所述图像处理模块用于降低噪声对图像质量的影响，增加图像对比度以增强边缘特性，提高后续两幅图像之间立体匹配的精确度；

所述立体匹配模块用于存储从各LED灯的OOK(On-Off Keying)编码到坐标的映射关系，通过解码图像中各LED灯的编码信息，找到空间内各LED灯在两幅图像中的匹配点，并获得左右两幅图像上匹配点在图像坐标系中的坐标，设定所述坐标分别为(a,b)和(a',b')；

所述坐标及倾角计算模块用于对获得的坐标进行处理，得到左右相机在三维空间坐标系的坐标，并计算手持设备的倾角；

所述双目相机为两个内外参数相同的CMOS相机，双目相机在拍照过程中对图像像素进行逐行扫描，从而识别并提取图像所中包含的信息。

2.一种基于双目技术的室内可见光自定位系统的定位方法，其特征在于：所述坐标及倾角计算模块在工作中具体实现过程包括以下步骤：

第1步，图像采集：

在设定空间内固定安装三个坐标不同且已知具体位置的LED灯，分别为LED₁、LED₂和LED₃，各LED灯在空间直角参考坐标系内的坐标分别为L₁(m₁,n₁,q₁)、L₂(m₂,n₂,q₂)和L₃(m₃,n₃,q₃)；以OOK编码I₁、I₂、I₃对LED₁、LED₂和LED₃进行调制，再用一定的载波频率f₀对编码后的每个LED灯进行调制，使得每个LED灯的发射频率大于1kHz以保证其变化无法被人眼察觉，并可以有效地保护视力；若CMOS相机的扫描频率为f_s，则每个LED灯的载波频率f₀为f_s/n(n为任意正整数)，同时将每个LED灯的OOK编码与坐标之间的映射关系存储到立体匹配模块中；在待定位设备上安装两个内外参数相同的相机，使其构成一对双目相机；相机在同一时刻从不同角度拍摄天花板，得到两张图像分别为左图像和右图像；

第2步，获得相机内外参数和匹配点坐标：

双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d、相机的焦距f，左右相机的旋转矩阵R、R'，左右相机的平移矩阵T、T'等参数；图像处理模块对图像进行处理，增加图像的对比度以增强边缘特性；立体匹配模块将两幅图像转化成灰度图并对其模糊化处理，然后使其通过一个单边OTSU滤波器，找到每个LED灯在图像上的对应子区域并从中解码得到LED_i(i＝1,2,3)在左右两幅图像中的匹配点的坐标分别为P_i(u_i，v_i)、P_i'(u_i'，v_i')；

第3步，计算摄像头的坐标：

设定：左右相机在世界坐标系的坐标分别为S＝(x，y，z)、S'＝(x'，y'，z')，由抽象出的双目相机与每个LED灯之间的几何关系，列出包含未知左右相机坐标的方程组分别为：

其中：将方程组分别整理成AX＝B和A'X'＝B'的矩阵形式，则有：

其中：A、B、A'、B'是由实际参数确定的已知向量，X、X'为需要求解的未知向量，并通过最小均方算法(Least Mean Square，LMS)或最小二乘法(Least Square Method，LS)求解该矩阵方程，进而得到左右摄像头的坐标；

第4步，计算设备的倾角。

3.如权利要求2所述一种基于双目技术的室内可见光自定位系统的定位方法，其特征在于：第3步中，用LMS算法求解左相机坐标的具体步骤包括：

S1，X(n)表示n次迭代后的估计系数X；

S2，设置步长μ，μ的取值范围为0<μ<2/λ_max；其中：λ_max是A的自相关矩阵的最大特征值；

S3，设置初始估计系数X(0)为任意值；

S4，判断是否满足误差项e(n)＝B-AX(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出左相机的z轴坐标至此，左相机坐标的计算完成；若否，则执行下一步；

S5，更新系数X(n+1)＝X(n)+μA^He(n)并循环S4，直到满足最小误差条件；所述A^H表示矩阵A的共轭转置。

4.如权利要求2所述的一种基于双目技术的室内可见光自定位系统的定位方法，其特征在于：第3步中，用LMS算法求解右相机坐标的具体步骤包括：

U1，X'(n)表示n次迭代后的估计系数X'；

U2，设置步长μ'，μ'的取值范围为0<μ'<2/λ'_max，其中，λ'_max是A'的自相关矩阵的最大特征值；

U3，设置初始估计系数X'(0)为任意值；

U4，判断是否满足误差项e'(n)＝B'-A'X'(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出右相机的z轴坐标至此，左相机坐标的计算完成；若否，则执行下一步；

U5，更新系数X'(n+1)＝X'(n)+μ'A'^He'(n)并循环U5，直到满足最小误差条件；所述A'^H表示矩阵A'的共轭转置。

5.如权利要求2所述的一种基于双目技术的室内可见光自定位系统的定位方法，其特征在于：第3步中，用LS算法求解左相机坐标的具体步骤包括：

Q1，残差值e＝B-AX，残差的平方f(X)表示为：

f(X)＝e²＝(B-AX)²＝(B-AX)^T(B-AX)；

Q2，对f(X)求导并令其等于零，即：

Q3，得到X的解X＝(A^TA)^-1A^TB，根据已求得的横纵坐标可以推算出左相机的z轴坐标至此，左相机坐标的计算完成。

6.如权利要求2所述的一种基于双目技术的室内可见光自定位系统的定位方法，其特征在于：第3步中，用LS算法求解右相机坐标的具体步骤包括：

W1，残差值e'＝B'-A'X',残差的平方：

f(X')＝e'²＝(B'-A'X)²＝(B'-A'X')^T(B'-A'X')；

W2，对f(X')求导并令其等于零，即：

W3，得到X的解X'＝(A'^TA')^-1A'^TB'，根据已求得的横纵坐标可以推算出右相机的z轴坐标至此，右相机坐标的计算完成。

7.如权利要求2所述的一种基于双目技术的室内可见光自定位系统的定位方法，其特征在于：第4步中，计算摄像头倾角的具体步骤包括：

V1，令n₁＝[0,a,0]，将其转换成世界坐标系的坐标n₁，由[n₂,1]＝[n₁,1]*W计算得到，其中转换矩阵

V2,设备的倾角