CN100580597C - 基于超声波源的二维定位控制*** - Google Patents

Abstract

一种基于超声波源的二维定位控制***，包括两个静止超声波基站、安装在受控二维平面作业的机械上的超声波收发器和定位控制器，超声波基站在收到超声波信号后转发超声波信号，定位控制器包括距离计算模块，用于计算当前点与两个超声波基站的距离L1和L2，目标点与两个超声波基站的距离L3和L4；数据存储模块，用于存储两个静止超声波基站之间的距离L、L1和L2、L3和L4；定位移动控制模块，用于当机械从当前点移动到目标点时，通过X轴和Y轴移动来实现，将所述X轴和Y轴移动距离控制指令输出到机械的X轴受控电机和Y轴受控电机。本发明提供一种定位精度高、实用性强的基于超声波源的二维定位控制***。

Description

基于超声波源的二维定位控制***

技术领域

本发明涉及一种二维定位控制***。

背景技术

比较常见的无人农业作机或机器人是通过无线电磁波通信技术实现数据命令和视频图像信号的远程通信和控制，存在的问题有两个：一是现场***装置往往是受到无线电磁波远程操作的，机器人的无线电磁操作将极大的可能影响***装置，甚至引起误引爆；二是电磁波技术在定位上存在一定的困难，像无人农业作机的自动运作往往需要比较规则路经的场地运作，对自身的定位要求比较高。

发明内容

为了克服已有的二维定位***的定位精度低的不足，本发明提供一种定位精度高、实用性强的基于超声波源的二维定位控制***。

本发明解决其技术问题所采用的技术方案是：

一种基于超声波源的二维定位控制***，包括两个静止超声波基站、安装在受控二维平面作业的机械上的超声波收发器和定位控制器，所述超声波基站在收到超声波信号后转发超声波信号，所述定位控制器包括：距离计算模块，用于依据超声波收发器发出超声波信号的时间和接收到静止超声波基站转发的超声波时间，依据超声波的传播速度，计算得到当前点与两个超声波基站的距离L1和L2，目标点与两个超声波基站的距离L3和L4；数据存储模块，用于存储两个静止超声波基站之间的距离L、当前点与两个超声波基站的距离L1和L2、以及目标点与两个超声波基站的距离L3和L4；

定位移动控制模块，用于当机械从当前点移动到目标点时，通过X轴和Y轴移动来实现，其中：

$X=|X1-X2|=|\frac{L^2+{L1}^2-{L2}^2}{2L}-\frac{L^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{L1}^2-{L2}^2-{L3}^2+{L4}^2}{2L}\right|---\left(5\right)$

$Y=|Y1-Y2|=|\sqrt{{L1}^2{\left(\frac{L^2+{L1}^2-{L2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{L^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

将所述X轴和Y轴移动距离控制指令输出到机械的X轴受控电机和Y轴受控电机。

本发明的技术构思为：超声波技术的引入正好解决了现有技术中存在的问题，超声波属于一种机械波，当超声波在空间传播时，如果遇到障碍物就能反射，传播的速度和声波类似，如果记录带调制的超声波发出和接收的时间差，就能比较简单的测量到和障碍物的距离。

如图1，构建了两个静止的超声波基站JZ1、JZ2，每个基站可以同时收发超声波信号，而且发出的超声波信号是带编码调制的，可以被移动超声波源有辨别的接收。可移动超声波源是架构在可移动机械M上的，该移动机械M可以是步进电机驱动的机器人或农业作机，可移动超声波源也可以同时收发超声波信号，发出的超声波信号也是带编码调制的。在该二维平面内，基站和可移动超声波源发出的带编码调制超声波信号足够的强，即采用大功率超声波发生器，可移动超声波源发出的超声波信号同时可以被两个基站接收。如图2，当可移动超声波源在A点时，可以通过式1和式2得到：

A点的坐标(X1，Y1)，其中：

$\cos \mathrm{\α}=\frac{L^2+{L1}^2-{L2}^2}{2L.L1}$

$X1=\frac{L^2+{L1}^2-{L2}^2}{2L}---\left(1\right)$

$Y1=\sqrt{{L1}^2-{\left(\frac{L^2+{L1}^2-{L2}^2}{2L}\right)}^2}---\left(2\right)$

L是两个静止基站间的距离；L1是移动超声波源和第一静止基站JZ1的距离；L2是移动超声波源和第二静止基站JZ2的距离；α是移动超声波源和第一静止基站JZ1的夹角；

可移动超声波源在B点时，可以通过式3和式4得到：

B点的坐标(X2，Y2)，其中：

$\cos \mathrm{\β}=\frac{L^2+{L3}^2-{L4}^2}{2L.L3}$

$X2=\frac{L^2+{L3}^2-{L4}^2}{2L}---\left(3\right)$

$Y2=\sqrt{{L3}^2-{\left(\frac{L^2+{L3}^2-{L4}^2}{2L}\right)}^2}---\left(4\right)$

L是两个静止基站间的距离；L3是移动超声波源和第一静止基站JZ1的距离；L4是移动超声波源和第二静止基站JZ2的距离；β是移动超声波源和第一静止基站JZ1的夹角；

当移动超声波源从A点移动到B点时，可以通过X轴和Y轴分别移动实现，从点A(X1，Y1)移动到点B(X2，Y2)在X轴上需要移动：

$X=|X1-X2|=|\frac{L^2+{L1}^2-{L2}^2}{2L}-\frac{L^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{L1}^2-{L2}^2-{L3}^2+{L4}^2}{2L}\right|---\left(5\right)$

从点A(X1，Y1)移动到点B(X2，Y2)在Y轴上需要移动：

$Y=|Y1-Y2|=|\sqrt{{L1}^2{\left(\frac{L^2+{L1}^2-{L2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{L^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

通过以上的方法可实现可移动超声波源在二维平面内的自由移动和定位。

本发明的有益效果主要表现在：定位精度高、实用性强。

附图说明

图1是移动超声波源和静止超声波基站双向通信的二维图。

图2是可移动超声波源在二维平面内的定位图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1和图2，一种基于超声波源的二维定位控制***，包括两个静止超声波基站JZ1、JZ2、安装在受控二维平面作业的机械M上的超声波收发器和定位控制器，所述超声波基站在收到超声波信号后转发超声波信号，所述定位控制器包括：距离计算模块，用于依据超声波收发器发出超声波信号的时间和接收到静止超声波基站转发的超声波时间，依据超声波的传播速度，计算得到当前点A与两个超声波基站的距离L1和L2，目标点B与两个超声波基站的距离L3和L4；数据存储模块，用于存储两个静止超声波基站之间的距离L、当前点与两个超声波基站的距离L1和L2、以及目标点与两个超声波基站的距离L3和L4；

定位移动控制模块，用于当机械M从当前点A移动到目标点B时，通过X轴和Y轴移动来实现，其中：

$X=|X1-X2|=|\frac{L^2+{L1}^2-{L2}^2}{2L}-\frac{L^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{L1}^2-{L2}^2-{L3}^3+{L4}^2}{2L}\right|---\left(5\right)$

$Y=|Y1-Y2|=|\sqrt{{L1}^2{\left(\frac{L^2+{L1}^2-{L2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{L^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

将所述X轴和Y轴移动距离控制指令输出到机械的X轴受控电机和Y轴受控电机。

以可移动小型农业割稻机为例，先在稻田边确定两个静止基站点JZ1、JZ2，安装好大功率静止超声波发生器后并测得两基站点的距离，在移动小型农业作机上安装移动超声波发生器。假设通过程序设定，需要农业作机在起点(3，0)处垂直X轴沿Y轴方向前进到点(3，10)处停止，可移动超声波源只要实时的测量和两个静止基站的距离，通过内部的定位移动控制模块计算，得到X轴和Y轴上需要移动的距离，指挥电机转动就可以顺利到达目的地。

Claims (1)

1、一种基于超声波源的二维定位控制***，其特征在于：包括两个静止超声波基站、安装在受控二维平面作业的机械上的超声波收发器和定位控制器，所述超声波基站在收到超声波信号后转发超声波信号，所述定位控制器包括：

距离计算模块，用于依据超声波收发器发出超声波信号的时间和接收到静止超声波基站转发的超声波时间，依据超声波的传播速度，计算得到当前点与两个超声波基站的距离L1和L2，目标点与两个超声波基站的距离L3和L4；

数据存储模块，用于存储两个静止超声波基站之间的距离L、当前点与两个超声波基站的距离L1和L2、以及目标点与两个超声波基站的距离L3和L4；

定位移动控制模块，用于当机械从当前点移动到目标点时，通过X轴和Y轴移动来实现，其中：

$X=|X1-X2|=|\frac{L^2+{L1}^2-{L2}^2}{2L}-\frac{L^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{L1}^2-{L2}^2-{L3}^3+{L4}^2}{2L}\right|---\left(5\right)$

$Y=|Y1-Y2|=|\sqrt{{L1}^2-{\left(\frac{L^2+{L1}^2-{L2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{L^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

将所述X轴和Y轴移动距离控制指令输出到机械的X轴受控电机和Y轴受控电机。