[发明专利]一种基于声纳的机器人跟踪方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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