基于協作機械臂的航電開關柔性檢測算法

苗浩原，朱笑笑，周章勇，孫 濤，曹其新

（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，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)存在非對角元素，會導致機械臂各關節之間存在耦合作用力。但對于開關夾持這一場景，由于機械臂切換至阻抗控制后，由工具完成后續操作動作，機械臂本身的加速度和速度較小，因此此處產生的耦合作用相對較弱，對末端阻抗特性的影響十分有限。同樣的，考慮到機械臂在夾持開關后，后續操作動作由工具完成，機械臂本身無需運動，即x?d和x?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 大類開關，按鈕、撥桿和旋轉開關。第一種算法基于外力閾值檢測，通過廣義動量觀測器估計末端工具所受外力，并在控制機械臂運動時設置操作力閾值，實現對按鈕和撥桿開關的二次定位和安全操作。第二種算法基于阻抗控制，在末端工具夾持開關時切換機械臂控制模式為低阻抗模式，使工具順應操作內力，實現對旋轉開關的柔性夾持操作。

通過在模擬艙內和實際聯試環境下進行的開關檢測實驗，證明了該算法能夠在實際航電聯試環境中完成對不同種類開關的有效檢測，且具備較好的誤差適應性。另外，第二種算法能夠顯著降低開關操作開關時產生的內力，大大提高開關檢測的安全性。值得注意的是，針對同一類型、不同尺寸的開關，本文算法僅需調節不同的行程大小和力閾值大小即可，具備一定的通用性。