基于协作机械臂的航电开关柔性检测算法

苗浩原，朱笑笑，周章勇，孙 涛，曹其新

（1.上海交通大学机械与动力工程学院，上海 200240；2.国营芜湖机械厂航电部，芜湖 241007；3.南京航空航天大学自动化学院，南京 211106；4.上海航翼高新技术发展研究院有限公司，上海 200433）

航空电子系统是飞机驾驶舱上所有电子系统的总和，一个最基本的航空电子系统由操控、通信、导航和显示管理等多个系统和部件组成。经过定期保养和维修后航电系统部件在装机前必须经历航电联调联试，为的是验证各系统之间通讯接口及功能的正确性。航电系统作为现代飞机中的控制核心，其联调联试的重要性不言而喻。航电联调联试往往在另外搭建的航电联试环境，即模拟座舱中进行。环境中的各类设备按照实际装机方式连接，加以仿真的外部动态激励，以完成航电系统接口及功能的全面考察。舱内放置各类航电控制和显示面板，用于人工操作面板上的开关和观察显示屏上的图像变化。

当前航电联试任务需要人工完成所有测试流程，操作固定、步骤繁琐、耗时长。若能够开发自动化航电检测系统，则可以使测试人员摆脱繁重的检测任务，并解决由于疲劳、疏忽等人为因素造成的误检测和漏检测，提高检测效率和准确性。另外，航电系统不断提高的集成化程度，使得自动化测试成为了航电性能测试的重要趋势［1］。国内外已有较多对自动化检测系统的研究，但往往都集中在软件设计或检测系统本身的自动化设计层面，很少有研究关注目前仍需人为进行大部分检测操作这一问题。

有一些国内外机构尝试使用机器人代替人，完成航电系统的自动化检测。国内电子科技大学在2017 年开发了一套检测系统和软件框架［2］，使用ABB 六轴机械手臂完成按钮的点击等操作。国外的德国汉莎技术（Lufthansa Technik，LHT）公司于2019 年开发了基于工业机器人的检测系统RoC⁃CET［3］，通过多摄像头和精密末端力传感器完成机舱检测任务，主要用于民航客机的空客、波音等机型的检测。上述检测系统存在以下几个问题：使用示教的方法控制工业机器人完成开关的检测，较为繁琐，且系统无法快速部署至新机型；机器人为工业机械臂，体积较大，需要安装在护栏内，即需要搭建另外的检测舱，实用性较低。

本文使用配备关节力矩传感器的协作型轻量化机械臂KUKA LBR iiwa 加装RGBD 相机对航电开关进行快速定位和安全检测。主要关注机器人检测系统中的核心问题，即如何设计机械臂的操作算法，通过协作臂本体的力感知能力，实现更加安全、柔顺的开关操作。据了解，国内外较少有使用协作臂力感知能力对开关进行操作的相关研究，大多通过示教的方法［2⁃3］进行检测，或设计柔性夹具［4］，通过夹具的柔性完成对开关的安全操作。但上述方法均存在部署时间长及缺乏可扩展性等问题。

由于机械臂需要与开关发生接触，因此本文主要研究带接触的机械臂操作问题，这也是机器人研究领域中热点和难点问题。在丰富接触下的机械臂操作研究领域，学界主要聚焦于轴孔装配及打磨等应用。主要分为基于模型和基于学习的两大类方法。前者较为传统，往往通过3 种方法的组合来实现机械臂的带接触操作：建立接触模型［5⁃6］、视觉定位进行引导［7］以及使用预先设定的控制策略［8］（柔顺控制、力位混合控制［9］或自适应控制［6］）。后者近年来较为流行，主要通过模仿人工示教数据，通过各类模型提取人工示教轨迹中的特征［10］，再让机器人进行复现，或在与环境的交互中学习，通过设置奖励机制让机器人自己学习和探索动作策略，如各类强化学习算法［11⁃13］。

上述研究往往假设环境较为简单、理想，机械臂有较大的运动和调整空间。而航电开关检测环境则较为复杂、开关排列紧密，而且开关本身还具有种类多（共3 类，即按压型、拨动型和旋转型）、体积小、位置固定无法任意移动的特点，这对机械臂操作算法提出了两点要求：算法需要考虑在复杂环境中的实用性；需要具备对不同种类开关和误差的适应性。

因此，本文提出两种基于力觉的开关操作算法用来检测不同种类开关：基于外力阈值检测的开关接触定位与操作算法，用于处理开关的二次定位以及安全操作；基于阻抗控制的开关柔性操作算法，用于处理被夹持开关的柔性操作。两种算法在KUKA LBR iiwa 机械臂上实现，末端不加装六维力矩传感器，仅通过各轴的关节力矩传感器对机械臂与开关接触时受到的外力进行估计。通过开关操作的实际实验表明该算法具备较好的鲁棒性和适应性，能够提高开关检测的成功率和安全性。

1 基于外力阈值检测的开关接触定位与操作算法

1.1 机械臂末端的接触外力估计算法

本文使用KUKA LBR iiwa 工业协作机械臂，使用谐波减速器，在对机械臂进行动力学建模时，需要考虑其柔性。因此当运动的机械臂受到末端外力作用时，其动力学方程可以简化表示为［14］

式中：θ∈Rn为经过减速后的等效电机位置；q，q̇，q̈∈Rn为连杆位置、速度和加速度；M(q)∈Rn×n为对称、正定的惯性矩阵；B∈Rn×n为电机惯量矩阵；C(q，q̇)q̇∈Rn为离心力和科氏力向量；C(q，q̇)∈Rn×n为克里斯托弗尔符号表示的矩阵；g(q)∈Rn表示重力向量；τ∈Rn为连杆侧的弹性力矩向量；τm∈Rn为经过减速器放大后的等效电机指令力矩向量；τf∈Rn为减速器处摩擦力向量；τext∈Rn为末端外力在关节连杆处产生的等效力矩向量，满足τext=JT(q)Fext；J(q)∈R6×n为末端连杆的雅克比矩阵；Fext∈R6为笛卡尔坐标系中的末端外力；K为考虑关节柔性时的关节刚度矩阵。另外，KUKA LBR iiwa 的关节力矩传感器安装在连杆侧，因此其测量值τJ即为减速器对连杆施加的力矩τ。

根据柔性机械臂的广义动量观测器方法［15］，定位广义动量p∈Rn为p=M(q)q̇，则其导数为

式（7）可以看作是以r为变量，τext为输入的一阶系统，其传递函数为

当Ko足够大时，r≈τext，r以指数形式收敛到τext。因此，通过计算r即可估计出末端对应的关节力矩τext，而末端外力则可以通过Fext=J-T(q)τext求得，其中包括力和力矩分量两部分。

从式（6）可以看出，该计算无需其他传感器，只需要系统状态q，q̇，连杆侧力矩τ。由于加装了力矩传感器，连杆侧力矩可以由测量值τJ代替［16］，而且无需考虑关节摩擦力τf。

需要注意，在式（6）中需要对CT(q，q̇)进行计算。通常的动力学矩阵C(q，q̇)，g(q)由迭代牛顿欧拉法（Recursive Newton⁃Euler，RNE）计算得到，但无法得到CT(q，q̇)，因此需要使用改进牛顿欧拉法（Modified Newton⁃Euler，MNE）求解［17］。

使用基于动量观测器的外力估计方法，无需计算加速度，不会受到加速度估计的噪声影响，且无需计算惯性矩阵的逆，计算量小，可用于机械臂的实时运动。

1.2 基于力阈值判断的开关接触定位与操作算法

首先定义开关操作时的坐标系（图1）。F为法兰坐标系，设定末端工具的进给方向为x轴，张开方向为y轴，从而工具中心点（Tool center point，TCP）坐标系为T，面板坐标系为P。

图1 机械臂末端坐标系定义Fig.1 Coordinates definition of end effector

通过上述外力估计算法，各类开关在操作时的接触力Fext，x的曲线变化如图2 所示。

图2 示教控制机械臂操作开关时得到的TCP 坐标系下的末端笛卡尔外力Fig.2 End⁃effector Cartesian external force in TCP frame when operating button through demostration

从外力的变化情况可以看出，在接触发生的Ⅰ时刻存在明显的力突变；而且对于各种类开关，都需要接触力达到特定大小（Ⅱ时刻），才能成功触发开关。因此，可以通过设定力阈值，判断机械臂是否已经与环境发生了接触，或判断机械臂是否达到了所需的操作力。具体的力阈值设定方法如下：

在控制机械臂向目标位置进行笛卡尔运动时，实时检测所受的外力，并设置所受的末端外力阈值为Fth，x。若实时估计的外力超过阈值，则触发信号cd(Fext，x)为1，让机械臂停止运动

cd(Fext，x)为0 时，表示机械臂所受外力未超过阈值，则继续向目标位置运动。

外力阈值的选取可以通过开关产品说明、测力计测量，或通过示教的方法控制机械臂操作开关，同时记录实时的位置和估计的外力曲线，选择合适的力阈值。针对不同种类开关，只需设计不同的运动方式并设置力阈值大小，即可实现开关操作。该方法具有一定的通用性。

2 基于阻抗控制的开关柔性操作算法

上述基于力阈值判断的操作方法能够解决大部分开关操作的问题，但对于需要使用工具夹持的开关操作来说，要求较高的定位精度。当定位误差存在时，由于在位置控制模式下机械臂末端刚性大，机械臂会对被夹持开关施加极大的内力。使用阻抗控制则能够降低机械臂末端的刚度，使机械臂顺应接触外力，增加操作的柔性和安全性。

2.1 阻抗控制原理与实现

使用笛卡尔阻抗控制［18］，将机械臂末端建模为二阶质量弹簧阻尼系统，通过设置所需的质量系数、刚度系数或阻尼系数，改变机械臂关节或末端表现出的质量、刚度和阻尼特性，达到期望的与外界交互的动力学特性。其控制目标方程为

式中：x͂=x-xd表示机器人末端当前笛卡尔位姿与期望笛卡尔位姿的误差，x，xd∈R6分别为末端当前和期望位姿在笛卡尔空间的最小表示法。Md、Dd、Kd分别为期望的笛卡尔空间惯性矩阵、阻尼矩阵和刚度矩阵。

机器人动力学方程在笛卡尔空间下可以表示为

式中：Fτ为电机输入力矩τ在笛卡尔空间中对应的输入向量，满足τ=JT(q)Fτ，J(q)∈R6×n为末端连杆的雅克比矩阵；Fext为笛卡尔空间所受的末端外力；τext=JT(q)Fext。Λ(x)，μ(x，ẋ)和Fg(x)分别为笛卡尔空间下的惯性矩阵，离心/科氏力矩阵和重力向量

把式（10）代入式（11）中，并令Md=Λ(x)（避免需要使用腕部力传感器获得Fext），可以得到简化后的阻抗控制方程为

使用简化后的控制器无法人为定义二阶系统对外表现出的质量特性，但对于本文顺应夹持时的定位误差而言，主要关注阻抗控制的刚度和阻尼特性，质量特性可以忽略。

2.2 基于阻抗控制的开关柔性夹持操作

当机械臂通过力阈值检测接触面板并上抬后，将控制方法切换为上述阻抗控制，然后夹爪闭合，继续完成对开关的旋转操作。xd设置为期望的末端位置，即切换阻抗控制前末端的实际位置，也是开关定位模块给出的开关位置。夹爪夹持开关受力图如图3 所示。假设夹爪和末端法兰为刚性连接，夹爪视作机械臂末端连杆的一部分，且环境中开关的刚性足够。在操作前（图3（a）），机械臂已经到达了目标位置（指令下发的位置，该目标位置存在定位误差），此时夹爪中心位置为xd。当夹爪闭合时，受到环境中的开关的阻碍，开关对末端施加作用力Fext，使末端实际位置偏移至x。机械臂为了修正当前与目标位置间的误差x͂，会对开关施加反作用力为F′ext（图3（b））。

图3 夹持开关时的夹爪受力分析Fig.3 Gripper force analysis when the switch is clamped

为了使机械臂末端在存在定位误差x͂时，不产生较大的修正力，而是顺应定位误差造成的外力，采用式（13）的阻抗控制方法，控制机械臂完成开关夹持，并将刚度矩阵中的位置分量设置为较小值。由于操作开关时，定位误差不会引起夹爪姿态的变化，刚度矩阵中的旋转分量可以设置为较高值，使末端保持原有姿态即可。

另外，控制器（13）回代动力学方程后，系统的阻抗特性方程变为

式中惯性矩阵Λ(x)存在非对角元素，会导致机械臂各关节之间存在耦合作用力。但对于开关夹持这一场景，由于机械臂切换至阻抗控制后，由工具完成后续操作动作，机械臂本身的加速度和速度较小，因此此处产生的耦合作用相对较弱，对末端阻抗特性的影响十分有限。同样的，考虑到机械臂在夹持开关后，后续操作动作由工具完成，机械臂本身无需运动，即ẍd和ẋd为0，因此阻抗控制器可以做进一步简化

2.3 末端偏差的表示方法

在阻抗控制实现时还需要考虑机械臂末端偏差x͂的位置和姿态分量的表示方法。位置误差参考系的选择决定了阻抗系数的意义，即是在哪个坐标系下的阻抗行为。由于使用阻抗控制的意义是为了能够柔顺操作开关，因此选择在开关坐标系定义阻抗行为，即末端工具的期望坐标系D下表示位置误差，位置误差表示为x͂t：=pD，如图4 所示。为了便于标注，图示选为夹爪闭合前，夹爪闭合后末端位置即在T处，期望坐标即为D。

图4 位置误差参考系定义Fig.4 Coordinate definition of translational error

而姿态部分的表示法则较为复杂，因为对于末端旋转矩阵RST∈SO(3)来说，没有全局的最小参数表示法，因此需要选取如欧拉角、轴角法、单位四元数或李群李代数来表达姿态的旋转矩阵。本文使用单位四元数(s，v)来表示姿态误差［19］，单位四元数能够给出旋转矩阵的无奇异表示。将姿态误差表示为对应四元数误差的虚部向量，设v为旋转矩阵RST对应的单位四元数的虚部向量，则x͂r：=v。

3 实验与分析

为了评价算法在控制机械臂检测航电开关时的有效性、成功率以及鲁棒性，搭建了航电检测模拟环境，并设计了3 组实验。前两组分别对两种算法进行验证，最后的综合实验使用机器人检测系统平台，配合显示屏识别模块进行实际的航电检测，在实际进行自动化检测时，本文算法具备较好的实用性和较高的开关操作成功率。

3.1 实验系统

实验环境为仿制的航电检测模拟舱，如图5 所示。各个航电设备面板以及显示屏安装在舱内，需要使用机器人检测系统对面板上的开关进行操作，并识别显示屏上对应的图像变化，以完成自动化航电联调联试。

图5 仿制的航电检测模拟舱Fig.5 Replica of avionics detection simulation cockpit

基于2 个KUKA LBR iiwa 七自由度机械臂搭建机器人检测系统，如图6 所示。机械臂末端安装了手眼系统：RealSense 深度相机及光源用于开关定位以及开关状态识别，多功能操作工具用于操作开关，通过旋转、夹爪闭合以及机械臂末端运动的配合，能够实现全部3 大类开关的检测动作。经过设计，两个机械臂安装的位置能够使机械臂工作空间覆盖舱内的全部开关。实验所用工控机CPU 为i7⁃6700 3.40 GHz，内存为16 GB，操作系统为Ubuntu 18.04。

图6 双臂机器人航电检测系统Fig.6 Two-arm robot system for avionics detection

通过机器人操作系统（Robot operating sys⁃tem，ROS）框架，将系统各算法模块视为“节点”，实现分布式通信和调用。本文提出的算法属于其中的操作模块，由C++实现，通过KUKA LBR iiwa 提供的开发接口与机械臂进行通信。另外还有显示屏图像识别模块，主要基于Halcon 识别数量大、种类多的图像元素，用于检测开关操作后显示屏上图像是否发生了期望的变化，因此能够在综合实验中证明开关操作的有效性。

3.2 外力阈值检测实验

为了验证外力阈值检测方法可以实现对各类开关的检测，并能够顺应一定的定位误差，本实验控制机械臂和末端工具，对按钮和拨杆两类开关进行检测。

实验开始前，首先通过示教引导机器人运动至各类开关附近的固定预操作位置，然后使用力阈值检测算法控制机械臂对各类开关进行操作。对于需要设置力阈值的机械臂运动轨迹（即可能接触开关或面板的运动轨迹），根据预操作位置距离开关的大致位置，设置大于实际距离的机械臂运动目标位置。针对两类开关的操作参数设置见表1。

表1 两类开关的操作参数Table 1 Operation parameters of two types of buttons

检测按钮和拨杆开关时，估计的工具末端外力和位置曲线，以及4 个标注时刻对应的夹爪实际位置如图7、8 所示。

可以看到在操作两类开关的Ⅰ至Ⅱ时刻，机械臂执行向下运动25 mm，正在靠近面板等待接触。在Ⅱ时刻，估计的外力超过了阈值5 N 限制，因此使机械臂停止了运动。此时机械臂还未达到目标的25 mm，说明使用外力阈值可以对面板进行二次定位。而在操作拨杆时，与面板的接触力已远大于5 N，这是由于夹爪与面板发生速度较快的刚性碰撞造成的，使得信号cd(Fext，x)被触发到控制机械臂停止过程中（100～200 ms）夹爪依然在对面板施加力。在实际检测中，该碰撞由于时间较短，力不是太大，远不会对面板造成损坏。该问题可以通过减慢末端运动速度，减缓力的增长，或在末端加装软胶增加一定的缓冲，减小施加的力来解决。

对于按钮开关，在时刻Ⅲ，此时机械臂刚接触开关。在时刻Ⅳ前，开关已经被充分按下，但外力未到设定的阈值；而当检测到外力超过设定阈值后（图7 时刻Ⅳ），机械臂即停止运动，保证了开关被成功触发。

图7 使用力阈值检测方法控制机械臂操作按压型开关时的TCP 笛卡尔外力与TCP 末端位置的对比图Fig.7 Comparison of TCP Cartesian external force and position when operating a press button using the proposed algorithm

对拨杆开关来说，当对拨杆施加的力大于拨杆档位的触发力时，拨杆会在内部弹簧的作用下立刻复位至下一档位。因此在图8 中会出现两个力峰值，但由于该峰值均未超过力阈值11 N，因此拨杆可以执行完15 mm 的拨动距离。

图8 使用力阈值检测方法控制机械臂操作拨杆开关时的TCP 笛卡尔外力与TCP 末端位置的对比图Fig.8 Comparison of TCP Cartesian external force and position when operating a switchbutton using the proposed algorithm

该实验说明了力阈值方法检测开关的有效性，能够实现二次定位，并限制操作力的大小。

3.3 阻抗控制柔性夹持实验

上述力阈值方法仅对按钮、拨杆开关适用，但对于需要夹持的旋转开关而言，若存在开关中心的定位误差，则会在夹爪闭合和后续旋转时，对开关施加极大的内力。因此设计本实验来验证阻抗控制算法，能够顺应定位误差，减小操作内力，实现操作的柔性。

通过示教使机械臂运动至开关正上方，并打开夹爪，当使用机械臂末端工具夹持并旋转开关时，对比是否使用阻抗控制估计的外力曲线，如图9所示。

图9 对旋钮进行夹持和旋转操作时使用阻抗控制前后末端所受的外力对比图Fig.9 Comparison of external forces of the end effector before and after using impedance control when clamping and rotating a rotary button

Ⅰ至Ⅱ时刻，机械臂执行向下运动25 mm，正在靠近面板等待接触。在Ⅱ时刻，估计的外力超过了阈值5 N 限制，机械臂停止运动，阻抗控制在时刻Ⅲ选择开启或不开启。在时刻Ⅳ，夹爪闭合夹持住旋钮，并开始旋转250°，直至时刻Ⅴ。

可以看出，即使在定位较为准确的情况下，使用纯位置控制对开关进行旋转操作会产生极大的内力，yz方向分别为19.74 N 和20.70 N。而使用本文方法，则能够显著地减小操作内力，yz方向所受的力分别为2.36 N 和2.69 N。

为了证明基于阻抗控制开关夹持算法的误差顺应性，通过人为引入定位误差，即让机械臂在开关上方的初始位置发生小范围变化，再记录使用阻抗控制让夹爪旋转开关时，末端yz方向受到的最大外力。结果如表2 所示，当在开关附近±5 mm内进行旋转开关的夹持和旋转，其最大外力均不超过10 N，体现了较好的操作柔顺性。

表2 在开关xy 方向±5 mm 内进行旋转开关夹持和旋转时末端所受yz 方向的最大外力Table 2 The maximum external forces in y and z axes when the end effector clamps and rotates a rota⁃ry button within 5 mm range in x or y direction

3.4 开关检测综合实验

为了验证算法能够在实际机器人系统中完成开关操作，使用操作算法完成航电检测综合实验。假设已经通过视觉定位或手动测量的方法估计出了各类开关位置，但可能存在1～3 mm 的定位误差，下一步需要使用两种开关操作算法对各类开关进行检测，综合实验的流程图如图10 所示。图中W为世界坐标系，P为面板坐标系，B为开关坐标系，O为预操作坐标系。

图10 综合实验流程图Fig.10 Flowchart of the comprehensive experiments

实验使用机械臂操作模块并配合显示屏图像识别模块，对某一实际检测流程中的共计125 个开关操作任务和96 个图像识别任务进行综合检测。开关操作任务包含对4 个面板上，共计3 大类、43个开关的检测，包括40 个按钮开关（存在不同尺寸）、2 个两位拨杆开关以及1 个旋钮开关。通过图像识别模块可以判断开关是否被触发，即开关操作任务是否成功执行。各类开关的操作成功率见表3。

表3 针对某一实际检测流程操作各类开关的成功率Table 3 Success rate statistics of operating various kinds of button in a real testing procedure

结果表明，开关操作任务的成功率较高，122次操作中仅漏检测了某一显示屏按钮。误操作原因是显示屏四周存在凸台，在操作位于边缘位置的按钮时夹爪旋转的一部分误与凸台发生了接触，导致超过了按动的力阈值，算法误认为已经完成了按动。后续可以通过夹爪旋转至某一不发生接触的角度解决此问题。

通过3 组开关操作实验，验证了本文提出的开关操作算法能够配合机器人检测系统，完成航电开关的自动化检测，具备较好的误差顺应性以及很好的操作成功率。整体机器人检测系统能够降低部署时间，大大提高了系统的实用性和可扩展性。

4 结论

在开发基于协作机械臂的航电联调联试系统时，本文针对机械臂在检测航电开关时存在环境复杂、需要适应不同种类开关与定位误差等问题，提出了两种基于关节力传感器的机械臂柔性控制算法，以检测航电联试环境中主要的3 大类开关，按钮、拨杆和旋转开关。第一种算法基于外力阈值检测，通过广义动量观测器估计末端工具所受外力，并在控制机械臂运动时设置操作力阈值，实现对按钮和拨杆开关的二次定位和安全操作。第二种算法基于阻抗控制，在末端工具夹持开关时切换机械臂控制模式为低阻抗模式，使工具顺应操作内力，实现对旋转开关的柔性夹持操作。

通过在模拟舱内和实际联试环境下进行的开关检测实验，证明了该算法能够在实际航电联试环境中完成对不同种类开关的有效检测，且具备较好的误差适应性。另外，第二种算法能够显著降低开关操作开关时产生的内力，大大提高开关检测的安全性。值得注意的是，针对同一类型、不同尺寸的开关，本文算法仅需调节不同的行程大小和力阈值大小即可，具备一定的通用性。