改进人工势场法的洗浴机器人擦洗路径规划

程龙 王欣 吴迪 冯志乾

摘 要：针对洗浴机器人末端执行器与人体直接接触擦洗的自主导航问题进行了研究，提出一种基于人体点云的改进人工势场法三维覆盖路径规划算法。首先，在传统人工势场法基础上增设障碍物引力势场，使机器人能够贴近障碍物表面向目标点移动，并且通过添加虚拟目标点的方法克服了凹陷区域产生的局部极小值问题。然后，将人体点云切段分割后投影至平面，结合改进的人工势场法完成对人体表面的覆盖路径规划。以人体模型点云为对象进行仿真实验及对比，结果表明提出算法可以快速有效地完成基于人体点云的三维覆盖擦洗路径规划。

关键词：洗浴机器人； 接触擦洗； 路径规划； 人工势场法； 点云

中图分类号：TP241   文献标志码：A

文章编号：1001-3695（2023）09-030-2760-05

doi：10.19734/j.issn.1001-3695.2023.02.0052

Scrubbing path planning of bathing robot based onimproved artificial potential field method

Cheng Longa， Wang Xina， Wu Dib， Feng Zhiqiana

（a.School of Mechanical Engineering， b.School of Computer Science & Technology， Dalian University of Technology， Dalian Liaoning 116024， China）

Abstract：Aiming at the autonomous navigation of the end effector of the bathing robot in direct contact with the human body， this paper proposed a three-dimensional coverage path planning algorithm which applied to the improved artificial potential field method based on the human point cloud. Firstly， it added the obstacle gravitational potential field to the basis of the traditional artificial potential field method， so that the robot could press close to the obstacle surface and moved toward to target point. And by adding virtual target points，it overcame the local minimum problem caused by the concave region. Then， it segmented the human point cloud and projected it onto a plane， combined with the improved artificial potential field method to complete the coverage path planning for the human body surface. Conducting simulation experiments based on a human model point cloud， the experimental results show that the proposed algorithm can quickly and effectively complete the 3D coverage scrubbing path planning.

Key words：bathing robot; contact scrubbing; path planning; artificial potential field method; point cloud

0 引言

老年人身體机能降低容易受到外伤或疾病的侵扰导致自主行动能力丧失，这种丧失自理能力的失能老人需要护理人员照顾其日常生活起居［1］。随着人口老龄化加剧 ［2］，护理人员的护理工作日益繁重，尤其对于失能老人的洗浴工作往往要耗费大量的人力和物力［3］。因此研发面向失能老人的洗浴机器人对减轻护理人员工作负担、节约人力具有重要意义［4， 5］。

目前，国内外研究者致力于实现末端执行器与人体接触擦洗的方式以获得较好的洗浴效果和舒适度。陈雅［6］设计了一款通过弹性绳使洗浴海绵紧贴人体皮肤进行接触擦洗的洗浴机器人，其能够有效去除人体表面油脂类污渍，但依靠弹性使海绵贴紧人体的方式其安全性、舒适性和精度都相对较差。Dometios等人［7］提出了一种基于在线点云的洗浴机器人末端执行器实时运动规划方法，该方法基于点云信息通过导航函数调整二维空间中预定义的擦洗路径在垂直人体表面方向上的距离，但只能完成对于人体局部皮肤的路径规划。接着，Dometios等人［8］提出一种将视觉感知和CC-DMP相融合的运动规划方法，该方法能够在完成洗浴机器人末端执行器擦洗路径规划的同时配有专业化的擦洗动作以提高舒适性，但其依然只能面向人体局部区域进行擦洗。Papageorgiou等人［9］在NF方法基础上构建排斥势场完成洗浴机器人末端执行器实时运动规划，其生成的路径具有较高的灵活性能够避开人体表面不可清洗区域，但还是只能完成人体局部皮肤的擦洗路径规划。Huang等人［10］研发了一种利用深度摄像头和软触觉传感器跟踪人体轮廓的机器人洗浴擦洗系统，擦洗过程中末端执行器与人体始终保持一定压力相接触，但其无法获取全局场景信息作出覆盖擦洗人体全身的自主导航，只能沿着单一方向前进。虽然这些研究基于视觉或触觉等方式实现了精准舒适的擦洗运动，但仅能应用于人体局部区域，没有实现覆盖人体全身的自主导航，使用时需要人为作出先验准备和主动调整。人体表面曲面复杂多样，开展全身擦洗自主导航的研究是十分必要的。

路径规划是实现机器人自主导航的关键技术，常用的路径规划算法主要有人工势场法［11］、A*算法［12］、RRT算法［13］和智能仿生算法［14］等。其中人工势场法具有计算量小、实时性好、路径平滑等优点，但人工势场法不可避免地会产生局部极小值问题，尤其容易因进入凹形区域而陷入局部极小值。徐小强等人［15］通过添加预测距离和设置虚拟目标点的方式克服了局部极小值问题，同时引入安全距离减少了不必要的路径；牛秦玉等人［16］通过添加虚拟障碍物的方式破坏斥力和引力平衡从而跳出局部极小值点；李庆华等人［17］提出了一种用于克服凹形障碍物的组合算法，陷入局部极小值后通过切换A*算法跳出凹形区域；许万等人［18］通过凹形障碍补齐方式避免机器人进入凹形区域而产生局部极小值问题。以上研究能够有效解决局部极小值问题，但生成的路径点并不能完全满足擦洗路径规划的要求。

为此，针对洗浴机器人末端执行器擦洗人体全身的自主导航问题，提出一种基于人体点云的改进人工势场法全局擦洗路径规划算法。首先通过增设障碍物引力势场和添加虚拟目标点的方式，使路径点能够覆盖障碍物并克服进入凹陷区域产生的局部极小值问题。然后基于人体点云进行切段分割，结合改进人工势场法依次完成分割后各点云段的三维覆盖路径规划。

1 问题描述

洗浴机器人如图1所示，仅对人体脖子以下部位进行清洗，头部除外。洗浴机器人由浴床和擦洗装置组成，浴床供洗浴者仰卧，擦洗装置可以视为一个三自由度机械臂，能够带动末端执行器实现上下、左右和前后运动。末端执行器由柔软浴球固定到转盘上组成，电机驱动转盘带动浴球旋转实现擦洗动作，如图2所示。末端执行器与人体表面保持适当距离，使柔性浴球能够以一定压力作用于人体表面。擦洗过程中末端执行器的有效清洗面为浴球受压变形后与人体接触面积，浴球移动形成具有一定宽度的条形清洗区域。为使清洗区域尽可能全面覆盖人体，需要基于人体表面特征进行合理的擦洗路径规划，路径既要尽可能全面覆盖人体表面，又要具有足够的安全性和舒适性。

擦洗路径规划相较于移动路径规划在环境建模和路径生成原则上有一定区别。擦洗路径规划的目的是沿着人体复杂曲面生成路径点以实现洗浴机器人擦洗人体的自主导航，为保证其安全性和精确性，实时获取人体点云作为环境模型以还原人体表面特征。大部分路径规划算法无法直接基于点云离散空间点进行路径规划，而人工势场法由于障碍物斥力势场具有一定作用范围这一特性，将各个离散点作为障碍物构建斥力势场能够相互重叠构成一个整体斥力势场，所以人工势场法能够适用于基于人体点云的路径规划任务。但是为了保证末端执行器擦洗的有效性，不同于传统人工势场法，生成的擦洗路径应是一条沿着人体复杂曲面延伸的曲线，其以尽可能全面覆盖障碍物轮廓为目标，并不是一条最短路径。同时，人工势场法路径规划过程中容易因进入人体表面凹形区域而陷入局部极小值，因此有必要改进人工势场法以适用于洗浴机器人的擦洗路径规划。

增设障碍物引力势场后机器人所受障碍物引力和斥力交替变化导致运动方向反复变化。由于每次添加路径点的步长为固定值，生成的路径规划轨迹存在如图4（a）所示的锯齿状波动，所以需要对轨迹进行平滑处理。人工势场法路径规划步长设置可以非常小，这样生成的路径点较为细密且远远超出洗浴机器人所需精度，在步长取值较小的情况下增大路径点取样间隔即可减小轨迹波动，如图4（b）所示为间隔5个点取样的路径规划轨迹。

2.3 克服局部极小值改进方法

人工势场法无可避免地会产生局部极小值问题，局部极小值问题是指在势场中某个位置斥力和引力合力为零，此时无法判断出下一步移动方向，因此会在此处停滞或徘徊而无法到达目标点［19］。增设障碍物引力势场的人工势场法会使机器人贴近障碍物表面运动，如果障碍物表面特征比较复杂，容易移动到凹陷区域而陷入局部极小值。如图5所示是一个不规则障碍物，此时机器人所受障碍物各个方向斥力的合力与目标点引力大小相等、方向相反，陷入局部极小值。

ρ（p，pg）表示当前机器人与目标点之间的距离，ρ（pn，pn+5）表示第n个路径点和第n+5个路径点之间的距离，s表示步长，当ρ（p，pg）＞2s且ρ（pn，pn+5）＜1.5s时，可以判断此时陷入了局部极小值，机器人当前位置为局部极小值点。

针对改进人工势场法路径规划过程中因进入凹陷区域而产生的局部极小值问题，采用添加虚拟目标点的方法加以解决。添加虚拟目标点的位置通过以下方法确定：针对如图5所示的局部极小值问题，首先连接局部极小值点和目标点，作该线段的垂直平分线，然后沿着该垂直平分线以与障碍物边界的交点为起点取距离ρo（斥力作用范围）作为虚拟目标点，如图6所示。虚拟目标点替换原目标点重新构造人工势场进行路径规划，此时若跳出了局部极小值，则路径规划到达虚拟目标点后再替换回原目标点继续完成剩余路径规划；若未跳出局部极小值，则连接局部极小值点和虚拟目标点作垂直平分线，按照上述方法继续添加新的虚拟目标点，直至跳出局部极小值。

3 基于点云的三维覆盖路径规划

擦洗路径要避免与环境中的人和物体发生碰撞，同时还要紧贴人体表面完成接触擦洗，因此环境建模应尽可能精确地反映出人体表面结构特征。为此，需实时获取人体深度信息，对深度信息进行滤波、分割、融合处理后，获得人体点云。

基于点云的路径规划通常先将点云曲面重构，然后在曲面重構基础上通过NURBS［20］、CAM等方法生成路径点。点云的采样点数以万计，曲面重构不仅会引入额外计算量降低路径规划速度，还会引入重构误差降低路径规划精度，因此提出一种无须曲面重构直接基于人体点云的三维覆盖路径规划算法。

如图7所示的兔子点云表面特征与人体相似，使用该点云对三维覆盖路径规划方法进行说明。首先在点云空间中放置一系列一定间距的切割平面将点云切段分割，垂直于x轴的切割平面沿x轴方向间隔dx距离将点云切段分割，如图7所示为其中两相邻切割平面及中间点云段。

然后依次将分割后各点云段所包含的三维空间点沿x轴方向投影到两切割平面之间的中间面上，即将点云段中的三维空间点压缩到中间二维平面，如图8（a）所示点云段沿x轴方向投影到两切割平面之间的中间面上，三维点云段被压缩为离散像素点组成的二维图像。将每个像素点都视为一个障碍物，按照第2章中阐述的改进人工势场法构造人工势场，其路径规划轨迹如图8（b）所示。此外，路径点相对障碍物表面的法向量方向就是该点在障碍物斥力势场中梯度方向，如图9所示。路径规划轨迹在三维点云空间中如图10所示，路径点从起始点开始贴近兔子表面到达目标点。

最后依次遍历所有分割后的点云段，最终兔子点云的三维覆盖路径规划结果如图11（a）所示。分割平面间隔dx设置越小，切段分割后点云段宽度越小，点云表面特征越不容易因投影至平面而被遮盖，路径规划的精度就越高，如图11（b）所示是减小dx后的三维覆盖路径规划轨迹。分割平面间隔dx设置过小会导致三维覆盖路径规划轨迹过于细密，末端执行器有效清洗面积重叠率过高，大大延长对人体的擦洗时间，因此应合理设置分割平面间隔dx。基于点云的改进人工势场法三维覆盖路径规划方法流程如图12所示。

4 仿真实验及对比分析

通过微软的Kinect-v2深度相机获取如图13（a）所示人体模型的多帧深度数据，经过滤波去噪、点云分割和点云融合等得到如图13（b）所示人体模型点云［21］。以该点云作为实验对象，在MATLAB中分别对改进人工势场法和三维覆盖路径规划算法的有效性进行实验验证。首先对基于人体点云的三维覆盖路径规划算法的有效性进行验证，然后将多种人工势场法和第2章中改进人工势场法进行仿真对比实验，分别对改进人工势场法的覆盖路径规划问题和克服局部极小值问题进行验证。实验中所用算法的各项参数名称及数值如表1所示。

4.1 三维覆盖路径规划仿真实验

将上述三维覆盖路径规划算法应用于人体模型点云进行实验验证，切割平面的间隔距离dx设置为100 mm。如图14（a）所示是基于人体模型点云的三维覆盖路径规划结果，路径规划轨迹无碰撞到达目标点且全面有效地覆盖了除头部、背部以外人体全身各个位置，实验结果表明该方法可以有效地完成对于人体点云的三维覆盖路径规划。

4.2 覆盖路径规划仿真实验及对比分析

将如图14（b）所示点云段1作为实验对象，分别采用传统人工势场法、文献［18］中凹陷障碍物补全的人工势场法和第2章中改进的人工势场法进行仿真对比实验，仿真结果如图15所示。由图15（a）可知，使用传统人工势场法进行路径规划，路径规划轨迹对障碍物覆盖率较低，作为擦洗路径会导致人体有些位置不能被有效清洗。由图15（b）可知，通过凹陷障碍物补全的人工势场法得到的路径轨迹避开了凹陷区域，有效缩短了路程，但擦洗路径规划以尽可能覆盖障碍物为目的，以往以获取最优路径为目的改进人工势场法无法有效适用。而图15（c）中，使用增设障碍物引力势场的人工势场法进行路径规划，路径点从起始点开始直到目标点都始终保持一定距离紧贴障碍物表面，其路径规划轨迹能够作为覆盖人体的擦洗路径。

如图16（a）（b）所示是基于图15（c）实验环境下分别减小和增大斥力作用范围ρo的路径规划结果。图16（a）所示路径规划轨迹相对图15（c）更加靠近障碍物，图16（b）所示路径规划轨迹相对图15（c）更加远离障碍物，由此可见，通过调整斥力作用范围ρo可以调节路径点与障碍物的距离。

4.3 克服局部极小值仿真实验及对比分析

以图14（c）所示点云段2为实验对象，分别采用传统人工势场法、文献［18］中凹形障碍物补齐的人工势场法、文献［17］中人工势场与A*组合算法和2.3节中添加虚拟目标点的人工势场法进行克服局部极小值对比仿真实验，实验结果如图17所示。由图17（a）可知，传统人工势场法路径规划过程中移动到人体凹陷区域时陷入局部极小值，最终在该位置停滞导致路径规划失败。由图17（b）可知，凹形障碍物补齐的人工势场法通过对凹形障碍物的预处理可以达到不陷入局部极小值的目的，但这也使得路径点不能沿着人体表面生成，无法满足擦洗路径规划的要求。由图17（c）可知，组合算法首先通过人工势场法进行路径规划，陷入局部极小值后通过A*算法跳出局部极小值区域，虽然能有效跳出局部极小值，但A*算法生成的路径点紧贴人体表面且有较多转折点，无法满足擦洗路径规划的要求。如图17（d）所示，陷入局部极小值后根据2.3节规则添加虚拟目标点，虚拟目标点替换原目标点并重新构造人工势场，此时成功跳出局部极小值继续前进，到达虚拟目标点后替换回原目标点并重新构造人工势场完成剩余路径规划。擦洗路径规划因需要贴近人体复杂表面移动而陷入凹形区域，采用添加虚拟目标点的方式可以有效解决因进入凹形区域而产生的局部极小值问题，且能够保证生成擦洗路径的有效性。

5 结束语

针对洗浴机器人末端执行器对人体全局的自动擦洗功能，提出一种基于人体点云的改进人工势场法擦洗路径规划算法。首先在传统人工势场法基础上增设障碍物引力势场，使路径规划轨迹能够覆盖障碍物轮廓，并通过添加虚拟目标点的方法，解决了因进入凹形障碍物而产生的局部极小值问题。然后获取人体点云进行切段分割，结合改进人工势场法依次完成分割后各点云段的覆盖路径规划，同时避免了点云曲面重构会引入额外计算量和重构误差的问题。最后对改进人工势场法和基于人体点云的三维覆盖路径规划算法进行了仿真实验验证。实验结果表明，该算法能够有效完成洗浴机器人自动擦洗功能所需的三维覆盖路径规划任务。有关点云切割平面间隔宽度dx的确定，其如何自适应于人体表面特征以保证清洗精度与覆盖度，将在后续工作中进行深入研究。

参考文献：

［1］王梅梅， 張先庚， 王红艳， 等. 我国失能老人长期照护现状及对策［J］. 全科护理， 2015，13（31）： 3112-3114. （Wang Meimei， Zhang Xiangeng， Wang Hongyan， et al. Long-term care of disabled elderly situation and countermeasures［J］. Chinese General Practice Nursing， 2015，13（31）： 3112-3114.）

［2］Department of Economic and Social Affairs， United Nations. World population prospects 2019： highlights ［EB/OL］. （2019-06-17）. https：//www.un.org/en/desa/world-population-prospects-2019-highlights.

［3］Liu Huaqing， Byles J E， Xu Xiaoyue， et al. Evaluation of successful aging among older people in China：results from China health and retirement longitudinal study［J］. Geriatrics & Gerontology International， 2016，17（8）： 1183-1190.

［4］杨振巍. 浅谈医疗机器人及发展前景［J］. 科技创新导报， 2018， 15（12）：104-105. （Yang Zhenwei. Discussion on medical robot and its development prospect［J］. Science and Technology Innovation Herald， 2018，15（12）： 104-105.）

［5］李奇林， 曹宇， 茆宁， 等. 护理机器人技术发展综述［J］. 江苏理工学院学报， 2020，26（4）： 62-70. （Li Qilin， Cao Yu， Mao Ning， et al. Summary of the development of nursing robot technology［J］. Journal of Jiangsu University of Technology， 2020，26（4）： 62-70.）

［6］陈雅. 洗浴辅助机器人的擦洗结构设计及其清洁效果评价［D］. 杭州： 浙江大学， 2021. （Chen Ya. Development of a scrubbing structure and cleanliness evaluation method for the bath assisted robot［D］. Hangzhou： Zhejiang University， 2021.）

［7］Dometios A C， Papageorgiou X S， Arvanitakis A， et al. Real-time end-effector motion behavior planning approach using on-line point-cloud data towards a user adaptive assistive bath robot［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2017： 5031-5036.

［8］Dometios A C， Zhou You， Papageorgiou X S， et al. Vision-based online adaptation of motion primitives to dynamic surfaces： application to an interactive robotic wiping task［C］//Proc of IEEE Robotics and Automation Letters. Piscataway， NJ： IEEE Press， 2018： 1410-1417.

［9］Papageorgiou X S， Dometios A C， Tzafestas C S， et al. Towards a user adaptive assistive robot： learning from demonstration using navigation functions［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2021： 965-970.

［10］Huang I， Chow D， Bajcsy R. Soft tactile contour following for robot-assisted wiping and bathing［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2022： 7797-7802.

［11］劉珂， 董洪昭， 张丽梅， 等. 基于改进人工势场法的物流无人配送车路径规划［J］. 计算机应用研究， 2022，39（11）： 3287-3291. （Liu Ke， Dong Hongzhao， Zhang Limei， et al. Path planning for logistics unmanned delivery vehicles based on improved artificial potential field method［J］. Application Research of Computers， 2022，39（11）： 3287-3291.）

［12］杜婉茹， 王潇茵， 贾福凯， 等. 基于多层双向A*的未知环境路径规划算法研究［J］. 计算机应用与软件， 2019，36（12）： 261-267. （Du Wanru， Wang Xiaoyin， Jia Fukai， et al. Path planning in unknow environment based on multi-layer BI-A* algorithm［J］. Computer Applications and Software， 2019，36（12）： 261-267.）

［13］裴以建， 楊超杰， 杨亮亮. 基于改进RRT*的移动机器人路径规划算法［J］. 计算机工程， 2019，45（5）： 285-290. （Pei Yijian， Yang Chaojie， Yang Liangliang. Path planning algorithm for mobile robot based on improved RRT*［J］. Computer Engineering， 2019， 45（5）： 285-290.）

［14］林韩熙， 向丹， 欧阳剑， 等. 移动机器人路径规划算法的研究综述［J］. 计算机工程与应用， 2021， 57（18）： 38-48. （Lin Hanxi， Xiang Dan， Ouyang Jian， et al. Review of Path planning algorithms for mobile robots［J］. Computer Engineering and Applications， 2021，57（18）： 38-48.）

［15］徐小强， 王明勇， 冒燕. 基于改进人工势场法的移动机器人路径规划［J］. 计算机应用， 2020，40（12）： 3508-3512. （Xu Xiaoqiang， Wang Mingyong， Mao Yan. Path planning of mobile robot based on improved artificial potential field method［J］. Journal of Computer Applications， 2020，40（12）： 3508-3512.）

［16］牛秦玉， 李美凡， 赵勇. 改进人工势场法的AGV路径规划算法研究［J］. 机床与液压， 2022， 50（17）： 19-24. （Niu Qinyu， Li Meifan， Zhao Yong. Research on improved artificial potential field method for AGV path planning［J］. Machine Toll & Hydraulics， 2022，50（17）： 19-24.）

［17］李庆华， 尤越， 沐雅琪， 等. 一种针对大型凹型障碍物的组合导航算法［J］. 电子与信息学报， 2020， 42（4）： 917-923. （Li Qinghua， You Yue， Mu Yaqi， et al. Integrated navigation algorithm for large concave obstacles［J］. Journal of Electronics & Information Technology， 2020，42（4）： 917-923.）

［18］许万， 程兆， 朱力， 等. 一种基于改进人工势场法的局部路径规划算法［J］. 电子测量技术， 2022，45（19）： 83-88. （Xu Wan， Cheng Zhao， Zhu Li， et al. A local path planning algorithm based on improved artificial potential field method［J］. Electronic Measurement Technology， 2022， 45（19）： 83-88.）

［19］梁献霞， 刘朝英， 宋雪玲， 等. 改进人工势场法的移动机器人路径规划研究［J］. 计算机仿真， 2018， 35（4）： 291-294. （Liang Xianxia， Liu Chaoying， Song Xueling， et al. Research on improved artificial potential field approach in local path planning for mobile robot［J］. Computer Simulation， 2018， 35（4）： 291-294.）

［20］张江涛. 基于点云数据处理的打磨机器人运动规划策略研究［D］. 天津： 天津理工大学， 2022. （Zhang Jiangtao. Research on motion planning strategy of 6-DOF industrial robot based on point cloud data processing［D］. Tianjin： Tianjin University of Technology， 2022.）

［21］陈聪梅. 基于Kinect的三维点云数据处理［D］. 苏州：苏州大学， 2013. （Chen Chongmei. Studies on the processing of 3D point cloud data based on Kinect［D］. Suzhou： Soochow University， 2013.）

收稿日期：2023-02-23；修回日期：2023-04-14  基金项目：大连理工大学成都研究院资助项目（XM01006）

作者简介：程龙（1998-），男，山东泰安人，硕士，主要研究方向为机器人机械臂路径规划；王欣（1972-），女（通信作者），天津人，副教授，硕导，博士，主要研究方向为工业机器人与动作规划（wangx@dlut.edu.cn）；吴迪（1972-），男，辽宁大连人，副教授，博导，博士，主要研究方向为基于5G平台的医疗机器人制造技术；冯志乾（1996-），男，河南焦作人，硕士，主要研究方向为移动机器人路径规划．