基于激光雷达的嵌入式护理床自动对接系统*

李素姣，杨皓文，孟巧玲△，王海涛，喻洪流

(1.上海理工大学康复工程与技术研究所，上海 200093;2.上海康复器械工程技术研究中心，上海 200093;3.民政部神经功能信息与康复工程重点实验室，上海 200093)

1 引 言

据研究，目前我国失能老人已经达到4 200万左右，预计2030年将达到6 000万[1]。在2015年，我国老年人平均带残存活时间约为2～3年，老年人产生失能问题的比例达到10.48%～13.31%，自主移动困难[2]。为解决失能患者自主移动问题，一些高校和研究机构研发了智能可分离式护理床，其中床椅的自动对接是保障失能患者安全归位的关键技术之一。

为解决智能可分离式护理床的自动对接技术，国内外学者投入了大量的研究工作。Mascaro等[3]最早提出了可重构的轮椅床系统，该系统基于力传感器的对接控制策略，通过力传感器逐步调整床体的姿态从而实现床椅对接[4]；Zou等[5]、Ye等[6]先后提出的两种对接方式均采用安装在轮椅上方的视觉传感器，并采用直线跟踪算法实现床椅的对接；胡木华等[7]采用激光雷达对接与巡线对接的方式，并结合开门式，旋转座椅式等轮椅动作，实现了床椅的侧方分离对接；李秀智等[8-9]提出了一种基于视觉与超声波数据融合的床椅侧边对接策略，利用标定点视觉传感器确定水平位置中心，超声波传感器获取当前深度信息关系，借此获取床椅相对位姿信息。综上，目前床椅对接仍以侧方对接方式为主，单侧对接的结构问题不利于失能患者翻身，且人工挪动易造成二次伤害[10-11]。在床椅对接策略方面，基于力传感器的床椅对接，对于初始床椅位置、姿态和接触点具有严格要求，对接准确率低；超声波与红外测距传感器提供的深度信息较少，精度较高的对接任务往往难以完成；基于视觉的床椅对接，需要在使用环境中预设标准点，并在床椅上方安装视觉传感器，对光线使用环境要求较高。

针对以上问题，本研究设计了一种基于激光雷达的嵌入式床椅自动对接系统，床椅对接分为局部定位和精准对接两步。前期利用智能轮椅自主导航移动至床体框架前部目标区域，到达目标点，后向激光雷达对预置的床体人工标志进行特征提取与局部定位，保证对接定位的精度和鲁棒性，同时提出相应的PID路径跟踪算法，保证床椅对接过程中的稳定性，从而实现床椅的安全精准对接。

2 机构和策略

2.1 床椅对接结构

为避免患者移动造成二次伤害，并实现精准对接，设计的轮椅床为嵌入式结构，见图1。整体分为床体模块和轮椅模块，并进行相关的有限元和运动学分析，验证了机械结构的合理性和可行性。在床体中部安装辅助激光雷达定位的人工标志反光板(见图2)，激光雷达反光板为两边等长的且成90°夹角的双边形结构为激光雷达定位提供了充分的角点与直线特征，定位过程中只需通过确定双边的直线结构方程，求解出角点位置便可获取到当前床椅相对位姿，从而控制轮椅进行对接动作。此外，利用双边呈90°这个先验知识，可对前一步获取到的双边结果进行验证，此结构极大简化了激光雷达数据处理难度，提高了定位算法的鲁棒性。

图1 床椅机械结构Fig.1 Mechanical structure of bed chair

图2 反光板结构Fig.2 Reflector structure

2.2 床椅对接策略

2.2.1局部定位算法 激光的扫描匹配即通过求解坐标转换关系,将连续扫描的两帧或多帧激光点云统一到同一坐标系中(scan-to-scan)，或者将当前扫描点云与已建立的地图进行配准(scan-to-map)，从而最终恢复出载体的位置和姿态的变化[12-13]。目前从测距传感器中提取的特征大多是线段、角点或圆等几何基元，直线与角点在特征提取的过程中拥有计算简单、鲁棒性高等特点，故采用基于直线与角点特征的激光匹配方法[14-15]。未知环境下的直线提取有三个主要问题：有多少条线；哪些点属于哪条线；给定属于直线的点，如何估计直线模型参数。针对直线提取中的三个主要问题，为保证床椅自动对接过程中的定位精度与速度，本研究在床体框架中预置了双边直角结构，以增加先验的直线与角点信息，即直线的长度与直线夹角的角度，从而解决了直线提取过程中的前两个问题。对于第三个问题，采用最小二乘法进行对已提取的直线进行线性拟合，估计直线的模型参数。常见的激光雷达直线数据提取算法在处理不同情况的问题过程中各有优劣[16]，本研究从复杂性、计算速度、正确性以及精确性对以下6种算法进行评估，研究结果见表1。

表1 二维激光雷达直线提取算法比较Table 1 Comparison of two-dimensional lidar straight line extraction algorithms

由表1可以看出，分割合并法在运算速度、错误率等方面都有较好表现，合并分割算法的实现见图3，主要包含6个步骤：

图3 合并分割算法Fig.3 Merge segmentation algorithm

步骤1：初始化集合S1由N个点组成。将S1放入列表L。

步骤2：将一条直线L拟合到中的下一个集合Si。

步骤3：检测距离直线最远距离的dP的点P。

步骤4：如果dP小于一个阈值，继续(转到步骤2)。

步骤5：否则将在P的Si分裂为Si1和Si2，并以Si1和Si2取代L中的Si，返回步骤2。

步骤6: 当L中的所有集合(直线段)都被检测出，合并共线段。

(1)

(2)

轮椅床框中预设了V型人工标志，在利用完成数据滤波后，采用分割合并的方法能够快速而简洁的对激光雷达目标特征的提取。见图4，在激光雷达坐标系中，激光雷达数据以及人工标志的位置，在经过合并分割算法后，采用最小二乘法对激光雷达数据进行数据拟合从而得到激光雷达坐标系中预置人工标志中的两个边的的直线方程L1、L2，并通过对直线方程的处理获得L1L2交点O的坐标，即人工标志中的直角在激光雷达坐标系的坐标，以及L1L2两直线夹角的角平分线LP，并可以获得直线LP与轮椅目标位姿之间的夹角θ，这是进行轮椅对接运动控制的重要参数。

图4 激光雷达数据处理Fig.4 Lidar data processing

利用合并分割算法对激光雷达的数据处理后，为进一步实现局部定位，即智能轮椅与床体框架的相对位置关系，需要进行激光雷达与坐标系和机器人坐标系之间的坐标转换，通过位姿变换后，得到图5，以反光板顶角为中心建立直角坐标系，则智能轮椅在该坐标系下的位姿坐标可以用 (x，y，θ)进行表示，坐标中的x、y、θ与图4中x、y、θ的一致，经过坐标系变换之后保持了一致性。可以看出θ表示了智能轮椅朝向与理想位姿的角度偏差，x表示智能轮椅当前理想位姿的水平偏差，y表示了智能轮椅与理想位姿的垂直距离偏差。由此便获得智能轮椅与床体的相对位置关系，通过运动控制算法可实现实床椅的自动对接。

图5 局部定位示意图Fig.5 Schematic diagram of local positioning

2.2.2精准对接控制 对接过程中采用后向行驶，以实现床椅的对接。本系统中护理床采用的中央嵌入式结构，在保证患者的床椅分离便捷性的同时，也对床椅对接的控制算法提出了更高的要求。传统的控制算法中常采用规划对接路径后，采用路径跟踪算法实现控制[19]，但由于规划路径的复杂性，使得路径跟踪效果一般都很差。本系统中，床椅对接动作在执行前，采用智能导航方式使得智能轮椅运动到床体前端中部，因此床椅对接的距离较近，对接运动距离较短。故直接采用PID控制算法将智能轮椅位姿坐标(x,y,θ)的加权值作为控制输入从而控制智能轮椅实现床椅对接。采用反馈控制算法避免了路径跟踪精度较差的问题，同时保证了对接的流畅性。而直接采用PID进行对接控制，势必会出现在控制初期大幅震荡后趋于平稳的现象，这对患者在使用体验的过程中是极差的，因此我们将偏差函数定义为：

(3)

式中，误差函数与智能轮椅位姿、理想位姿的偏差值θ、x的加权值成正比，与y成反比，有效地避免了控制前期大幅震荡的问题。

3 实验结果与分析

为验证床椅对接有效性，本研究从激光雷达数据处理和床椅对接两个方面进行定性的验证。

在实现床椅自动对接的过程中，局部定位的精度是关键，而本系统采用的分割合并算法在预置人工标志的基础上取得了较好的效果。图6(a)为激光雷达数据在可视化界面Rviz中的显示，激光雷达的扫描角度270°，扫描间隔为1°，得到270个扫描点；通过采用机器人操作系统(ROS)[20]自带的滤波算法实现对激光雷达数据的滤波[21]，滤波结果见图6(b)，有明显间断点的数据被有效滤除；并采用合并分割算法对滤波后的数据进行处理，见图6(c)，完成分割后的直线采用不同颜色进行标志，可以看出，该方法有效地对直线与角点数据进行了提取；图6(d)通过对预置人工标志上激光雷达数据进行紫色标注，与其他激光雷达点进行区分，将人工标志中心位置通过较大颗粒进行标注，可以看出本研究所采用的激光雷达算法在实际的运用过程中依然保证了较高的稳定性与精度。

图6 激光雷达处理可视化(a).激光雷达数据;(b).数据滤波;(c).特征提取;(d).对接结果Fig.6 Visualization of lidar processing(a).lidar data;(b).data filtering;(c).feature extraction;(d).docking result

本研究采用基于反馈的PID控制算法直接对智能轮椅当前位置坐标与理想位置坐标进行反馈控制。在床椅对接过程中，床椅通过自主导航运动到床体前方，见图7(a)，并开始自动对接过程。开启对接动作后，对激光雷达数据进行处理并获得智能轮椅局部定位信息，即床椅相对位姿，通过PID算法进行自身位姿调节，见图7(b)，智能轮椅以较快的速度平稳地向床体中心位置靠拢，通过不断地调整自身位姿顺利进入床体，见图7(c)，并最终完成床椅的对接动作，整个过程中，智能轮椅保证平稳运行，且完成对接后智能轮椅位于床体中部，并与床体保证较小的间隙，见图7(d)。

图7 床椅对接过程(a).完成自主导航动作;(b).开始床椅对接动作;(c).进入床体;(d).完成对接Fig.7 Docking process of bed and chair(a).complete autonomous navigation;(b).start the bed-chair docking action;(c).enter the bed body;(d).complete docking

4 结论

针对当前轮椅床结构和对接问题，本研究提出了一种基于激光雷达的嵌入式床椅自动对接系统，重点研究了床椅自动对接过程中的局部定位算法与运动控制算法，局部定位算法则重点研究了对激光雷达数据直线特征的提取与拟合，通过合并分割算法结合预置人工标志有效地提高了直线提取的速度与鲁棒性。运动控制算法则直接采用了PID控制算法，但结合对误差函数的处理依然起到了很好的控制效果。最后通过实验验证了本研究所提出的基于激光雷达的中央嵌入式床椅对接策略的有效性，保障了床椅的结构安全性、对接准确性，一定程度上解决了失能患者的自主移动问题。