1. 问题陈述
机器人定位问题从根本上是一个坐标变换和几何计算的问题。我们首先建立两个坐标系：房间坐标系\mathcal{F}和机器人坐标系\mathcal{R}。这两个坐标系的关系是理解整个问题的关键。
房间环境用线段集合[\mathbf{a}_m, \mathbf{b}_m]描述，其中m = 1, \ldots, M。每个线段都是有向的，从\mathbf{a}_m指向\mathbf{b}_m，这个方向性决定了线段的哪一侧能够反射超声波。根据右手定则，当沿着从\mathbf{a}_m到\mathbf{b}_m的方向前进时，右侧是可见的反射面，左侧则被遮挡。
机器人的状态用向量\mathbf{p} = (p_1, p_2, p_3)^T表示。这里p_1和p_2是机器人在房间坐标系\mathcal{F}中的位置坐标，通常对应机器人质心的x和y坐标。p_3是机器人的朝向角，表示机器人坐标系\mathcal{R}相对于房间坐标系\mathcal{F}的旋转角度。这三个参数完全确定了两个坐标系之间的刚体变换关系。
机器人上安装了N个超声波传感器。每个传感器在机器人坐标系\mathcal{R}中有固定的位姿\mathbf{s}_i^{\mathcal{R}} = (s_{i,1}^{\mathcal{R}}, s_{i,2}^{\mathcal{R}}, s_{i,3}^{\mathcal{R}})^T。这里(s_{i,1}^{\mathcal{R}}, s_{i,2}^{\mathcal{R}})是传感器i在机器人坐标系中的位置，s_{i,3}^{\mathcal{R}}是传感器相对于机器人前进方向的安装角度。这些都是已知的固定参数，在机器人制造时就确定了。
现在我们获得了测量向量\mathbf{y} = (y_1, \ldots, y_N)^T，其中y_i是第i个传感器测量到的距离值。定位问题就是：给定测量值\mathbf{y}和环境地图（线段集合），求解机器人的真实位姿\mathbf{p}^*，或者说找到一个最优估计\hat{\mathbf{p}}。
2. 第一步：构建传感模型
2.1 基本假设与坐标变换
传感模型的构建基于两个关键假设。第一，超声波沿直线传播，不考虑衍射和散射效应。第二，将超声波束简化为零孔径的射线，忽略波束的实际扩散角。这些简化使得问题变成了纯几何计算。
首先需要解决的是坐标变换问题。传感器的位姿在机器人坐标系\mathcal{R}中是已知的，但要计算距离，必须将其转换到房间坐标系\mathcal{F}中。这个变换包含两个步骤：先旋转，后平移。
对于传感器i在房间坐标系中的位置，我们有：
这两个公式的推导来自二维旋转矩阵。当一个点(s_{i,1}^{\mathcal{R}}, s_{i,2}^{\mathcal{R}})绕原点旋转角度p_3时，新坐标为：
展开矩阵乘法得到：
然后加上平移量(p_1, p_2)，就得到了上述的s_{i,1}(\mathbf{p})和s_{i,2}(\mathbf{p})。
传感器的朝向变换更简单：
这个公式表明传感器在房间坐标系中的绝对朝向等于机器人朝向p_3加上传感器相对于机器人的朝向s_{i,3}^{\mathcal{R}}。这是角度的简单叠加，因为旋转是可交换的。
2.2 反射侧判定
确定传感器位置后，下一步是判断传感器相对于每个线段的位置关系。这涉及到一个关键的几何判定：传感器是否在线段的反射侧？
行列式判定法的核心是：
这个行列式实际上计算的是两个向量\overrightarrow{ab}和\overrightarrow{a\mu}构成的平行四边形的有向面积。展开计算：
行列式的符号有明确的几何意义：
- 当\det > 0时，从\overrightarrow{ab}逆时针旋转到\overrightarrow{a\mu}的角度小于180度，点\mu在线段左侧
- 当\det < 0时，从\overrightarrow{ab}顺时针旋转到\overrightarrow{a\mu}，点\mu在线段右侧（反射侧）
- 当\det = 0时，三点共线
对于传感器i和线段m，令\tilde{\mathbf{s}}_i = (s_{i,1}(\mathbf{p}), s_{i,2}(\mathbf{p}))^T，判定条件变为：
这个条件的物理意义是：如果传感器不在反射侧（行列式非负），则超声波无法被该线段反射，距离设为无穷大。
2.3 射线与线段的交点计算
当确认传感器在反射侧后，需要计算超声波射线与线段的交点。这是整个传感模型的核心计算。
从传感器位置(s_{i,1}, s_{i,2})出发，沿方向s_{i,3}的射线参数方程为：
这里t是沿射线的距离参数。当t=0时，我们在传感器位置；随着t增大，我们沿着角度s_{i,3}的方向前进。\cos s_{i,3}和\sin s_{i,3}是单位方向向量的分量。
线段[\mathbf{a}_m, \mathbf{b}_m]的参数方程需要特别注意：
这是线性插值的标准形式。当t'=0时，(x_1, x_2) = (a_{m,1}, a_{m,2})；当t'=1时，(x_1, x_2) = (b_{m,1}, b_{m,2})。参数t' \in [0,1]的约束确保点在线段上而不是延长线上。
将参数方程展开重写：
这更清楚地显示了从点\mathbf{a}_m沿向量\mathbf{b}_m - \mathbf{a}_m移动t'倍的过程。
2.4 求解交点参数
射线与线段相交意味着存在参数t和t'使得两个参数方程表示同一点：
重新整理，将未知数移到左边，常数移到右边：
注意这里(a_{m,1} - b_{m,1})是负的(b_{m,1} - a_{m,1})，这样写是为了后续计算方便。
这是一个标准的二元一次方程组，可以用克拉默法则求解。首先求t'，通过消去t：
从第一个方程：t = \frac{a_{m,1} - s_{i,1} - (a_{m,1} - b_{m,1})t'}{\cos s_{i,3}}
代入第二个方程并化简，最终得到：
这个公式的分子是传感器到线段起点的向量与传感器朝向的叉积，分母是线段方向向量与传感器朝向的叉积。几何上，这给出了交点在线段上的相对位置。
类似地，求解t：
这个公式的分子是两个向量的叉积，分母确保了除法的合法性（非零表示射线与线段不平行）。
2.5 交点有效性判定与距离计算
求得参数后，必须验证交点的有效性：
- t' \in [0,1]：确保交点在线段上而不是延长线上
- t \geq 0：确保交点在射线的正向上而不是反向延长线上
只有两个条件都满足时，交点才是物理上有意义的。此时交点坐标为(x_1(t), x_2(t))，传感器到该线段的距离为：
这就是欧几里得距离公式。注意这里实际上d = t，因为t本身就是沿射线的距离参数。
最后，传感器的测量值是到所有线段距离的最小值：
这个最小化操作的物理意义是：超声波遇到的第一个障碍物（最近的）会产生反射，后面的障碍物被遮挡。这完成了从机器人位姿\mathbf{p}到传感器测量y_{m,i}的完整正向模型。