一种机器人室内定位方法与流程

本发明涉及地面移动机器人在室内环境中的导航控制领域，更具体地涉及一种机器人室内定位方法。

背景技术：

现有智能移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标的自主导航运动，从而完成预定任务的机器人系统。要实现机器人自主导航运动，必须要解决环境建模、实时定位、路径规划、运动控制等一系列问题；其中，移动机器人必须具有定位的能力，其目的就是确定机器人在运行环境中相对于世界坐标系的位置及航向。

目前存在多种定位技术，例如GPS定位技术、惯性导航系统定位技术以及超宽带(UWB，Ultra Wide Band)技术等。然而，在室内环境下，机器人无法使用GPS进行定位，而使用惯性导航系统成本较高且不能保证长时间定位精度。

超宽带技术具有对信道衰落不敏感、发射信号功率谱密度低、低截获能力等特点，能够提供分米级定位精度。然而，在室内环境下，由于空间尺寸较小，无线系统多径效应强，高精度定位部署困难；并且，由于建筑支柱、隔断墙体、玻璃、家具等遮挡物体的存在，更加恶化了无线定位的工作环境。因此，机器人在室内环境下自主移动不能单纯依靠超宽带定位系统，应与其他定位方式进行融合。

另外，一般室内机器人由两个直流伺服电机驱动左右主动轮，通过左右轮差速实现转向运动。当以安装在两个伺服电机轴端的光电编码器输出进行航位推算时，由于电机减速器存在间隙，而且车轮与地面之间不可避免的存在打滑现象，尤其在转弯时滑动更为明显。因此航位推算的轨迹为理想的输出轨迹，在转弯时航向误差有明显增大的趋势。

技术实现要素：

鉴于以上问题，本发明提供了一种结合了超宽带定位和航位推算的特点的机器人室内定位方法：当超宽带信号正常时，采用无迹卡尔曼滤波(Unscented Kalman Filter，UKF)方法对超宽带、编码器、电子罗盘信息进行数据融合，在获得位置、航向的同时对航位推算的航向失准角进行估算；当超宽带信号异常时，通过航位推算进行位姿解算，由于失准角可以实时补偿，因此可以保证航位推算的定位定向精度。

根据本发明的一方面，提供了一种机器人室内定位方法，包括：

数据存储步骤，用于存储上一时刻的编码器数据以及超宽带数据；

数据采集步骤，用于通过设置在驱动所述机器人的左右轮的电机的轴端的光电编码器采集当前时刻的编码器数据以及通过超宽带定位系统采集当前时刻的超宽带数据；

增量数据计算步骤，用于根据所存储的上一时刻的编码器数据和超宽带数据以及所采集的当前时刻的编码器数据和超宽带数据来计算里程增量和位置增量；以及

定位信息评估步骤，用于根据所计算的里程增量和位置增量来判断所述编码器数据和所述超宽带数据是否异常，从而确定使用无迹卡尔曼滤波(UKF)方法、航位推算方法或由所述超宽带定位系统获得的超宽带位姿信息来获得所述机器人在当前时刻的定位信息。

根据实施例，所述定位信息评估步骤包括：如果所述里程增量为零并且所述位置增量不为零，则判断所述编码器数据正常并且所述超宽带数据异常，从而确定使用所述航位推算方法来获得所述机器人在当前时刻的位置和航向，其中使用所述航位推算方法来获得所述机器人在当前时刻的位置和航向包括：装订航向失准角的步骤，以使得所述航位推算方法使用当前时刻的编码器数据和已装订的航向失准角来计算所述机器人在当前时刻的位置和航向。

根据实施例，所述定位信息评估步骤包括：如果所述位置增量小于预定值并且所述里程增量的绝对值大于等于预定倍数的所述位置增量，则判断所述编码器数据异常并且所述超宽带数据正常，从而使用所述超宽带位姿信息获得所述机器人在当前时刻的位置以及由所述电子罗盘确定的当前时刻的航向。

根据实施例，所述定位信息评估步骤包括：如果所述里程增量的绝对值小于预定值并且所述位置增量大于等于预定倍数的所述里程增量的绝对值，则判断所述编码器数据正常并且所述超宽带数据异常，从而确定使用所述航位推算方法来获得所述机器人在当前时刻的位置和航向，其中使用所述航位推算方法来获得所述机器人在当前时刻的位置和航向包括：装订航向失准角的步骤，以使得所述航位推算方法使用当前时刻的编码器数据和已装订的航向失准角来计算所述机器人在当前时刻的位置和航向。

根据实施例，所述定位信息评估步骤包括：如果所述里程增量的绝对值小于预定值并且所述位置增量小于预定倍数的所述里程增量的绝对值，则判断所述编码器数据正常并且所述超宽带数据正常，从而使用所述无迹卡尔曼滤波(UKF)方法来获得所述机器人在当前时刻的位置和航向，其中使用所述无迹卡尔曼滤波(UKF)方法来获得所述机器人在当前时刻的位置和航向包括通过设置在机器人上的电子罗盘采集当前时刻的航向数据的步骤，以使得所述无迹卡尔曼滤波(UKF)方法使用当前时刻的编码器数据、超宽带数据以及航向数据来计算所述机器人在当前时刻的位置、航向以及航位推算失准角。

根据实施例，在判断所述编码器数据正常并且所述超宽带数据异常的情况下，如果连续多次满足所述里程增量的绝对值小于预定值并且所述位置增量小于预定倍数的所述里程增量的绝对值，则判断所述编码器数据正常并且所述超宽带数据正常，从而使用所述无迹卡尔曼滤波(UKF)方法来获得所述机器人在当前时刻的位置和航向，其中使用所述无迹卡尔曼滤波(UKF)方法来获得所述机器人在当前时刻的位置和航向包括通过设置在机器人上的电子罗盘采集当前时刻的航向数据的步骤，以使得所述无迹卡尔曼滤波(UKF)方法使用当前时刻的编码器数据、超宽带数据以及航向数据来计算所述机器人在当前时刻的位置、航向以及航位推算失准角。

根据实施例，所述定位信息评估步骤包括：如果所述里程增量的绝对值大于等于预定值并且所述位置增量大于等于所述预定值，则判断所述编码器数据异常并且所述超宽带数据异常，从而停止计算所述机器人在当前时刻的位置数据和航向数据并且输出上一时刻的位置和航向。

根据实施例，所述数据存储步骤包括：存储所获得的所述机器人在当前时刻的位置、航向以及航向失准角以用于下一时刻的计算。

根据本发明的实施例，所述倍数优选为10倍，并且所述预定值优选为15cm。

根据本发明的实施例，所述里程增量ds根据以下公式计算：

其中，L＝π·D·η·P为航位推算的比例系数，其中编码器精度为P(单位：每脉冲转数PPR)，驱动电机的减速比为η，车轮直径为D(单位：m)，两车轮轮间距为w(单位：m)；并且N_l和N_r为上一时刻k-1至当前时刻k左右车轮编码器输出的脉冲增量。

根据本发明的实施例，所述位置增量dl根据以下公式计算：

其中，(x_k-1，y_k-1)为前一时刻k-1的超宽带位置，(x_k，y_k)为当前时刻k的超宽带位置。

根据本发明的实施例，所述航位推算方法根据以下公式来获得机器人在当前时刻的位置数据和航向数据：

其中，(x_dk，y_dk)和为当前时刻k的机器人的位置和航向，(x_dk-1，y_dk-1)和为上一时刻k-1的机器人的位置和航向，并且为航向偏差，是高斯白噪声。

附图说明

图1是根据本发明实施例的机器人室内定位方法的总体流程图。

具体实施方式

图1示出根据本发明实施例的机器人室内定位方法的总体流程图。如图1所示，在已经获得上一时刻的编码器数据以及超宽带数据的情况下，通过设置在驱动机器人的左右轮的电机的轴端的光电编码器来采集当前时刻的编码器数据以及通过超宽带定位系统采集当前时刻的超宽带数据，采样间隔例如为100ms。基于上述数据分别计算采样间隔内的里程增量ds和位置增量dl。根据ds、dl判断定位信息可用性，并采取不同的定位策略：

当根据超宽带数据判断航位推算异常时，通常是车轮严重打滑或空转，此时以超宽带位姿信息为准；

当超宽带数据和编码器数据均在有效值范围内，此时通过UKF进行组合定位解算，并估算航位推算的失准角；

当根据编码器信息判断超宽带数据异常时，可能超宽带信号受到遮挡，或到达信号有效区域边界，此时以航位推算位姿信息为准(需初始装订航位失准角)。

以下详细说明ds、dl的计算、定位信息的评估、UKF算法以及航位推算方法：

1、ds、dl计算方法

记两个编码器精度为P(单位：每脉冲转数PPR)，驱动电机的减速比为η，车轮直径为D(单位：m)，两车轮的轮间距为w(单位：m)。令L＝π·D·η·P，为航位推算的比例系数。设k-1至k时刻左右车轮编码器输出的脉冲增量为N_l和N_r，则根据编码器信息计算的里程增量ds的计算公式如下：

设k-1时刻超宽带位置为(x_k-1，y_k-1)，k时刻超宽带位置为(x_k，y_k)，则根据超宽带信息计算的位置增量dl的计算公式如下：

2、定位信息评估

根据ds和dl的定位信息评估准则如下：

如果ds＝0，dl≠0，机器人应处于静止状态，而超宽带由于信号扰动存在位置跳变，视为“编码数据正常，超宽带数据异常”，定位信息以航位推算为准；

如果dl<15cm，|ds|≥10dl，机器人存在打滑或空转现象，判定为“编码数据异常，超宽带数据正常”，定位信息以超宽带定位为准；

如果|ds|<15cm，dl≥10|ds|，认为超宽带数据存在较大跳变，判定“编码数据正常，超宽带数据异常”，定位信息以航位推算为准；

在“编码数据正常，超宽带数据异常”状态下，如果连续10次满足|ds|<15cm，dl<10|ds|，则认为超宽带恢复正常，判定“编码数据正常，超宽带数据正常”，定位信息以UKF为准；

如果|ds|≥15cm，dl≥15cm，机器人运行超过最大速度上限，可能已处于失控状态，停止位置解算并且输出上一时刻的位置和航向。

3、UKF算法

(1)状态方程和观测方程

滤波状态其中(x，y)为机器人的位置坐标；为机器人当前航向，包括根据航位推算获得的机器人航向和由于传动间隙和打滑造成的航位推算的航向偏差状态方程如下：

其中，是高斯白噪声，COV[w(k)，w(l)]＝Q·δ_kl。N_R、N_L分别为左右轮驱动电机的编码器脉冲，可看作状态方程的控制变量。上述状态方程可简记为：

取测量值Z＝[z₁，z₂，z₃]分别为超宽带测量的XY轴坐标，和电子罗盘测量的航向(已转换到超宽带定位坐标系下)。则测量方程如下：

Z(k)＝H·X(k)+v(k) (8)

其中，

测量噪声v＝[v₁，v₂，v₃]为高斯白噪声，COV[v(k)，v(l)]＝R·δ_kl。

将UKF计算过程分为Sigma点计算、一步预测和测量更新三部分，滤波的过程就是这三部分运算的反复迭代。

(2)Sigma点计算

Sigma采样点计算公式如下：

尺度因子λ＝α²(n+κ)-n，其中n为状态维数,κ为次级尺度因子，通常取为0。为第k步时状态误差方差阵，则表示矩阵的平方根矩阵的第i行。

利用各采样点计算的权重设定如下：

其中β＝2。当用于向量估计时，选用加权系数当用于矩阵估计时，选用加权系数

(3)一步预测

(4)测量更新

4、航位推算

k时刻根据机器人的位置(x_dk，y_dk)和航向的递推公式如下：

其中，(x_dk-1，y_dk-1)和为上一时刻k-1的机器人的位置和航向，并且为航向偏差，是高斯白噪声。

如上所述，本发明提供了一种结合了超宽带定位和航位推算的特点的机器人室内定位方法：当超宽带信号正常时，采用无迹卡尔曼滤波(Unscented Kalman Filter，UKF)方法对超宽带、编码器、电子罗盘信息进行数据融合，在获得位置、航向的同时对航位推算的航向失准角进行估算；当超宽带信号异常时，通过航位推算进行位姿解算，由于失准角可以实时补偿，因此可以保证航位推算的定位定向精度。

显然，上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而这些属于本发明的精神所引伸出的显而易见的变化或变动仍处于本发明的保护范围之中。