技术领域
[0001] 本
发明涉及一种
移动机器人定位方法，特别涉及一种面向车间托盘搬运的多移动机器人编队协作定位方法。
背景技术
[0002] 随着
机器人技术的不断发展，人们意识到通过多机器人的合作能以更小的代价完成相对复杂的任务。采用多移动机器人编队协作托盘搬运方法可有效解决因生产车间物料的尺寸、形状、重量等因素影响，而导致搬运策略异常繁杂的问题；还可降低搬运系统对搬运移动机器人复杂功能的要求，减少搬运系统成本；此外，在搬运编队队形设计时，只需考虑标准托盘尺寸和最大载重量等因素，而不受搬运
工件复杂形状特征的影响，从而减小了搬运编队队形设计的复杂度，因此具有广泛的应用价值。
[0003] 精确的移动机器人定位，即精确地确定移动机器人的
位姿，是保证协作搬运复杂任务成功执行的关键。文献“基于有源RFID的室内移动机器人定位系统.清华大学学报:自然科学版,2010(5):673-676.”提供了一种由设置于移动机器人上的有源射频模
块读写器和RFID
电子标签构成的室内移动机器人定位系统和方法。但是该方法在面向车间环境时需要布置大量的RFID标签，导致成本过高，且定位
精度易受环境中的金属以及
水份干扰，因此该定位方法并不适用于车间环境。
发明内容
[0004] 为了克服现有移动机器人定位方法实用性差的不足，本发明提供一种面向车间托盘搬运的多移动机器人编队协作定位方法。该方法通过布置人工路标辅助移动机器人定位，且布置人工路标同时兼顾定位成本与定位精度两方面因素，在需高精度定位区域，路标布置
密度稠密，而在无需高精度定位区域，路标布置密度稀疏。移动机器人团队在搬运过程中当
传感器观测到固定路标时，移动机器人可通过观测路标定位以及机器人之间的相互观测实现高精定位。当移动机器人团队传感器观测不到固定路标时，可通过观测通道两侧边线实现定位。由于在本发明中利用车间通道辅助移动机器人定位，减少了固定路标的布置密度，因而节省了搬运系统的投入成本；且在需要高精定位的区域，通过移动机器人之间的相互协作进一步提升了搬运团队的定位精度，实用性好。
[0005] 本发明解决其技术问题所采用的技术方案：一种面向车间托盘搬运的多移动机器人编队协作定位方法，其特点是包括以下步骤：
[0006] 当固定路标在移动机器人团队传感器的观测范围时，
[0007] 步骤一、移动机器人Ri应用
里程计数据，根据上一时刻的估计，预测当前时刻移动机器人Ri的状态；
[0008] 步骤二、移动机器人Ri观测当前环境中路标及与之通信的移动机器人Rj；
[0009] 步骤三、基于移动机器人Ri对路标的预测观测值和真实观测值之间的差值，应用
扩展卡尔曼滤波方法，或粒子滤波方法更新自身估计，并估计与之通信的机器人状态，且将相应的估计发送给与之对应的移动机器人；
[0010] 步骤四、移动机器人Ri应用协方差交叉方法，或协方差联合方法，融合来自与之通信的移动机器人Rj对自身的估计。
[0011] 当移动机器人团队观测不到固定路标时，由于在车间中主干通道的两侧布置有与主干通道不同
颜色的边线，主移动机器人通过视觉传感器识别边线，确定自身相对于通道y方向的定位，以防搬运团队冲出主干通道；而主机器人在通道的x方向需依靠里程计进行定位，为了便于保持搬运编队队形，此时从移动机器人的定位依靠与主移动机器人的通信和观测主移动机器人的相对
位置来确定自身坐标定位。首先从移动机器人与主移动机器人通信获取主移动机器人的位置信息；然后通过传感器观测主移动机器人，获得相对于主移动机器人的距离和
角度；最后基于观测数据和主移动机器人的坐标位置完成从移动机器人的坐标定位。
[0012] 本发明的有益效果是：该方法通过布置人工路标辅助移动机器人定位，且布置人工路标同时兼顾定位成本与定位精度两方面因素，在需高精度定位区域，路标布置密度稠密，而在无需高精度定位区域，路标布置密度稀疏。移动机器人团队在搬运过程中当传感器观测到固定路标时，移动机器人可通过观测路标定位以及机器人之间的相互观测实现高精定位。当移动机器人团队传感器观测不到固定路标时，可通过观测通道两侧边线实现定位。由于在本发明中利用车间通道辅助移动机器人定位，减少了固定路标的布置密度，因而节省了搬运系统的投入成本；且在需要高精定位的区域，通过移动机器人之间的相互协作进一步提升了搬运团队的定位精度，实用性好。
[0013] 下面结合
附图和具体实施方式对本发明作详细说明。
附图说明
[0014] 图1是本发明方法
实施例三移动机器人协作搬运托盘实例示意图。
[0015] 图2是本发明方法布置的辅助定位装置示意图。
[0016] 图3是本发明方法实施例中移动机器人传感器示意图。
[0017] 图4是本发明方法实施例中通信机制示意图。
[0018] 图5是本发明方法实施例中搬运团队观测圆柱形路标定位示意图。
[0019] 图6是本发明方法实施例中搬运团队观测主干通道两侧黄色边线定位示意图。
[0020] 图7是本发明方法实施例三移动机器人协作搬运运动控制示意图。
具体实施方式
[0021] 参照图1-7。本发明面向车间托盘搬运的多移动机器人编队协作定位方法具体步骤如下：
[0022] 辅助定位系统由位于搬运通道两侧、具有固定间距的红色圆柱形路标以及搬运通道两侧的两侧黄色标线组成。其中，红色圆柱形路标在直道的间距为15m，在弯道的间距为5m。同时，每个路标上都标注有唯一的白色代码。移动机器人通过识别路标上的代码确定该路标在车间中的精确坐标位置。
[0023] 移动机器人携带的传感器包含
激光雷达、单目视觉传感器、里程计和
陀螺仪。其中，视觉传感器在6m的距离内可有效识别路标上的白色代码，激光雷达的有效观测范围为8m。
[0024] 多移动机器人协作搬运的通信机制设置为：主移动机器人可与上位机通信获取任务信息，从移动机器人只能与主移动机器人相互通信，从移动机器人之间不通信。
[0025] 基于托盘搬运的多移动机器人编队协作定位方法的具体实施步骤如下：
[0026] 步骤1、任务获取。上位机向主移动机器人发布任务信息：搬运任务为从D点搬运零件到O点；所需从移动机器人数目为2个；采用编队搬运队形为三角形；全局路径规划为M-J-D-E-G-L-P-O-N-M。
[0027] 步骤2、主移动机器人编队队形组织。主移动机器人在获取到任务信息后，采用拍卖的方式将搬运任务分配给2个从移动机器人，并组织从移动机器人完成三角形编队。
[0028] 步骤3、搬运任务执行。搬运团队到达托盘存放区，以编队的形式钻入转运托盘底部，并保持该队形从M点驶向搬运任务的起始点D；托起矩形零件按路径D-E-G-L-P-O执行搬运任务；货物搬运完成后从O点返回到M点。搬运过程中移动机器人的协作定位与运动控制步骤如下：
[0029] 步骤3.1、移动机器人协作定位。在移动机器人运行中，每个移动机器人t时刻的状态为Xt＝[pt,θt]，其中pt＝[xt,yt]为坐标状态，θt为朝向状态。
[0030] 步骤3.1.1、当移动机器人搬运团队视觉传感器能观测到红色圆柱形路标时，主移动机器人依靠观测路标以及移动机器人之间的互观测实现坐标状态pt定位；由于从移动机器人不能被主移动机器人观测，其采用观测路标实现坐标状态pt定位。
[0031] 1).主移动机器人通过路标与互观测定位步骤：
[0032] a)主移动机器人读取里程计数据，根据上一时刻估计的坐标状态 和其对应的协方差 预测当前时刻的坐标状态 和协方差
[0033] b)主移动机器人通过视觉传感器观测当前环境中的固定路标，并由其上的白色代码确定该固定路标在车间中的精确坐标位置；然后通过激光雷达观测该路标，测得主移动机器人相对于固定路标的距离和角度；
[0034] c)基于主移动机器人对路标的预测观测值和真实观测值之间的差值，应用扩展卡尔曼滤波方法更新主移动机器人估计得到坐标状态 和协方差
[0035] d)应用协方差交叉融合方法，融合来自于与之通信的从移动机器人1、从移动机器人2对主移动机器人的坐标估计 得到t时刻主移动机器人的坐标状态 和协方差 其中， 表示从移动机器人1估计的主移动机
器人坐标状态和协方差， 表示从移动机器人2估计的主移动机器人坐标状态和协方差。
[0036] 2).从移动机器人1、从移动机器人2坐标定位方法一致，以从移动机器人1为例：
[0037] a)从移动机器人1读取里程计数据，根据上一时刻估计的坐标状态 和其对应的协方差 预测当前时刻的坐标状态 和协方差
[0038] b)识别环境中的红色圆柱形路标，并通过激光雷达观测该路标以及主移动机器人，测得相对于该路标和主移动机器人的距离和角度；
[0039] c)基于从移动机器人1对路标的预测观测值和真实观测值之间的差值，应用扩展卡尔曼滤波方法更新从移动机器人1估计得到坐标状态 和协方差 并估计主移动机器人得到坐标状态 和协方差 且将该估计发送给主移动机器人。
[0040] 步骤3.1.2、当机器人团队的视觉传感器不能观测到红色圆柱形路标时，采用如下坐标定位策略。
[0041] 1).主移动机器人的坐标定位步骤如下：
[0042] a)主移动机器人根据上一时刻估计的坐标状态 和其对应的协方差 预测当前时刻的坐标状态 和协方差
[0043] b)主移动机器人通过视觉传感器观测主干道两侧的黄色边线，测得自身相对于两侧黄色边线的角度；
[0044] c)基于主移动机器人对边线的预测观测值和真实观测值之间的差值，应用扩展卡尔曼滤波更新y方向坐标估计得到当前时刻的坐标状态 和协方差
[0045] 2).从移动机器人1、从移动机器人2坐标定位方法一致，以从移动机器人1为例：
[0046] a)从移动机器人1读取里程计数据，根据上一时刻估计的坐标状态 和协方差预测当前时刻的坐标状态 和协方差
[0047] b)从移动机器人1与主移动机器人通信获取主移动机器人的坐标状态[0048] c)从移动机器人1通过激光雷达观测主移动机器人，获得相对于主移动机器人的距离和角度；
[0049] d)基于从移动机器人1对主移动机器人的预测观测值和真实观测值之间的差值，应用EKF更新从移动机器人估计得到坐标状态 和协方差
[0050] 步骤3.1.3、移动机器人朝向定位：通过读取陀螺仪中的数据完成t时刻移动机器人的朝向定位θt。
[0051] 步骤3.2、移动机器人协作搬运运动控制主要分为主移动机器人轨迹
跟踪控制和从移动机器人编队队形保持。
[0052] 步骤3.2.1、主移动机器人轨迹跟踪控制：主移动机器人根据t时刻位姿 和上位机规划的期望位姿 建立跟踪误差方程，利用轨迹跟踪控制
算法计算主移动机器人下一时刻的线速度vl和
角速度wl，控制主移动机器人沿着规划的线路运动。
[0053] 步骤3.2.2、从移动机器人编队队形保持：从移动机器人1、从移动机器人2编队队形保持方法一致，以从移动机器人1为例：从移动机器人1首先与主移动机器人通信，获取t时刻主移动机器人的位姿 以及与主移动机器人保持的期望距离 和期望角度 然后利用主移动机器人的位姿 期望距离 和期望角度 生成与之期望队形结构的虚拟跟随移动机器人位姿 最后，基于虚拟跟随移动机器人位姿 和从移动机器人1当前时刻位姿 建立队形误差方程，运用
控制器算法计算出从移动机器人1的线速度vf1和角速度wf1，使队形中的从移动机器人1能够与主移动机器人
维持期望队形。
