基于UWB室内定位的迎宾机器人系统研究

，,2，，,2，,2

(1.苏州大学机电工程学院，江苏 苏州 215021；2.苏州纳米科技协同创新中心，江苏 苏州 215021)

0 引言

机器人的研究、开发和应用是彰显一个国家高新技术能力的重要指标[1]，“机器人革命”有望成为“第三次工业革命”的一个切入点和增长点。根据“十三五”规划，到2020年我国服务机器人产业年销售额将超过300亿元，并重点推进服务机器人在消防、医疗、公共服务等领域的发展[2]，因此越来越多的专家、学者开始研究室内移动机器人相关技术。迄今为止，国内外学者已对类人型迎宾机器人、火灾机器人[3-5]、护理机器人等[6]室内移动机器人做了大量研究，并取得了一定的成果。

室内定位与路径规划是室内移动机器人研究的重点方向。目前，用于室内定位的方法主要包括：通过无线信号的RSS(received signal strength)信息进行位置指纹匹配，然后利用曲线拟合和位置搜索算法实现室内定位[7-8]；利用机器人视觉，基于神经递质调节发育算法等，对机器人进行强化学习，最终实现视觉定位[9]；基于激光雷达对移动机器人进行全局定位[10-11]等。但目前各种方法大都存在定位精度低、无法满足动态环境等缺点。UWB具有定位精度高、受非视距影响小等优点。因此，设计了一种基于UWB的室内定位系统，研究并设计了基于UWB定位的迎宾机器人系统，基于该系统研究机器人的路径跟踪算法，实现了机器人的室内精确定位和路径跟踪。

1 系统总体结构

本文自主研制的移动机器人系统如图1所示，主要包括圆形自主移动底盘及人形外壳、UWB定位系统、电子罗盘、运动控制系统、无刷直流电机及驱动器、433M无线通信单元和供电电源等。

图1 迎宾机器人系统组成

自主移动机器人主体部分包括移动底盘和外壳，移动底盘采用两轮差速驱动方式，安装于底盘后侧，通过控制两驱动轮的转速，实现机器人的直行和转弯运动。前侧安装2个万向轮，可以根据地面情况自动调节高度，以保证机器人在不平坦路面上的平稳性。

UWB定位系统由安装在室内固定位置的4个定位基站，以及安装于机器人正上方(与两驱动轮中心在同一垂直轴线上)的定位标签组成，实现机器人的实时定位功能。

运动控制系统主要完成自主移动机器人的直线行走、转弯等运动控制，主要由运动控制板、电子罗盘、无刷直流电机和驱动器等组成。其中，全姿态电子罗盘水平安装于移动底盘上部的中心位置，为机器人提供航向角信息。

2 机械系统

自主研制的移动机器人机械系统如图2所示，主要包括移动底板、跷跷板式万向轮机构、无刷电机、驱动轮和机器人外壳等。

图2 机器人机械系统主要部件

2个万向轮安装于移动底板的前侧位置，具有独特的跷跷板式结构。当地面倾斜或凹凸不平时，两万向轮根据地面的高度自动调节自身高度，保证机器人的平稳性。两驱动轮固定在底板上，通过联轴器与无刷电机的转动轴连接。铅蓄电池组安装于底板中心位置，确保移动底盘的重心在中心位置，并为整个系统提供能源。

移动机器人的前进和转向运动，通过两驱动轮的差速控制实现。当两驱动轮的转速相同且同向时，机器人沿直线向前行驶；当两驱动轮的转速不同但同向时，机器人向低速轮一侧转弯；当两驱动轮转速相同但反向时，机器人实现原地自转运动。

3 控制系统

机器人控制系统硬件结构如图3所示，主要包括UWB定位系统和运动控制系统。

图3 机器人控制系统硬件结构

3.1 UWB定位系统

室内复杂环境下，由于障碍物的存在以及多径效应等因素的影响，使得室内定位变得相对困难。基于UWB的定位系统因独特的通信原理，使其具有定位精度高、功耗低、抗干扰性能强和多径分辨能力强等优点。

本文研制了基于UWB的定位系统，主要包括4个定位基站和1个定位标签。UWB通信模块采用DecaWave公司的DWM1000模块，该模块基于业内精度最高的室内定位芯片DW1000。4个定位基站安装在室内的固定位置，定位标签安装在机器人顶部，为机器人提供位置信息。

UWB定位系统采用三边定位原理求解待定位标签的坐标。在实际应用环境中，由于外部环境以及系统测量误差等因素的影响，以基站为中心、标签与基站的距离为半径的圆不一定相交于同一点位置(标签位置)。因此在实际计算标签的坐标时，若相邻基站之间有相交点，则先求得相邻基站之间靠近其他基站的交点坐标；若相邻基站之间没有相交点，则求得两基站坐标的中心点坐标。然后以求得的4个坐标的均值作为待定位标签的测量坐标值。为进一步消除测量的随机误差，采用卡尔曼滤波算法对测量坐标值进行滤波处理。

UWB定位系统的程序流程如图4所示。首先测得k时刻标签与4个基站的距离R1，R2，R3，R4，求解相邻基站之间的交点或中心点坐标P1，P2，P3，P4，以4个坐标点的均值为当前时刻实际测得的标签坐标P′，经过卡尔曼滤波之后作为k时刻最终的标签坐标P。

图4 UWB定位程序流程

3.2 运动控制系统

运动控制系统以STM32F103ZET6微控制器为控制核心，其与UWB定位系统、电子罗盘以及433M无线通信模块通过RS232接口连接。无刷直流电机和驱动器均采用SPG公司的XF系列电机。运动控制器接收定位系统的位置信息以及电子罗盘的航向角信息，通过路径跟踪算法对当前位姿进行校正。

运动控制系统如图5所示，主要通过控制两驱动电机的速度实现机器人的直线行驶和原地转弯功能。在沿直线行驶时，主控制板通过定位标签获取的位置信息以及预设直线路径，计算机器人当前位置到路径的最短距离，并与电子罗盘的航向角信息一起作为路径跟踪算法的反馈输入，其输出作用于左右电机，通过同向差速控制实现机器人的直线路径跟踪。在转弯运动时，仅将期望航向角作为参考输入，通过控制左右电机反向同速转动，完成期望的转弯运动。

图5 运动控制系统

4 混合路径跟踪算法

双轮差速移动机器人路径跟踪系统是一个单输入双输出的系统。常规的PID控制器在解决这此类系统控制问题时，往往无法获得较为理想的稳态控制精度，而当系统误差较小时，线性二次型最优控制器能够获得较好的控制效果，因此提出了一种融合PID和LQR最优控制的混合路径跟踪算法。如图6所示，在较大偏差时，采用PD控制器，快速消除较大的系统误差；当误差较小时，采用LQR最优控制器，以获得更好的稳态性能。

图6 控制系统的算法流程

4.1 自主移动机器人运动学模型

移动机器人的简化模型如图7所示，主要由2个后驱动轮和2个前万向轮构成，并假定两驱动轮中心位置为机器人位置。在UWB定位系统的笛卡尔坐标系下，l为设定路径，d为左右驱动轮中心距，v为前进速度，vL为左轮转速，vR为右轮转速。假设t时刻机器人偏航距为ed，偏航角为eθ，并规定左偏为负，右偏为正。

图7 移动机器人运动学模型

机器人左右轮转速为：

(1)

系统状态变量为x=[eθed]T，则常系数状态方程为：

(2)

系统的输出方程为：

(3)

4.2 混合路径跟踪控制算法

PID控制策略因其算法简单、鲁棒性好、可靠性高等，被广泛应用于过程控制和运动控制。但对于多输入多输出系统，PID控制往往很难获得令人满意的性能。基于二次型指标函数的LQR控制策略在小偏差下具有较好的性能，因此本文提出了一种融合PID和LQR最优控制的混合路径跟踪算法。

在大偏差状态下，即eθ≥eθth或ed≥edth时，采用PID控制器将误差快速衰减至小偏差状态。控制器将偏航距ed和偏航角eθ进行加权融合，作为控制器输入，通过式(4)计算控制量u。

(4)

其中，e=q1ed+q2eθ。

在小偏差状态下，即eθ

(5)

选取加权阵Q=diag(q1,q2)，且q1>0，q2>0；R=1。

系统状态空间方程如式(2)和式(3)所示。因为

(6)

所以系统可控；rank(Q)=2，且特征值均大于零，满足二次型控制器的附加条件。

取哈密顿函数为：

(7)

则由文献[12]可知，协态方程为：

(8)

控制方程为：

(9)

黎卡提矩阵代数方程为：

-KA-ATK+KBR-1BTK-Q=0

(10)

解式(10)即可得到K值，再将其代入式(9)即可得最优控制为：

u=-R-1BTKx=-Gx

(11)

G为最优反馈增益阵。

综上所述，混合路径跟踪控制算法的控制量为：

(12)

5 实验与结果

迎宾机器人实验场地如图8所示。在长8 m、宽6 m的室内环境下，对自主研制的室内移动机器人进行了UWB室内定位实验和基于UWB室内定位的路径跟踪实验。

图8 实验场景

5.1 UWB室内定位实验结果与分析

对自主设计的室内定位系统和室内定位算法进行实验验证，实验中设置4个定位基站，其坐标分别为(0，0)、(0，500)、(500，0)、(500，500)，以cm为单位。在位置(317，217.5)、(135.5，360)、(256.5，312.5)处分别进行定位实验，实验结果分别如图9、图10和图11所示。

图9 坐标(317，217.5)的定位结果

图10 坐标(135.5，360)的定位结果

如图9、图10和图11所示，实线表示测量的原始坐标数据，虚线表示KF滤波之后的坐标数据。3个测试点的坐标定位值，经滤波之后较原始数据更加平滑；由表1可知，KF滤波之后，定位结果的标准差(STD)更小。综上，经滤波之后取得了小于1 cm的重复性定位精度。表2分别列出了10个测试位置的定位误差。由表2可知，X轴和Y轴的最大定位误差分别为-4.4 cm和-4.8 cm，其中误差均值分别为-0.4 cm和-2.7 cm，具有小于5 cm的定位精度。

图11 坐标(256.5，312.5)的定位结果

测试点X轴STDY轴STD原始数据/cmKF滤波/cm原始数据/cmKF滤波/cm11.4810.6601.6380.82221.6151.0851.6440.96131.3070.6401.4940.61641.3730.5231.4520.43051.6700.6431.4000.42561.2520.4321.1610.33171.2940.5281.4590.39181.5560.5011.6970.76991.1550.5391.3180.464101.2570.4461.4050.425

表2 10个测试点的定位误差

5.2 路径跟踪实验结果与分析

首先对提出的混合路径跟踪控制算法进行仿真实验。MATLAB数值仿真结果如图12所示。其中，设定前进速度v=0.5 m/s，系统初始状态为eθ=-0.5 rad，ed=0.5 m，加权阵Q=diag(0.8,3.5)，R=1.5。从仿真结果可知，在系统运行4 s时，系统偏差已趋近于零。

图12 路径跟踪控制器仿真结果

然后，通过迎宾机器人进行直线路径跟踪试验。设定行进速度为v=0.5 m/s，系统初始状态为eθ=-0.4 rad，ed=1 m，选取性能指标的加权阵Q=diag(0.6,3.0)，R=2.5，伺服周期Ts=0.1 s。试验结果如图13、图14、图15和图16所示。

图13 混合路径跟踪算法的动态跟踪误差响应曲线

图13和图14分别为混合路径跟踪算法和PID算法的动态响应曲线，由图可知，混合路径跟踪算法在7 s左右跟踪误差已趋于稳定，PID算法在11 s左右趋于稳定，因此混合路径跟踪算法具有更好的动态响应特性。图15和图16分别为稳态响应曲线，对比可知，混合跟踪算法稳态精度要优于PID算法。

图14 PID算法的动态跟踪误差响应曲线

图15 混合路径跟踪算法的稳态跟踪误差响应曲线

图16 PID算法的稳态跟踪误差响应曲线

综上所述，提出的混合路径跟踪算法能通过UWB定位系统提供的坐标信息，以及电子罗盘提供的航向角信息有效地跟踪设定路径，跟踪误差小于5 cm。

6 结束语

设计了一种基于UWB室内定位系统和一种基于UWB室内定位的迎宾机器人系统。将定位标签安装于迎宾机器人上方位置，通过定位系统输出的位置信息以及电子罗盘输出的航向角信息进行室内直线路径跟踪。实验结果表明，所设计的移动机器人软硬件系统运行稳定，室内定位系统定位精度小于5 cm，重复精度小于1 cm，能有效消除外界干扰；基于UWB定位的混合路径跟踪算法能够准确跟踪直线路径，跟踪误差小于5 cm，实时性高。本文的室内定位技术研究不仅可用于迎宾机器人，还可应用于其他室内移动机器人和工业搬运机器人等。