基于姿态与距离的室内定位

基于姿态和距离估计的室内定位研究

摘要

本文提出一种室内定位方法，该方法结合了由 SOKUIKI传感器获取的距离信息、利用姿态传感器进行的姿态估计以及建筑物结构信息。我们同时估计终端的姿态和终端到周围反射表面的距离。根据初步测量获得的结构信息，将平方误差和最小的点确定为位置的估计值。本文通过实验获取的测量数据，评估了提出方法的定位精度。

引言

近年来，室内定位的需求不断增加，例如商业设施内的导航和工厂内人流线路分析。另一方面，测距传感器作为一种室内定位方法已引起关注。测距传感器是一种将激光束照射到水平面，并根据被物体反射后的反射光到达时间来计算与物体之间距离的传感器。本文假设将装有测距传感器的终端安装在行人手臂上，而传统定位方法则是将其固定在室内结构上。然而，由于行走时终端的姿态会发生变化，因此在终端中加装由加速度计和陀螺仪传感器组成的姿态传感器，可在利用测距传感器进行距离测量的同时估计终端的姿态，以应对姿态变化。这样可将测距空间从二维平面扩展到三维空间，充分利用由墙壁和物体构成的房间结构信息。然后通过将每个时刻的姿态和距离信息与房间结构信息进行匹配，实现对行人位置的估计。因此，该方法无需多个昂贵设备，仅用一个终端即可完成位置估计。本文通过在真实环境中开展测量实验，基于获得的测量数据和房间结构信息进行位置估计，评估该定位方法的精度。

II. 坐标系与旋转

我们定义两个坐标系和一个旋转。首先，图1中 $x - y - z$ 的三维正交坐标系所定义的坐标系称为世界坐标系，房间结构由该坐标系定义。接下来，图1中 $\tilde{x} - \tilde{y} - \tilde{z}$ 的三维正交坐标系所定义的坐标系称为物体坐标系，该坐标系相对于世界坐标系经过原点移动和旋转得到。我们假设终端位于物体坐标系的原点处。换句话说，终端在世界坐标系中的位置为 $o = [o_x, o_y, o_z]$。我们还定义了旋转轴 $n = [n_x, n_y, n_z]$ 和旋转角 $\theta$，用于在三维空间中执行坐标系的旋转。在这种情况下，我们使用四元数 $q = [\cos \frac{\theta}{2}, n_x \sin \frac{\theta}{2}, n_y \sin \frac{\theta}{2}, n_z \sin \frac{\theta}{2}]^T$ 来表示该旋转。

III. 房间结构信息和反射表面

A. 房间结构信息

本文假设存在墙壁、地板、天花板以及能够反射 SOKUIKI传感器激光束的物体等反射表面。这些反射表面可能是相互连接的，也可能是独立的，每个反射表面均由一组三角形表示。

B. 终端与反射表面之间的距离

在 $x - y$ 世界坐标系平面中，指向 $x$ 轴正方向的方向向量逆时针旋转 $\phi_l (l = 0, \dots, L - 1)$ 使得该方向向量变为 $u(\phi_l)$。此外，方向向量 $v(\phi_l)$，其中 $u(\phi_l)$ 乘以 $q$ 及其共轭 $q^\star$，并旋转和平移至 $o$ 的位置，可表示为
$$
v(\phi_l) = q u(\phi_l) q^\star + o. \tag{1}
$$
接着，从 $o$ 沿 $v$ 方向延伸的线段与反射面的交点称为反射点。$o$ 与反射点之间的第 $l$ 个距离表示为 $d_l$。我们还将 $L$ 个距离表示为 $d = [d_0, \dots, d_{L-1}]^T$。

IV. 提出方法

A. 观测模型

本文假设房间结构信息通过先验测量准确获得。姿态传感器采用作为树莓派扩展的 Sense HAT，终端姿态通过 IMU 滤波器[1]进行估计。使用北阳电机公司生产的 UST-10LX 作为测距传感器，用于估计终端与反射表面之间的距离 $d$。

B. 物体坐标系原点 $o$ 的估计

在本文中，我们将平方误差和 $f(\tilde{o})$ 定义为估计准则，当假设 $o$ 等于 $\tilde{o}$ 时，用于估计物体坐标系的原点 $o$，如下所示。
$$
f(\tilde{o}) = |\hat{d} - d(\tilde{o})|^2 \tag{2}
$$
$\hat{d}$ 是 $d$ 和 $d(\tilde{o})$ 的估计值，表示 $\tilde{o}$ 与各反射点之间的距离。
$$
\hat{o} = \arg\min_{\tilde{o} \in \mathbb{R}^3} f(\tilde{o}) \tag{3}
$$
本文中，我们使用网格搜索算法[2]作为搜索算法来确定 $\hat{o}$。下面展示了网格搜索算法。

步骤1. 将 $x-y$ 平面划分为宽度为 $d_{gw}[m]$ 的网格，并在每个网格的中心放置一个候选点。
步骤2. 对每个候选点，计算 $f(\tilde{o})$。
步骤3. 使 $f(\tilde{o})$ 最小的点被确定为估计点 $\hat{o}$。

$z$-坐标的候选点在主体直立时固定为终端离地面的高度。

V. 定位实验

在本文中，我们验证了提出方法的估计结果，并基于真实环境中测量实验获得的数据评估了估计精度。
$d_{gw} = 0.5\,\text{m}$ 的估计结果如图2所示。图2显示了从 $z$ 轴正方向（即天花板）观察的房间视图。左图为一个样本

红圈指向的真实位置及其估计值（由蓝圈指向的候选点中选出的绿圈）。在本实验中，我们从房间的西南向东北进行了测量。此处假设真实点 $o$ 在测量开始到结束的行走路线上的运动为匀速直线运动。作为估计精度的衡量标准，我们使用每个时刻距离误差 $|\hat{o} - o|[\text{m}]$ 的平均值。右图显示了行走实验中 $|\hat{o} - o|$ 随经过时间的变化性能。结果证实，估计值分布在行走路线周围。另一方面，我们也发现了一些远离行走路线的估计值。此类估计误差的原因被认为与测距传感器的测量误差、姿态数据的测量误差以及房间结构信息的测量误差有关。最终确认，估计的平均误差为 0.63 m。

VI. 结论

本文提出了一种室内位置估计方法，该方法结合了房间结构信息（如墙壁或物体）、终端的姿态传感器获取的姿态信息以及测距传感器获取的距离信息。此外，还在真实环境中进行了测量实验，以验证估计结果并评估提出方法的准确性。未来我们将在不同的行走路线和房间中进行实验。
本工作由 JSPS KAKENHI 资助，项目编号 JP19K12037。