测量目标坐标点的计算

1、表述定义

定义目标位置的坐标为 $(x^T_2,y^T_2)$，2号传感器的坐标为 $(x^2_2,y^2_2)$、3号传感器的坐标点为 $(x^3_2,y^3_2)$、4号传感器的坐标点为 $(x^4_2,y^4_2)$。2、3、4号传感器测得的实际距离定义为 $D_2$、 $D_3$、 $D_4$。点 $(x^3_2,y^3_2)$和点 $(x^4_2,y^4_2)$之间的距离为 $L_1$，点 $(x^2_2,y^2_2)$和点 $(x^3_2,y^3_2)$之间的距离为 $L_2$。定义 $L_1$相对 $X$轴旋转的角度为 $\alpha_1$， $L_2$相对 $X$轴旋转的角度为 $\alpha_2$。 $D_3$与 $L_1$的夹角为 $\beta_1$， $D_2$与 $L_2$的夹角为 $\beta_2$。

2、传感器工作状态描述

3号传感器处于收发一体模式，2、4号传感器处于接收模式。3号传感器发射完超声波后，和2、4号传感器一样都处于接收状态，等待超声波的回波信号，分别产生3个飞行时间，分别为 $TOF_2$、 $TOF_3$、 $TOF_4$。

对上述状态的图像化描述，如下图所示：

3、测量距离计算

假设T℃温度时的声速为 $V_T$，建立距离关系式
$\begin{matrix} D_2 + D_3 = V_T*TOF_2 \\ D_3 + D_3 = V_T*TOF_3 \\ D_4 + D_3 = V_T*TOF_4 \end{matrix}$
第一步计算距离 $D_3$
$D_3 = \frac{V_T*TOF_3}{2}$
接下来计算距离 $D_2$和 $D_4$：
$D_2 = V_T*TOF_2 - D_3\\ D_4 = V_T*TOF_4 - D_3$
当然，在实际工程应用中，可以不考虑这个误差，直接算出这三个距离值。
$D_2 = \frac{V_T*TOF_2}{2} \\ D_3 = \frac{V_T*TOF_3}{2} \\ D_4 = \frac{V_T*TOF_4}{2}$

4、目标位置计算

1. 根据坐标点 $(x^3_2,y^3_2)$和 $(x^4_2,y^4_2)$的位置，计算坐标点 $(x^T_2,y^T_2)$。

• 计算角度 $\alpha$
  $\alpha_1 = \arctan \frac{y^4_2 - y^3_2}{x^4_2 - x^3_2}$
  $\alpha_2 = \arctan \frac{y^3_2 - y^2_2}{x^3_2 - x^2_2}$
• 计算角度 $\beta$

根据余弦定理计算得
$\beta_1 = \arccos \frac{D_3^2 + L_1^2 - D_4^2}{2*D_3*L_1}$
$\beta_2 = \arccos \frac{D_2^2 + L_2^2 - D_3^2}{2*D_2*L_2}$

• 计算目标位置坐标

3、4号传感器组合测量结果为

$\left[ \begin{matrix} x^T_2\\y^T_2 \end{matrix} \right]= \left[ \begin{matrix} x^3_2\\y^3_2 \end{matrix} \right]+ \left[ \begin{matrix} D_3*\cos (\alpha_1+\beta_1) \\ D_3*\sin (\alpha_1+\beta_1) \end{matrix} \right]$

2、3号传感器组合测量结果为

$\left[ \begin{matrix} x^T_2\\y^T_2 \end{matrix} \right]= \left[ \begin{matrix} x^2_2\\y^2_2 \end{matrix} \right]+ \left[ \begin{matrix} D_2*\cos (\alpha_2+\beta_2) \\ D_2*\sin (\alpha_2+\beta_2) \end{matrix} \right]$

5、计算过程中的常数

由于传感器的安装位置是固定的，所以上述计算过程中，很多参数是确定的常量。

• 已知传感器安装位置点

通过上述信息，可以计算出传感器的距离 $L_1$和 $L_2$，以及对应的旋转角度 $\alpha_1$和 $\alpha_2$。

6、软件代码

        /// <summary>
        /// 计算超声波的距离
        /// </summary>
        /// <param name="m_318Data">输入原始超声波的原始数据</param>
        /// <param name="Distance">计算实际距离</param>
        private void DistanceCalculate(LIN_STP318_ReadData[] m_318Data,ref double [] Distance)
        {
            Distance[1] = m_318Data[1].TOF * 0.0157;//cm
            Distance[0] = m_318Data[0].TOF * 0.0314 - Distance[1];//cm
            Distance[2] = m_318Data[2].TOF * 0.0314 - Distance[1];//cm
        }

        //目标位置计算
        /// <summary>
        /// 目标位置坐标点计算
        /// </summary>
        /// <param name="base_position">基准点坐标点</param>
        /// <param name="base_angle">基准角度(弧度)</param>
        /// <param name="base_L">基准长度(cm)</param>
        /// <param name="distance1">临边长度(cm)</param>
        /// <param name="distance2">对角长度(cm)</param>
        /// <param name="TargetPosition">输出最终的目标位置坐标</param>
        private void TargetPositionCalculate(Axis base_position,double base_angle,double base_L,double distance1, double distance2,ref Axis TargetPosition)
        {
            double beta = Math.Acos((Math.Pow(base_L, 2) + Math.Pow(distance1, 2) - Math.Pow(distance2, 2)) / (2 * base_L * distance1));
            TargetPosition.x = base_position.x + distance1 * Math.Cos(base_angle + beta);
            TargetPosition.y = base_position.y + distance1 * Math.Sin(base_angle + beta);
        }