文章目录
测量目标坐标点的计算1、表述定义2、传感器工作状态描述3、测量距离计算4、目标位置计算5、计算过程中的常数6、软件代码
测量目标坐标点的计算
1、表述定义
定义目标位置的坐标为
(
x
2
T
,
y
2
T
)
(x^T_2,y^T_2)
(x2T,y2T)，2号传感器的坐标为
(
x
2
2
,
y
2
2
)
(x^2_2,y^2_2)
(x22,y22)、3号传感器的坐标点为
(
x
2
3
,
y
2
3
)
(x^3_2,y^3_2)
(x23,y23)、4号传感器的坐标点为
(
x
2
4
,
y
2
4
)
(x^4_2,y^4_2)
(x24,y24)。2、3、4号传感器测得的实际距离定义为
D
2
D_2
D2、
D
3
D_3
D3、
D
4
D_4
D4。点
(
x
2
3
,
y
2
3
)
(x^3_2,y^3_2)
(x23,y23)和点
(
x
2
4
,
y
2
4
)
(x^4_2,y^4_2)
(x24,y24)之间的距离为
L
1
L_1
L1，点
(
x
2
2
,
y
2
2
)
(x^2_2,y^2_2)
(x22,y22)和点
(
x
2
3
,
y
2
3
)
(x^3_2,y^3_2)
(x23,y23)之间的距离为
L
2
L_2
L2。定义
L
1
L_1
L1相对
X
X
X轴旋转的角度为
α
1
\alpha_1
α1，
L
2
L_2
L2相对
X
X
X轴旋转的角度为
α
2
\alpha_2
α2。
D
3
D_3
D3与
L
1
L_1
L1的夹角为
β
1
\beta_1
β1，
D
2
D_2
D2与
L
2
L_2
L2的夹角为
β
2
\beta_2
β2。
2、传感器工作状态描述
3号传感器处于收发一体模式，2、4号传感器处于接收模式。3号传感器发射完超声波后，和2、4号传感器一样都处于接收状态，等待超声波的回波信号，分别产生3个飞行时间，分别为
T
O
F
2
TOF_2
TOF2、
T
O
F
3
TOF_3
TOF3、
T
O
F
4
TOF_4
TOF4。
对上述状态的图像化描述，如下图所示：
3、测量距离计算
假设T℃温度时的声速为
V
T
V_T
VT，建立距离关系式
D
2
+
D
3
=
V
T
∗
T
O
F
2
D
3
+
D
3
=
V
T
∗
T
O
F
3
D
4
+
D
3
=
V
T
∗
T
O
F
4
\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}
D2+D3=VT∗TOF2D3+D3=VT∗TOF3D4+D3=VT∗TOF4 第一步计算距离
D
3
D_3
D3
D
3
=
V
T
∗
T
O
F
3
2
D_3 = \frac{V_T*TOF_3}{2}
D3=2VT∗TOF3 接下来计算距离
D
2
D_2
D2和
D
4
D_4
D4：
D
2
=
V
T
∗
T
O
F
2
−
D
3
D
4
=
V
T
∗
T
O
F
4
−
D
3
D_2 = V_T*TOF_2 - D_3\\ D_4 = V_T*TOF_4 - D_3
D2=VT∗TOF2−D3D4=VT∗TOF4−D3 当然，在实际工程应用中，可以不考虑这个误差，直接算出这三个距离值。
D
2
=
V
T
∗
T
O
F
2
2
D
3
=
V
T
∗
T
O
F
3
2
D
4
=
V
T
∗
T
O
F
4
2
D_2 = \frac{V_T*TOF_2}{2} \\ D_3 = \frac{V_T*TOF_3}{2} \\ D_4 = \frac{V_T*TOF_4}{2}
D2=2VT∗TOF2D3=2VT∗TOF3D4=2VT∗TOF4
4、目标位置计算
根据坐标点
(
x
2
3
,
y
2
3
)
(x^3_2,y^3_2)
(x23,y23)和
(
x
2
4
,
y
2
4
)
(x^4_2,y^4_2)
(x24,y24)的位置，计算坐标点
(
x
2
T
,
y
2
T
)
(x^T_2,y^T_2)
(x2T,y2T)。
计算角度
α
\alpha
α
α
1
=
arctan
y
2
4
−
y
2
3
x
2
4
−
x
2
3
\alpha_1 = \arctan \frac{y^4_2 - y^3_2}{x^4_2 - x^3_2}
α1=arctanx24−x23y24−y23
α
2
=
arctan
y
2
3
−
y
2
2
x
2
3
−
x
2
2
\alpha_2 = \arctan \frac{y^3_2 - y^2_2}{x^3_2 - x^2_2}
α2=arctanx23−x22y23−y22计算角度
β
\beta
β
根据余弦定理计算得
β
1
=
arccos
D
3
2
+
L
1
2
−
D
4
2
2
∗
D
3
∗
L
1
\beta_1 = \arccos \frac{D_3^2 + L_1^2 - D_4^2}{2*D_3*L_1}
β1=arccos2∗D3∗L1D32+L12−D42
β
2
=
arccos
D
2
2
+
L
2
2
−
D
3
2
2
∗
D
2
∗
L
2
\beta_2 = \arccos \frac{D_2^2 + L_2^2 - D_3^2}{2*D_2*L_2}
β2=arccos2∗D2∗L2D22+L22−D32
计算目标位置坐标
3、4号传感器组合测量结果为
[
x
2
T
y
2
T
]
=
[
x
2
3
y
2
3
]
+
[
D
3
∗
cos
(
α
1
+
β
1
)
D
3
∗
sin
(
α
1
+
β
1
)
]
\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]
[x2Ty2T]=[x23y23]+[D3∗cos(α1+β1)D3∗sin(α1+β1)]
2、3号传感器组合测量结果为
[
x
2
T
y
2
T
]
=
[
x
2
2
y
2
2
]
+
[
D
2
∗
cos
(
α
2
+
β
2
)
D
2
∗
sin
(
α
2
+
β
2
)
]
\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]
[x2Ty2T]=[x22y22]+[D2∗cos(α2+β2)D2∗sin(α2+β2)]
5、计算过程中的常数
由于传感器的安装位置是固定的，所以上述计算过程中，很多参数是确定的常量。
已知传感器安装位置点
传感器号坐标位置实际坐标点2号
(
x
2
2
,
y
2
2
)
(x^2_2,y^2_2)
(x22,y22)
(
360
,
30
)
(360,30)
(360,30)3号
(
x
2
3
,
y
2
3
)
(x^3_2,y^3_2)
(x23,y23)
(
360
,
−
30
)
(360,-30)
(360,−30)4号
(
x
2
4
,
y
2
4
)
(x^4_2,y^4_2)
(x24,y24)
(
346
,
−
70
)
(346,-70)
(346,−70)
通过上述信息，可以计算出传感器的距离
L
1
L_1
L1和
L
2
L_2
L2，以及对应的旋转角度
α
1
\alpha_1
α1和
α
2
\alpha_2
α2。
参数计算值
L
1
L_1
L142.3792(cm)
L
2
L_2
L260(cm)
α
1
\alpha_1
α1250.71(°)
α
2
\alpha_2
α2270(°)
6、软件代码
///
/// 计算超声波的距离
///
/// 输入原始超声波的原始数据
/// 计算实际距离
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
}
//目标位置计算
///
/// 目标位置坐标点计算
///
/// 基准点坐标点
/// 基准角度(弧度)
/// 基准长度(cm)
/// 临边长度(cm)
/// 对角长度(cm)
/// 输出最终的目标位置坐标
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);
}