[发明专利]基于两盏LED灯及角度传感器的定位算法及定位系统

权利要求书

1.一种基于两盏LED灯及角度传感器的定位算法，其特征在于，包括如下步骤：

S1：拍摄两盏LED灯得到图片，对捕获的所述两盏LED灯进行解码，获取所述两盏LED灯的位置信息，并计算所述两盏LED灯的几何参数信息；

S2：根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系，结合步骤S1中的所述两盏LED灯的位置信息以及所述两盏LED灯的几何参数信息，并根据所述两盏LED灯的成像平面与水平面之间的夹角，求得摄像头中心的Z坐标；

S3：根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系，结合步骤S2求得的所述摄像头中点的Z坐标，计算得到摄像头中点的X坐标与Y坐标。

2.根据权利要求1所述的基于两盏LED灯及角度传感器的定位算法，其特征在于，在步骤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)。

3.根据权利要求2所述的基于两盏LED灯及角度传感器的定位算法，其特征在于，在步骤S1中，所述计算所述两盏LED灯的几何参数信息，包括：

通过图像处理技术，计算所述两盏LED灯的几何参数信息；

所述几何参数信息包括：所述两盏LED灯的成像椭圆的中心位置坐标(i1,j1)、(i2,j2)，所述两盏LED灯的成像椭圆中心分别到成像平面中心之间的距离d1、d2，所述两盏LED灯的成像椭圆的半长轴的长度a1、a2，LED灯的半径r。

4.根据权利要求3所述的基于两盏LED灯及角度传感器的定位算法，其特征在于，步骤S2包括：

S2.1：利用角度传感器获得摄像头的倾斜角t，所述倾斜角t即成像平面与水平面之间的夹角；

S2.2：计算所述摄像头的透镜中心到每盏LED灯的成像椭圆的长轴所在的水平面之间垂直距离h；

S2.3：根据透镜中心到LED等所在的平面的垂直距离与透镜中心到成像椭圆的长轴所在的水平面之间的垂直距离h之比等于LED灯的半径与成像椭圆半长轴之比，得到摄像头中点的Z坐标。

5.根据权利要求4所述的基于两盏LED灯及角度传感器的定位算法，其特征在于，步骤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。