基于多源信息融合的床椅机器人自主对接技术和方法研究

房志东，梅珑，金来

(1.安徽三联机器人科技有限公司，合肥 230081；2.安徽三联学院，合肥 230601)

1 引言

随着经济发展和人民生活水平提高，失能卧床人群的照护需求剧增，尤其交通事故造成的残疾失能人群以及失能老人照护问题越来越受到社会关注[1]。根据2020年民政部数据，我国仅有50余万名养老机构护理员，远不能满足失能人群的照护需求，同时相关的照护技能和护理设备严重缺失[2]。基于机器人和计算机技术的飞速发展，使代替护理员的智能康护机器人成为可能，一种床椅分离式机器人护理床由此创新地提出[3]，此床可以满足失能人群的移位和部分护理功能，可由护工或受护者操作进行护理和康复训练，床椅分离后可变成一张多功能移动轮椅，进行多功能体位变换康护，可以无障碍移位外出，轮椅返回可方便与床体对接合并成整床。然而床椅机器人的自护性需要解决床椅柔性、安全和精准的分离和对接的难题，目前市场可见分离式护理床基本全为人工辅助的非智能对接方式，人工对接方式一方面费时费力，自动化程度低，大大增加照护人员体力负担，另一方面影响受护者尊严和出行便利性，每次轮椅归位还要请求护理人员协助合并，自主独立性弱。为了实现床椅机器人的自主对接，本文拟尝试采用超声等多源信息融合与定位导航技术，依托机器人伺服系统实现床椅机器人轮毂电机精准控制，旨在提升自主对接的精度和成熟度。

2 方法

床椅机器人自主对接首先需要基于通信和控制功能构建自主对接的床、椅硬件系统和单元；然后以床体为参照建立相对坐标系，通过超声传感器组等方式构建TOA(Time of Arrival，时间达到)定位系统，运用轮毂编码器输出脉冲信号对轮椅运动状态推算，以IMU(Inertial Measurement Unit，惯性测量单元)加速度角速度信号进行航位解析，通过EKF(Extended Kalman Filter，扩展卡尔曼滤波器)方式进行多源定位信号融合，实现最优精确位姿估计。其间涉及移动轮椅闭环控制模型以及融合位姿与期望位置校对，通过路径规划和运动控制完成轮椅的精确驱动，最终完成自主对接。

自主对接的性能测试以对接时间、对接间隙、对接间隙角度，对接成功率、适应性等指标进行衡量。

(1)对接时间测量：床椅一体机进入可对接状态，移动床椅和固定床体1米距离以上，完成对接前的到达预定区域位置和姿态准备，用秒表开始计时t1，直到移动床椅、固定床体完成对接，并及时调整到平躺姿态，计时结束t2i，则对接时间为(t2-t1)，时间取整到1 s。

(2)对接间隙距离：测量床椅一体机对接成功后，移动床椅和固定床体的中间间隙距离，用游标卡尺测量床头、床中和床尾三个地方，取平均值。

(3)对接间隙角度测量：床椅一体机对接成功后，用角度仪测量移动床椅和固定床体偏斜角度。

(4)对接成功率：参考对接时间测量标准，n次对接实验，首次成功m次，首次不成功二次成功s次，则一次对接成功率m/n，二次对接成功率(m+s)/n,n不小于20。

(5)适应性：以不同起始对接位置和航向角实验测试对接成功率，反应系统对接适应的鲁棒性。

3 系统设计

3.1 硬件系统设计

整个床椅电气控制系统较为复杂，包括轮椅主控器、床侧主控器、手控器、驱动器、电源转换器和传感器组、无线通信装置等。这里主要探讨的是床椅如何对接，故说明电控系统中对接功能部分的构成。

如图1所示，为床椅机器人床体完成对接控制的系统框图，包括一个床体处理器单元，和处理器单元连接的核心模块包括无线通信收发模块U，自研超声发射传感器模块A、B和C，因为B、C和A的内部构造相同，图中内部构造省略。超声发射传感模块包括信号产生单元、放大单元和发射单元，分别由微处理器、放大器和发射探头电路连接构成；床体处理器单元根据无线通信模块U的超声控制信息，进行逻辑运算，控制A、B和C传感模块的工作状态。无线通信收发模块U，一方面将床体处理器单元产生的数据信息发送给轮椅无线收发模块，另一面给接收轮椅无线模块发送的各类状态和控制数据信息。

图1 机器人床体对接系统组成图

如图2所示，为机器人轮椅部分完成合并的系统组成图，包括一个轮椅处理器单元，和轮椅处理器连接的包括无线通信收发模块V，自研超声接收传感器模块L和R；以及红外激光测距传感器模块G和H。超声接收传感器模块L和R由可以接收床体超声发射传感模块发射信号的接收单元，和对接收信号进行预处理单元，经过预处理的测距信号通过轮椅处理器进行分析解算，形成距离信息。红外激光测距模块G、H是一个发射和接收一体化模块，内部的发射模块完成信号的产生、放大和发射，发射信号经过外部物体产生反射信号；内部接收模块接收发射产生的反射信号，然后经过放大、增益调整等预处理操作产生信号发送给轮椅处理器单元，经过逻辑运算，得到距离信息。

图2 机器人轮椅对接系统组成图

3.2 融合定位方法

3.2.1 室内坐标标定模型

以俯视角度，对位置空间进行简化，将床椅机器人床体部分简化为一个L型，轮椅部分简化成矩形，另因床体和轮椅同处在一个地面，垂直方向上误差基本可以忽略，因此简化为二维水平面上几何解算关系。如图3所示，以L型床体部分内测边缘交叉点为原点O，以长边为y轴，短边为x轴，建立以床体为参考的坐标系。床体部分，固定安装了自研发的超声发射模块A、B和C，坐标分别为A(a1，b1)、B(a2，b2)、C(a3，b3)，各模块的位置因设备组装完成而确定。移动轮椅部分，以朝床体方向前方分别固定安装两个可以接收床体超声发射信号的自研超声接收模块L和R，记LR的中点M，定义坐标分别为L(xl，yl)、R(xr、yr)、M(xm，ym)；同时在轮椅右侧安装两个红外激光测距模块G和H，坐标为G(xg，yg)和H(xh，yh)。AB、AC之间距离在床椅机器人中为固定常数，记为dab和dac。

图3 床椅机器人坐标系构建

3.2.2 超声传感器定位

超声定位系统下轮椅的位姿以M点坐标和轮椅航向角确定。床体控制器通过无线收发模块U和轮椅无线收发模块V通信，当轮椅发出全局定位指令，在得到轮椅侧传过来的确认信息后，以特定周期规律Ts循环控制发射模块A、B、C发射信号，A、B、C发射互斥，Ts较小通常为十毫秒级。例如收到指令后，启动A，关闭B，此时A发射信号被轮椅侧接收模块L和R接收，经过轮椅处理单元解算完成传输时间tal、tar提取，得AR的距离dar和AL的距离dal。

Ts时间之后，床体控制器通过无线收发模块U和轮椅无线收发模块V通信，通知即将关闭A开启B，在得到轮椅传过来的确认信息后，启动B关闭A。此时B发射的信号被轮椅侧接收模块L和R接收，经过硬件信号调理，进入轮椅处理器完成传输时间tbl、tbr提取，得到BR的距离dbr和BL的距离dbl表达式如(1)和(2)所示。

(1)

(2)

c=331.5+0.607K

(3)

上式(3)中c为声速，等式表达了当前时刻环境温度K的声速c的补偿关系。

R、L传感器中点的坐标，以及轮椅的航向角，就可以准确测算相对床体轮椅的位姿。定义x轴负向逆时针与RL中垂线夹角定义为轮椅航向角θ。在RAB三点组成的三角形RAL中，由几何关系得

(4)

(5)

令y轴正向顺时针与AB边与夹角为α，y轴正向顺时针与AR与夹角为β，同时结合床椅机器人已确定传感器的位置分布，可得

α=∠RAB+β

(6)

(7)

(8)

于是得到R(xr,yr)为

(9)

同理得到L(xl,yl)位置为

<1)，且各件产品是否为不合格品相互独立．

(10)

于是得到位姿观测量

(11)

3.2.3 IMU定位原理

基于IMU航位推算是一种以牛顿力学为基础的相对定位方法，六轴IMU包括三轴加速度和三轴角速度信息，对加速度的一次积分得到速度，二次积分得到相对位移，角速度一次积分可以获得旋转角度。利用角度和速度信息计算得到载体坐标系下的位姿。基于IMU的位置推算不受外部环境影响，更新速率快短时间精度高，全天侯定位，在超声定位非视距误差较大情况下，可以对系统的位姿进行校正，当超声定位置信度较高，可以校准航位推算的初始值。

(12)

其中C1、C2、C3分别为

(13)

3.2.4 编码器信号定位实现

编码器是一种精确的位移信号测量传感器，在左右驱动轮各安装一高精度编码器，轮毂转动会输出密集编码脉冲信号，通过对单位时间编码信号测量，即可推算出移动轮椅的的位置和航向信息。设左右驱动轮编码器参数为K线/圈，驱动轮半径为已知量Rd，左右驱动轮横向间距D。定义k时刻轮椅位姿为(xm(k)，ym(k)，θ(k))，Δt时间即k+1时刻后位姿为(xm(k+1)，ym(k+1)，θ(k+1))，Δt时间左右轮里程计输出脉冲分别为Nl(k)，Nr(k)，依据运动学推算得Δt时间左右轮移动距离ΔS1(k)、ΔSr(k)和线速度v(k)、ω(k)旋转角速度(k)、分别为

(14)

轮椅的移动距离ΔS1r,旋转角度Δθ(k)和旋转半径R(k)为

(15)

由数学知识可得Δt时间轮椅位姿变化量Δxm(k)、Δym(k)

(16)

3.2.5 扩展卡尔曼多源信息融合定位

基于超声波定位方式存在干扰和非视距误差等因素，特别目前越来越多电子设备的应用，超声工作频率附近的干扰源将直接导致定位系统失灵，致使定位精度下降或直接不能定位。编码器脉冲定位相对可靠，也存在重要缺陷，例如轮毂打滑将产生较大位移、角度推算误差，同时也是一种相对定位方法。通常一种定位方法很难考虑周全，通过采用超声、IMU、轮毂编码器脉冲以及红外多传感器融合方式进行组合导航，为连续、可靠和精确的定位提供可能。常用多传感融合方法有加权平均法、贝叶斯归纳推理、模糊逻辑、卡尔曼滤波(含扩展型)和神经网络算法等，可以对测距信息、全局定位信息融合，实现被测对象的全面环境感知和描述。

如图4所示采用EKF方案的基于多传感器融合定位方法原理图，超声模组构建TOA定位系统，输出基于测距信息获得的位置、速度等位姿信息，激光红外传感器输出测距信息，IMU传感器输出的加速度、角速度信息，经过捷联解算模块获得轮椅位置、速度、航向角等参量，轮毂编码器脉冲信号经过推算输出位姿信息，同时将IMU和编码器输出的测距变量做比较后输出伪距率信息，以上各类信息进入卡尔曼滤波器进行滤波运算融合，最终输出最优定位结果。此外，EKF滤波器输出位姿补偿信息需要对编码器位姿解算和IMU推算模块进行反馈补偿。

图4 基于EKF的融合定位框图

3.3 运动控制设计

3.3.1 控制模型

移动轮椅相对位姿可通过多传感器测算融合获取，轮椅已知位姿情况下导航到目标位置还需要精准对接控制方法，也应具有一定的鲁棒性，整个系统核心部分是对接控制器和融合定位算法。如图5所示为闭环对接控制模型框图，采用双闭环控制策略，系统期望的轮椅位姿与融合定位系统输出的观测位姿量进行比较，比较量输入到对接控制器，对接控制器包括路径规划和运动控制算法，路径规划模块获得位姿比较量，输出下一目标位姿参考量，运动控制算法模块接收位姿参考量，运用基于PID的多闭环控制策略，应用inv-park、park进行电压变换、clark进行电流变换等运算方法[5]，输出旋转空间PWM信号量驱动轮毂电机运转，通过安装在电机上的编码器反馈的编码脉冲信号，一方面与超声红外传感器系统、IMU进行组合融合定位输出观测位姿量，一方面输入到对接控制器，及时参与电机的闭环控制。最终完成超低速状态下，简洁可靠的高精准对接。

图5 对接闭环控制模型图

闭环控制模型的核心在于对接控制器，对接控制器主要包括路径规划和运动控制算法。路径规划算法有人工势场、栅格法、蚁群法和神经网络法等[6]，栅格法包括Dijkstra、A-star和D-star。床椅机器人对接的环境相对不复杂，规划区域空间较小，路径规划可采用正向搜索方式的A-star算法比较合适，A-star是一种启发式搜索算法，是目前最稳定和高效的算法之一。运动控制模块采用伺服技术，应用基于PID算法的位置、速度、电流三环控制，PID参数的整定方法对控制精度和稳定性影响很大。基于上节编码器脉冲信号反馈的推算公式，对公式进行微分和拉式变换，可得轮椅的运动学模型[7]，根据运动学模型进行反馈控制。

3.3.2 床椅对接设计

由于床椅对接的特点是对接环境可靠简单无障碍物，对接精度要求非常高，一般不用考虑中间障碍物或不同房间非可视定位线路规划问题。根据需要可采用不同对接控制策略，分别对效果进行评估。本研究依据对接环境的特点，提出一种多目标点区间分段对接的简洁高效方案，以{x=d1，y=d2，d1和d2为设定正常数}将对接区域分为Ⅰ、Ⅱ、Ⅲ、Ⅳ，根据经验选取已知的6个较小圆形目标区域Wi(Ni，ri)(i=1,2,…,6)，Ni为预设坐标点，ri为区域半径。开始自动对接区域的起始条件M需满足x>d1(Ⅰ和Ⅳ)。整个对接区域被分为W1W2、W2W3、W3W4、W4W5、W5W6五个区间段。对接基本过程描述如下。

(1)起始位姿检测：轮椅接收用户的指令，以ABRL和ACRL传感器组循环组合定位，因传感器角度无法定位时可旋转左右驱动轮，直至有一组可靠位置结果输出。判读轮椅所处区域位置是否为Ⅰ和Ⅳ。

(2)航向调整：所处区域为Ⅳ时，对接控制系统开始工作，轮椅自主移动到W1区域并调整航向角满足(0，π/2)，即M点在W1内(下同)进入下一步。当所处区域位于Ⅰ时直接进入下一步。

(3)自主循迹导航：对接控制系统持续工作，轮椅自主导航移动到W2区域(N2横坐标xw2=d1)，然后自主导航移动到W3，再移动到W4区域(N4纵坐标yw2=d2)，然后继续自主移动到W5。

(4)横向末程对接：在W5区域，轮椅侧边已经很接近床体侧边，航向角接近90度，此时过大或滞后调整轮毂运动，将可能导致轮椅床体碰擦。实验结果证明，此时应微调G、H与床体侧面的距离值为主要目标，通过控制轮毂达到较为理想的效果。设d3、d4为合适距离常数，ε1、ε2为合适较小误差正常数，控制轮椅满足下式持续移动。

(17)

(5)纵向末程对接：设d5为合适距离常数，ε3为合适较小误差正常数，当轮椅状态测量参数满足下式，轮毂电机停止运动，物理刹车装置启动，对接结束。

|dbr+dbl-2d5|≤ε3

(18)

(6)结束：通过轮椅向床体发出关闭尾门指令，床体关门，轮椅自动变形和床体合为整体，完成整个合体过程。

4 系统验证

以多传感器信息融合定位的方法提高了定位精度和定位可用性，对接精度可达厘米级；以双闭环控制策略完成了移动轮椅和床体柔性、安全和精准的自主对接。为验证导航定位精度和对接控制效果的可行性，研究人员准备了硬件和结构验证平台，实验在研发部门开发的样机上进行，如图6实物图，建立以床体为参考的坐标系，将移动轮椅置于表1所示的不同起始位置和航向角做了8组实验，用秒表对时间进行计时，用角度仪测量起始航向角和对接间隙，利用卡尺对横向和纵向误差进行测量。表1为对接实验数据统计，从实验结果来看，对接完成时间能很好保证2 min以内，且横向和纵向对接精度都达到了厘米级。本次8组实验全部一次对接成功，此外大量测试结果表明，一次对接成功率能达到90%以上。

图6 对接验证平台实物图

表1 床椅机器人对接实验结果

5 讨论

床椅自主对接可研究的定位方式还包括基于磁导航循迹、基于图像标记识别、激光雷达建图定位等。(1)基于磁导航定位技术[8]在传统AGV定位中应用较为成熟，一般定位精度良好，但对场地环境部署要求非常高，应用在床椅机器人中需在地面贴磁性条状物，在轮椅的前向和后向均需安装磁导航控制器识别引导床椅进行合并，很大程度上解决了人工辅助的合并问题，但问题也比较突出，一，贴在地面的磁性条状物和床体发生相对位置偏移时，将无法保证轮椅顺利合并，甚至出现碰撞风险；二，移动轮椅移动到导航磁条的起始端也是非常的难，依靠受护人在看不见地面磁条位置的情况下移动到起始端效率非低下；另外磁导航传感器和磁导航控制器本身成本也较高。(2)基于图像识别定位导航的方式在目前应用也比较广泛，某高校研究所将其在床椅机器人中进行应用，此方案一般都需要在地面贴显著性易识别的标记物，如纯色的条带、二维码[9]等，采用摄像头识别明显标记物，然后采用基于图像处理定位和导航控制算法，完成轮椅和床的自动合并，这种技术和磁导航技术一样，一存在标记物和床体发生相对位移，无法完成床椅自动合并；另一方面，基于图像识别的定位精准度可靠度可用性也满足不了要求，如采用高分辨率的专用相机，算力要求较高，相机和处理器的成本也相对较高。(3)采用激光雷达传感器以slam(Simultaneous localization and mapping，同步定位与建图)定位技术方案[10]，需要采用激光雷达等硬件成本高，且需事先建图使用受限，另外其精度约5cm，不能达到对接精度要求，还需要依赖其它传感器辅助。

床椅机器人应用对象是失能或需康复人群，对接后需要是一张完整可靠的床，因此使用安全性和对接精度要求非常高。自主对接的精度和安全性关键在于导航定位持续可用性、运动控制策略适配以及电机的精准控制。本文对基于多源传感器信息融合定位技术进行研究，应用超声传感器、红外激光、脉冲编码器、民用IMU等多类低成本传感器进行多系统融合定位，提高了定位系统输可用性和定位精度；研究了双闭环对接控制方法，使床椅对接更加柔性、安全、可靠，具有跟随床体移动的对接系统成功率更高。相对于上述磁导航、图像识别、激光slam等技术方案，本研究技术方法在对接成本、性能和鲁棒性都具有优势：(1)采用的是普通超声模组、红外以及民用级IMU等低成本传感器完成机器状态的感测和定位，没有激光雷达、专用摄像头、高性能处理器等，方案成本低；(2)相对于slam定位精度不能满足要求，采用多源融合定位方式，定位结果可靠性、可用性和精度高，能满足使用要求；(3)相对于磁导航控制和基于图像识别方式，不在床椅、地面之外的环境添加额外的标记物、设备等，对地面设施部署要求点，适用环境要求低，场景适用性强；(4)具有优秀的鲁棒适应性，以床体为参考系，移动轮椅随床体位置移动而自适应调整轨迹，对于床体位置经常有移动的场景，仍然可以完成高精度、高成功率的自动对接，大大提高了对接的可用性。

6 结语

本文采用超声、红外激光、IMU、编码器脉冲等传感器信息进行多传感融合的定位解算，以床体为参考坐标系完成轮椅的位姿推算，通过轮毂控制器实现床椅自主对接。此种床椅机器人自主对接技术和方法，有效降低了算法复杂度和系统硬件成本，提高了系统响应实时性。床椅机器人的广泛应用，将大大提升失能人群的康护水平，有效缓解社会康养护理员的人力资源缺口，对促进我国全人群健康质量和幸福指数具有重要的社会意义。