运动约束增强的可见光惯性紧组合定位系统

A Tightly-Coupled VLP/INS Integrated Positioning System Enhanced by Motion Constraints

摘要

可见光定位（VLP）由于其低成本、高精度、现有基础设施大量部署等优点而在室内定位研究与应用中受到广泛关注。然而，VLP系统在接收端倾斜或被遮挡的情况下定位性能严重下降。针对这一问题，设计了一个可见光惯性紧组合定位系统。该系统引入惯性测量单元以估计接收端倾角，从传感器原始观测层面进行滤波融合以削弱遮挡的影响，并通过附加运动约束来增强定位性能。系统在实际测试中平均定位精度达10.27 cm，在遮挡和倾斜情形下相比纯VLP精度分别提升了35.34%和63.19%，综合性能优于现有方法。

Abstract

Visible light positioning (VLP) has received widespread attention in indoor positioning research and applications due to its advantages such as low cost, high accuracy, and extensive deployment in existing infrastructure. However, VLP systems experience a significant decline in localization performance when obstructed or tilted at the receiver end. To address this issue, a VLP/INS tightly coupled navigation system enhanced by adding motion constraints is proposed, which uses an inertial measurement unit for inclination estimation and applies filtering and fusion techniques at the raw observation level of the sensor to mitigate the effects of obstructions. In practical tests, the system achieved an average positioning accuracy of 10.27 cm, with accuracy improvements of 35.34% and 63.19% compared to pure VLP under occlusion and tilt situations, respectively, resulting in higher accuracy and robustness than existing methods.

Graphical abstract

关键词

室内定位 / 可见光定位 / 惯性导航 / 紧组合系统 / 运动约束

Key words

indoor positioning / visible light positioning / inertial navigation / tightly-coupled integrated system / motion constraints

0　 引 言

可见光定位（Visible Light Positioning，VLP）利用发光二极管（Light Emitting Diode，LED）发出的可见光信号进行定位，通常以相机或光电二极管（Photodiode，PD）作为接收器。受卷帘快门效应影响，基于低成本相机的VLP系统的动态定位性能难以得到有效保障^[1]。而PD成本低廉，实用性较高，因此大多数VLP系统采用PD作为接收装置。

如表1^[2]所示，与其他常见的室内定位技术相比，VLP技术具有以下优势：分米甚至厘米级的高精度定位、低功耗、LED成本低廉且已大量部署、易于布设等^[3-4]。VLP在诸多室内应用场景具有巨大潜力，包括室内行人导航、商场导购、资产设备追踪、停车场车位管理以及其他潜在应用领域^[4]。

VLP技术尽管拥有诸多优势，但仍存在一定的缺陷。基于单个PD的VLP系统通常依赖于接收端保持水平的假设，PD存在倾角时系统性能会严重下降^[5]。同时，直射链路的遮挡可能导致错误的观测，非视距（Non-Line-Of-Sight，NLOS）信号会对定位造成干扰。

为了应对PD倾角导致定位性能下降的问题，Zhou等^[6]基于非凸优化实现对PD的同时定位定姿，仿真表明高精度的定位结果要求密集的LED布设；Yasir等^[7]使用加速度计感知接收器静止时的倾斜角，从中提取入射角并实现定位，但此方法只支持静态或低动态定位；Zou等^[8]使用加权无迹卡尔曼滤波将基于倾斜模型的VLP与惯性导航系统（Inertial Navigation System，INS）结合，却将问题简化成了不普适的二维情形。此外，Zhang等^[9]提出了一种基于半参数化递归神经网络的可学习卡尔曼滤波以应对VLP信号中断，但接收端的姿态与位置变化都经过了假设简化。Li等^[10-11]分别使用无迹卡尔曼滤波和粒子滤波实现了VLP与INS的组合，一定程度上缓解了VLP遮挡与多径的影响。

尽管已经有许多关于单PD VLP与INS组合的研究^[7-11]，但它们基本都采用松组合（Loosely Coupled，LC）的耦合方式，即各子系统独立工作、导航解融合输出。与之相比，紧组合（Tightly Coupled，TC）系统直接针对传感器的原始观测进行融合，具有更高的精度与鲁棒性，对恶劣情景（倾斜/遮挡）的适应能力更强^[12]。Zhang等^[9]设计的组合系统虽然直接针对接收信号强度（Received Strength Signal，RSS）进行量测更新，但该系统假设PD是水平放置的，且只考虑二维的平面状态。此前的研究中已提出了基于RSS和惯性测量单元（Inertial Measurement Unit， IMU)的6自由度紧组合系统，但该系统基于图优化框架^[2]。与基于滤波的方法相比，图优化方法的复杂度更高，消耗计算资源更多^[12-13]；且该研究未考虑杆臂对系统的影响，推导的紧组合观测模型并不完善。

本文推导、完善了VLP/INS紧组合观测的误差扰动模型，设计了基于滤波框架的单PD VLP/INS紧组合定位定姿系统。该系统引入了INS进行姿态估计，消除了系统对水平假设的依赖；从原始观测层面进行数据融合，削弱了遮挡信号对定位的影响；利用载体运动状态的固有约束来构造虚拟观测，减小误差发散、增强系统定位性能。仿真和实际实验结果表明，本文设计的VLP/INS紧组合导航系统能有效处理信号遮挡和接收机倾角问题，综合考虑系统精度、鲁棒性和复杂度，其性能优于现有方法。

1　 运动约束增强的VLP/INS紧组合滤波算法

1.1　信道模型

图1为评估VLP系统性能的典型场景，该场景可以是一个包含N个LED灯和1个PD的空房间。N个LED灯固定在天花板上作为发射光信号的光源，采用频分复用调制方案，其调制频率已知，房间内的VLP接收端实时接收信号并对其进行处理。接收到的信号的傅里叶频谱中第i个LED对应的频率峰值即为该LED的接受信号强度RSS，记为P_i。

在LED和PD的视场（Field of View，FoV）内，可以基于Lambert定律对RSS进行建模^[14]：

P_i=\frac{\left(m_i+\mathrm{1}\right)A_R{P}_{\mathrm{T}i}}{\mathrm{2}\mathrm{\pi }}\cdot \frac{\mathrm{c}\mathrm{o}{\mathrm{s}}^{m_i}\theta \mathrm{c}\mathrm{o}\mathrm{s}\Psi T_s\left(\Psi \right)g\left(\Psi \right)}{d_i^{\mathrm{2}}}(1)

式中，m_i是第i个LED的朗伯系数；A_R是PD的有效面积；P_{\mathrm{T}i}是第i个LED的发射功率；d_i是第i个LED和PD之间的距离；\theta是该LED灯对PD的辐射角；\Psi则是接收端的入射角，如图1所示。T_s(\Psi )是滤光片的增益，g(\Psi )是放置在PD前的聚光器的增益，没有滤光片和聚光器时，二者均为1。

在VLP系统中，PD接收光信号时引入的噪声主要有两种：散粒噪声和热噪声，通常建模为加性高斯白噪声^[4,14]。散粒噪声是由光电探测器的暗电流以及LED光和环境光引起的，热噪声则主要由导体中的自由电子或电荷载流子的不规则热运动引起。文献[4]对二者进行了详细讨论。以{\sigma }_{\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{t}}^{\mathrm{2}}表示散粒噪声方差，{\mathrm{\sigma }}_{\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}}^{\mathrm{2}}表示热噪声方差，则总噪声方差{\sigma }_{\mathrm{N}}^{\mathrm{2}}可以表示为：

{\sigma }_{\mathrm{N}}^{\mathrm{2}}={\sigma }_{\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{t}}^{\mathrm{2}}+{\sigma }_{\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}}^{\mathrm{2}}(2)

实际定位时，可以通过Allan方差^[5]分析确定信号的具体噪声水平。

1.2　VLP/INS紧组合

1.2.1　数据融合框架

在导航定位领域中常见的数据融合框架主要有滤波和图优化等^[12,13]。滤波框架基于递归贝叶斯估计，强调实时性和计算效率，如卡尔曼滤波、粒子滤波等。其优势在于架构简单、计算资源需求低，适合嵌入式设备和动态场景的实时状态更新。然而，其对噪声模型的准确性要求较高，且易受累积误差影响。相比之下，图优化基于图论和非线性优化，能够通过稀疏图结构高效处理全局一致性问题，常用于闭环检测和全局优化。其主要优势在于精度高、全局一致性强，但计算复杂度较高，且对初值敏感。

在实际应用中，滤波框架因其实时性强和资源需求低的特点，适合部署在低成本、计算性能受限的嵌入式设备中，能够在满足实时需求的同时进一步发挥VLP系统的低成本优势，而图优化则更适用于离线处理或高性能计算平台。因此，本文选择基于滤波框架构建VLP/INS紧组合定位系统，以实现低功耗、高效、实时的状态估计，确保系统能适用于消费级和资源受限场景。

1.2.2　系统模型

本文设计的VLP/INS紧组合导航系统基于误差状态卡尔曼滤波进行组合导航解算，采用的误差状态量\mathrm{\delta }\mathit{x}\left(t\right)包括导航状态量和IMU误差向量：

\begin{array}{c}\mathrm{\delta }\mathit{x}\left(t\right)={\left[\begin{array}{ccccc}{\left(\mathrm{\delta }{\mathit{r}}^n\right)}^{\mathrm{T}}& {\left(\mathrm{\delta }{\mathit{v}}^n\right)}^{\mathrm{T}}& {\mathit{\varphi }}^{\mathrm{T}}& {\mathit{b}}_g^{\mathrm{T}}& {\mathit{b}}_a^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\end{array}(3)

式中，\mathrm{\delta }{\mathit{r}}^n为导航坐标系n系下的位置误差向量；\mathrm{\delta }{\mathit{v}}^n为n系速度误差向量；\mathit{\varphi }为姿态误差向量；{\mathit{b}}_g为陀螺三轴零偏向量；{\mathit{b}}_a为加速度计三轴零偏向量；各状态分量均为时变量，为了书写简便省略了时间符号。系统状态方程可参见文献[15]，(4)~(13)式是对系统观测方程的详细推导。

在(1)式中，余弦值可用向量的内积来描述，故(1)式等价于：

P_i=\frac{\left(m_i+\mathrm{1}\right)A_R{P}_{\mathrm{T}i}}{\mathrm{2}\mathrm{\pi }}\cdot \frac{{\left({\mathit{u}}_i^n\cdot {\mathit{d}}_i^n\right)}^{m_i}\left({\mathit{u}}^n\cdot {\mathit{d}}_i^n\right)}{d_i^{\mathrm{3}+m_i}}(4)

式中，{\mathit{d}}_i^n为从第i个LED到PD的距离向量在n系下的投影，d_i为其模长；{\mathit{u}}_i^n为LED辐射平面的单位法向量在n系的下投影，一般为已知值；{\mathit{u}}^{\mathit{n}}为PD接收平面的单位法向量在n系下的投影。

进一步地，{\mathit{d}}_i^n、{\mathit{u}}^n分别取决于PD的位置和姿态，即：

{\mathit{d}}_i^n={\mathit{r}}_i^n-{\mathit{r}}^n={\mathit{r}}_i^n-{\mathit{r}}_I^n-{\mathit{C}}_b^n{\mathit{l}}^b(5)

{\mathit{u}}^n={\mathit{C}}_b^n{\mathit{u}}^b(6)

式中，{\mathit{r}}_i^n、{\mathit{r}}^n、{\mathit{r}}_I^n分别为第i个LED、PD和IMU的位置向量；{\mathit{l}}^b表示PD到IMU的杆臂在载体坐标系b系下的投影；{\mathit{C}}_b^n表示载体坐标系到b系的姿态旋转矩阵。

以标注符号“^”表示惯导推算值，其与真值相比存在一定误差，即\widehat{\mathit{x}}=\mathit{x}+\mathrm{\delta }\mathit{x}。对{\mathit{r}}_I^n、{\mathit{C}}_b^n进行误差扰动，有：

{\widehat{\mathit{C}}}_b^n=[\mathit{I}-(\mathit{\varphi }\times )]{\mathit{C}}_b^n(7)

{\widehat{\mathit{r}}}_I^n={\mathit{r}}_I^n+\mathrm{\delta }{\mathit{r}}^n(8)

其中，（\mathit{a}\times）表示向量\mathit{a}的反对称矩阵。

由(4)~(8)式，可得：

{\widehat{P}}_i=P_i+{\mathit{H}}_{r,i}\mathrm{\delta }{\mathit{r}}^n+{\mathit{H}}_{\varphi ,i}\mathit{\varphi }(9)

{\mathit{H}}_{r,i}=P_i{\left(-\frac{{\mathit{u}}^n}{{\mathit{d}}_i^n\cdot {\mathit{u}}^n}-\frac{m_i{\mathit{u}}_i^n}{{\mathit{d}}_i^n\cdot {\mathit{u}}_i^n}+\left(\mathrm{3}+m_i\right)\frac{{\mathit{d}}_i^n}{d_i^{\mathrm{2}}}\right)}^{\mathrm{T}}(10)

{\mathit{H}}_{\varphi ,i}=P_i\frac{{\left({\mathit{d}}_i^n\times {\mathit{u}}^n\right)}^{\mathrm{T}}}{{\mathit{d}}_i^n\cdot {\mathit{u}}^n}+{\mathit{H}}_{r,i}\left({\mathit{C}}_b^n{\mathit{l}}^b\right)\times(11)

式中，矩阵{\mathit{H}}_{r,i}、{\mathit{H}}_{\varphi ,i}为观测矩阵，反应了系统状态\mathit{r}、\mathit{\varphi }与第i个RSS观测量之间的变换关系。

以标注符号“~”表示观测量，RSS观测向量{\tilde{P}}_i可表示为：

{\tilde{P}}_i=P_i+{\mathrm{ϵ}}_{P_i}(12)

其中，{\mathrm{ϵ}}_{P_i}表示P_i的测量噪声。

将(9)、(12)式相减即可得到系统的观测方程：

\mathrm{\delta }{z}_{P_i}={\widehat{P}}_i-{\tilde{P}}_i={\mathit{H}}_{r,i}\mathrm{\delta }{\mathit{r}}^n+{\mathit{H}}_{\varphi ,i}\mathit{\varphi }-{\mathrm{ϵ}}_{P_i}(13)

文献[2]同样对RSS的扰动模型进行了推导，然而其并没有考虑到PD到IMU的杆臂的影响。与本文推导的结果相比，其推导的观测模型中对应的部分缺少了杆臂项，即(11)式等号右边第二项。在杆臂较长时，这可能会导致姿态估计异常，从而使得系统性能衰减。

系统的定位精度很大程度上取决于滤波参数设定是否合理，关键的几个参数包括测量噪声协方差矩阵\mathit{R}、过程噪声协方差矩阵\mathit{Q}、初始状态估计值{\widehat{\mathit{x}}}_{\mathrm{0}}及其协方差矩阵{\mathit{P}}_{\mathrm{0}}。初始状态可以在实验前通过现场标定确定。RSS的测量噪声方差可通过Allan方差分析获得，并基于测量信息调整权重^[16]。记第i个LED的RSS噪声方差为{\sigma }_{P_i}^{\mathrm{2}}，假设各调制信号之间互相独立，则：

\mathit{R}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\sigma }_{P_{\mathrm{1}}}^{\mathrm{2}},{\sigma }_{P_{\mathrm{2}}}^{\mathrm{2}},\cdots ,{\sigma }_{P_N}^{\mathrm{2}}\right)(14)

合理设定矩阵\mathit{Q}的关键是正确给定功率谱密度矩阵\mathit{q}。\mathit{q}由IMU误差模型参数决定，不考虑IMU各轴之间的差异，则：

\mathit{q}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(\mathrm{V}\mathrm{R}{\mathrm{W}}^{\mathrm{2}}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}},\mathrm{A}\mathrm{R}{\mathrm{W}}^{\mathrm{2}}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}},\frac{\mathrm{2}{\sigma }_{\mathrm{g}\mathrm{b}}^{\mathrm{2}}}{T_{\mathrm{g}\mathrm{b}}}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}},\frac{\mathrm{2}{\sigma }_{\mathrm{a}\mathrm{b}}^{\mathrm{2}}}{T_{\mathrm{a}\mathrm{b}}}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}}\right)(15)

其中，VRW为速度随机游走，ARW为角度随机游走；陀螺仪和加速度计零偏建模为一阶高斯马尔可夫过程，{\sigma }_{\mathrm{g}\mathrm{b}}和T_{\mathrm{g}\mathrm{b}}为陀螺零偏对应的标准差和相关时间，{\sigma }_{\mathrm{a}\mathrm{b}}和T_{\mathrm{a}\mathrm{b}}为加速度计零偏对应的标准差和相关时间。相关参数一般可以从厂家提供的数据手册上获得。

1.3　运动约束

对于卡尔曼滤波算法，除了使用传感器的测量直接构造量测方程外，还可以利用载体固有的运动约束引入新的观测类型。通过对载体的运动状态进行分析，如果其状态符合特定的运动情况，就可以构造对应的虚拟观测，从而增加观测冗余，提高滤波的精度和鲁棒性^[17]。本研究使用的附加约束包括零速更新（Zero Velocity Update，ZUPT）、零角速率更新（Zero Angular Rate Update，ZARU）^[18]、非完整约束（Nonholonomic Constraint，NHC）^[15]和高程约束（Height Constraint，HC）^[17]。

1.3.1　零（角）速更新

如果在导航过程中，载体经常保持静止，则可使用ZUPT和ZARU对系统进行修正。此时，可构造速度观测{\tilde{\mathit{v}}}^n={\left[\mathrm{0,0},\mathrm{0}\right]}^{\mathrm{T}}+{\mathbf{ϵ}}_v和角速度观测{\tilde{\mathit{\omega }}}_{ib}^b={\left[\mathrm{0,0},\mathrm{0}\right]}^{\mathrm{T}}+{\mathbf{ϵ}}_{\mathit{\omega }}，其中{\mathbf{ϵ}}_v和{\mathbf{ϵ}}_{\mathit{\omega }}为观测噪声。结合惯导的速度推算值{\widehat{\mathit{v}}}^n和角速度输出{\widehat{\mathit{\omega }}}_{ib}^b，则对应的观测方程分别为：

\mathrm{\delta }{\mathit{z}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}={\widehat{\mathit{v}}}^n-{\tilde{\mathit{v}}}^n=\mathrm{\delta }{\mathit{v}}^n-{\mathbf{ϵ}}_v(16)

\mathrm{\delta }{\mathit{z}}_{\mathrm{Z}\mathrm{A}\mathrm{R}\mathrm{U}}={\widehat{\mathit{\omega }}}_{ib}^b-{\tilde{\mathit{\omega }}}_{ib}^b=\mathrm{\delta }{\mathit{\omega }}_{ib}^b-{\mathbf{ϵ}}_{\omega }(17)

其中，\mathrm{\delta }{\mathit{\omega }}_{ib}^b为载体角速度误差，在只考虑IMU零偏的情况下，\mathrm{\delta }{\mathit{\omega }}_{ib}^b={\mathit{b}}_g。

1.3.2　非完整约束

一般的陆地车辆只要未发生跳动或侧滑，就满足车辆沿车轮转轴方向和路面垂直方向的速度为0的约束条件。若记车轮坐标系为v系，b系到v系的姿态旋转矩阵为{\mathit{C}}_b^v，驱动轮中心到IMU测量中心的杆臂为{\mathit{l}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^b，则惯导推算的v系速度{\widehat{\mathit{v}}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^v可描述为：

{\widehat{\mathit{v}}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^v={\mathit{C}}_b^v{\widehat{\mathit{C}}}_n^b{\widehat{\mathit{v}}}^n+{\mathit{C}}_b^v\left({\widehat{\mathit{\omega }}}_{ib}^b\times \right){\mathit{l}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^b={\mathit{v}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^v+{\mathit{C}}_b^v{\mathit{C}}_n^b\mathrm{\delta }{\mathit{v}}^n-{\mathit{C}}_b^v{\mathit{C}}_n^b\left({\mathit{v}}^n\times \right)\mathit{\varphi }-{\mathit{C}}_b^v\left({\mathit{l}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^b\times \right)\mathrm{\delta }{\mathit{\omega }}_{ib}^b(18)

v系的速度观测可表示为{\tilde{\mathit{v}}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^v={\left[{\mathit{v}}_{\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{w}\mathrm{o}\mathrm{r}\mathrm{d}},\mathrm{0,0}\right]}^{\mathrm{T}}+{\mathbf{ϵ}}_{\mathrm{N}\mathrm{H}\mathrm{C}}，其中，{\mathit{v}}_{\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{w}\mathrm{o}\mathrm{r}\mathrm{d}}表示载体前向速度，{\mathbf{ϵ}}_{\mathrm{N}\mathrm{H}\mathrm{C}}表示观测噪声，则观测向量和对应的观测方程可表示为：

\mathrm{\delta }{\mathit{z}}_{\mathrm{N}\mathrm{H}\mathrm{C}}={\widehat{\mathit{v}}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^v-{\tilde{\mathit{v}}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^v={\mathit{C}}_b^v{\mathit{C}}_n^b\mathrm{\delta }{\mathit{v}}^n-{\mathit{C}}_b^v{\mathit{C}}_n^b\left({\mathit{v}}^n\times \right)\mathit{\varphi }-{\mathit{C}}_b^v\left({\mathit{l}}_{\mathrm{w}\mathrm{h}\mathrm{e}\mathrm{e}\mathrm{l}}^b\times \right)\mathrm{\delta }{\mathit{\omega }}_{ib}^b-{\mathbf{ϵ}}_{\mathrm{N}\mathrm{H}\mathrm{C}}(19)

NHC算法不对前向速度进行约束，因此上述方程只取侧向和垂向分量。

1.3.3　高程约束

假如载体在平坦的室内地板上进行运动，其高程短时间内不会发生较大的变化，可以对高程施加约束。若以\widehat{h}表示惯导推算的高程，\tilde{h}表示高程测量值，{ϵ}_{\mathrm{H}\mathrm{C}}为观测噪声，则对应的观测方程为：

\mathrm{\delta }{z}_{\mathrm{H}\mathrm{C}}=\widehat{h}-\tilde{h}=\mathrm{\delta }h+{ϵ}_{\mathrm{H}\mathrm{C}}(20)

构造约束观测量时，若有可靠的高程先验信息，可采用固定高程（常值）进行约束^[17]；如果没有，则可以用上一个历元的高程来约束当前历元。

1.4　定位流程

本文设计的VLP/INS紧组合系统基于INS机械编排进行状态的预测递推，并使用PD的RSS观测和根据运动约束构造的虚拟观测来修正系统误差。图2给出了系统的具体工作流程。基于惯导机械编排获得载体的位置、速度和姿态，并计算RSS预测值，与RSS观测求差得到观测残差并输入滤波器，在运动状态符合特定约束时将对应的虚拟观测一并输入滤波器，数据融合后得到修正后的导航结果和系统误差。

需要注意的是，为了减小定位刚刚启动、状态尚未收敛之时的INS误差累积，实验开始时建议载体保持一段时间的静止状态。一方面，通过该时段的IMU输出可以粗略计算出零偏初值，以进行初步的校准；另一方面，利用该时段的零速信息进行滤波，也能有效校准INS、修正状态初值、抑制误差发散。

2　 仿真模拟

2.1　模拟配置

仿真测试的房间大小为\mathrm{5}\mathrm{m}\times \mathrm{5}\mathrm{m}\times \mathrm{5}\mathrm{m}，五个LED均匀分布在房间四角和中央。房间中有一个倾角为\mathrm{30}°的坡面，用于实现高度和俯仰角的变化。本文模拟的载体运动过程包括水平移动和向上爬坡，起点位于\left(\mathrm{5,0},\mathrm{0}\right)处。此外，还模拟了多次信号遮挡。为了使模拟尽可能真实，仿真时向IMU数据中添加了四倍于实际实验的噪声（实际实验相关参数见表2）；同时，向RSS仿真数据中也添加了白噪声，其标准差为0.1 lx。

2.2　仿真结果

图3展示了VLP系统、VLP/INS松组合（LC）和紧组合（TC）的仿真测试定位结果。图3（a）中，VLP轨迹在部分区域远远偏离了参考轨迹（Ref），松组合轨迹与参考结果也有一定偏差，紧组合结果则极为贴近参考轨迹。图3（b）中以灰色背景和粉色背景突出了遮挡和倾斜对应的时段。可以看到，在遮挡和倾斜情况下，VLP和松组合的定位误差迅速增大，一度达到5 m和0.85 m，而紧组合则没有因观测条件变化而发生明显的精度衰减。整体来看，紧组合的平均误差只有0.117 m，远胜于松组合（0.33 m）和VLP（1.05 m）。

仿真模拟的结果初步验证了VLP/INS紧组合系统在遮挡和倾斜情况下的优势。对于VLP而言，其精度严重依赖于RSS的观测质量。遮挡会导致RSS测距失真，倾斜则会使水平假设失效，二者都会导致VLP误差迅速增大。只有在无遮挡无倾斜的良好观测条件下时，VLP才是可用的。VLP/INS组合系统的精度和抗干扰能力明显高于VLP。这是由于组合系统一方面通过姿态估计消除了VLP对于水平假设的依赖，减弱了倾斜的影响；另一方面通过INS推算得出平滑的定位轨迹，减缓了异常RSS引起的误差，增强了系统对遮挡的忍耐能力。

VLP/INS组合系统中，紧组合明显优于松组合。其根本原因是紧组合能够更充分地利用传感器的原始数据、更有效地处理误差和噪声，因此有更高的稳定性、准确性和鲁棒性。一方面，紧组合的数据融合层次更深，能有效利用所有可用信息；而松组合则是各传感器分别处理得到独立导航解后再进行融合，对信息的利用并不充分，特别是当部分RSS观测不可用导致VLP精度下降甚至失效时，系统整体性能会受到较大的影响。另一方面，紧组合在统一的滤波器中考虑了所有传感器的误差模型，能更有效地处理各种误差和噪声；而松组合在各传感器独立处理阶段就可能引入了一些误差，这些误差在后续融合中可能无法完全补偿，从而影响最终的导航精度。

3　 实际实验

3.1　实验配置

如图4（a）所示，实验场地大小为\mathrm{6}\mathrm{m}\times \mathrm{8.4}\mathrm{m}\times \mathrm{2.8}\mathrm{m}，共安装了6盏CREE LED灯，其平面坐标分别为(1.37,1.65)、(3.73,1.62)、(3.19,3.40)、(1.96,3.38)、(1.35,5.19)、(3.69,5.79)，单位为m。如图4（b）所示，实验以一个移动机器人（AgileX Scout Mini）作为运动载体，其上安装有一个STM32驱动的PD（OPT101），一个IMU（Xsens MTi-3）和一个激光雷达（Velodyne VLP16）。PD和IMU安装在一个云台上，可通过旋转云台改变倾角，二者关键技术指标参见表2、3。参考基准的获取依赖于激光雷达和IMU采集的数据。通过先进的激光雷达惯性里程计算法（如FAST-LIO2）进行解算，可为本研究提供平均误差优于1 cm的高精度参考轨迹^[19]。

3.2　运动约束有效性验证

在正式实际实验前先基于一组实测数据（3.3节中的实验B数据）来验证运动约束算法的性能，对VLP/INS紧组合系统附加如下不同方案的约束并比较其定位结果：

方案1：无附加约束；

方案2：附加零角速率更新，三维角速度标准差均为0.001 rad/s；

方案3：在方案2基础上，附加零速更新，三维速度约束标准差均为0.001 m/s；

方案4：在方案3基础上，附加非完整约束，侧向、垂向速度标准差均为0.01 m/s；

方案5：在方案4基础上，附加高程约束，高程标准差为0.001 m。

图5展示了各方案的定位结果。从图5(a)可以看到，方案1、2、3轨迹相近，误差较大且主要体现在高程上；方案4、5则与参考轨迹（Ref）较为接近，高程误差得到了很好的抑制，这主要是由于附加了NHC垂向约束和高程约束。从图5(b)也可以看出，方案1精度最差，方案2、3略有改善，方案4、5则有明显的精度提升。

表4统计了各方案的平均定位误差。可以看到，各方案按顺序逐个增加运动约束，其定位误差也逐个减小；零速更新、零角速率更新带来的精度提升大致相当且比较微弱，非完整约束和高程约束则能带来较明显的精度提升。

总之，各种运动约束均能一定程度减小定位误差，其有效性得到了验证。

3.3　实验结果

本文在3.1节所述实验配置下进行了3个不同观测条件下的实际实验，并将系统定位结果与纯VLP系统、VLP/INS松组合系统的定位结果进行比较。3个实验分别为：PD未遭受遮挡也没有倾斜的良好观测条件下的实验A，PD遭受了一段时间遮挡的实验B，以及PD始终倾斜的实验C。各实验中对滤波（包括松组合和紧组合）施加的运动约束方案均为3.2节中的方案5。

3.3.1　实验A

实验A的观测环境是较为理想的，用于测试VLP/INS组合系统在良好观测条件下的定位性能。实验A开始之前，首先调整了PD和IMU所在云台的倾角，尽可能地确保其水平；进行实验时，尽量保证PD与各个LED之间没有人或其他物体遮挡，因此视距（LOS）信号几乎没有被遮挡。

图6展示了实验A的定位结果。从图6(a)可见，各系统定位轨迹和参考轨迹（Ref）都比较接近；图6(b)则显示各系统误差均比较小，大多在20 cm以内波动。表5展示了实验A中各系统的定位误差统计指标，包括平均误差、50、90分位误差和均方根误差（Root Mean Square Error，RMSE），从中可见，观测条件较为理想时，3个系统的定位性能是大致相当且较为良好的（~9 cm）。

3.3.2　实验B

实验B是遮挡实验，用于测试系统在遭遇光遮挡情况下的定位性能。遮挡会使VLP系统的RSS观测不准，导致VLP导航结果的可靠性大幅降低甚至不可用，但本文提出的紧组合VLP/INS系统一方面可以通过INS推算得出平滑的解决方案，另一方面可以通过对原始观测的充分利用提高对异常值的容忍度。

实验结果如图7所示。从图7(a)可见，整个定位过程中，除左上段以外，各系统的定位轨迹与参考轨迹基本一致；各系统轨迹在左上部分均有不同程度的偏移，这是因为此时载体遭遇了一段时间的遮挡，对应于图7(b)中的阴影时段（实验开始后42 s到48 s）。从图7(b)可见，在遭遇遮挡时，VLP精度衰减很严重，误差最大达到0.8 m；松组合同样有一定程度的精度衰减，误差一度达到0.5 m；而紧组合在遭遇遮挡时精度变化最小，与正常时段的差异并不明显。

从表6展示的实验B误差统计结果中也可看出，紧组合系统的定位性能最佳，受遮挡的影响最小。这与理论相符：VLP没有处理异常值的能力；而松组合通过INS推算能减缓精度衰减，但有观测数量要求，因此遮挡严重时仍无可奈何；紧组合则由于没有观测数量要求能充分利用每一个观测，对异常值有较高的容忍程度。

3.3.3　实验C

实验C是倾斜测试实验，用于测试系统在PD倾斜时的定位性能。倾斜会导致VLP定位的假设可靠性降低，模型与实际的符合程度下降，引起系统性误差。VLP/INS组合系统通过同时定位定姿，消除了系统对水平假设的依赖。为了测试各系统在接收器倾斜情况下的定位性能，将PD和IMU所在平台旋转了一个固定角度（16.5°）。在机器人运动过程中，倾角会随运动状态的变化而变化。

实验C的定位结果如图8。图8(a)中，VLP定位轨迹与参考轨迹的重合度明显低于实验A/B的无遮挡时段，从图8(b)中可见其误差基本在20 cm以上，这是因为模型的水平假设与系统不符导致VLP定位结果中包含较大的系统性偏差。对于两种组合系统，图中展示的定位轨迹和精度均较为接近，误差在0∼30 cm内波动，其性能与VLP相比均有明显提升。

由表7展示的实验C的误差统计结果可见，组合系统性能远胜于VLP系统，其中紧组合又优于松组合，相比VLP各指标精度提升均达60%以上。

表8对上述3个实际实验的平均定位误差进行了统计。其中，VLP/INS紧组合系统在3个场景下都是性能最优的系统，实验中的平均定位误差为10.27 cm，在遮挡和倾斜情形下相比纯VLP精度分别提升了35.34%和63.19%。

3.4　与现有工作对比

表9对比了本文与同行的研究工作。除本研究与文献[2]外，其他文献提出的系统均无法兼顾倾角估计和遮挡处理。本文方法在真实环境中的定位精度达到10.27 cm，仅略低于文献[2]和[6]。与文献[2]相比，虽然二者LED布设数量相同，但本研究选用的场地较大；与文献[6]相比，文献[6]要求的LED布设密度非常高，且方案尚未经过实测验证。此外，本研究以远低于文献[2]的计算复杂度，实现了与之相当的定位精度。文献[2]采用的是图优化方法，计算复杂度与耗时远高于本文方法^[13]。具体来说，若其单次解算的窗口包含T个时间步，状态向量的维度为N，迭代K次，则其计算复杂度高达O(K{T}^{\mathrm{3}}{N}^{\mathrm{3}})，而本研究在对应窗口内的计算复杂度仅为O(T{N}^{\mathrm{3}}) ^[20]。

综合考虑系统精度、鲁棒性和复杂度等各个方面，本研究的性能优于现有方法的性能。

4　 结 语

本文设计了首个使用滤波框架实现的单PD定位定姿VLP/INS紧组合导航系统，并通过对滤波附加运动约束增强系统的定位性能。仿真模拟和实际实验的结果均说明，系统即使在PD存在倾角或被遮挡的情况下，也能稳定实现10 cm左右的定位精度。该系统有效地弥补了VLP技术的短板，能以较低的成本提供快速、鲁棒、高精度的位置服务。在今后的工作中，将对系统状态量进行拓展，对目前VLP中通过预先标定获取的常量（如LED坐标、朗伯系数等）进行建模以实现更稳定、准确的估计。

参考文献

[1] FANG J B， YANG Z， LONG S， et al. High-speed indoor navigation system based on visible light and mobile phone［J］. IEEE Photonics Journal， 2017， 9（2）： 8200711. DOI： 10.1109/JPHOT.2017.2687947 .

[2] SUN X， ZHUANG Y， YANG X S， et al. Tightly-coupled VLP/INS integrated navigation by inclination estimation and blockage handling［EB/OL］. ［2024-04-28］. DOI: 10.1186/s43020-025-00158-9 .

[3] ALAM F， FAULKNER N， LEGG M， et al. Indoor visible light positioning using spring-relaxation technique in real-world setting［J］. IEEE Access， 2019， 7： 91347-91359. DOI： 10.1109/ACCESS.2019.2927922 .

[4] ZHUANG Y， HUA L C， QI L N， et al. A survey of positioning systems using visible LED lights［J］. IEEE Communications Surveys & Tutorials， 2018， 20（3）： 1963-1988. DOI： 10.1109/COMST.2018.2806558 .

[5] ZHUANG Y， HUA L C， WANG Q， et al. Visible light positioning and navigation using noise measurement and mitigation［J］. IEEE Transactions on Vehicular Technology， 2019， 68（11）： 11094-11106. DOI： 10.1109/TVT.2019.2943517 .

[6] ZHOU B P， LAU V， CHEN Q C， et al. Simultaneous positioning and orientating for visible light communications： Algorithm design and performance analysis［J］. IEEE Transactions on Vehicular Technology， 2018， 67（12）： 11790-11804. DOI： 10.1109/TVT.2018.2875044 .

[7] YASIR M， HO S W， VELLAMBI B N. Indoor positioning system using visible light and accelerometer［J］. Journal of Lightwave Technology， 2014， 32（19）： 3306-3316. DOI： 10.1109/JLT.2014.2344772 .

[8] ZOU Q， XIA W W， ZHU Y P， et al. A VLC and IMU integration indoor positioning algorithm with weighted unscented Kalman filter［C］//2017 3rd IEEE International Conference on Computer and Communications （ICCC）. New York： IEEE Press， 2017： 887-891. DOI： 10.1109/CompComm.2017.8322671 .

[9] ZHANG R， LIU Z C， QIAN K M， et al. Outage bridging and trajectory recovery in visible light positioning using insufficient RSS information［J］. IEEE Access， 2020， 8： 162302-162312. DOI： 10.1109/ACCESS.2020.3020874 .

[10] LI Z T， YANG W H， XIAO L H， et al. Integrated wearable indoor positioning system based on visible light positioning and inertial navigation using unscented Kalman filter［C］//2019 11th International Conference on Wireless Communications and Signal Processing （WCSP）. New York： IEEE Press， 2019： 1-6. DOI： 10.1109/WCSP.2019.8928098 .

[11] LI Z T， YANG A Y， LV H C， et al. Fusion of visible light indoor positioning and inertial navigation based on particle filter［J］. IEEE Photonics Journal， 2017， 9（5）： 7906613. DOI： 10.1109/JPHOT.2017.2733556 .

[12] ZHUANG Y， SUN X， LI Y， et al. Multi-sensor integrated navigation/positioning systems using data fusion： From analytics-based to learning-based approaches［J］. Information Fusion， 2023， 95： 62-90. DOI： 10.1016/j.inffus.2023.01.025 .

[13] WEN W S， KAN Y C， HSU L T. Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization［C］// Proceedings of the 32nd International Technical Meeting of the Satellite Division of the Institute of Navigation（ION GNSS+ 2019）. Miami： Institute of Navigation. 2019： 3019-3032. DOI： 10.33012/2019.17129 .

[14] CHEN B D， JIANG J J， GUAN W P， et al. Performance comparison and analysis on different optimization models for high-precision three-dimensional visible light positioning［J］. Optical Engineering， 2018， 57（12）： 125101. DOI： 10.1117/1.oe.57.12.125101 .

[15] 丁龙阳， 旷俭， 陈起金， 等. GNSS拒止环境下的手机车载定位研究［J］. 哈尔滨工程大学学报， 2023， 44（10）： 1712-1719. DOI： 10.11990/jheu.202207060 .

[16] DING L Y， KUANG J， CHEN Q J， et al. Vehicle positioning based on smartphones in a GNSS-denial environment［J］. Journal of Harbin Engineering University， 2023， 44（10）： 1712-1719. DOI： 10.11990/jheu.202207060（Ch ）.

[17] YANG Y， HE H， XU G. Adaptively robust filtering for kinematic geodetic positioning［J］. Journal of Geodesy， 2001， 75（2）： 109-116. DOI： 10.1007/s001900000157 .

[18] 李彦杰， 杨元喜， 何海波. 附加约束条件对GNSS/INS组合导航结果的影响分析［J］. 武汉大学学报（信息科学版）， 2017， 42（9）： 1249-1255. DOI： 10.13203/j.whugis20150526 .

[19] LI Y J， YANG Y X， HE H B. Effects analysis of constraints on GNSS/INS integrated navigation［J］. Geomatics and Information Science of Wuhan University， 2017， 42（9）： 1249-1255. DOI： 10.13203/j.whugis20150526（Ch ）.

[20] 王伟， 马晞茗， 王敬民， 等. 基于自适应PDR补偿的UWB/MIMU组合行人定位算法［J］. 仪表技术与传感器， 2023（6）： 100-108. DOI： 10.3969/j.issn.1002-1841.2023.06.017 .

[21] WANG W， MA X M， WANG J M， et al. UWB/MIMU combined pedestrian location algorithm based on adaptive PDR compensation［J］. Instrument Technique and Sensor， 2023（6）： 100-108. DOI： 10.3969/j.issn.1002-1841.2023.06.017（Ch ）.

[22] XU W， CAI Y X， HE D J， et al. FAST-LIO2： Fast direct LiDAR-inertial odometry［J］. IEEE Transactions on Robotics， 2022， 38（4）： 2053-2073. DOI： 10.1109/TRO.2022.3141876 .

[23] FANG X， WANG C， NGUYEN T M， et al. Graph optimization approach to range-based localization［J］. IEEE Transactions on Systems， Man， and Cybernetics： Systems， 2021， 51（11）： 6830-6841. DOI： 10.1109/TSMC.2020.2964713 .

基金资助

国家重点研发计划(2022YFE0139300)

湖北省自然科学基金杰出青年项目(2021CFA040)

苏州市科技计划项目(ZXL2021237)