基于两盏led灯及角度传感器的定位算法及定位系统
阅读说明：本技术 基于两盏led灯及角度传感器的定位算法及定位系统 (Positioning algorithm and positioning system based on two LED lamps and angle sensor ) 是由 安富通 关伟鹏 伍文飞 于 2020-07-20 设计创作，主要内容包括：本发明公开一种基于两盏LED灯及角度传感器的定位算法及定位系统,通过拍摄两盏LED灯得到图片,对捕获的所述两盏LED灯进行解码,获取所述两盏LED灯的位置信息,并计算所述两盏LED灯的几何参数信息,根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系,结合两盏LED灯的位置信息以及两盏LED灯的几何参数信息,并根据两盏LED灯的成像平面与水平面之间的夹角,求得摄像头中心的Z坐标,根据求得的摄像头中点的Z坐标,计算得到摄像头中点的X坐标与Y坐标。本发明基于双灯进行定位,接收端在水平或倾斜状态均可实现三维定位,定位精确,丰富了现有的定位算法,拓展了应用场景,有广阔的市场价值。(The invention discloses a positioning algorithm and a positioning system based on two LED lamps and an angle sensor, wherein pictures are obtained by shooting the two LED lamps, the two captured LED lamps are decoded to obtain position information of the two LED lamps, geometric parameter information of the two LED lamps is calculated, the Z coordinate of the center of a camera is obtained according to the geometric relationship among the two LED lamps, the camera and imaging ellipses of the two LED lamps, the position information of the two LED lamps and the geometric parameter information of the two LED lamps are combined, the included angle between the imaging plane of the two LED lamps and the horizontal plane is obtained, and the X coordinate and the Y coordinate of the midpoint of the camera are obtained by calculation according to the obtained Z coordinate of the midpoint of the camera. The invention carries out positioning based on double lamps, the receiving end can realize three-dimensional positioning in a horizontal or inclined state, the positioning is accurate, the existing positioning algorithm is enriched, the application scene is expanded, and the market value is wide.)
技术领域
本发明涉及可见光通信技术领域，尤其涉及一种基于两盏LED灯及角度传感器的定位算法及定位系统。
背景技术
可见光通信技术利用的是调制好的高频闪烁的肉眼无法分辨的LED灯，可以在现有的照明灯的基础上进行改造就可以实现。通过对传统技术的不断改进，使得传输速度不断加快，建设费用不断降低，受干扰程度越来越小，而且安全性高，应用场景越来越广泛。
可见光通信定位技术，则是对该技术特点的很好体现。如今CMOS图像传感器已经嵌入到各样式的电子设备中，尤其是智能手机终端，将其作为可见光成像通信中的光接收器件，快速捕获信息并完成信息的无线传输。再加以利用空间几何关系，利用可见光进行定位的技术也就应运而生。
然而利用LED灯进行定位，在单灯技术下实现效果较差，且目前不能实现三维空间定位。三灯定位技术需求较多的LED灯，存在计算量大，捕获完整图像较为困难等问题。而目前所有的双灯定位技术，也只是实现了接收端在水平状态下的定位，导致应用场景有限，不能普及。
因此，探索新的双灯定位技术，尤其是一般状态即接收端处于任意倾斜状态下实现三维定位就很有必要。
发明内容
本发明的目的是克服现有技术的不足，提供一种基于两盏LED灯及角度传感器的定位算法及定位系统，基于双灯实现接收端位于倾斜状态下的精确定位。
本发明的技术方案如下：提供一种基于两盏LED灯及角度传感器的定位算法，包括如下步骤：
S1：拍摄两盏LED灯得到图片，对捕获的所述两盏LED灯进行解码，获取所述两盏LED灯的位置信息，并计算所述两盏LED灯的几何参数信息；
S2：根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系，结合步骤S1中的所述两盏LED灯的位置信息以及所述两盏LED灯的几何参数信息，并根据所述两盏LED灯的成像平面与水平面之间的夹角，求得摄像头中心的Z坐标；
S3：根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系，结合步骤S2求得的所述摄像头中点的Z坐标，计算得到摄像头中点的X坐标与Y坐标。
进一步地，在步骤S1中，所述摄像头拍摄两盏LED灯得到图片，对捕获的所述两盏LED灯进行解码，获取所述两盏LED灯的位置信息，包括：
S1.1：在LED灯的发射端，利用PWM调制方式，控制输入PWM的频率、相位差，得到ID互不相同的LED灯，ID的具体形式是黑白条纹相间，其特征包括条纹的粗细和条纹的数目，不同的LED灯的ID展示出来的特征不同；
S1.2：拍摄两盏LED灯得到图片；
S1.3：通过图像处理技术处理拍摄得到的图片，解码得到两盏LED灯的面积、白色条纹数、黑白条纹占空比特征，以及两盏LED灯分别在拍摄图片中的几何位置信息；
S1.4：根据获取到的两盏LED灯的特征信息，与预先建立的本地数据库中的LED灯的ID进行匹配，得到所拍摄的两盏LED灯的具***置坐标(x1,y1,0)、(x2,y2,0)。
进一步地，在步骤S1中，所述计算所述两盏LED灯的几何参数信息，包括：
通过图像处理技术，计算所述两盏LED灯的几何参数信息；
所述几何参数信息包括：所述两盏LED灯的成像椭圆的中心位置坐标(i1,j1)、(i2,j2)，所述两盏LED灯的成像椭圆中心分别到成像平面中心之间的距离d1、d2，所述两盏LED灯的成像椭圆的半长轴的长度a1、a2，LED灯的半径r。
进一步地，步骤S2包括：
S2.1：利用角度传感器获得摄像头的倾斜角t，所述倾斜角t即成像平面与水平面之间的夹角；
S2.2：计算所述摄像头的透镜中心到每盏LED灯的成像椭圆的长轴所在的水平面之间垂直距离h；
S2.3：根据透镜中心到LED等所在的平面的垂直距离与透镜中心到成像椭圆的长轴所在的水平面之间的垂直距离h之比等于LED灯的半径与成像椭圆半长轴之比，得到摄像头中点的Z坐标。
进一步地，步骤S2.2包括：
S2.2.1：当LED灯的成像椭圆位于倾斜的成像平面的高侧时，
h＝(f-d·tant(t))·cost；
当LED灯的成像椭圆位于倾斜的成像平面的低侧时，
h＝(d·tant(t)+f)·cost；
其中，f为透镜的焦距，d为LED灯的成像椭圆中心到成像平面中心之间的距离；
S2.2.2：由于无法得知两盏LED灯的成像椭圆位于成像平面的高侧还是低侧，因此计算得到四个h，分别为h11,h12,h21,h22。
进一步地，步骤2.3包括：
S2.3.1：根据透镜中心到LED等所在的平面的垂直距离与透镜中心到成像椭圆的长轴所在的水平面之间的垂直距离h之比等于LED灯的半径与成像椭圆半长轴之比，并结合步骤S2.2.2计算出的四个h，得到以下四个公式：
根据以上四个公式得到摄像头中心的四个Z坐标值Z1、Z2、Z3、Z4；
S2.3.2：将摄像头中心的四个Z坐标值Z1、Z2、Z3、Z4进行两两做差并取绝对值，对差值的绝对值最小的两个Z坐标取平均值，将平均值作为摄像头中心的Z坐标。
进一步地，步骤S3包括：
S3.1：根据几何关系，LED灯的半径与成像椭圆的半长轴之比，等于透镜中心到LED灯的圆心的距离与透镜中心到成像椭圆中心的距离之比，得到两个公式：
S3.2：联立以上两组公式，并代入步骤S2求得的Z坐标值，求出摄像头中心的X与Y坐标值。
进一步地，步骤3.2包括：
S3.2.1：公式1与公式2相当于是以(x1，y1)和(x2，y2)为圆心的两个圆，通常两圆相交的交点有两个，分别记为(X1，Y1)、(X2，Y2)，联立公式1与公式2，得到(X1，Y1)、(X2，Y2)的坐标，其中只有一个交点的坐标为摄像头的X、Y坐标；
S3.2.2：规定两盏LED灯的圆心坐标(x2，y2)指向(x1，y1)的向量为向量1，对两交点(X1，Y1)、(X2，Y2)进行旋转变换处理，向量1与X轴之间的夹角记为角0，取角0的相反数记为角t，利用旋转角公式得到(X11，Y11)、(X22，Y22)的坐标，所述旋转角公式如下：
其中(X11，Y11)为(X1，Y1)经过旋转变换处理后的新的坐标，(X22，Y22)为(X2，Y2)经过旋转变换处理后的新的坐标；
若Y11大于Y22，则(X1，Y1)位于向量1的左侧，(X2，Y2)位于向量1的右侧；若Y11小于Y22，则(X1，Y1)位于向量1的右侧，(X2，Y2)位于向量1的左侧；
S3.2.3：将角0进行转换成为向量1与Y轴正方向的夹角，记为角1，范围为0～360°，再由角度传感器测出接收端的AZIMUTH角，即为接收端与正北方向的夹角，记为角2，范围为0～360°，所规定的坐标轴的Y轴正方向与地理的正北方向之间的夹角记为角3，范围0～360°，角1与角3进行运算处理，得到向量1与正北方向的夹角，记为角4，利用角度传感器获取相机的ROLL角，范围-90°～90°；
S3.2.4：在ROLL角>0的情况下：如果角4与角2差值的绝对值在90°～270°之间，摄像头最终的定位坐标就选择左侧的坐标点，否则就选择右侧坐标作为摄像头最终的定位坐标；
在ROLL角<0的情况下：如果角4与角2差值的绝对值在90°～270°之间，摄像头最终的定位坐标就选择右侧的坐标点，否则就选择左侧坐标作为摄像头最终的定位坐标。
本发明还提供一种定位系统，包括接收端、处理器以及存储在所述处理器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以上所述的基于两盏LED灯及角度传感器的定位算法及定位系统；
所述接收端包括摄像头以及角度传感器，所述摄像头用于拍摄两盏LED灯得到图片；
所述角度传感器用于感应摄像头倾斜的角度。
采用上述方案，本发明基于双灯进行定位，接收端在水平或倾斜状态均可实现三维定位，定位精确，丰富了现有的定位算法，拓展了应用场景，有广阔的市场价值。
附图说明
图1为本发明的应用示意图。
图2为当LED灯的成像椭圆位于成像平面的高侧时的成像示意图。
图3为当LED灯的成像椭圆位于成像平面的低侧时的成像示意图。
图4为相似关系图。
图5为实际应用场景图。
图6为联立公式1与公式2的示意图。
图7为经过旋转变换处理后的示意图。
图8为联立大地坐标系的示意图。
图9为本发明的流程示意图。
具体实施方式
以下结合附图和具体实施例，对本发明进行详细说明。
请参阅图1、图5以及图9，本发明提供一种基于两盏LED灯及角度传感器的定位算法，包括如下步骤：
S1：拍摄两盏LED灯得到图片，对捕获的所述两盏LED灯进行解码，获取所述两盏LED灯的位置信息，并计算所述两盏LED灯的几何参数信息。
S2：根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系，结合步骤S1中的所述两盏LED灯的位置信息以及所述两盏LED灯的几何参数信息，并根据所述两盏LED灯的成像平面与水平面之间的夹角，求得摄像头中心的Z坐标。
S3：根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系，结合步骤S2求得的所述摄像头中点的Z坐标，计算得到摄像头中点的X坐标与Y坐标。
在步骤S1中，所述摄像头拍摄两盏LED灯得到图片，对捕获的所述两盏LED灯进行解码，获取所述两盏LED灯的位置信息，包括：
S1.1：在LED灯的发射端，利用PWM调制方式，控制输入PWM的频率、相位差，得到ID互不相同的LED灯，ID的具体形式是黑白条纹相间，其特征包括条纹的粗细和条纹的数目，不同的LED灯的ID展示出来的特征不同。通过可见光通信编码技术对室内的LED灯进行调制，使LED灯发出肉眼无法识别的高频灯光，同时不影响照明使用，不同的调制频率对应不同的LED-ID，做到了照明、通信两用，并将室内LED灯的位置以及其对应的ID进行记录。
S1.2：拍摄两盏LED灯得到图片，可以采用手机的CMOS摄像头进行拍摄。
S1.3：通过图像处理技术处理拍摄得到的图片，解码得到两盏LED灯的面积、白色条纹数、黑白条纹占空比特征，以及两盏LED灯分别在拍摄图片中的几何位置信息。
S1.4：根据获取到的两盏LED灯的特征信息，与预先建立的本地数据库中的LED灯的ID进行匹配，得到所拍摄的两盏LED灯的具***置坐标(x1,y1,0)、(x2,y2,0)。
在步骤S1中，所述计算所述两盏LED灯的几何参数信息，包括：
通过图像处理技术，计算所述两盏LED灯的几何参数信息。
所述几何参数信息包括：所述两盏LED灯的成像椭圆的中心位置坐标(i1,j1)、(i2,j2)，所述两盏LED灯的成像椭圆中心分别到成像平面中心之间的距离d1、d2，所述两盏LED灯的成像椭圆的半长轴的长度a1、a2，以及LED灯的半径r。室内每盏LED灯均为同规格的LED灯，即每盏LED灯的半径相等。
步骤S2包括：
S2.1：利用角度传感器获得摄像头的倾斜角t，所述倾斜角t即成像平面与水平面之间的夹角。
S2.2：计算所述摄像头的透镜中心到每盏LED灯的成像椭圆的长轴所在的水平面之间垂直距离h，具体包括如下步骤：
S2.2.1：由于成像平面为倾斜的平面，LED灯的成像椭圆可能会位于成像平面的高侧或者成像平面的低侧，无法得知。请参阅图2，当LED灯的成像椭圆位于倾斜的成像平面的高侧时，
h＝(f-d·tant(t))·cost；
请参阅图3，当LED灯的成像椭圆位于倾斜的成像平面的低侧时，
h＝(d·tant(t)+f)·cost；
其中，f为透镜的焦距，d为LED灯的成像椭圆中心到成像平面中心之间的距离。
S2.2.2：由于无法得知两盏LED灯的成像椭圆位于成像平面的高侧还是低侧，因此每盏LED灯能计算出两个h，两盏灯共计算得到四个h，分别为h11,h12,h21,h22。
S2.3：根据透镜中心到LED等所在的平面的垂直距离与透镜中心到成像椭圆的长轴所在的水平面之间的垂直距离h之比等于LED灯的半径与成像椭圆半长轴之比，得到摄像头中点的Z坐标。具体包括如下步骤：
S2.3.1：根据透镜中心到LED等所在的平面的垂直距离与透镜中心到成像椭圆的长轴所在的水平面之间的垂直距离h之比等于LED灯的半径与成像椭圆半长轴之比，并结合步骤S2.2.2计算出的四个h，得到以下四个公式：
根据以上四个公式得到摄像头中心的四个Z坐标值Z1、Z2、Z3、Z4。
S2.3.2：将摄像头中心的四个Z坐标值Z1、Z2、Z3、Z4进行两两做差并取绝对值，对差值的绝对值最小的两个Z坐标取平均值，将该平均值作为摄像头中心的Z坐标。
步骤S3包括：
S3.1：请参阅图4，根据几何关系，LED灯的半径与成像椭圆的半长轴之比，等于透镜中心到LED灯的圆心的距离与透镜中心到成像椭圆中心的距离之比，由此构建两对相似关系：
S3.2：联立以上两组公式，并代入步骤S2求得的Z坐标值，求出摄像头中心的X与Y坐标值，由此，得到摄像头的三维坐标。
具体地，步骤3.2包括：
S3.2.1：请参阅图6，公式1与公式2相当于是以(x1，y1)和(x2，y2)为圆心的两个圆，通常两圆相交的交点有两个，分别记为(X1，Y1)、(X2，Y2)，需要判断哪个交点为摄像头的最终定位坐标。联立公式1与公式2，得到(X1，Y1)、(X2，Y2)的坐标，其中只有一个交点的坐标为摄像头的X、Y坐标。
S3.2.2：规定两盏LED灯的圆心坐标(x2，y2)指向(x1，y1)的向量为向量1，对两交点(X1，Y1)、(X2，Y2)进行旋转变换处理，向量1与X轴之间的夹角记为角0，取角0的相反数记为角t，利用旋转角公式得到(X11，Y11)、(X22，Y22)的坐标，所述旋转角公式如下：
其中(X11，Y11)为(X1，Y1)经过旋转变换处理后的新的坐标，(X22，Y22)为(X2，Y2)经过旋转变换处理后的新的坐标。
请参阅图7，由于进行旋转变换处理后，(X11，Y11)与(X22，Y22)两点关于经旋转变换后的X轴对称，因此若Y11大于Y22，则(X11，Y11)位于X轴正方向的左侧，(X22，Y22)位于X轴正方向的右侧，也就是(X1，Y1)位于向量1的左侧，(X2，Y2)位于向量1的右侧；若Y11小于Y22，则(X11，Y11)位于X轴正方向的右侧，(X22，Y22)位于X轴正方向的左侧，也就是(X1，Y1)位于向量1的右侧，(X2，Y2)位于向量1的左侧。
S3.2.3：请参阅图8，将角0进行转换成为向量1与Y轴正方向的夹角，记为角1，范围为0～360°(以正北方向为基准，向东旋转为正方向)。再由角度传感器测出接收端的AZIMUTH角，即为接收端与正北方向的夹角，记为角2，范围为0～360°(以正北方向为基准，向东旋转为正方向)。所规定的坐标轴的Y轴正方向与地理的正北方向之间的夹角记为角3，范围0～360°(以正北方向为基准，向东旋转为正方向)。角1与角3进行运算处理，得到向量1与正北方向的夹角，记为角4，利用角度传感器获取相机的ROLL角，范围-90°～90°。
S3.2.4：在ROLL角>0的情况下：如果角4与角2差值的绝对值在90°～270°之间，摄像头最终的定位坐标就选择左侧的坐标点，否则就选择右侧坐标作为摄像头最终的定位坐标；
在ROLL角<0的情况下：如果角4与角2差值的绝对值在90°～270°之间，摄像头最终的定位坐标就选择右侧的坐标点，否则就选择左侧坐标作为摄像头最终的定位坐标。
本发明还提供一种定位系统，包括接收端、处理器以及存储在所述处理器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以上所述的基于两盏LED灯及角度传感器的定位算法及定位系统。
所述接收端包括摄像头以及角度传感器，所述摄像头用于拍摄两盏LED灯得到图片。
所述角度传感器用于感应摄像头倾斜的角度。
综上所述，本发明基于双灯进行定位，接收端在水平或倾斜状态均可实现三维定位，定位精确，丰富了现有的定位算法，拓展了应用场景，有广阔的市场价值。
以上仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。