基于三个已知点的Yaw和Pitch求解未知点F的坐标

问题明确 #

已知三个固定点坐标：P1=(x1,y1,z1)、P2=(x2,y2,z2)、P3=(x3,y3,z3)，以及未知点F到每个Pi的偏航角(Yaw) ai 和俯仰角(Pitch) bi，需要求解F的坐标(Fx,Fy,Fz)。

数学建模 #

先明确通用坐标系定义（以ENU坐标系为例：X=北，Y=东，Z=天顶）：

• Yaw角ai：目标Pi在XY平面的投影与X轴（北）的顺时针夹角
• Pitch角bi：目标Pi与XY平面的夹角（向上为正）

根据角度计算从F指向Pi的单位方向向量vi=(vix,viy,viz)：

vix = cos(ai) * cos(bi)
viy = sin(ai) * cos(bi)
viz = sin(bi)

对于每个Pi，向量Pi - F与vi共线，因此存在正标量ki（F到Pi的距离）满足：

x1 - Fx = k1 * v1x
y1 - Fy = k1 * v1y
z1 - Fz = k1 * v1z

x2 - Fx = k2 * v2x
y2 - Fy = k2 * v2y
z2 - Fz = k2 * v2z

x3 - Fx = k3 * v3x
y3 - Fy = k3 * v3y
z3 - Fz = k3 * v3z

求解步骤 #

1. 求解Fx和Fy #

从P1、P2的XY平面方程消去k1、k2：

• 由P1的前两式整理得：v1y*Fx - v1x*Fy = v1y*x1 - v1x*y1
• 由P2的前两式整理得：v2y*Fx - v2x*Fy = v2y*x2 - v2x*y2

这是二元一次方程组，设系数矩阵A = [[v1y, -v1x], [v2y, -v2x]]，常数项向量B = [v1y*x1 - v1x*y1, v2y*x2 - v2x*y2]，直接通过矩阵求逆解得：[Fx, Fy]^T = A^{-1} * B。

2. 求解Fz #

用P1的方程计算k1（优先选择非零分量避免除以零）：

• 若v1x ≠ 0，则k1 = (x1 - Fx)/v1x
• 否则用k1 = (y1 - Fy)/v1y

代入z方向方程得：Fz = z1 - k1 * v1z

3. 验证与优化 #

用P3验证解的合理性：
计算k3_candidate = (x3 - Fx)/v3x（或对应非零分量），检查k3_candidate * v3y ≈ y3 - Fy且k3_candidate * v3z ≈ z3 - Fz，同时k1、k2、k3均为正数（距离不能为负）。

若验证不通过（角度存在测量误差），则用最小二乘法拟合最优解：
构造误差函数：

error(Fx,Fy,Fz) = Σ[( (xi - Fx)/vix - (yi - Fy)/viy )² + ( (xi - Fx)/vix - (zi - Fz)/viz )²]

使用非线性优化算法（如Levenberg-Marquardt）最小化误差函数，得到最优的F坐标。

注意事项 #

• 若你的Yaw/Pitch坐标系定义不同（如Pitch向下为正、Yaw逆时针旋转），只需调整单位向量的分量符号即可，核心逻辑不变。
• 若存在多个解（如对称位置），需结合场景约束（如F在地面以上）筛选合理解。

内容的提问来源于stack exchange，提问作者DoogyHtw