CN112164113A - 一种室内定位中新颖的可见光通信辅助的透视四线方法 - Google Patents

Abstract

本发明提供了一种室内定位中新颖的可见光通信辅助的透视四线方法，属于无线通信技术领域。具体包括：首先构建包括LED灯具发射器和待定位相机接收器的场景，待定位相机获得LED灯具四个顶点投影的像素坐标后变换到图像坐标；然后，在CCS中估计LED灯具的方向；计算LED灯具的四个顶点在CCS中的相机坐标；接着利用LED灯具在CCS和WCS中的方向，计算待定位相机在x^c轴和y^c轴对应的旋转角；利用LED灯具在CCS中的相机坐标，计算WCS中待定位相机的z坐标；最后，基于线性最小二乘法计算待定位相机z^c轴对应的旋转角、以及WCS中待定位相机的x坐标和y坐标，最终得到待定位相机在WCS中的姿态和位置。本发明只需要单个灯具进行定位，在不同灯具宽度和图像噪声下，精度性能优于P4L法和CA‑RSSR。

Description

一种室内定位中新颖的可见光通信辅助的透视四线方法

技术领域

本发明属于无线通信技术领域，具体涉及一种室内定位中新颖的可见光通信辅助的透视四线方法。

背景技术

随着诸如位置跟踪、导航和机器人运动控制等基于位置服务的兴起，精确的室内定位变得越来越重要。在本研究领域中，可见光定位(Visible Light Positioning，VLP)和基于计算机视觉的定位以其高精度、低成本的特点得到了越来越多的关注，见参考文献[1][2]。

典型的VLP算法包括近邻法[3]、指纹法[4]、到达时间法(Time of Arrival，TOA)[5]、到达角法(Angle of Arrival，AOA)[6]、接收信号强度法(Received SignalStrength，RSS)[7]和图像传感器法[8]。

但是在这些算法中，只有图像传感器法才能估计接收器的姿态。此外，图像传感器法、TOA法、AOA法和RSS法需要多个灯具来定位[1]，当可用灯具不足时，这是不可行的。另一方面，近邻法和指纹法可以利用单个灯具估计接收器的位置。然而，近邻法的精度低；在指纹法中，歧义问题严重，因此精度也不高[9]。文献[9]已经证明，在指纹法中至少需要三个灯具来减小歧义的影响；RSS法因其高精度和低成本的特点被广泛应用[7]。然而，RSS法依赖于精确的信道模型，这是它们面临的最严重问题。在RSS法中一个通常的假设是LED的辐射模式为朗伯模型，这对于许多灯具可能不太适用，特别是当使用了灯罩时[10]。同时，在实际中，估计的信道增益会受到阳光、灰尘和阴影的影响[11]。

另一方面，典型的基于计算机视觉的定位方法包括透视n线法(Perspective-n-Line，PnL)和透视n点法(Perspective-n-Point，PnP)。PnL法通过分析3维(3-Dimensional，3D)参考线和它们的2维(2-Dimensional，2D)投影之间的n个对应关系(即3D-2D对应关系)来进行。与利用点特征的PnP法相比，PnL法具有更高的检测精度，并且对(部分)遮挡具有更强的鲁棒性[12]。然而，PnL法存在一些固有的挑战：首先，PnL法需要3D-2D对应关系，这是很难获得的。在已有的PnL法研究中，直接给出了3D-2D对应关系，这在实际中是不现实的。为了规避这一挑战，文献[2]提出了一种方法，用于在垂直线多于水平线的场景下去寻找3D-2D对应关系。但是，这种方法不适用于矩形信标部署在天花板上的场景，因为不同方向线的数目没有显著差异。此外，信标的坐标很难知道，这意味着无法获得相机的绝对位置。

[1]T.-H.Do and M.Yoo,“An in-depth survey of visible lightcommunication based positioning systems,”Sensors,vol.16,no.5,pp.678,May 2016.

[2]T.Goto,S.Pathak,Y.Ji,H.Fujii,A.Yamashita,and H.Asama,“Line basedglobal localization of a spherical camera in manhattan worlds,”in 2018 IEEEInt.Conf.Robotics Automation(ICRA),2018,pp.2296–2303.

[3]C.Sertthin,T.Ohtsuki,and M.Nakagawa,“6-axis sensor assisted lowcomplexity high accuracy-visible light communication based indoor positioningsystem,”IEICE Trans.Commun.,vol.93,no.11,pp.2879–2891,Nov.2010.

[4]K.Qiu,F.Zhang,and L.Ming,“Let the light guide us:VLC-basedlocalization,”IEEE Robot.Autom.Mag.,vol.23,no.4,pp.174–183,Dec.2016.

[5]T.Q.Wang,Y.A.Sekercioglu,A.Neild,and J.Armstrong,“Positionaccuracy of time-of-arrival based ranging using visible light withapplication in indoor localization systems,”J.Lightw.Technol.,vol.31,no.20,pp.3302–3308,Oct.2013.

[6]B.Zhu,J.Cheng,Y.Wang,J.Yan,and J.Wang,“Three-dimensional VLCpositioning based on angle difference of arrival with arbitrary tilting angleof receiver,”IEEE J.Sel.Areas Commun.,vol.36,no.1,pp.8–22,Jan.2018.

[7]L.Bai,Y.Yang,C.Guo,C.Feng,and X.Xu,“Camera assisted receivedsignal strength ratio algorithm for indoor visible light positioning,”IEEECommun.Lett.,vol.23,no.11,pp.2022–2025,Nov.2019.

[8]Y.Li,Z.Ghassemlooy,X.Tang,B.Lin,and Y.Zhang,“A VLC smartphonecamera based indoor positioning system,”IEEE Photon.Technol.Lett.,vol.30,no.13,pp.1171–1174,Jul.2018.

[9]A.M.Vegni and M.Biagi,“An indoor localization algorithm in asmall-cell LED-based lighting system,”in Proc.IEEE Int.Conf.IndoorPosition.Indoor Navig.(IPIN),2012,pp.1–7.

[10]F.Miramirkhani and M.Uysal,“Channel modeling and characterizationfor visible light communications,”IEEE Photonics J.,vol.7,no.6,pp.1–16,Dec.2015.

[11]Z.Dong,T.Shang,Y.Gao,and Q.Li,“Study on VLC channel modelingunder random shadowing,”IEEE Photonics J.,vol.9,no.6,pp.1–16,Dec.2017.

[12]Alexander V.,Jan F.,and Francesc M.-N.,“Accurate and linear timepose estimation from points and lines,”in Europ.Conf.Comp.Visi.(ECCV),2016,pp.583–599.

发明内容

本发明针对以上问题，提出了一种室内定位中新颖的可见光通信辅助的透视四线方法，将可见光通信获得的坐标信息与计算机视觉中的几何信息结合起来，基于单视图几何和线性最小二乘法计算相机的绝对位置和姿态；本发明只需要单个灯具，不依赖于信道模型，且不需要3D-2D对应关系。

所述的定位方法具体步骤如下：

步骤一、构建包括一个LED灯具发射器和一个待定位相机接收器的场景，同时定义四种定位坐标系；

四种坐标系分别是像素坐标系(Pixel Coordinate System，PCS)o^p-u^pv^p、图像坐标系(Image Coordinate System，ICS)oⁱ-xⁱyⁱ、相机坐标系(Camera Coordinate System，CCS)o^c-x^cy^cz^c和世界坐标系(World Coordinate System，WCS)o^w-x^wy^wz^w。

场景中的发射器被安装在天花板上，是一个由4个顶点P_i(i∈{1,2,3,4})构成的支持可见光通信的矩形LED灯具，WCS中灯具的第i个顶点的坐标为

四条3D参考线L_ij((i,j)∈{(1,2),(2,3),(3,4),(4,1)})是灯具的边。

场景中的接收器是一个标准的针孔相机，与灯具不共面。因此，发射器和接收器构成了一个四棱锥o^c-P₁P₂P₃P₄。

步骤二、待定位相机获得LED灯具四个顶点投影的像素坐标后，基于单视图几何将像素坐标变换到图像坐标；

LED灯具的第i个顶点P_i在待定位相机的图像平面上投影点为p_i，像素坐标为

L_ij在图像平面上的2D投影为l_ij。

投影点p_i的图像坐标

表示如下

其中，d_x是图像平面上x轴方向每个像素的物理尺寸，d_y是图像平面上y轴方向每个像素的物理尺寸。此外，u₀,v₀ ^T是ICS原点oⁱ的像素坐标，称为主点。

步骤三、利用LED灯具四个顶点投影的图像坐标，基于单视图几何、平面几何和立体几何，在CCS中估计LED灯具的方向；

在CCS中LED灯具的方向用矩形P₁P₂P₃P₄的单位法向量表示，即：

其中，

和

A、B和C是CCS中矩形P₁P₂P₃P₄的一般形式方程的系数；m和n是利用LED灯具四个顶点投影的图像坐标，并基于单视图几何、平面几何和立体几何推导过程的中间变量。

步骤四、基于立体几何计算LED灯具的四个顶点在CCS中的相机坐标；

LED灯具的第i个顶点P_i的相机坐标

表示为：

其中

A_ij、B_ij和C_ij((i,j)∈{(1,2),(2,3),(3,4),(4,1)})是顶点P_i、P_j和o^c构成的侧面Π_ij的一般形式方程的系数；

其中，

M₁₂＝W₁,W₂,W₃ ^T，M₂₃＝W₂,W₃,W₄ ^T，M₃₄＝W₃,W₄,W₁ ^T，M₄₁＝W₄,W₁,W₂ ^T；S是灯具的面积。

步骤五、基于单视图几何，利用LED灯具在CCS和WCS中的方向，计算待定位相机在x^c轴对应的旋转角

和y^c轴对应的旋转角；

基于单视图几何，灯具在WCS中的单位法向量为

在CCS中的单位法向量为

转换关系为：

为CCS到WCS的旋转矩阵，即相机的姿态；

R_X表示沿x^c轴的旋转矩阵，

是x^c轴对应的旋转角。

R_Y表示沿y^c轴的旋转矩阵，

θ是y^c轴对应的旋转角。

R_Z表示沿z^c轴的旋转矩阵，

ψ是z^c轴对应的旋转角。

因此有：

通过求解上式即可得到待定位相机的x^c轴对应的旋转角

和y^c轴对应的旋转角θ。

步骤六、通过可见光通信将LED灯具四个顶点的世界坐标发送至待定位相机，然后利用LED灯具在CCS中的相机坐标，计算WCS中待定位相机的z坐标；

具体步骤如下：

首先，针对LED灯具的第i个顶点P_i，相机坐标为

世界坐标为

然后，世界坐标与相机坐标的转换为：

是待定位相机的3D世界坐标，即待定位相机的位置；

WCS中待定位相机的z坐标计算如下：

其中，c₁，c₂和c₃是通过x^c轴和y^c轴的旋转矩阵相乘后，得到的矩阵元素值，即：

步骤七、基于线性最小二乘法计算待定位相机z^c轴对应的旋转角ψ、以及WCS中待定位相机的x坐标t_x和y坐标t_y，最终得到待定位相机在WCS中的姿态和位置。

首先，将未知参数设为X＝[cosψ,sinψ,t_x,t_y]^T，X是X的估计。

线性最小二乘法的未知参数方程为：

X＝A_ij ^TA_ij ^-1A_ij ^Tb_ij

其中，A_ij＝[A_i,A_j]^T，b_ij＝[b_i,b_j]^T(i,j∈{1,2,3,4},i≠j)，其中

每个b_ij＝[b_i,b_j]^T(i,j∈{1,2,3,4},i≠j)，分别对应6个A_rs(r,s∈{1,2,3,4},r≠s)，得到6个解，其中有一个解X_ij即为b_ij对应正确的解X。

针对6个不同的b₁₂，b₁₃，b₁₄，b₂₃，b₂₄，b₃₄，总共可以获得6组36个解，通过对这6组中6个最接近的解X_ij求平均得到最优结果，表示如下：

最终，得到待定位相机的姿态和位置，即

和

本发明的优点有：

1.一种室内定位中新颖的可见光通信辅助的透视四线方法，只需要单个灯具进行定位，并且不依赖于信道模型；

2.一种室内定位中新颖的可见光通信辅助的透视四线方法，不需要已知3D-2D对应关系；

3.一种室内定位中新颖的可见光通信辅助的透视四线方法，在不同灯具宽度和图像噪声下，精度性能优于P4L法和CA-RSSR。

附图说明

图1为本发明一种室内定位中新颖的可见光通信辅助的透视四线方法的流程图；

图2为本发明在四种坐标系下的VLP系统框图；

图3为本发明在不同灯具宽度下VP4L、P4L法和CA-RSSR的定位误差对比图；

图4为本发明在不同灯具宽度下VP4L、P4L法的方向误差对比图；

图5为本发明在不同图像噪声下VP4L、P4L法和CA-RSSR的定位误差对比图；

图6为本发明在不同图像噪声下VP4L、P4L法的方向误差对比图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图对本发明作进一步的详细和深入描述。

本发明是一种室内定位中新颖的可见光通信(Visible Light Communication，VLC)辅助的透视四线方法(Visible Light Communication Assisted Perspective-Four-Line，V-P4L)，将VLC得到的坐标信息与计算机视觉中的几何信息结合起来，在基于平面和立体几何的相机坐标系下，首先利用几何信息，对单个矩形LED灯具的方向和坐标进行估计；然后利用VLC将灯具的世界坐标信息发送至待定位相机。接着利用单视图几何和线性最小二乘法(Linear Least Square，LLS)求出待定位相机在世界坐标系中的姿态

和位置

分别对应六个未知数

θ和ψ以及t_x、t_y和t_z，即根据LED顶点的各种坐标去求解这六个未知数的过程。

所述的定位方法如图1所示，具体步骤如下：

步骤一、构建包括一个LED灯具发射器和一个待定位相机接收器的场景，同时定义四种定位坐标系；

如图2所示，四种坐标系分别是像素坐标系(Pixel Coordinate System，PCS)o^p-u^pv^p、图像坐标系(Image Coordinate System，ICS)oⁱ-xⁱyⁱ、相机坐标系(CameraCoordinate System，CCS)o^c-x^cy^cz^c和世界坐标系(World Coordinate System，WCS)o^w-x^wy^wz^w。

在PCS、ICS和CCS中，坐标轴u^p、xⁱ和x^c相互平行，坐标轴v^p、yⁱ和y^c也相互平行。此外，oⁱ和o^c在同一条直线上；

CCS的原点o^c与ICS的原点oⁱ之间的距离为焦距f，因此在CCS中图像平面的z坐标为z^c＝f；另外，o^p在图像平面的左上角。

场景中的发射器被安装在天花板上，是一个由4个顶点P_i(i∈{1,2,3,4})构成的支持可见光通信的矩形LED灯具。四条3D参考线L_ij((i,j)∈{(1,2),(2,3),(3,4),(4,1)})是灯具的边。

假设灯具垂直向下，则WCS中灯具的单位法向量

其中(·)^T表示矩阵的转置。此外，

是WCS中灯具的第i个顶点的坐标。假设在发射端是已知的，可以通过可见光通信在接收端获得。

场景中的接收器是一个标准的针孔相机，与灯具不共面。因此，发射器和接收器构成了一个四棱锥o^c-P₁P₂P₃P₄。在四棱锥o^c-P₁P₂P₃P₄中，P_i是矩形P₁P₂P₃P₄的第i个顶点，CCS的原点o^c为四棱锥的顶点，矩形P₁P₂P₃P₄为底面，同时定义Π_ij为侧面，由顶点P_i,P_j(i,j∈{(1,2),(2,3),(3,4),(4,1)})和o^c构成。

步骤二、待定位相机获得LED灯具四个顶点投影的像素坐标后，基于单视图几何将像素坐标变换到图像坐标；

在相机中，LED灯具的第i个顶点P_i在待定位相机的图像平面上投影点为p_i，L_ij在图像平面上的2D投影为l_ij。

需要注意的是，许多现有的PnL法假设3D-2D对应关系

是提前已知的，这在实际中过于理想。而在本发明中，3D-2D对应关系是未知的。

第i个顶点的投影点p_i的像素坐标为

该坐标可由相机通过图像处理获得。基于单视图几何，投影点p_i的图像坐标

表示如下

其中，d_x是图像平面上x轴方向每个像素的物理尺寸，d_y是图像平面上y轴方向每个像素的物理尺寸。此外，u₀,v₀ ^T是ICS原点oⁱ的像素坐标，称为主点。

相机的内部参数，包括u₀,v₀ ^T、u轴和v轴上的归一化焦距

和

可以被提前校准；从CCS到WCS的转换可以表示为：

其中，

和

分别表示相机在WCS中的姿态和位置；定位的任务就是求出

和

步骤三、利用LED灯具四个顶点投影的图像坐标，基于单视图几何、平面几何和立体几何，在CCS中估计LED灯具的方向；

在ICS中，给定l_ij的点法式方程可以表示为

xⁱcosφ_ij+yⁱsinφ_ij＝ρ_ij (3)

其中，

是l_ij上一点的图像坐标，φ_ij是yⁱ轴沿逆时针方向至l_ij的旋转角，ρ_ij是oⁱ到l_ij的距离。由于p_i和p_j在l_ij上，因此基于p_i和p_j的图像坐标可以获得φ_ij和ρ_ij。另外，在l_ij上存在两点s_ij,1和s_ij,2，相机坐标分别为

和

有了Π_ij上的s_ij,1、s_ij,2和o^c，可以将CCS中Π_ij表示为以下的一般形式

A_ijx^c+B_ijy^c+C_ijz^c＝0 (4)

其中，A_ij＝fcos_ij，B_ij＝fsin_ij，C_ij＝-ρ_ij。

在CCS中，矩形P₁P₂P₃P₄的一般形式方程可以表示为

Ax^c+By^c+Cz^c＝1 (5)

其中，A、B和C是未知常数。矩形P₁P₂P₃P₄的法向量可以表示为n^c＝(A,B,C)^T。

在CCS中，设

(2,3表示Π_ij的法向量，

表示L_ij的方向向量。由于L_ij是平面P₁P₂P₃P₄和平面Π_ij的交线，因此

可以计算为

基于立体几何，可以得到：

定义

和

通过求解公式(6)可将m和n表示为A_ij、B_ij和C_ij的函数。因此，CCS中矩形P₁P₂P₃P₄的单位法向量(即灯具的方向)可以表示为

其中，

步骤四、基于立体几何计算LED灯具的四个顶点在CCS中的相机坐标；

由于P₁是矩形P₁P₂P₃P₄、Π₁₂和Π₄₁的交点，因此相机坐标

可以计算为

其中

因此，相机坐标

可以表示为如下统一的形式

其中

公式(9)中唯一的未知数是C，基于立体几何来计算：四棱锥o^c-P₁P₂P₃P₄的体积计算为

其中S是灯具的面积，是提前已知的。此外，

是o^c到矩形P₁P₂P₃P₄的距离。对于三棱锥o^c-P₁P₂P₃来说，它的体积可以计算如下

其中，detM是矩阵M的行列式，

将公式(9)带入公式(10)，可以得到

其中，

其中M₁₂＝W₁,W₂,W₃ ^T。其它三个三棱锥o^c-P₂P₃P₄、o^c-P₃P₄P₁和o^c-P₄P₁P₂的体积可以分别表示为V₂、V₃和V₄，可以通过相同的方法获得，分别为

其中，

M₂₃＝W₂,W₃,W₄ ^T，M₃₄＝W₃,W₄,W₁ ^T，M₄₁＝W₄,W₁,W₂ ^T；

由于

因此C可以计算如下

将公式(11)带入公式(9)，可以得到P_i(i∈{1,2,3,4})的相机坐标。

利用可见光通信将LED灯具4个顶点的世界坐标发送至待定位相机，然后基于LED灯具的方向和坐标信息，利用单视图几何和线性最小二乘法求出世界坐标系中待定位相机的位置和姿态。具体来说，可以通过以下步骤来实现：

步骤五、基于单视图几何，利用LED灯具在CCS和WCS中的方向，计算待定位相机在x^c轴对应的旋转角

和y^c轴对应的旋转角θ；

设R_X、R_Y和R_Z分别表示沿x^c轴、y^c轴和z^c轴的旋转矩阵。给出R_X、R_Y和R_Z如下

其中，

θ和ψ分别是x^c轴、y^c轴和z^c轴对应的未知欧拉角，从CCS到WCS的旋转矩阵

可以表示为：

基于单视图几何，灯具在WCS中的单位法向量为

在CCS中的单位法向量为

二者之间的关系可以表示为

因此，可以得到：

通过求解上式即可得到待定位相机的x^c轴对应的旋转角

和y^c轴对应的旋转角θ。

步骤六、通过可见光通信将LED灯具四个顶点的世界坐标发送至待定位相机，然后利用LED灯具在CCS中的相机坐标，计算WCS中待定位相机的z坐标；

具体步骤如下：

基于P_i(i∈{1,2,3,4})的相机坐标，P_i的世界坐标可以表示为

其中，

是提前已知的，可以通过VLC在接收端获得，

可以在步骤四中计算，

是待定位相机的3D世界坐标，即待定位相机的位置；设

可以将公式(17)改写如下

在公式(18)中，有4个未知参数ψ、t_x、t_y和t_z。如果3D-2D对应关系是提前已知的，可以很容易地利用4个顶点的世界坐标和相机坐标获得4个未知参数。然而，在本发明中，出于对实际考虑，3D-2D对应关系是未知的。因此，只能从公式(18)中得到

和t_z之间的关系为

这是因为所有LED都被安装在天花板上，且高度相同，即

WCS中待定位相机的z坐标可以计算如下

步骤七、基于线性最小二乘法计算待定位相机z^c轴对应的旋转角ψ、以及WCS中待定位相机的x坐标t_x和y坐标t_y，最终得到待定位相机在WCS中的姿态和位置。

公式(18)中仍然有3个未知参数ψ、t_x和t_y。这些未知参数可以通过LLS估计进行计算，可以用矩阵形式表示如下

A_ijX＝b_ij (20)

其中，A_ij＝[A_i,A_j]^T，b_ij＝[b_i,b_j]^T(i,j∈{1,2,3,4},i≠j)，其中

X＝[cosψ,sinψ,t_x,t_y]^T (22)

因此，未知参数可以表示为

X＝A_ij ^TA_ij ^-1A_ij ^Tb_ij (24)

其中，X是X的估计。

由于3D-2D对应关系不是提前已知的，给定一个A_rs和b_ij(r,s,i,j∈{1,2,3,4},r≠s,i≠j)，并不知道它们准确的对应关系。幸运的是，有4个A_r(r∈{1,2,3,4})和b_i(i∈{1,2,3,4})，这意味着总共有

个不同的A_rs和b_ij(r,s,i,j∈{1,2,3,4},r≠s,i≠j)。

因此，对于每个b_ij＝[b_i,b_j]^T(i,j∈{1,2,3,4},i≠j)，分别对应6个A_rs(r,s∈{1,2,3,4},r≠s)，可以得到6个解，其中有一个解X_ij即为b_ij对应正确的解X。当考虑6个不同的b_ij时，总共可以获得6组共36个解。为了提高V-P4L的精度性能，最后估计的X可以通过对6组中6个最接近的解X_ij求平均值进行计算，可以表示如下

用这种方法，可以分别得到相机的姿态和位置，即

和

虽然V-P4L要求LED灯具为矩形，但是V-P4L对部分遮挡具有很好的鲁棒性，这对于移动的情况是很有意义的。例如，如果P₁的投影p₁被障碍所阻挡，不在图像平面上，p₁的像素坐标可以通过l₁₂和l₄₁的交点来确定，因此V-P4L仍然可以估计相机的位置和姿态。

实施例：

本发明分别对VP4L、P4L法和CA-RSSR这三种方法进行仿真和分析，系统参数如表1所示。

表1

参数	值
		房间大小(长×宽×高)	5m×5m×3m
灯具的长	120cm
		灯具的宽	20cm-100cm
LED半功率角，Φ<sub>1/2</sub>	60°
		相机的主点	u<sub>0</sub>,v<sub>0</sub>＝(320,240)
相机的归一化焦距	f<sub>u</sub>＝f<sub>v</sub>＝800
		CA-RSSR中PD和相机之间距离，d<sub>PC</sub>	1cm

假设可见光信号采用开关键控调制，灯管的长为120cm，灯具宽度在不同的仿真下是不同的。所有统计结果都独立运行10000次以上，每次仿真运行时，在房间内随机选择接收器的位置。针孔相机已经提前校准，图像噪声被建模为高斯白噪声。由于图像噪声会影响灯具在图像平面上投影的像素坐标，因此对相同位置的20幅图像进行处理得到像素坐标。

由于CA-RSSR需要5个LED进行3D定位，因此假设灯具中心有一个额外的LED，这在V-P4L和P4L法中并不需要。此外，CA-RSSR依赖于完美的朗伯模型，然而，VLC信道模型与朗伯模型相比是不同的，即使使用接近理想的朗伯模型的LED，在某些情况下，这种差别可以超过100％。因此，本发明为CA-RSSR中的朗伯模型设置一个10％以内的随机偏差。

另一方面，P4L利用一个矩形灯具来估计相机的位置和姿态，假设相机已知3D-2D对应关系，然而，矩形信标无法将坐标传递给相机，这使得该假设并不实际。此外，基于垂直和水平直线数目的差异，利用相机捕捉天花板上的信标来寻找3D-2D对应关系的方法也不实际。因此，本发明为3D-2D对应关系设置一个10％以内的随机偏差。

将定位误差定义为

其中，

和

分别是接收器真实的世界坐标和估计的世界坐标。此外，将方向误差定义为

OE＝|Θ_true-Θ_est| (27)

其中，Θ_true和Θ_est分别是真实的旋转角和估计的旋转角。

首先评估了灯具的宽度对V-P4L定位精度的影响。该性能是由定位误差的均值来表示，宽度从20cm至100cm。图像噪声被建模为期望为0、标准差为2像素的高斯噪声。如图3所示，在三种算法中，V-P4L能够获得最优的性能。对于V-P4L来说，无论灯具的宽度是多少，定位误差的均值都能保持在23cm以下。而对于CA-RSSR，随着灯具宽度从20cm增加至100cm，定位误差的均值从102cm减少至68cm。此外，对于PnL法，定位误差的均值从105cm减少至38cm。如图3所示，三种算法的定位精度都随着宽度的增加而增加。此外，使用单个宽度大于20cm的LED灯具，V-P4L的定位误差的均值总是小于18cm，因此V-P4L可以应用于流行的室内灯具。

由于姿态估计的精度也会受到LED灯具宽度的影响，所以比较了V-P4L和P4L法在不同灯具宽度下沿x轴、y轴和z轴的方向误差的均值。如图4所示，对于V-P4L和P4L法来说，在LED灯具的所有宽度下，沿x轴和y轴的方向误差的均值均保持在4°以下。此外，对于V-P4L，随着灯具宽度的增加，沿z轴的方向误差的均值从4.5°减少至1.5°。相比之下，对于P4L法，沿z轴的方向误差的均值从10.2°减少至6.8°。因此，与P4L法相比，V-P4L可以获得更高的姿态估计精度。

然后评估了当灯具宽度为40cm时，图像噪声对V-P4L定位性能的影响。图像噪声被建模为高斯白噪声，期望为0，标准差σ_n，从0变化至4像素。不同图像噪声下定位误差的均值，如图5所示，对于V-P4L来说，当σ_n为0像素时，定位误差的均值为0cm。这说明V-P4L的定位误差完全是由图像噪声引起的。此外，对于V-P4L，随着图像噪声从0增加至4像素，定位误差的均值从0cm增加至18cm。而对于P4L法，定位误差的均值从30cm增加至40cm。此外，对于CA-RSSR，定位误差的均值约为90cm。因此，与P4L法和CA-RSSR相比，V-P4L可以获得更高的位置估计精度。

最后比较了V-P4L和P4L法在不同图像噪声下的姿态估计精度。图像噪声从0变化至4像素时沿x轴、y轴和z轴的方向误差的均值，如图6所示，V-P4L在0至4像素范围内表现很好，沿x轴、y轴和z轴的方向误差的均值小于3.5°。相比之下，对于P4L法，沿z轴方向误差的均值从6°增加至8°。因此，与P4L法相比，不论图像噪声是多少，V-P4L可以获得更稳定、更精确的姿态估计。

Claims (4)

1.一种室内定位中新颖的可见光通信辅助的透视四线方法，其特征在于，具体步骤如下：

步骤一、构建包括一个LED灯具发射器和一个待定位相机接收器的场景，同时定义四种定位坐标系；

步骤二、待定位相机获得LED灯具四个顶点投影的像素坐标后，基于单视图几何将像素坐标变换到图像坐标；

步骤三、利用LED灯具四个顶点投影的图像坐标，基于单视图几何、平面几何和立体几何，在CCS中估计LED灯具的方向；

在CCS中LED灯具的方向用矩形P₁P₂P₃P₄的单位法向量表示，即：

其中，

和

A、B和C是CCS中矩形P₁P₂P₃P₄的一般形式方程的系数；m和n是利用LED灯具四个顶点投影的图像坐标，并基于单视图几何、平面几何和立体几何推导过程的中间变量；

步骤四、基于立体几何计算LED灯具的四个顶点在CCS中的相机坐标；

LED灯具的第i个顶点P_i的相机坐标

表示为：

其中，

A_ij、B_ij和C_ij((i,j)∈{(1,2),(2,3),(3,4),(4,1)})是顶点P_i、P_j和o^c构成的侧面Π_ij的一般形式方程的系数；

其中，

M₁₂＝W₁,W₂,W₃ ^T，M₂₃＝W₂,W₃,W₄ ^T，M₃₄＝W₃,W₄,W₁ ^T，M₄₁＝W₄,W₁,W₂ ^T；S是灯具的面积；

步骤五、基于单视图几何，利用LED灯具在CCS和WCS中的方向，计算待定位相机在x^c轴对应的旋转角

和y^c轴对应的旋转角θ；

基于单视图几何，灯具在WCS中的单位法向量为

在CCS中的单位法向量为

转换关系为：

为CCS到WCS的旋转矩阵，即相机的姿态；

R_X表示沿x^c轴的旋转矩阵，

是x^c轴对应的旋转角；

R_Y表示沿y^c轴的旋转矩阵，

θ是y^c轴对应的旋转角；

R_Z表示沿z^c轴的旋转矩阵，

ψ是z^c轴对应的旋转角；

因此有：

通过求解上式即可得到待定位相机的x^c轴对应的旋转角

和y^c轴对应的旋转角θ；

步骤六、通过可见光通信将LED灯具四个顶点的世界坐标发送至待定位相机，然后利用LED灯具在CCS中的相机坐标，计算WCS中待定位相机的z坐标；

具体步骤如下：

首先，针对LED灯具的第i个顶点P_i，相机坐标为

世界坐标为

然后，世界坐标与相机坐标的转换为：

是待定位相机的3D世界坐标，即待定位相机的位置；

WCS中待定位相机的z坐标计算如下：

其中，c₁，c₂和c₃是通过x^c轴和y^c轴的旋转矩阵相乘后，得到的矩阵元素值，即：

步骤七、基于线性最小二乘法计算待定位相机z^c轴对应的旋转角ψ、以及WCS中待定位相机的x坐标t_x和y坐标t_y，最终得到待定位相机在WCS中的姿态和位置；

首先，将未知参数设为X＝[cosψ,sinψ,t_x，t_y]^T，X是X的估计；

线性最小二乘法的未知参数方程为：

X＝A_ij ^TA_ij ^-1A_ij ^Tb_ij

其中，A_ij＝[A_i,A_j]^T，b_ij＝[b_i,b_j]^T(i,j∈{1,2,3,4},i≠j)，其中

每个b_ij＝[b_i,b_j]^T(i,j∈{1,2,3,4},i≠j)，分别对应6个A_rs(r,s∈{1,2,3,4},r≠s)，得到6个解，其中有一个解X_ij即为b_ij对应正确的解X；

针对6个不同的b₁₂，b₁₃，b₁₄，b₂₃，b₂₄，b₃₄，总共可以获得6组36个解，通过对这6组中6个最接近的解X_ij求平均得到最优结果，表示如下：

最终，得到待定位相机的姿态和位置，即

和

2.如权利要求1所述的一种室内定位中新颖的可见光通信辅助的透视四线方法，其特征在于，所述步骤一中，四种坐标系分别是像素坐标系(Pixel Coordinate System，PCS)o^p-u^pv^p、图像坐标系(Image Coordinate System，ICS)oⁱ-xⁱyⁱ、相机坐标系(CameraCoordinate System，CCS)o^c-x^cy^cz^c和世界坐标系(World Coordinate System，WCS)o^w-x^wy^wz^w。

3.如权利要求1所述的一种室内定位中新颖的可见光通信辅助的透视四线方法，其特征在于，所述步骤一中，场景中的发射器被安装在天花板上，是一个由4个顶点P_i(i∈{1,2,3,4})构成的支持可见光通信的矩形LED灯具，WCS中灯具的第i个顶点的坐标为

四条3D参考线L_ij((i,j)∈{(1,2),(2,3),(3,4),(4,1)})是灯具的边；

场景中的接收器是一个标准的针孔相机，与灯具不共面；因此，发射器和接收器构成了一个四棱锥o^c-P₁P₂P₃P₄。

4.如权利要求1所述的一种室内定位中新颖的可见光通信辅助的透视四线方法，其特征在于，所述步骤二中，LED灯具的第i个顶点P_i在待定位相机的图像平面上投影点为p_i，像素坐标为

L_ij在图像平面上的2D投影为l_ij；

投影点p_i的图像坐标

表示如下

其中，d_x是图像平面上x轴方向每个像素的物理尺寸，d_y是图像平面上y轴方向每个像素的物理尺寸；此外，u₀,v₀ ^T是ICS原点oⁱ的像素坐标，称为主点。

Images