室内主动式服务机器人控制系统研究与设计

李 艳, 鱼 晨, 戴庆瑜

(1.陕西科技大学 电气与控制工程学院， 陕西 西安 710021; 2.陕西农产品加工技术研究院, 陕西 西安 710021)

0 引言

我国已经进入老龄化社会，越来越多的老年人面临独居的问题.目前因人口老龄化导致劳动力严重不足，从事护理工作的人员数量已经无法满足老龄化社会的需求[1].服务机器人能够有效解决护工资源短缺的问题，更加便捷的为独居老人提供看护与服务[2].因此，国内外大量学者对服务机器人展开了研究.赵东辉等[3]将传感网络与服务机器人相结合搭建了智能家居环境，有效提升了老年人日常生活的便捷性.穆小奇等[4]研发了一种助行机器人并建立系统动力学模型，对其动态稳定性展开了研究.有效解决了现有助行机器人防跌倒措施不完善的问题.Yoann Charlon等[5]介绍了一种用于室内定位和老年人健康监测的嵌入式衣服遥测系统，通过将移动节点嵌入衣服中完成对老人步态速度、运动时间、行走轨迹的监测，进一步用于临床应用.陈思宇等[6]结合物联网等技术研发了一款具备语音交互、康复游戏等互动功能的陪伴服务型机器人.然而现有的助老服务机器人普及度不高，同时存在被动服务、操作复杂等缺点，不能主动的对老人进行服务.

因此，本文针对独居老人记忆力衰退容易忘记服药以及突发疾病需立刻服药(心肌梗塞、心绞痛等疾病发病时，需要立刻停止一切活动并尽快服用速效救心丸等药物来缓解症状.)等问题.在研究超宽带(Ultra Wide Band，UWB)定位技术、路径规划技术在室内复杂环境的应用基础上，以履带式移动机器人为开发平台，结合可穿戴健康设备设计了一种面向独居老人的主动式服务机器人，它能在到达服药时间或监测到体征异常时主动为老人送药，能有效保障老人的安全与健康.

1 系统总体方案设计

根据对服务机器人系统的功能需求分析，搭建系统如图1所示的结构框图.整个系统分为三个部分：①服务对象部分：由UWB定位标签1、智能手环组成.②室内架设部分：由UWB定位基站A、B、C组成.③服务机器人部分：由机器人主控制器、激光雷达、UWB定位标签2、无线通信模块、时钟模块、语音播放模块、电机驱动控制器、直流电机、锂电池供电模块组成.

系统的工作原理为：时钟模块为服务机器人提供时间信息，在到达服药时间时，通过语音播放模块进行语音播报，提醒老人服药；无线通信模块用于获取智能手环监测到的老人体征信息以及在异常情况时向外界发送求助信息；UWB定位标签与各基站完成实时信息交互测得两者之间的距离，在到达服药时间或体征异常时，通过相应定位算法计算出机器人和老人的位置，主控制器通过路径规划算法规划最优路径，并向电机驱动控制器发送移动指令，电机驱动控制器输出脉宽调制波(Pulse Width Modulation,PWM)控制服务机器人的左右电机，实现自主移动，激光雷达用于对突然出现的障碍物进行探测.锂电池模块为服务机器人系统持续工作提供电能.远程调试单元用于实现对控制器的远程调试与编程.

图1 系统整体设计框图

2 UWB室内定位与路径规划技术

2.1 UWB室内定位

室内定位是指在室内环境中确定出目标的位置信息，目前常见的有蓝牙、UWB、超声波、WiFi等技术.能够准确的获取老人及自身位置信息是服务机器人完成基本功能的前提.相比于室外空旷环境，室内环境较为复杂，因此需要具有较高的定位精度以及较强的穿透能力.UWB技术是通过发送和接收具有纳秒或纳秒级以下的极窄脉冲来传输数据，具有定位精度高、穿透能力强、抗干扰效果好等优点[7].UWB室内定位可分为测距和定位两个阶段.在测距阶段，利用飞行时间法(Time Of Flight，TOF)计算出各基站与标签之间的距离信息；定位阶段则是在已知各基站与标签之间的距离后通过相应定位算法求解标签的位置坐标.

2.1.1 TOF测距及其误差分析

TOF测距法是通过测量设备间信号的飞行时间，再乘以电磁波的传播速度来计算距离(即d=TF-C)，由于要获取到精确的时间戳，因此需要基站与标签之间时钟严格同步[8].目前常用的有：单边双向测距(Single-sided Two Way Ranging，SS-TWR)、对称双边双向测距(Symmetrical Double-Sided Two Way Ranging，SDS-TWR).

(1)SS-TWR及误差分析

SS-TWR是一种最简单的能够消除设备之间时钟不同步的测距方法.其测距原理如图2所示.

图2 SS-TWR测距

在图2中，设备A在t1时刻向设备B发送测距请求，设备B在t2时刻收到该请求后经处理延时TR，在t3时刻向设备A发送ACK信号，设备A在t4时刻收到该信号，记整个过程耗时为TA，信号飞行时间为TF.则有：

(1)

(2)

故测距误差Δd可表示为：

Δd=ΔTF·C

(3)

由式(2)和(3)可以得出，测距误差Δd与两设备之间的时钟漂移比例之差呈线性相关，且Δd随着TR的增大而增大.

(2)改进SDS-TWR及误差分析

针对SS-TWR方法中时钟漂移引起的计时误差，采用SDS-TWR算法对该误差具有一定的削弱作用[9].其测距原理如图3所示.

图3 SDS-TWR测距

SDS-TWR是在SS-TWR的基础上又进行了一次设备B发起的反向测距.它能进一步克服时钟漂移的影响，但是该方法测距所需时间是原方法的两倍多.而改进SDS-TWR测距算法在发送上一测距过程确认信息的同时发送下一过程的测距请求，既克服了时钟漂移带来的计时误差，又减少了测距所需时间.其测距原理如图4所示.

图4 改进SDS-TWR测距

(4)

(5)

故测距误差Δd′可表示为：

(6)

在如图5所示的环境中对测距算法效果进行验证.在0～12 m范围内每隔1 m作为一次测距点，每个点位取50组数据，取其平均值作为测距结果，得到图6所示的测距误差分布.

图5 测距试验场地

图6 测距误差分布

由图6可知，相比于SS-TWR算法，改进SDS-TWR算法在减少了测距时间的同时也提高测距精度.且两种测距方法的实际误差与前文的理论分析相符合.

2.1.2 改进三边测量定位算法

在获得各基站与标签之间的距离信息后，利用三边测量定位算法求解标签坐标，其几何模型如图7所示.

图7 三边测量几何图示

在理想情况下，信号在传播过程中不存在干扰，三个圆的交点即为标签O所在位置，设其坐标为(x,y)，已知图中的三个基站BS1、BS2、BS3的坐标为(x1,y1)、(x2,y2)、(x3,y3)：测得标签O到各个基站的距离为R1、R2、R3.故有几何关系：

(7)

求解方程组(7)即可得到标签O的位置坐标.然而室内环境较为复杂，测距结果易受多径效应、非视距(Non-Line Of Sight，NLOS)干扰的影响，使得测量值往往大于真实值，导致三个圆两两相交产生一个公共区域，实际测量图如图8所示.

图8 三边测量实际图示

此时方程组无解，无法求出标签的位置坐标，针对这一问题采用最小二乘法求解标签O的坐标.展开(7)式，将含有未知量x,y的项移到等式一侧可得：

(8)

其矩阵形式可表示为Qθ=B，其中：

(9)

令测量过程中的误差为N，则：

F(θ)=‖N‖2=NTN=(Qθ-B)T(QB-B)

(10)

对上式求导，令导数为0，则：

(11)

解得：

θ=(QTQ)-1QTB

(12)

向量θ的前两项就是标签的坐标(x,y)，其与实际坐标之间误差的平方和最小，该方法进一步提高了室内复杂环境下UWB定位的精度与鲁棒性.

2.2 基于改进A*和人工势场的混合路径规划算法

在获取位置信息后，机器人需要规划一条最优的路径到达老人身边.由于室内环境的不确定性，机器人在移动过程中可能存在原本地图中不存在的障碍物，因此本节对履带式移动机器人运动学模型及其运动约束条件进行分析[10,11]，在此基础上设计了一种混合路径规划算法，在避障的同时保证路径为全局最优.

2.2.1 履带式移动机器人运动学模型及其运动约束条件

通过将履带式移动机器人简化为二维的运动学问题来建立其运动学模型.以X0Y为全局坐标系，选取履带式移动机器人上的xcoyc坐标系来对其运动过程进行描述，如图9所示.

图9 履带式机器人运动学模型

图9中：θ为履带式机器人的方向角；ω为机器人转动角速度；VA和VB分别为左右两侧履带的线速度；l为两侧履带的中心距；O为机器人质心；V为质心O沿前进方向的线速度.

则有：

(13)

式(13)中:R为履带式机器人转动半径；ωA、ωB分别为两侧履带驱动力的转动角速度.

故可得，履带式机器人的线速度和角速度为：

V=(VA+VB)/2

(14)

ω=(ωA+ωB)/l

(15)

假定转弯过程中两侧履带不发生侧滑，则履带车的运动学方程为：

(16)

履带式机器人在移动过程中受自身驱动系统的约束，角速度的突变会导致履带严重打滑，系统应尽量避免其发生突变.假定履带式机器人驱动轮角加速度的最大值为αm，则其运动约束条件可表示为：

(17)

同时，由式(13)～(15)可得：

(18)

将式(18)中两式相加并取导可得

(19)

式(19)中：a为驱动轮线加速度，α为驱动轮角加速度.故可得线加速度和角加速度的约束条件：

(20)

(21)

2.2.2 改进A*算法

A*算法是一种启发式搜索算法，能实现机器人在静态环境下的全局路径规划[12].其代价函数可表示为

f(n)=g(n)+h(n)

(22)

式(22)中：g(n)为起始位置到当前位置n的实际值；h(n)为当前位置n到目标位置的估计值，启发函数可定义为

(23)

式(23)中：(xn,yn)表示当前节点所处栅格的中心坐标，(xg,yg)表示目标节点所处栅格的中心坐标.然而A*算法规划的路径存在冗余节点较多、转折角过大等缺点，不利于机器人在室内环境平稳、高效的移动.针对以上缺点，受文献[13]和[14]启发，做出如下改进：

(1)提取拐点.在A*算法搜索的路径基础上对冗余路径点进行删除，提取路径拐点.具体为：从与起点不相邻的一个路径点开始，判断该节点与前一节点是否在同一直线上，若不在，则判定前一节点非冗余路径点.反之，判定前一节点为冗余路径点，此时删除前一路径点，并对路径进行更新.同理，对路径上的所有节点进行判别，删除冗余路径点.最终生成一条仅含起点、拐点以及目标点的移动路径.

(2)删除冗余拐点.在提取拐点后，路径中可能还存在冗余拐点，在此基础上引入可视点检测法删除冗余拐点.即路径上不相邻的两个拐点相连穿过障碍物，则判定两点不可视.反之，判定两点可视，将这两点之间的连线作为新路径保存.

综合服务机器人、障碍物的大小以及安全距离，设定栅格地图大小为0.5*0.5m.A*算法与改进A*算法的仿真结果如图10、11所示.算法性能参数对比如表1所示.

图10 A*算法

图11 改进A*算法

表1 算法性能参数对比

综合图10、11以及表1可知，改进A*算法有效减少了冗余路径点.拐点减少率达到50%，路径长度缩短5%，转折角度降低率达到51.8%.有效的实现了路径的进一步优化，这将有助于履带式机器人在室内移动过程中更加平稳、高效.

2.2.3 人工势场法

人工势场法的基本思想是：目标和障碍物分别对机器人产生引力与斥力，综合两者的合力来进一步控制机器人的运动[15].其几何模型如图12所示，仿真环境为10×10 m,仿真结果如图13所示.

图12 人工势场原理图

图13 人工势场仿真图

引力、斥力场函数分别定义如式(24)、(25)所示：

(24)

(25)

式(24)、(25)中：Katt,Krep分别为引力、斥力场常量，X=(x,y)为机器人当前位置的坐标向量；Xgoal=(x,y)为目标点坐标向量；Xobs=(x,y)为障碍物坐标向量；ρ(X,Xgoal)为机器人和目标点连线的距离；ρ(X,Xobs)为机器人和障碍物之间连线的距离；ρo为障碍物的最大影响距离.

机器人所受到的引力Fatt和斥力Frep：

Fatt(X)=-Uatt(X)=-Kattρ(X,Xgoal)

(26)

Frep(X)=-Urep(X)=

(27)

机器人所受的合力Ftotal(X)

Ftotal(X)=Fatt(X)+Frep(X)

(28)

2.2.4 混合路径规划算法

本文所述的混合路径规划算法流程为：先利用改进A*算法规划出一条移动路径，将路径拐点依次作为移动过程中的子目标点，若探测到原地图中不存在的障碍物，则采用人工势场法进行实时局部路径规划，反之，则按照原路径行驶.当即将到达子目标点时，再以下一子目标点作为目标点，直至移动到全局目标点.该算法不仅能有效躲避障碍物，还能保证路径为全局最优.

依据实验室内部环境搭建相应栅格地图，针对不同情况对该算法进行了仿真验证，如图14所示.其中图14(a)为无障碍物时仿真结果，图14(b)为有障碍物时仿真结果.黄色方格为模拟新增障碍物.

(a)无障碍物时仿真结果

(b)有障碍物时仿真结果图14 混合路径规划

3 系统硬件设计

本文设计的服务机器人硬件部分主要分为下位机系统和上位机系统，上位机系统由树莓派控制器、无线通信模块、激光雷达、UWB定位标签、时钟模块、语音播放模块组成，上位机控制器采用树莓派(Raspberry Pi4.0)4代B版，利用树莓派自带的WiFi模块将树莓派与电脑PC端通过远调试单元连接起来，树莓派调试界面如图15所示.

图15 树莓派调试界面

下位机系统由STM32控制器、电机驱动电路组成.服务机器人系统硬件框图如图16所示.

图16 系统硬件框图

服务机器人开始工作后，先通过UWB定位基站与标签进行实时通信测得两者之间的距离，在到达服药时间和智能手环监测到体征异常时，上位机利用改进三边测量定位算法解算老人及机器人的位置坐标.然后利用改进A*算法进行路径规划，将该路径以及移动指令经串口通信发送给下位机STM32.下位机根据获得的数据输出相应的PWM波控制电机驱动电路，进一步控制电机的运动状态，实现服务机器人的自主移动.同时编码器将实时测得的速度数据通过中断发送给下位机，利用运动控制算法实现闭环反馈，对移动过程中激光雷达探测到的障碍物通过人工势场法进行避障.

4 系统软件设计

服务机器人部分作为整个系统的核心，其主程序流程如图17所示.工作过程主要如下：

图17 系统主程序流程图

(1)完成系统的初始化工作.

(2)机器人获取各传感器的数据信息，具体包括UWB模块之间的距离信息、时钟模块的时间信息、无线通信模块获取智能手环监测到的老人体征信息.

(3)机器人判断体征是否异常，若是，需完成紧急送药功能；若否，判断是否到达预设服药时间，若到达，通过语音播放模块播放录音信息，完成医疗提醒功能.

(4)调用UWB定位子程序，主控制器通过(2)中得到的距离值解算出机器人与老人的位置坐标.

(5)调用路径规划子程序，通过改进A*算法规划出一条从机器人到老人的移动路径.对移动过程中的障碍物利用人工势场法进行规避.

(6)判断是否到达老人身边，若到达，则整个主程序停止运行.若未到达，则返回继续执行(4)～(6)直至到达老人身边.

5 系统测试

服务机器人的整体实物图如图18所示，笔者在搭建完实验平台后，对其功能进行了测试，包括室内定位测试、路径规划测试、模块功能测试.

图18 服务机器人实物图

5.1 室内定位测试

5.1.1 空旷环境下定位性能测试

在如图19所示的空旷环境下对定位性能进行测试.其中，图19(a)为实际环境、图19(b)为平面图.基站A、B、C的坐标分别为(0,0)、(450,0)、(0,350).实验环境内无障碍物遮挡，基站与标签高度一致，多径效应和非视距干扰对定位精度的影响很小.

(a)实际环境

(b)实际环境平面图图19 实验场地一

图19(b)中标签所处位置为本次实验测量点.每个测量点取20组数据，取其平均值作为最终测量值，定位结果如表2所示.

表2 空旷环境下定位结果及误差

由表2可得，在空旷环境下，改进三边测量定位算法最大误差为12.45 cm，最小误差为9.90 cm.相较于传统三边测量算法定位精度得到了进一步提高.

5.1.2 多径效应和NLOS环境下定位性能测试

由于实验环境内存在桌、椅、柜子等物品，同时调整基站与标签处于不同高度.信号在传输过程中会受多径效应和NLOS的干扰.在如图20所示的环境下进行实验.其中，图20(a)为实验室实际环境、图20(b)为实验室平面图.基站A、B、C的坐标分别为(0,0)、(800,0)、(400,700).

(a)实际环境

(b)实际环境平面图图20 实验场地二

图20(b)中标签所处位置为本次实验测量点.每个测量点取20组数据，取其平均值作为最终测量值，定位结果如表3所示.

表3 多径效应与NLOS环境下定位结果及误差

由表3可得，在多径效应和NLOS干扰情况下，改进三边测量定位算法最大误差为16.55 cm，最小误差为13.45 cm.与传统三边测量定位算法相比，改进三边测量定位算法有效的保障了定位的准确性，符合服务机器人高定位精度的需求，有助于服务机器人更精准的到达老人身边.

5.2 路径规划测试

5.2.1 静态环境中的路径规划测试

为了测试服务机器人的自主移动能力，在如图21所示的静态环境下对路径规划算法效果进行了实验.其中，图21(a)为实际环境，图21(b)为无障碍物情况下路径规划算法的仿真效果.

(a)实际环境

(b)无障碍物时仿真结果图21 静态环境下路径规划

利用手机对机器人实际移动过程进行连拍并选取部分关键图片.如图22(a)～(f))展示了移动中未探测到障碍物按照原路径行驶的过程.

(a)按全局路径行驶 (b)未探测到障碍物 (c)前方路径无障碍物

(d)仍未探测到障碍物 (e)按原路径行驶 (f)到达服务对象身边图22 静态环境下路径规划实验过程

5.2.2 存在随机障碍物情况下的路径规划测试

在完成静态路径规划的基础上，将动态障碍物布置在全局规划路径上，用来测试机器人避开随机障碍物的能力，如图23所示.其中，图23(a)为实际环境，图23(b)为有障碍物时路径规划算法的仿真效果.

(a)实际环境

(b)有障碍物时仿真结果图23 存在随机障碍物情况下的路径规划

图24(a)～(d)展示了移动中激光雷达探测到障碍物时利用人工势场法避障的过程；图24(e)～(f)展示了移动中未探测到障碍物按照原路径行驶的过程.

(a)前方路径存在障碍物 (b)人工势场法实时规划局部路径 (c)按规划的局部路径行驶

(d)前方路径无障碍物 (e)按原路径行驶 (f)到达服务对象身边图24 动态环境下路径规划实验过程

5.3 模块功能测试

5.3.1 时钟模块功能测试

时钟模块为服务机器人提供时间信息，在到达服药时间时，机器人能主动为老人提供送药服务.时钟模块的测试结果如图25所示.

图25 时间信息

5.3.2 语音播放模块模块功能测试

在到达预设服药时间后，机器人在自主移动至老人身边的同时能通过语音播报来提醒老人及时服药.语音播放模块存储的的录音信息如图26所示.

图26 录音信息

5.3.3 体征监测功能测试

利用可穿戴健康设备对老人的体征进行监测，监测到异常情况时向机器人发送该异常数据，监测结果如图27所示，为智能手环监测到的体征信息.机器人在接收到异常数据后进一步完成紧急送药功能.

图27 智能手环监测到的体征信息

5.3.4 异常报警功能测试

在完成紧急送药功能的同时，机器人通过无线通信模块向外界发送异常信息.如图28所示，为老人情况异常时监护人收到的报警信息.

图28 异常报警信息

6 结论

本文设计的面向独居老人的主动式服务机器人具有以下特点:

(1)能在预设服药时间或体征异常时主动为老人进行送药服务，并将异常情况发送至监护人处.

(2)采用UWB定位技术有效解决了在室内复杂环境下定位精度不高的问题.

(3)利用混合路径规划算法不仅可以使路径为全局最优，还能有效躲避障碍物.

测试表明：本文所设计的服务机器人系统在多径效应以及非视距干扰严重的室内环境中具有较高的定位精度；能够在移动过程中对原本地图中不存在的障碍物进行规避，在具备良好的自主移动性能的同时能够完成定时送药和紧急送药功能，有效地保障了老人的安全与健康.