[发明专利]一种基于室内全局定位的多移动机器人编队控制方法

摘要

本发明提供了一种基于室内全局定位的多移动机器人编队控制方法，涉及机器人导航领域，本发明通过安装在固定信标和移动信标上的超声波发生接收器，计算出编队运动中各个移动机器人的位置坐标，并由此获得各移动机器人的实时位姿，控制跟随机器人的跟踪位姿，从而实现基于领航跟随型的多机编队作业任务。本发明解决了移动机器人自主导航中的基础性定位问题；该定位方法具有容易部署、简单实用、定位精度高、可扩展性好等特点，适用于单个移动机器人的自主导航，也可适用于多移动机器人的编队协作，同时也支持无人系统的三维坐标定位，为无人机、空地协同等的室内定位提供使能技术支撑。

主权项

一种基于室内全局定位的多移动机器人编队控制方法，其特征在于包括下述步骤：步骤1、本发明包括一套室内全局定位系统和N个移动机器人，其中室内全局定位系统包括四个固定信标、一台定位基站控制器和N个搭载在移动机器人上的移动信标，将四个固定信标安装在高于移动机器人运动平面的墙面上，在避免遮挡的情况下使固定信标两两相对且不在同一平面内，并保证移动机器人运动区域无遮挡；固定信标是整个室内定位系统用来构建全局坐标系的基准，接收每个移动信标发出的超声波信号并记录其ID码，在接收过程中四个固定信标将会接收所有移动信标的超声波信号；移动信标携带唯一的ID码以标识每个移动机器人，并以固定频率发射超声波信号，该固定频率设定在10‑20Hz之间；所述固定信标和移动信标均采用相同结构，均包含一个正方形底座和5个超声波发生接收器，5个超声波发生接收器分别安装在正方形底座的四边和中心，其中一个超声波发生接收器安装在底座中心位置且发生接收探头竖直向上，其余四个安装在正方形底座各边的中点处，且四个超声波发生接收器的发生接收探头与正方形底座保持15度仰角向上倾斜，超声波发生接收器以实现信标的全向发送和接收，同时每个移动信标配备角度姿态传感器，可动态测量移动机器人的偏角信息，移动信标和固定信标都有各自唯一的ID标识码，整套定位系统同时测定所有的移动信标的位置坐标，在定位系统的覆盖区域内，定位系统利用固定信标对所有的移动信标进行实时测量，从而满足多机编队运动中对各个移动机器人的位置坐标计算需求；定位基站控制器是整个室内全局定位系统的中心控制器，其信号覆盖定位区域内的所有固定信标和移动信标；步骤2、在室内全局定位系统初始化过程中，以地面为基准对各固定信标的高度进行标定，并选择其中任一固定信标作为Z轴，以相邻的任一固定信标的正下方为X轴建立三维坐标系，其他各固定信标以该坐标系为基准，确定其余固定信标在坐标系中的位置坐标，整个坐标系建立完成后，设置固定信标为接收模式；步骤3、采用三边测量法对移动信标的位置坐标进行计算：固定信标接收移动信标的超声波信号并记录接收到该信号的时间点；定位基站控制器通过计算移动信标到达各固定信标的时间间隔，依此计算出该移动信标和四个固定信标之间的直线距离分别为d1、d2、d3、d4，当某一移动信标发射的超声波信号被3个或3个以上的固定信标接收到后，基站控制器通过三边测量法计算得到该移动信标在全局坐标系内的位置信息；详细计算步骤为：假设定位系统在初始标定时，从4个固定信标中任选3个，即可得1号固定信标、2号固定信标和3号固定信标的坐标分别为(0,0,z1)、(a,0,z2)、(0,b,z3)，当这3个固定信标分别接收到移动信标发送的超声波信号时，依据三边测量计算原理，该移动信标的当前位置坐标(X,Y,Z)可计算如下：X2+Y2+(Z-z1)2=d12(X-a)2+Y2+(Z-z2)2=d22X2+(Y-b)2+(Z-z3)2=d32---(1)]]>即通过式(1)计算，即可得到某一移动信标在全局坐标内的当前位置坐标(X,Y,Z)；步骤4、构建基于超声波信标的室内全局定位系统，用以获取各移动机器人的实时位姿；1)部署室内全局定位系统：将固定信标安装在四个顶角位置，选择其中两个固定信标作为坐标系基准建立三维坐标系，移动信标分别安装在每个移动机器人上，移动信标与移动机器人控制器均采用串口通讯连接方式；定位基站控制器实时计算各移动机器人的位置坐标信息，并结合偏角信息以位姿数据包的形式下传至对应的移动信标；2)移动机器人解析位姿数据包：移动机器人通过串口通讯方式读取移动信标接收到的位姿数据包，采用奇偶校验位进行判断，并判断校验位是否正确，当校验位为1则为正确，校验位为0则为错误，当校验位信息正确，则移动机器人将正确的位姿坐标信息存入处理器的缓存中，并进行字符串格式的数据类型转换为浮点型的位姿数据，并存入位姿数组中，如果校验位信息错误，将舍弃此次位姿坐标信息；3)位姿数据依次存储：随着移动机器人的自主运动，其位置信息进行实时更新，通过定位基站控制器向各移动信标发送并解析位姿数据包后，位姿坐标信息遵循“先进先出”的顺序原则依次更新到移动机器人控制器的位姿坐标数组中；步骤5：移动机器人的初始化移动机器人的初始化主要包括：分配领航、跟随两种移动机器人角色，即任意选择一个机器人为领航机，其它均为跟随机器人，根据需求设定移动机器人编队需要保持的队形图案，配置领航机器人为发送数据状态，跟随机器人为接收数据状态；设计移动机器人运动控制器，实现多机器人编队作业任务，详细步骤如下：1)根据移动机器人车体结构及驱动方式，建立移动机器人运动学模型，并依此构建多移动机器人编队的运动学模型；2)基于上述超声波信标的室内全局定位方法，实时获取领航机器人和跟随机器人的当前位姿；再根据预设的多机编队队形几何信息，计算出各跟随机器人当前时刻的理想位姿，将跟随机器人任意放置，然后，跟随机器人依据各自的理想位姿和实际位姿，计算得到各跟随机器人的跟踪位姿误差；最后，运用控制方法，设计运动控制器使跟随机器人的跟踪位姿误差满足精度要求，依此控制跟随机器人的跟踪位姿误差满足精度要求，从而实现基于领航跟随型的多机编队作业任务。