感知算法-超声波三角定位测量方法

测量目标坐标点的计算

1、表述定义

定义目标位置的坐标为$(x_2^T,y_2^T)$，2号传感器的坐标为$(x_2^2,y_2^2)$、3号传感器的坐标点为$(x_2^3,y_2^3)$、4号传感器的坐标点为$(x_2^4,y_2^4)$。2、3、4号传感器测得的实际距离定义为$D_2$、$D_3$、$D_4$。点$(x_2^3,y_2^3)$和点$(x_2^4,y_2^4)$之间的距离为$L_1$，点$(x_2^2,y_2^2)$和点$(x_2^3,y_2^3)$之间的距离为$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个飞行时间，分别为$TO{F}_2$、$TO{F}_3$、$TO{F}_4$。

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

3、测量距离计算

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

4、目标位置计算

1. 根据坐标点$(x_2^3,y_2^3)$和$(x_2^4,y_2^4)$的位置，计算坐标点$(x_2^T,y_2^T)$。

• 计算角度$\alpha$
  $${\alpha }_1=\arctan \frac{y_2^4-y_2^3}{x_2^4-x_2^3}$$
  $${\alpha }_2=\arctan \frac{y_2^3-y_2^2}{x_2^3-x_2^2}$$
• 计算角度$\beta$

根据余弦定理计算得
$${\beta }_1=\arccos \frac{D_3^2+L_1^2-D_4^2}{2\ast D_3\ast L_1}$$
$${\beta }_2=\arccos \frac{D_2^2+L_2^2-D_3^2}{2\ast D_2\ast L_2}$$

• 计算目标位置坐标

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

$$\left[\begin{array}{c}{x}_2^T\\ y_2^T\end{array}\right]=\left[\begin{array}{c}{x}_2^3\\ y_2^3\end{array}\right]+\left[\begin{array}{c}{D}_3\ast \cos ({\alpha }_1+{\beta }_1)\\ D_3\ast \sin ({\alpha }_1+{\beta }_1)\end{array}\right]$$

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

$$\left[\begin{array}{c}{x}_2^T\\ y_2^T\end{array}\right]=\left[\begin{array}{c}{x}_2^2\\ y_2^2\end{array}\right]+\left[\begin{array}{c}{D}_2\ast \cos ({\alpha }_2+{\beta }_2)\\ D_2\ast \sin ({\alpha }_2+{\beta }_2)\end{array}\right]$$

5、计算过程中的常数

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

• 已知传感器安装位置点

传感器号	坐标位置	实际坐标点
2号	$(x_2^2,y_2^2)$	$(360,30)$
3号	$(x_2^3,y_2^3)$	$(360,-30)$
4号	$(x_2^4,y_2^4)$	$(346,-70)$

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

参数	计算值
$L_1$	42.3792(cm)
$L_2$	60(cm)
${\alpha }_1$	250.71(°)
${\alpha }_2$	270(°)

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);
        }