基于视觉传感的医疗服务机器人位姿精度测试系统的研制

【作者】胡晟，吴贤欢，陈旭雯

1 上海市医疗器械检验研究院，上海市，201318

2 浙江谱麦科技有限公司，宁波市，315040

0 引言

随着智能传感与物联网技术的发展，在智能医疗领域，服务机器人正逐步取代人力，完成一些简单的日常任务[1]。尤其是在新冠疫情期间，医疗机构的无接触消杀需求激增，提升了医疗服务机器人的普及率。而在未来，体育场、机场等人员密集场景的消杀防疫需求也会进一步提升。其相关的研究正得到越来越广泛的重视[2-3]。而对于这种医疗服务机器人而言，绝对定位精度是一项重要的性能指标[4]。目前常用的确定移动机器人定位的方法是使用SLAM技术，通过获取固定在机器人上的多源传感器信息进行数据融合，从而进行自定位[5-7]。然而，该方法只能估计机器人在建立地图上的定位，而无法准确得到定位精度信息[8]。因此，我们提出了一种基于多目视觉传感的医疗服务机器人位姿精度测试方法，该测试方案能够高精度地、实时地测试位姿精度，可以为医疗服务机器人厂商的研发提供准确的判断依据。

1 基于视觉传感的医疗服务机器人测试系统

基于视觉传感的医疗服务机器人测试系统由视觉传感单元、医疗服务机器人、测试环境以及测试分析模块等组成（见图1）。其中医疗服务机器人是被测对象，机器人本体上安装有激光扫描仪，可以根据扫描的图像得到环境地图信息，自动规划路线并实现避障[9]。本系统采用多目红外视觉传感相机进行摄像，能够实时获取视觉靶标的定位信息。同时，在医疗服务机器人表面固定有多个视觉靶标，可以通过多目红外视觉传感相机获取视觉靶标的三维笛卡尔坐标信息，通过一定策略构造机器人工具坐标系，得到机器人的测量位姿。此外。医疗服务机器人也根据自身激光扫描仪自定位，估计出在虚拟地图上的坐标值，结合测量的位姿信息，得到医疗服务机器人的绝对位姿精度。

图1 基于视觉传感的医疗服务机器人测试系统Fig.1 Service robot testing system based on vision sensing

为了保证测试系统的精度和测试效率，将型号为TRD-01医疗服务机器人放置在医疗服务机器人测试系统进行验证。医疗服务机器人及障碍物，如图2所示。从图中可以看出，在机器人上安装了多个视觉靶标，超过了空间建系所需的最小个数（3个）。通过测量计算冗余的视觉靶标，通过最小二乘法进行最优拟合，进一步减少由于测量不确定度引入的误差。此外，在测试环境中安放了一些障碍物，来验证医疗服务机器人空间覆盖率性能。

图2 医疗服务机器人及障碍物Fig.2 Medical service robot and obstacles

2 多目视觉传感转站原理及参数辨识

医疗服务机器人测试系统采用多台红外相机进行同时测量。不同相机的测量坐标系不同，转站表示将不同红外相机对应地从测量系转换到主测量系下以进行数据统一的过程。多个红外相机同时进行测量时，选择其中一个相机为主站，其他相机作为从站。主站的测量坐标系不进行坐标变换，从站的测量坐标系经过坐标变换与主站坐标系进行统一[10]。

以3个红外相机A、B、C测量为例（见图3），在视场范围内下的视觉靶标记为pi，也称作公共测量点，i为视觉靶标的序号。

图3 多目视觉传感转站示意图Fig.3 Schematic diagram of multi-eye vision sensor transfer station

首先将红外相机A设为主站，分析主站相机A与从站相机B的转站关系。公共测量点pi在主站测量系下的齐次坐标向量记作，在从站测量系下的坐标向量记为，则存在以下关系。

其中，是主站测量系到从站测量系的坐标转换矩阵，X、Y和Z表示坐标变换中的平移变换参数，Ψ、θ和φ表示旋转变换参数，对应于欧拉角绕X，Y和Z轴方向的转动角度。

当红外视觉传感相机的数量大于等于两个时，需要使用式(1)和(2)进行转站处理（将所有的从站测量数据转换到主站测量系下），从而使所有测量数据统一在主站测量系下描述。实现转站需要辨识式(2)坐标转换矩阵中包含的6个平移旋转参数，分别是Ψ、θ、φ、X、Y和Z。参数辨识方法如下：选择所有视觉传感器视场空间中的任意多个公共测量点（推荐选择7个公共测量点）；对于每一个公共测量点，均可以根据式(1)列出3个等式方程组；代入不同视觉传感的测量数据，得到包含6个待辨识位移旋转参数的非线性等式方程组（对应7个公共测量点共21个方程）；由于方程组的方程个数远大于待辨识参数个数，可以采用高斯-牛顿法求其最小二乘解[11]。

3 数据融合与工具坐标系建立

图4描述了基于视觉传感的医疗服务机器人测试系统中服务机器人基坐标系{b}、主测量坐标系{m}以及坐标原点的示意图。机器人的位姿测量数据是依据测量的多个视觉靶标信息融合得到的。此外，由于基于视觉传感的测量数据是描述在主测量坐标系{m}下的，对于医疗服务机器人工作面的位姿精度表示问题，需要将主测量坐标系与服务机器人基坐标系在零位状态下进行对齐，才能进一步对机器人位姿精度评价，而在这个过程中，多目相机数据融合以及工具坐标系的建立是十分关键的步骤。

图4 基于视觉传感的服务机器人测试系统示意图Fig.4 Schematic diagram of service robot testing system based on vision sensing

3.1 多目相机数据融合

在多目相机转站的过程中，需要将不同红外相机从测量坐标系转换到主测量坐标系下以进行数据统一。不同的红外相机都能得到同一组视觉靶标的测量信息数据，需要对测量得到的多组数据进行数据融合。根据相机到视觉靶标的距离对多组测量数据进行归一化处理。

其中，lA、lB和lC分别表示对应相机到视觉靶标的距离，下标i对应不同位型下的数据。由于相机的测量精度与相机到测试点位的距离平方呈正相关关系，因此采用该参数作为归一化系数。后续所有的测量点位都是基于多目相机测量信息进行归一化处理后的数据。

3.2 工具坐标系建立

图4中的服务机器人工作表面上安装有三个视觉靶标a、b和c，需要对其进行构建工具坐标系，才能通过视觉传感系统实时准确获取服务机器人工作面的位姿信息。在第i个位型下，三个视觉靶标中心点坐标经过数据融合在主测量系下的坐标为，其中下标a、b和c表示对应的视觉靶标。工具坐标系的原点可以表示为：

此外在第i个位型下，使用三个视觉靶标的位置信息合成服务机器人工作面的姿态信息，可将姿态描述为3×3矩阵Qi：

其中，ai、oi、ei均为3×1的列向量：

其中，{·}3×1表示取该列向量的前3项组成的3×1列向量。3×3矩阵表示的姿态Qi也可以转化为欧拉角的表示方法：

式中，Qi(j,k)表示Qi矩阵的第j行第k列的元素。γai、γbi、γci为单位为rad的欧拉角。

4 服务机器人位姿精度测试

采用多红外视觉传感相机方案对图1所示型号为ABIR X8医疗服务机器人的工作面位姿精度进行测试。表1列出了任意选取的5个特征点位的位姿精度测试结果。由表可看出，5个点位数据中，最大的定位误差为1.578 mm，最大定位角度误差为0.478°。

表1 服务机器人位姿精度测量结果Tab.1 Experimental results of pose accuracy

为进一步测试服务机器人工作面的轨迹精度，令服务机器人在如图5所示的障碍物环境中运行进行精度和覆盖率测试。图5中的灰色部分代表障碍物，白色部分为地面。

图5 系统测试环境Fig.5 System test environment

图6描述了医疗服务机器人在移动过程中的轨迹路线，通过测量记录机器人的轨迹，并结合机器人的位姿信息，可以给厂家提供控制优化数据。图7则结合移动机器人的物理特性得出了机器人的空间覆盖率信息，经过测试，该机器人的空间覆盖率为80%以上。此数据对医疗服务类机器人标准制定意义很大。

图6 医疗服务机器人轨迹测试Fig.6 Medical service robot trajectory test

图7 医疗服务机器人覆盖率测试Fig.7 Medical service robot coverage test

6 结论

我们针对医疗移动服务机器人，提出了一种基于多目视觉传感对其工作面进行位姿精度测试的方法。本方法采用多目视觉传感器测量固定在服务机器人工作面上的靶标，通过非线性最小二乘法对测量坐标系与机器人基坐标系进行对齐，并融合多目图像数据来实现工作面位姿精度测试。最后，将本视觉测试方案在医疗服务机器人上进行试验验证，可以准确得到其轨迹精度以及空间覆盖率信息。本测试系统和方法满足国标GB/T 38124 《服务机器人性能测试方法》项目和精度的要求。填补了国内这一块研究的空白，指出了未来相关研究的方向。对未来厂家和企业进行医疗服务类机器人的研究具有很大的参考意义，相关设备可以为提高国内医疗服务机器人的精度提供一臂之力。