CN107422725A - 一种基于声纳的机器人跟踪方法 - Google Patents

Abstract

本发明提出一种基于声纳的机器人跟踪方法，采用非接触单向直接测距方法和收发分离的设计，声纳发射器负责发射超声波，声纳接收器负责接收超声波，使用无线通信的方法同步发射器和接收器的本地时间信号，通过计算超声波到达的时间差值来计算目标的距离并进行定位。该方法实现了单一传感器多种用途，既实现了产品功能又降级制造成本，同时可以实时快速的探测到外界环境中障碍物的信息；而且探测没有盲区，多种传感器测量的参数互补实用，能实时可靠的为机器人控制中心提供最真实的外部环境参数，以便机器人控制中心实时的优化行进线路和及时快速的避障。该方法以较低的成本和简单的结构实现了多项功能，定位跟踪准确，应用价值很高。

Description

一种基于声纳的机器人跟踪方法

技术领域

本发明属于移动机器人跟踪定位、导航、避障技术领域，特别是一种基于声纳的机器人跟踪方法。

背景技术

为了使机器人服务于特定的目标对象，帮助人们完成任务，要求这类机器人应该具有目标识别和自主导航的功能。目前具有目标识别和自主导航的机器人普遍通过“机器视觉系统”识别目标，并辅助以其它传感器来完成自主导航功能。

“机器视觉”是对人的视觉功能进行模拟，获取目标信息，并对目标信息进行处理。根据视觉传感器数据不同，分为“单目视觉”和“双目视觉”等。“单目视觉系统”对目标的表达主要体现在二维图像的明暗度、纹理、表面轮廓线、阴影等，它在成像过程中损失了成像点的深度信息，这是这类系统一个严重缺点。另外“单目视觉”的感知范围有限，缩小焦距增大广角往往需要增大图像畸变为代价，这就是这类系统的另一个缺点。“双目视觉”符合生物视觉的结构，在三维信息获取上有优势，但是双目摄像的结构复杂，算法不成熟。而且“机器视觉”通过提取目标图像数据，通过3D建模及大量复杂算法才能实现，对于硬件平台的要求高，成本也就居高不下。

目前“机器视觉”的实现往往借助于PC来进行大量算法计算，基于嵌入式的“机器视觉”应用还处在研究阶段。即使有应用，效果也不理想，在跟踪时很容易跟丢目标。

发明内容

本发明旨在于提出一种基于声纳的机器人跟踪方法，该方法能够实现移动机器人基于声纳实现定位、跟踪与避障功能，以较低的成本和简单的结构实现了多项功能，避免损失成像点的深度信息和图像畸变，定位跟踪准确。

为实现上述发明目的，本发明采用以下技术方案：

本发明是一种基于声纳的机器人跟踪方案采用非接触单向直接测距方法和收发分离的设计，声纳发射器负责发射超声波，声纳接收器负责接收超声波，使用无线通信的方法同步发射器和接收器的本地时间信号，通过计算超声波到达的时间差值来计算目标的距离并进行定位。

具体来说，本发明的一种基于声纳的机器人跟踪方法包括以下步骤：

1)在移动机器人及目标上安装声纳收发器；所述移动机器人上的一个声纳收发器作为协调器。

2)所述协调器接收移动机器人的运动控制器发出的目标识别和定位指令，然后所述协调器发回目标识别和定位数据至运动控制器，所述运动控制器根据协调器发回的目标识别和定位数据确定目标方位并计算出所述机器人与目标之间的距离。

3)所述协调器接收移动机器人的运动控制器发出的测距指令，然后所述协调器发回测距数据至运动控制器，所述运动控制器根据协调器发回的测距数据确定所述移动机器人和目标之间是否有障碍物，如果有障碍物，则计算出所述移动机器人与障碍物之间的距离。

4)所述运动控制器根据目标识别和定位数据以及测距数据判断出状态和计算出数据并规划跟随路径，并依据跟随路径发送动作指令序列给所述移动机器人的伺服控制器，由所述伺服控制器驱动所述移动机器人上的驱动装置实现跟随动作。

5)所述移动机器人在跟随目标的过程中，通过所述运动控制器不断发出目标识别和定位指令、测距指令获得目标的位置和距离以及行走环境中障碍物的信息，从而不断地实时更新所述移动机器人的行走路径，达到定位、路径规划、自主跟随和动态避障的目的。

进一步的，所述协调器获得目标识别和定位数据的过程为：

21)所述移动机器人上安装有两个声纳收发器，其中一个声纳收发器为协调器，另一个声纳收发器为第二路由节点；所述目标上的声纳收发器为第一路由节点；所述协调器接收到所述运动控制器发送的识别和定位指令时，以协调器身份标识号ID0为第一位数据，以所述运动控制器发出的时间同步指令和要求第一路由节点发射声波指令为后续位数据；第一位数据和后续位数据组成第一帧命令序列，所述协调器上的无线信号发射模块将第一帧命令序列以广播命令的形式发送出去。然后所述协调器等待第一路由节点确认命令，当协调器接收到确认命令后，延时一段时间后开始计时并等待接收声波信号。当所述协调器接收到声波信号后，停止计时，并由所述协调器读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离S1，把距离数据S1暂存到数据缓冲区中。

22)所述第一路由节点接收到协调器发送的所述第一帧命令序列，以第一路由节点的身份标识号ID1为第一位数据，以第一位数据ID1和确认指令组成第二帧命令序列，所述第一路由节点上的无线信号发射模块将第二帧命令序列以广播命令的形式发送出去，延时一段时间后发送一串声波。声波发送结束后，所述第一路由节点停止发送并进入等待命令状态。

23)所述第二路由节点接收到协调器发送的所述第一帧命令序列和第一路由节点发送的所述第二帧命令序列后，延时一段时间后开始计时并等待接收第一路由节点发射的声波信号。当所述第二路由节点接收到声波信号后，停止计时，并由所述第二路由节点读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离S2，把距离数据S2暂存到数据缓冲区中。

24)所述第二路由节点计算得到距离数据S2后，以所述第二路由节点的身份标识号ID2为第一位数据，以距离数据S2为第二位组成第三帧命令序列，所述第二路由节点上的无线信号发射模块将第三帧命令序列发给协调器。

25)所述协调器接收到第二路由节点发送的所述第三帧命令序列，然后从第三帧数据中解析出第二路由节点测量的距离数据S2。所述协调器以协调器身份标识号ID0为第一位数据、距离数据S1为第二位数据、距离数据S2为第三位数据，这三种数据按顺序组成第四帧数据序列发送给机器人运动控制器。

26)所述“渡越时间探测测距法”原理为：即在声速已知的情况下，通过测量超声波回声所经历的时间来获得距离。其原理图如图5所示。首先声纳发射器向空气中发射一串声波脉冲，声波遇到被测物体后反射回来。已知声速为C，若能测到第一个回波到达的时间与发射声波脉冲的时间差t，利用公式

即可算出传感器和反射点之间的距离S，测量距离

当s＞＞h时，d≈s.一般来说测距仪器采用收发同体传感器，故h＝0,则

已知声速v＝344m/s(20℃时)，则公式进一步简化

S＝172*t (26-4)

所述机器人在目标跟踪过程中感知障碍物的过程为：

31)所述协调器在接收到运动控制器发送的测距指令时，以协调器身份标识号ID0为第一位数据，所述协调器将第一位数据和测距命令组成第四帧命令序列，所述协调器上的无线信号发射模块将所述第四帧命令序列发送给第二路由节点，然后所述协调器等待第二路由节点的测量结果。

32)所述第二路由节点收到协调器发来的所述第四帧命令序列，所述第二路由节点先发送一串声波，声波发送结束后打开定时器计时并等待接收回波信号，当第二路由节点接收到回波信号后，停止计时并由第二路由节点读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离数据S3，所述第二路由节点以身份标识号ID2为第一位数据，测距命令为第二位数据，距离数据S3为第三位数据组成第五帧命令序列，所述第二路由节点上的无线信号发射模块将所述第五帧命令序列发送给协调器。然后所述第二路由节点停止工作并进入等待命令状态。

33)所述协调器接收到第二路由节点的所述第五帧命令序列后解析出距离数据S3暂存在数据缓冲器中，然后所述协调器发送一串声波，声波发送结束后打开定时器计时并等待接收回波信号；当所述协调器接收到回波信号后，停止计时并读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离S4，所述协调器将读出的定时数据、第五帧命令序列以及特定标识符组成测距数据。

进一步的，所述步骤4)中运动控制器根据目标识别和定位数据以及测距数据判断出状态和计算出数据规划跟随路径包括以下步骤：

41)机器人避障控制流程：

判断前方是否有障碍，如果没有障碍，全速前进；如果前方有障碍，判断障碍距离，如果障碍距离近，停止前进，如果前方障碍距离远，快速前进；对障碍左右空间进行判断，如果左边更开阔，左转前进，如果右边更开阔，右转前进。

42)机器人跟随目标控制流程：

探测是否有物体，如果没有物体，不进行目标路径规划；如果探测到物体，辨别该物体是目标还是障碍，如果是目标，跟踪目标，跟踪目标的路线上有障碍时，根据避障流程避障；如果辨别出是障碍而不是目标，判断障碍是否在前方，如果障碍不在前方，不进行目标路径规划，如果障碍在前方，根据避障流程避障，此时也不进行目标路径规划。

本发明的基于声纳的机器人跟踪方法，把对目标的识别转化成对特定信号的识别，对特定信号的识别要容易得多，简单得多。这样大大提高了识别率和定位的准确性，同时声纳收发器兼顾测距功能。声纳收发器既可以实现对目标的识别和定位，也可以测量障碍物的距离，实现单一传感器多种用途，既实现了产品功能又降级制造成本。同时可以实时快速的探测到外界环境中障碍物的信息，而且探测没有盲区，多种传感器测量的参数互补实用，能实时可靠的为机器人控制中心提供最真实的外部环境参数，以便机器人控制中心实时的优化行进线路和及时快速的避障。

附图说明

图1为本发明提出的一种基于声纳的机器人跟踪方法的原理图；

图2为本发明的声纳定位目标示意图；

图3为本发明机器人避障控制流程图；

图4为本发明机器人跟随目标控制流程图。

图5为本发明提出的超声波测距原理图

具体实施方式

下面结合附图，对本发明的一种基于声纳的机器人跟踪方法进行详细说明。需要说明的是，本文的“第一”、“第二”等仅为了区分相关的术语，并不表示重要性的高低。

如图1所示，本发明的一种基于声纳的机器人跟踪方法，采用的移动机器人由运动控器、多个声纳收发器、伺服控制器、辅助传感器、机械传动等模块组成。由运动控制器根据声纳收发器探测的数据做目标定位，路径规划和实时避障。

本发明的方法采用多个声纳传感器定位测距，为了定位目标的准确位置，尽可能减小误差，机器人上的两个声纳收发器分布距离尽可能大一些，最好直线距离大于15cm(因为声纳接收器的盲区一般在15cm左右)。同时两个声纳收发器在机器人上的安装高度最好离地60cm—80cm(因为机器人上的声纳收发器要接收目标佩带在皮带上小型声纳收发器的信号，一般人的腰部到地面高度大概是60cm---80cm)。另外其它辅助传感器应该安装在声纳传感器的探测盲区中，这样可以尽可能减小整个机器人的探测盲区，机器人在跟随过程中能全面准确的感知外部环境中障碍物的信息，从而更好的实现障碍功能。

本发明的一种基于声纳的机器人跟踪方法包括以下步骤：

1)在移动机器人及目标上安装声纳收发器；移动机器人上的一个声纳收发器作为协调器。本实施例以人为目标为例。

2)协调器接收移动机器人的运动控制器发出的目标识别和定位指令，然后协调器发回目标识别和定位数据至运动控制器，运动控制器根据协调器发回的目标识别和定位数据确定目标方位并计算出机器人与目标之间的距离。

3)协调器接收移动机器人的运动控制器发出的测距指令，然后协调器发回测距数据至运动控制器，运动控制器根据协调器发回的测距数据确定移动机器人和目标之间是否有障碍物，如果有障碍物，则计算出移动机器人与障碍物之间的距离。

4)运动控制器根据目标识别和定位数据和测距数据判断出状态和计算出数据规划跟随路径，并依据跟随路径发送动作指令序列给移动机器人的伺服控制器，由伺服控制器驱动移动机器人上的驱动装置实现跟随动作。

5)移动机器人在跟随目标的过程中，通过运动控制器不断发出目标识别和定位指令、测距指令获得目标的位置和距离以及行走环境中障碍物的信息，从而不断地实时更新移动机器人的行走路径，达到定位、路径规划、自主跟随和动态避障的目的。

如图2所示，协调器获得目标识别和定位数据的过程为：

21)移动机器人上安装有两个声纳收发器，其中一个声纳收发器为协调器，另一个声纳收发器为第二路由节点；目标上的声纳收发器为第一路由节点；协调器接收到运动控制器发送的识别和定位指令时，以协调器身份标识号ID0为第一位数据，以运动控制器发出的时间同步指令和要求第一路由节点发射声波指令为后续位数据；第一位数据和后续位数据组成第一帧命令序列，协调器上的无线信号发射模块将第一帧命令序列以广播命令的形式发送出去。本实施例的无线信号发射模块均选择Zigbee模块，需要说明的是，其他种类的无线发射模块也可实现本实施例中的Zigbee模块的功能，可以作为Zigbee模块的替换。然后协调器等待第一路由节点确认命令，当协调器接收到确认命令后，延时一段时间后开始计时并等待接收声波信号。当所述协调器接收到声波信号后，停止计时，并由所述协调器读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离S1，把距离数据S1暂存到数据缓冲区中。

22)所述第一路由节点接收到协调器发送的所述第一帧命令序列，以第一路由节点的身份标识号ID1为第一位数据，以第一位数据ID1和确认指令组成第二帧命令序列，所述第一路由节点上的无线信号发射模块将第二帧命令序列以广播命令的形式发送出去，延时一段时间后发送一串声波。声波发送结束后，所述第一路由节点停止发送并进入等待命令状态。

23)所述第二路由节点接收到协调器发送的所述第一帧命令序列和第一路由节点发送的所述第二帧命令序列后，延时一段时间后开始计时并等待接收第一路由节点发射的声波信号。当所述第二路由节点接收到声波信号后，停止计时，并由所述第二路由节点读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离S2，把距离数据S2暂存到数据缓冲区中。

24)所述第二路由节点计算得到距离数据S2后，以所述第二路由节点的身份标识号ID2为第一位数据，以距离数据S2为第二位组成第三帧命令序列，所述第二路由节点上的无线信号发射模块将第三帧命令序列发给协调器。

25)所述协调器接收到第二路由节点发送的所述第三帧命令序列，然后从第三帧数据中解析出第二路由节点测量的距离数据S2。所述协调器以协调器身份标识号ID0为第一位数据、距离数据S1为第二位数据、距离数据S2为第三位数据，这三种数据按顺序组成第四帧数据序列发送给机器人运动控制器。

所述机器人在目标跟踪过程中感知障碍物的过程为：

31)所述协调器在接收到运动控制器发送的测距指令时，以协调器身份标识号ID0为第一位数据，所述协调器将第一位数据和测距命令组成第四帧命令序列，所述协调器上的无线信号发射模块将所述第四帧命令序列发送给第二路由节点，然后所述协调器等待第二路由节点的测量结果。

32)所述第二路由节点收到协调器发来的所述第四帧命令序列，所述第二路由节点先发送一串声波，声波发送结束后打开定时器计时并等待接收回波信号，当第二路由节点接收到回波信号后，停止计时并由第二路由节点读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离数据S3，所述第二路由节点以身份标识号ID2为第一位数据，测距命令为第二位数据，距离数据S3为第三位数据组成第五帧命令序列，所述第二路由节点上的无线信号发射模块将所述第五帧命令序列发送给协调器。然后所述第二路由节点停止工作并进入等待命令状态。

33)所述协调器接收到第二路由节点的所述第五帧命令序列后解析出距离数据S3暂存在数据缓冲器中，然后所述协调器发送一串声波，声波发送结束后打开定时器计时并等待接收回波信号；当所述协调器接收到回波信号后，停止计时并读出计时数据t，然后根据“渡越时间探测测距法”计算出机器人到障碍物的距离S4，所述协调器将读出的定时数据、第五帧命令序列以及特定标识符组成测距数据。

4、根据权利要求1所述的基于声纳的机器人跟踪方法，其特征在于，所述步骤4)中运动控制器根据目标识别和定位数据以及测距数据判断出状态和计算出数据规划跟随路径包括以下步骤：

如图3所示，41)机器人避障控制流程：

判断前方是否有障碍，如果没有障碍，全速前进；如果前方有障碍，判断障碍距离，如果障碍距离近，停止前进，如果前方障碍距离远，快速前进；对障碍左右空间进行判断，如果左边更开阔，左转前进，如果右边更开阔，右转前进；

如图4所示，42)机器人跟随目标控制流程：

探测是否有物体，如果没有物体，不进行目标路径规划；如果探测到物体，辨别该物体是目标还是障碍，如果是目标，跟踪目标，跟踪目标的路线上有障碍时，根据避障流程避障；如果辨别出是障碍而不是目标，判断障碍是否在前方，如果障碍不在前方，不进行目标路径规划，如果障碍在前方，根据避障流程避障，此时也不进行目标路径规划。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。

Claims (4)

1.一种基于声纳的机器人跟踪方法，其特征在于，包括以下步骤：

1）在移动机器人及目标上安装声纳收发器；所述移动机器人上的两个声纳收发器中一个声纳收发器作为协调器、另一个声纳收发器作为路由节点2；目标上安装的声纳收发器作为路由节点1；

2）所述协调器接收移动机器人的运动控制器发出的目标识别和定位指令，然后所述协调器发回目标识别和定位数据至运动控制器，所述运动控制器根据协调器发回的目标识别和定位数据确定目标方位并计算出所述机器人与目标之间的距离；

3）所述协调器接收移动机器人的运动控制器发出的测距指令，然后所述协调器发回测距数据至运动控制器，所述运动控制器根据协调器发回的测距数据确定所述移动机器人和目标之间是否有障碍物，如果有障碍物，则计算出所述移动机器人与障碍物之间的距离；

4）所述运动控制器根据目标识别和定位数据以及测距数据判断出状态和计算出数据规划跟随路径，并依据跟随路径发送动作指令序列给所述移动机器人的伺服控制器，由所述伺服控制器驱动所述移动机器人上的驱动装置实现跟随动作；

5）所述移动机器人在跟随目标的过程中，通过所述运动控制器不断发出目标识别和定位指令、测距指令获得目标的位置和距离以及行走环境中障碍物的信息，从而不断地实时更新所述移动机器人的行走路径，达到定位、路径规划、自主跟随和动态避障的目的。

2.根据权利要求1所述的基于声纳的机器人跟踪方法，其特征在于，所述协调器获得目标识别和定位数据的过程为：

21）所述移动机器人上安装有两个声纳收发器，其中一个声纳收发器为协调器，另一个声纳收发器为第二路由节点；所述目标上的声纳收发器为第一路由节点；所述协调器接收到所述运动控制器发送的识别和定位指令时，以协调器身份标识号 ID 为第一位数据，以所述运动控制器发出的时间同步指令和要求第一路由节点发射声波指令为后续位数据；第一位数据和后续位数据组成第一帧命令序列，所述协调器上的无线信号发射模块将第一帧命令序列以广播命令的形式发送出去；然后所述协调器等待第一路由节点确认命令，当协调器接收到确认命令后，延时后开始计时并等待接收声波信号；当所述协调器接收到声波信号后，停止计时，并由所述协调器读出计时数据暂存到数据缓冲区中；

22）所述第一路由节点接收到协调器发送的所述第一帧命令序列，以第一路由节点的身份标识号 ID 为第一位数据，以第一位数据和确认指令组成第二帧命令序列，所述第一路由节点上的无线信号发射模块将第二帧命令序列以广播命令的形式发送出去，延时后发送一串声波；声波发送结束后，所述第一路由节点停止发送并进入等待命令状态；

23）所述第二路由节点接收到协调器发送的所述第一帧命令序列和第一路由节点发送的所述第二帧命令序列后，延时开始计时并等待接收声波信号；当所述第二路由节点接收到声波信号后，停止计时，并由所述第二路由节点读出计时数据暂存到数据缓冲区中；

24）所述第二路由节点读出计时数据后，以所述第二路由节点的身份标识号 ID 为第一位数据，以第一位数据和测量结果组成第三帧命令序列，所述第二路由节点上的的无线信号发射模块将第二帧命令序列发给协调器；

25）所述协调器将接收到第二路由节点发出的所述第三帧命令序列、所述协调器测量的数据以及特定标识符组成目标识别和定位数据。

3.根据权利要求1所述的基于声纳的机器人跟踪方法，其特征在于，所述协调器获得测距数据的过程为：

31）所述协调器在接收到运动控制器发送的测距指令时，以协调器身份标识号 ID 为第一位数据，所述协调器将第一位数据和测距命令组成第四帧命令序列，所述协调器上的无线信号发射模块将所述第四帧命令序列发送给第二路由节点，然后所述协调器等待第二路由节点的测量结果；

32）所述第二路由节点收到协调器发来的所述第四帧命令序列，所述第二路由节点先发送一串声波，声波发送结束后打开定时器定时并等待接收反射信号，当第二路由节点接收到反射信号后，停止定时并由第二路由节点读出定时数据，然后以第二路由节点身份标识号 ID 为第一位数据，第一位数据和测距结果命令以及读出的定时数据组成第五帧命令序列，所述第二路由节点上的无线信号发射模块将所述第五帧命令序列发送给协调器；然后所述第二路由节点停止工作并进入等待命令状态；

33）所述协调器接收到第二路由节点的所述第五帧命令序列后暂存在数据缓冲中； 所述协调器发送一串声波，声波发送结束后打开定时器定时并等待接收反射信号；当所述协调器接收到反射信号后，停止定时并读出定时数据，所述协调器将读出的定时数据、第五帧命令序列以及特定标识符组成测距数据。

4.根据权利要求1所述的基于声纳的机器人跟踪方法，其特征在于，所述步骤4）中运动控制器根据目标识别和定位数据以及测距数据判断出状态和计算出数据规划跟随路径包括以下步骤：

41）机器人避障控制流程：

判断前方是否有障碍，如果没有障碍，全速前进；如果前方有障碍，判断障碍距离，如果障碍距离近，停止前进，如果前方障碍距离远，快速前进；对障碍左右空间进行判断，如果左边更开阔，左转前进，如果右边更开阔，右转前进；

42）机器人跟随目标控制流程：

探测是否有物体，如果没有物体，不进行目标路径规划；如果探测到物体，辨别该物体是目标还是障碍，如果是目标，跟踪目标，跟踪目标的路线上有障碍时，根据避障流程避障；如果辨别出是障碍而不是目标，判断障碍是否在前方，如果障碍不在前方，不进行目标路径规划，如果障碍在前方，根据避障流程避障，此时也不进行目标路径规划。