一种机器人小车动态跟踪定位系统及方法
【专利摘要】本发明提供一种机器人小车动态跟踪定位系统及方法，定位系统包括超高频无线电遥控标签、主控板和至少两个超高频无线电基站；其中：各个基站布置于机器人小车上，且各个基站的连线为设定的几何图形；可移动的遥控标签携带于车主的固定位置；所述主控板安装于机器人小车的固定基座上，所述遥控标签与各个基站通信测距，各个基站进一步与主控板通信，根据获得的测距数据得出机器人小车相对于车主的方位。本发明实现了机器人小车相对于车主的准确定位。
【专利说明】
一种机器人小车动态跟踪定位系统及方法
技术领域
[0001] 本发明涉及机器人小车的自主跟随领域，具体涉及一种机器人小车动态跟踪定位 系统及方法。
【背景技术】
[0002] 现有机器人小车多采用遥控模式运动，操控者的双手被遥控器占用而无法进行其 他活动。若使机器人小车在非遥控状态下自主随行操控者运动，则需要机器人小车具有实 时判断自己与操作者相对位置的能力。而由于机器人小车与操作者的距离往往较小，甚至 实际距离的数值本身都属于误差范围，若小车基于采用传统的如直线测距或者GPS定位等 方式计算出的定位结果对操作者进行跟随，往往存在一定程度的偏差甚至错误。
【发明内容】
[0003] 有鉴于此，本发明提供一种机器人小车动态跟踪定位系统及方法，用于准确确定 机器人车和被跟随者(车主）的相对位置。
[0004] 本发明采用的技术方案具体为：
[0005] -种机器人小车动态跟踪定位系统，包括超高频无线电遥控标签、主控板和至少 两个超尚频无线电基站;其中：
[0006] 各个基站布置于机器人小车上，且各个基站的连线为设定的几何图形；可移动的 遥控标签携带于车主的固定位置；
[0007] 所述主控板安装于机器人小车的固定基座上，所述遥控标签与各个基站通信测 距，各个基站进一步与主控板通信，根据获得的测距数据得出机器人小车相对于车主的方 位。
[0008] 在上述机器人小车动态跟踪定位系统中，各个所述基站处于同一高度平面内。一 种机器人小车动态跟踪定位方法，包括如下步骤：
[0009] S10)通信测距步骤：
[0010] 以设定的几何图形布置于机器人小车上的n个基站和遥控标签周期性地通信测 距，各个基站根据接收到的测距信号得到其与遥控标签之间的距离分别为dl~dn，基于优 化算法对测距数据进行优化；以及
[0011] 评估各个基站接收到测距信号的质量等级；
[0012] S20)基站筛选步骤：
[0013] 各个基站将优化后的测距数据及其对应的测距信号的质量等级发送到主控板，主 控板选择质量等级符合要求的测距信号对应的基站作为有效定位数据基站，进行测距；
[0014] S30)定位步骤：
[0015] 主控板分别确定各个有效定位数据基站相对于小车的方位，并基于各个方位对应 的方位进一步确定出被跟随者相对于小车的位置；
[0016] 主控板通过与各基站周期性地通信，获取新的测距数据以及对应的测距信号的质 量等级；
[0017] 重复上述步骤，重新确定机器人小车相对被跟随者的位置。
[0018] 在上述机器人小车动态跟踪定位方法中，在步骤S10)中，测距数据的优化包括：
[0019] 针对环境温度和供电电压的影响，每次测距时，首先读取基站收发芯片的温度和 供电电压，然后根据温度和电压数值的变化量给出对应的补偿测量数据；
[0020] 为减小测距数据的波动性，对基站的测距数据进行限幅均值的滤波优化;具体地：
[0021] 各个基站和遥控标签周期性通信测距，得到其与遥控标签之间的距离分别为dl~ dn;具体地：
[0022]首先每个周期内，各个基站分别测一组N个与遥控标签之间的距离数据，如第一个 基站的距离数据为SN，…，33,32,31，如果本次测得的距离31和上次测距值32相比没有超出 预设的阈值X，则不对数据S1进行处理，反之则将S1的值替换为S1 = S2+X或者S1 = S2-X，对 其进行限幅操作；
[0023]其次对经限幅操作后的该组数据求均值，得到本周期的最终dl，即：dl = (Sl+S2 +---+SN)/N；
[0024]基于同样的原理，分别获得其他基站与遥控标签之间的距离。
[0025]在上述机器人小车动态跟踪定位方法中，基站收发芯片读取接收到的测距信号的 首达路径幅值A和平均噪声幅值M，定义测距信号的质量等级=A/M。本发明产生的有益效果 是：
[0026]本发明的动态跟踪定位方法原理简单，易于实现(计算量小，实现成本低），且可在 运动或者静止状态下，精度较高地对机器人小车相对车主的方位进行定位实现跟随。
【附图说明】
[0027]当结合附图考虑时，能够更完整更好地理解本发明。此处所说明的附图用来提供 对本发明的进一步理解，实施例及其说明用于解释本发明，并不构成对本发明的不当限定。 [0028]图1为本发明一种机器人小车动态跟踪定位方法的逻辑框图；
[0029]图2为本发明一种机器人小车动态跟踪定位方法的两个基站定位的原理图；
[0030]图3为两个基站无法定位的原理示意图；
[0031]图4为本发明一种机器人小车动态跟踪定位方法的三个基站定位原理图。
【具体实施方式】
[0032]下面结合附图及实施例对本发明的技术方案作进一步详细的说明。
[0033]本发明基于超高频无线电技术确定出车主相对机器人小车的方位之后，进一步实 现了机器人小车对车主的动态跟踪定位，动态定位系统主要包括超高频无线电遥控标签、 主控板和至少两个超尚频无线电基站(本实施例中以二个以基站为例）；其中：
[0034]主控板安装在机器人小车的固定基座上，三个基站对应等腰三角形的顶点分布在 机器人小车上，且三个基站处于同一高度平面内，移动的遥控标签则由车主(被跟随者)携 带。
[0035]实际应用中，可以根据精度要求，在每个基站位置处布置一组基站，或者也可以进 一步增加如每条边的中点等位置，或者以正方形、规则多边形层图形为基础来布置。采用两 个基站可进行180°范围内的定位，三个及以上的基站则既能实现180°范围内的定位，又可 实现360°范围的定位。当基站的个数越多，冗余的数据会提高定位可靠性和精度。
[0036]动态定位系统实现机器人小车定位相对车主的定位过程如图1所示，具体为：
[0037] S10)通信测距步骤：
[0038]三个基站和遥控标签周期性地通信测距，各个基站得到其与遥控标签之间的距离 分别为dl~d3,并基于优化算法对测距数据进行优化。同时评估各个基站接收到的测距信 号的质量等级;具体地：
[0039] 超高频无线电测距是基于无线信号在空中传输时间和无线信号的传输速度计算 距离。因为无线信号的传输速度非常快，传输时间测量上微小的误差都会引起测量数据的 极大变化。而机器人小车和被跟随者之间距离较短，采用超高频无线电直接测距误差较大， 且超高频无线电测距易受环境温度和供电电压的影响，因此需要对测距数据进行优化。
[0040] 针对环境温度以及供电电压等因素的影响，每次测距时，首先读取基站收发芯片 的温度和供电电压，然后根据温度和电压数值来相应的补偿测量数据。如：以环境温度30 °C，供电电压为3.3V时测得的数据作为基准数据，实际供电电压每变化IV，则测量数据对 应地变化3.35cm，温度每变化1°C，则测量数据对应的变化2.15cm。实际应用中，补偿参数以 及具体的补偿规则都可以根据实验数据或者基于经典理论进行设定。
[0041] 另外，为减小测距数据的波动性，本发明采用限幅均值滤波的优化算法对基站的 测距数据进行优化;具体地：
[0042]首先，对超高频无线电测距数据进行限幅操作，即：在周期性测距的过程中，在每 个周期内测一组数据（以每个周期内测32次为例），本时刻测得距离为S1，上一时刻测得距 离为S2，限幅阈值为X，则：
[0043] i)如果|S1_S2|彡X，变化值小于限幅阈值X，则不对S1进行额外处理；
[0044] ii)如果(S1_S2)>X，即S1大于S2且变化值大于限幅阈值X，则将S1的变化限定在X 内，S1 = S2+X;
[0045] iii)如果(S1-S2)〈-X，即S1小于S2且变化值小于限幅阈值X，则将S1的变化限定在 X内，Sl = S2-X〇
[0046] 上述用于判定优化数据的限幅阈值X的设定，主要依据测距频率和人的步行速度 决定。
[0047]其次，分别对测出的一组数据进行限幅操作之后，求均值得到最终的dl，即：dl = (Sl+S2+，"+S32)/32。
[0048]基于同样的原理，分别获得其他两个基站与遥控标签之间的距离d2、d3。
[0049] 各个基站的测距信号的质量等级的确定可以为：
[0050] 基站收发芯片读取接收到的首达路径测距信号的幅值A和平均噪声幅值M，如可以 将测距信号的质量等级定义为A/M，测距信号的质量等级值越大，则表示该基站的信号越可 与巨〇
[0051] 当然，也可以用其他能表征信号可靠性的相关参数或者参数的组合来得出各个基 站的测距信号的质量等级的优劣。
[0052] S20)基站筛选步骤：
[0053]各个基站将优化后的测距数据及其对应的测距信号的质量等级发送到主控板，主 控板选择质量等级较高的测距信号对应的基站作为有效定位数据基站进行测距。
[0054] 如果三个基站数据都符合预设质量标准时，则将其均作为有效定位数据进行定 位;此外:若只有两个或者一个基站的测距数据符合预设质量标准时，选用符合预设质量标 准的基站数据，不符合预设质量标准的，追溯其上一次符合预设质量标准的数据；
[0055] 若三个基站数据都不符合预设质量标准，则沿用上一时刻的测距数据用于定位。
[0056] S30)定位步骤：
[0057]筛选出有效定位数据后，主控制板运行定位算法，根据基站定位的原理确定机器 人小车与标签携带者的位置(距离、方向）。
[0058]重复上述步骤，重新确定机器人小车相对被跟随者的位置。
[0059] 两个基站定位的原理为：
[0060]如图2所示，两个基站平行布置于小车前进方向的一侧，两个基站依据自己的测距 距离，可以各自确定一个圆，两个圆相交，能确定两个交点，这两个交点既是标签可能的位 置。两个基站的定位方式无法在二维平面唯一确定一个定位点，因此两个基站的布置只能 用于确定方向（180°)的定位，如图2红色阴影部分所示，则能唯一确定定位点1为标签的位 置。
[0061]假设智能小车两个基站间距为Da，基站1和标签的距离为R1，基站2和标签的距离 为R2。以两个基站的中心为坐标轴零点建立笛卡尔直角坐标系，坐标系方向遵循右手法则， 由此可以计算出标签所在点的x轴和y轴坐标x，y;标签与基站连线中点的夹角0 (如图2所 示，〇〈0〈18〇);标签和基站连线中点的距离R。计算如下：
(1) (2) (3) (4)
[0066] 定位的过程中，可能会出现如图3所示无法定位的情况;即
[0067] 基站（1、2)的测距数据分别为R1和R2,由于测距误差的因素，以R1和R2为半径的2 个圆没有交点，定位失败。
[0068] 如本发明可以通过两个基站测距数据相互逼近的方法解决此类定位失败的情况。
[0069 ]当R1〈 R 2，通过计算无法得到定位点相应的x和y值，说明此时因测距误差无法定 位，误差来源于R1的测距比实际值偏小，而R2的测距比实际值偏大，两者差距太大导致定位 失败。此时将R1增加一个预设值C(R1 = R1+C)，R2减小一个预设值C(R2 = R2-C)，然后使用 新的R1和R2进行坐标运算。如果依然不能得到x和y值，说明定位依然不成功，重复上述测距 数据相互逼近的算法，直至算出x和y值。然后依据x和y值计算角度9和距离R。
[0070] 当R1>R2时，互相逼近的原理相同，在此不再赘述。在此基础上，进一步说明三个基 站的定位原理：
[0071] 如图4所示，基站（1、2、3)以等腰三角形的方式分布在智能小车上，基站（1、2)平行 地布置在小车行进方向的一侧，相距Da;基站3则布置在基站（1、2)的中线上，与基站1和基 站2连线的中点之间的距离为Db。设基站1和基站2连线中点为坐标轴零点，则基站（1、2、3) 的坐标分别为:基站l(-Da/2,0)，基站2(Da/2,0)，基站3(0，-Db)。三个基站的测距距离分别 为:Rl，R2，R3〇
[0072]基站1和基站2依据前述两个基站的定位计算方式，可以计算出两个定位点，其坐 标分别为定位点1 (x，y)，定位点2(x，-y)。
[0073] 此时可以计算出定位点1和定位点2到基站3的距离P1和P2:即
(5) (6)
[0076]比较PI、P2和基站3的测距值R3的差值的绝对值C1和C2;即
[0077] C1=|P1_R3| (7)
[0078] C2=|P2_R3| (8)
[0079] 如果C1〈C2,则定位点1则是最终计算的标签位置;反之，定位点2是最终计算的标 签位置。
[0080] 确定出唯一的定位点后，则依据其二维坐标，可按照上述两个基站定位情况的相 同计算方式，计算出定位点的角度e和距离R。此时利用三个基站的定位数据可实现360°范 围内的全向定位，定位角度0值的变化范围是_179〈0〈18〇°。
[0081] 以上结合附图对本发明的实施例进行了详细地说明，此处的附图是用来提供对本 发明的进一步理解。显然，以上所述仅为本发明较佳的【具体实施方式】，但本发明的保护范围 并不局限于此，任何对本领域的技术人员来说是可轻易想到的、实质上没有脱离本发明的 变化或替换，也均包含在本发明的保护范围之内。
【主权项】
1. 一种机器人小车动态跟踪定位系统，其特征在于，包括超高频无线电遥控标签、主控 板和至少两个超高频无线电基站;其中： 各个基站布置于机器人小车上，且各个基站的连线为设定的几何图形； 可移动的遥控标签携带于车主的固定位置； 所述主控板安装于机器人小车的固定基座上，所述遥控标签与各个基站通信测距，各 个基站进一步与主控板通信，根据获得的测距数据得出机器人小车相对于车主的方位。2. 根据权利要求1所述的机器人小车动态跟踪定位系统，其特征在于，各个所述基站处 于同一高度平面内。3. -种机器人小车动态跟踪定位方法，其特征在于，包括如下步骤： S10)通信测距步骤： 以设定的几何图形布置于机器人小车上的η个基站和遥控标签周期性地通信测距，各 个基站根据接收到的测距信号得到其与遥控标签之间的距离分别为dl~dn，基于优化算法 对测距数据进行优化；以及 评估各个基站接收到测距信号的质量等级； S20)基站筛选步骤： 各个基站将优化后的测距数据及其对应的测距信号的质量等级发送到主控板，主控板 选择质量等级符合要求的测距信号对应的基站作为有效定位数据基站，进行测距； S30)定位步骤： 主控板分别确定各个有效定位数据基站相对于小车的方位，并基于各个方位对应的方 位进一步确定出被跟随者相对于小车的位置； 主控板通过与各基站周期性地通信，获取新的测距数据以及对应的测距信号的质量等 级； 重复上述步骤，重新确定机器人小车相对被跟随者的位置。4. 根据权利要求3所述的机器人小车动态跟踪定位方法，其特征在于，在步骤S10)中， 测距数据的优化具体为： 针对环境温度和供电电压的影响，每次测距时，首先读取各基站收发芯片的温度和供 电电压，然后根据温度和电压数值的变化量给出对应的补偿测量数据； 为减小测距数据的波动性，对基站的测距数据进行限幅均值的滤波优化;具体地： 各个基站和遥控标签周期性通信测距，得到其与遥控标签之间的距离分别为dl~dn; 具体地： 首先每个周期内，各个基站分别测一组N个与遥控标签之间的距离数据，如第一个基站 的距离数据为31 -，33,32,31，如果本次测得的距离31和上次测距值32相比没有超出预设 的阈值X，则不对数据Sl进行处理，反之则将Sl的值替换为SI = S2+X或者SI = S2-X，对其进 行限幅操作； 其次对经限幅操作后的该组数据求均值，得到本周期的最终dl，即：dl = (Sl+S2+··· + SN)/N； 基于同样的原理，分别获得其他基站与遥控标签之间的距离。5. 根据权利要求3所述的机器人小车动态跟踪定位方法，其特征在于，基站收发芯片读 取接收到的测距信号的首达路径幅值A和平均噪声幅值M，定义测距信号的质量等级=A/M。
【文档编号】G01S5/02GK105891774SQ201610188377
【公开日】2016年8月24日
【申请日】2016年3月29日
【发明人】王治彪, 毕树生, 杨春卫, 牛传猛
【申请人】北京九星智元科技有限公司