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

文章目录

测量目标坐标点的计算1、表述定义2、传感器工作状态描述3、测量距离计算4、目标位置计算5、计算过程中的常数6、软件代码

测量目标坐标点的计算

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、目标位置计算

根据坐标点$(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); }