基于預測模型的遙操作參數辨識及模型更新算法研究

薛載敬，宋荊洲

(北京郵電大學自動化學院，北京 100876)

1 引言

隨著空間活動的廣泛開展及空間機器人遙操作技術的不斷發展，對于主端操作體驗和遙操作系統的透明性和穩定性的要求越來越高。數據傳輸的大時延問題是空間遙操作的核心問題，圖形預測顯示技術是解決遙操作中時延問題的有效手段[1]。事先針對從端環境進行幾何建模可以有效提高實時視覺反饋，針對從端接觸模型建模實時力覺反饋可以更好地輔助操作者完成試驗任務。在過去的幾十年里，許多學者針對預測模型的參數辨識和模型更新問題進行了大量研究[2]。

遙操作系統中從端環境主要包含幾何特征和動力學特征，其中幾何特征的描述主要通過虛擬場景建模以及視頻傳輸來完成，動力學特征的體現主要通過力覺生成和反饋來完成，相比于單一反饋系統，結合視覺和力覺的反饋完成試驗任務的效率和準確度得到提高[3]。例如在空間站手套箱等任務場景中，存在擦玻璃、旋擰閥門等需要力覺反饋來提高操作效率和操作準確度的任務，純靠視覺完成任務的效率較低下，操作者的心理負擔較大。

環境動力學特征主要包含：動力學環境模型建模和參數辨識[4-5]。考慮到力覺反饋需要保證刷新頻率才能有更好的效果，因此接觸類型一般簡化為單點接觸[6]。Kelvin-Voigt模型將接觸力與位置的關系等效為線性彈簧和阻尼的并聯結構，由于其簡單有效和線性特性，在過去的環境動力學建模中得到了廣泛的應用。但是Kelvin-Voigt模型的一個顯著缺點是由于不連續性存在初始階段的力突變問題和恢復階段的反向力問題，限制了其在柔性對象上應用。非線性的Hunt-Crossley模型通過阻尼項與位置的耦合，解決了連續性問題，更接近實際物理性質，在柔性對象的建模中起到了很好的效果，但其非線性也增加了參數辨識的難度[7]。

在環境的動力學參數辨識方面，Singh等[8]提出了使用一種模型參考自適應控制器來估計環境的剛度；Love[9]等較早使用了遞歸最小二乘法來辨識環境剛度，并用于調整從端阻抗控制器的參數，改善性能；Diolaiti[10]率先將遞歸最小二乘法用于辨識Hunt-Crossley模型；Colton等[11]則使用了指數加權遞歸最小二乘法辨識非線性模型；Xu等[12]提出了有關末端執行器轉動參數辨識的相關研究方法，類似于接觸動力學參數辨識方法的處理方式，使用轉動慣量和角加速度的乘積作為主項表示扭矩，應用到立方體的轉動試驗中，有效地驗證了轉動參數的引入滿足遙操作操作任務場景復雜性和豐富性的要求。

對于基于預測模型的遙操作系統，從端環境發生變化的同時，模型參數同樣需要更新，傳遞回主端[13-14]。通常采用模型參數更新算法來避免此類情況發生，不直接更新從端反饋回來的模型參數，而是采取一種延遲或者漸變的方法控制參數變化，使得操作者可以很好地適應參數變化的場景，系統的穩定性也可以得到保證[15]，但存在參數更新效率較低以及輸出力反饋不連續，操作體驗較差等問題[16-17]。

為了在系統穩定下提高參數辨識效率，本文針對基于預測模型的遙操作中的參數辨識及模型更新算法展開研究。首先針對接觸進行環境建模和參數辨識的理論分析和試驗驗證，并將其擴展到扭轉動力學建模；然后針對環境模型參數在線誤差修正及更新，提出一種自適應漸進更新算法，在保持操作體驗的前提下提高參數更新的更新速度；最后，搭建遙操作試驗平臺驗證上述理論的準確性和實用性，完成簡單的試驗任務。

2 基于預測模型的遙操作控制方法

基于預測的遙操作控制方法是根據已知的從端執行器和接觸環境信息，在主端預先通過Unity3D等虛擬場景引擎構建幾何模型和環境動力學模型。地面操作者操縱主端機器人發送控制指令信息至從端機器人的同時，相應的運動控制信號也將輸入主端的預設虛擬模型中實時響應，虛擬模型將生成對應匹配的圖像和力覺信息提供給地面操作者，形成臨場感體驗，從而將通信時延排除在外，輔助操作者在較高時延的情況下完成相應操作任務。這一手段也日益成為遙操作中時延克服的主要技術路線和研究重點。

基于預測模型遙操作系統工作流程結構如圖1所示，操作者于地面操作主端機器人，相應的控制信息經過通信環節傳遞至從端機器人的同時輸入到主端的預設仿真模型，主端預設模型根據運動狀態信息響應生成相應的預測力反饋給操作者。從端機器人的運動控制系統根據位置控制信號生成相應的電機控制信號驅動執行器工作。從端執行器運動的同時，根據力傳感器測量的接觸力信息和機械臂末端位置狀態信息實時在線辨識出相應的環境動力學參數并傳輸回給主端，修正虛擬預測環境模型[18]。

圖1 基于預測模型的遙操作系統Fig.1 Structure of the model-mediated teleoperation

3 扭轉接觸動力學環境建模及參數辨識

3.1 環境模型建模

在基于預測模型的遙操作系統中，從端執行器與環境發生相互作用的過程中會產生各種各樣的交互形態，比如接觸力以及力矩的產生。本文將相互作用的過程簡化為單點接觸，將六維的方向進行解耦，后續會針對復雜交互場景進行進一步深入研究，提供更加豐富的交互體驗。

選擇適合某種場景下交互環境的模型是保證參數辨識準確度的前提和基礎，選取的標準即為將從端執行器與環境的接觸抽象為單點接觸，接觸發生時環境是靜態且表面光滑，不同的穿透方向之間不存在耦合關系。在基于預測模型的遙操作系統中，用來描述執行器即機械臂末端與環境接觸的情況的模型主要有以下2種：K模型，Kelvin-Voigt(KV)模型。模型假設接觸力由物體的接觸部位變形產生，可以將這種情形轉化為彈簧模型或者彈簧阻尼模型。K模型主要表示接觸力和侵入深度間的關系，如式(1)[19]：

(1)

其中kK為材料接觸環境剛度，xe為接觸環境位置，x為當前末端位置，fK為接觸力。

KV模型引入了速度變量，相當于加入了阻尼效果，適用于剛性物理環境，如式(2)所示：

(2)

其中kKV為材料接觸環境剛度，xe為接觸環境位置，x為當前末端位置，fKV為接觸力。在從端執行器末端與環境進行接觸的過程中，除了接觸力參數的辨識之外，在某些比如擰閥門等需要力矩的場景下，合適的力矩反饋會使得操作更加精確。類比接觸力模型建模的方法，可以實現扭轉接觸動力學建模。扭轉接觸模型如式(3)[20]：

M=A·(θcur-θ0)

(3)

其中M為力傳感器采集的力矩值，A為扭轉剛度，θ0為初始接觸時末端姿態值，θcur為實時獲取的末端當前姿態值。整個辨識流程為：①從端執行器不斷跟隨主端手控設備運動；②當發生接觸時，記錄末端位姿初始值；③當不斷進行扭轉操作時，實時獲取當前位姿值，并實時獲取力傳感器的扭矩值；④采用上述的參數辨識算法進行計算得出扭轉剛度，并不斷通過通信鏈路傳輸到主端進行更新，使主端不斷獲取最新的參數值進行力覺反饋渲染。

3.2 模型參數辨識方法

由于機械臂末端運動過程中速度較慢，可以選取K模型作為接觸模型，扭轉接觸模型如公式(3)所述，在完成了環境動力學建模方法的選取后，模型的參數精度將決定遙操作系統中主端預測仿真模型與從端動力學實際模型之間的誤差。在空間遙操作實際任務執行過程中，由于通常難以預先獲取精確的環境動力學參數，即便是預先獲取的信息往往也存在較大誤差，故需要在實際任務執行過程中對從端接觸環境進行實時在線參數辨識，根據力傳感器測得的反饋力及執行器接觸的位置、速度等信息，求解環境模型參數并傳輸至主端，用于對預測模型進行修正[21]。

自擾動遞歸最小二乘法的核心思想是通過對參數辨識過程中的前向預測誤差實時檢測并與預設閾值進行比較，從而對待辨識環境狀態進行判斷。當實時檢測到的前向預測誤差大于預設閾值時，系統判斷接觸環境狀態發生改變，此時將根據前向預測誤差調節算法中附加項NINT()改變協方差矩陣，以提升算法對于環境變化的響應能力，保證算法的快速跟蹤性。而當實時的前向預測模型小于預設閾值時，算法將退化為無擾動增益遞歸最小二乘法，并同樣保證了辨識結果的有效收斂。因此，自擾動遞歸最小二乘法被認為兼具良好的收斂性和對變化系統的快速跟蹤性能。算法遞歸方程如式(4)：

(4)

(5)

4 虛擬場景模型參數在線修正

在基于預測模型的遙操作系統中通常需要根據從端結構化的環境再現一個虛擬場景，本文通過Unity3D虛擬引擎構建了虛擬場景，較為真實地還原從端場景環境。更新參數通過實時通信鏈路反饋到主端模型，由于誤差以及主從時延的存在，直接將從端環境模型的參數傳入主端操作模型，會導致操作者體驗的不連續，嚴重的會影響系統的穩定。例如，從端接觸參數的辨識過程中有干擾項的存在會使得參數發生某一時刻的突變，或者在某一個時間段內參數存在波動狀況，這時主端模型的參數在通過時延模塊傳輸獲取到從端辨識的參數，如果產生的反饋與上一時刻的反饋差距較大，則會影響操作者的操作穩定和安全，進而影響整個系統的穩定。

通常采用模型更新算法來控制模型參數誤差的修正，從而減小主端的突變力，提高系統穩定性，提高操作者的體驗。現有的模型更新算法包括無源性算法、漸進更新算法和基于渲染力調節的模型更新算法。但這些算法存在更新效率低和更新力不連續的問題。在此基礎上，本文提出一種新的自適應變速率漸進更新算法，以達到改善模型更新速度的效果。

在空間站手套箱等遙操作科學試驗環境下，擦玻璃、擦拭操作臺表面等試驗任務會存在表面有起伏及不同位置材料不同的場景，需要模型更新算法來展示接觸表面的變化。模型更新的參數通常包括兩部分，即環境位置和環境剛度。圖2(a)中左側較細的彈簧表示較低的環境剛度，右側較粗的彈簧表示較高的環境剛度，當環境模型的剛度發生突變時，相應的預測力也突變；圖2(b)中環境位置由下方的虛線突變為上方虛線，導致接觸力預測公式中侵入深度的突變，反應在圖中為彈簧起始位置的突變，最終導致預測力的突變。可見環境位置和環境剛度的突變都會導致預測力的突變。

圖2 環境模型突變Fig.2 Mutation of environmental model

漸進更新算法的核心思想是按照固定的變化速率進行更新，該方法能夠在得到新參數時即時開始更新，避免了參數更新時間過長的情況。該算法能夠通過調整參數變化速率調整參數更新速度與預測力變化速度之前的平衡。針對上述的參數更新如式(6)所示：

(6)

(7)

從公式可以看出，原有的漸進更新算法以固定速率進行更新，在一個采樣周期內判斷參數的變化范圍是否超過設定的閾值，在窗口內則不進行更新，在閾值窗口外，則進行速率為固定值的更新。該方法并未考慮到參數更新過程中的更新速率的動態變化，即應當在一個更新速率的范圍值動態選取相應的速率值進行迭代更新，既保證了參數更新的連續性，也增強了參數快速迭代更新和自適應的特性。

本文提出一種自適應變速率漸進更新算法，在實時迭代的過程中不斷調整參數更新速率，達到兼顧更新穩定性及快速響應特性的效果。算法的具體過程的偽代碼如下：

1： 輸入: X //當前主端空間內末端執行器位姿

Xe//環境位置

lastK//環境剛度

輸出：curF//當前時刻輸出力

2：Repeat{ //刷新周期內完成一次更新

4：P=Pmin+(|α|>=1?1:α)*(Pmax-Pmin)//當前周期內剛度更新速率

13：curF=curK*(X-Xe)}

14：until 程序退出

上述自適應變速率漸進更新算法在原有算法的基礎上，根據當前參數值和目標參數值的差距不斷調整更新速率，例如差距較大時更新速率較大，加快逼近目標值，差距較小時更新速率相應減小，自適應地調整更新速率。這樣既能夠保證更新參數的連續性，又能兼顧快速響應特性。如圖3所示，遙操作系統的主端模塊開啟監聽線程不斷接收從從端傳輸回的更新參數，主端模塊將當前參數值與目標參數值的差距作為更新速率的計算因子，并不斷更新當前參數值。由于人手可接收力覺反饋的連續性要求，在滿足提高反饋力渲染頻率的條件下，輸出力采取類似漸進更新的方式進行更新，防止輸出力不連貫導致系統不穩定，維持系統的穩定性和良好的交互體驗。

圖3 力渲染流程圖Fig.3 Flowchart of force rendering

5 試驗驗證與分析

為驗證上述理論并完成試驗操作，本文搭建了力反饋遙操作平臺，主端機器人設備為Geomagic Touch，使用電機來產生反饋力從而模擬觸覺并與虛擬對象交互；具有空間位置和姿態的六自由度輸入，三自由度的反饋力輸出；位置測量精度達到0.055 mm，標稱位置下的最大輸出力約為3.3 N。從端機器人為EC75七自由度機械臂。實物如圖4所示。軟件部分主要由以下模塊組成：中控界面模塊、視頻流傳輸模塊、Unity虛擬場景顯示模塊、機械臂控制模塊及手控器采集模塊等,總體流程圖如圖5所示。由于涉及到多模塊解耦以及異地運行問題，server和client端為一主多從關系，server進行數據廣播。視頻傳輸由于其數據量大和實時性的特點，采用RTP實時流媒體傳輸協議和h264編碼實現穩定傳輸。中控程序，手控器采集程序以及機械臂控制程序底層采用通用傳輸組件，均采用WPF創建應用，并發隊列模擬延時效果，組件以dll或者api的形式注入。

圖5 力反饋遙操作總體流程圖Fig.5 Overall flow chart of force feedback teleoperation

本文首先進行對海綿墊接觸試驗以及旋擰閥門試驗2個扭轉接觸試驗。前者比較4種不同參數辨識算法的差異和優劣；后者進一步驗證了自擾動遞歸最小二乘法的特性。然后進行擦拭表面等模型參數更新試驗，證明了本文提出的自適應漸進更新算法的優勢。

5.1 扭轉接觸試驗

在空間等需要遠程遙操作的任務場景中，通常會存在按壓開關、旋擰閥門等任務，對于力覺的交互精度和形式具有較高的要求。在執行器末端接觸到不同的接觸環境，需要實時的辨別接觸環境剛度來帶給主端可靠的交互體驗。同時，在類似旋擰閥門等操作任務中需要有扭轉力矩的主端反饋，能夠更好的完成試驗任務。本試驗平臺的末端為3D打印的帶有4個圓柱角的圓盤，可以用來模擬接觸和扭轉的試驗操作。

在已搭建的遙操作平臺從端模塊中，通過控制七自由度EC75機械臂對海綿墊的接觸試驗測量，時延設置為3 s，在接觸過程中記錄下接觸點的空間位姿坐標，并實時采集末端力傳感器的數據，在記錄當前位姿，并不斷計算位姿差，即當前位姿與接觸點位姿的空間坐標差，實時計算當前參數的辨識情況。其中末端實物如圖6所示。

圖6 末端實物圖Fig.6 Photo of end point

圖7 泡沫環境侵入深度和z軸力采集圖Fig.7 Diagram of intrusion depth in foamy environment and z-axis force acquisition

圖8 泡沫接觸剛度參數辨識圖Fig.8 Identification map of foam contact stiffness parameter

實際采集的數據圖如圖7所示。通過對采集的侵入深度以及力傳感器的數據進行處理分析，并分別使用自擾動遞歸最小二乘法(Self Perturbation Recursive Least Square，SPRLS)、指數加權遞歸最小二乘法(Exponential Weighting Recursive Least Square，EWRLS)、基于窗函數的遞歸最小二乘法(Recursive Least Square，BLS)、基于窗函數的最小四次均方算法(Based on window function Least Mean Square，BLMS)進行辨識，得到泡沫接觸環境剛度辨識結果如圖8。從圖8可以得出基于窗函數的遞歸最小二乘法在選取合適的窗口大小能夠實現平滑的過渡，但此窗口大小在不同的辨識環境下必然不同，窗口的存在使得參數的誤差變化得以緩沖，在某些誤差場景下具有實際意義。相較于BLS，基于窗函數的最小四次均方算法以及自擾動遞歸最小二乘法對于數據的變化相對敏感，反映在圖中即呈現接近于正弦的變化過程，證明了其快速跟蹤以及響應的特性。指數加權遞歸最小二乘法在容納誤差數據的情況下表現較差，指數因子的設置也會影響參數辨識的準確度。

綜上，可以同時結合自擾動遞歸最小二乘法的快速響應特性以及滑動窗口的容錯特性來使得實時參數辨識的過程中在準確性和實施響應特性中做平衡，達到最佳效果。

控制機械臂末端旋擰閥門，并記錄末端位姿信息和接觸力，如圖9所示。通過對原始數據的采集、處理和辨識，可觀察到在通過手控器操作機械臂末端扭轉的過程中會產生扭矩，并且隨著扭轉角度的增加扭矩呈現類線性關系，旋擰中扭轉剛度參數如圖10在4 N·mm/°附近，在20 s處扭轉到最大程度后反向旋擰，模擬擰松閥門的場景。從圖10中可以觀察到，由于傳感器設備以及機械臂運動誤差的存在，扭轉參數的實時辨識存在波動情況，自擾動遞歸最小二乘法展示了兼顧快速響應特性和最近窗口內均值的特性。

圖9 扭轉角度及末端扭轉力矩圖Fig.9 Diagram of torsion angle and end torsion moment

圖10 旋擰閥門扭轉剛度參數圖Fig.10 Diagram of torsional stiffness parameter of screw valve

5.2 模型參數更新試驗

在真實的遙操作系統中，由于主從時延的存在，遙操作主端操作者使用手控器得到的力覺反饋會有從無到有的突變；由于誤差的存在，實時辨識的環境參數會存在波動，反映到主端即力覺反饋的不連續性。基于上述原因，針對環境參數的更新策略是基于預測模型的遙操作系統中不可或缺的一部分。

模擬空間站手套箱等遙操作科學試驗環境下擦玻璃、擦拭操作臺表面等試驗任務，在遙操作系統的從端模塊中，設置試驗環境條件為：設置初始虛擬環境剛度為4400 N/m，每間隔10 s將虛擬環境剛度變為400和4400，試驗過程中保證環境位置為0 mm。虛擬環境剛度的更新速率范圍為到2000。在更新的過程中，實時選取相應速率系數進行迭代，在時延條件下并不斷傳輸回主端模塊進行力覺渲染輸出。

圖11 直接更新參數圖Fig.11 Diagram of direct update parameters

如圖11所示，直接更新參數法在模型參數發生更新的時刻會產生較大的突變輸出，同時較大的突變力會造成人手操作的負擔和危險，進而影響整個系統的穩定。在10 s處環境剛度突變到400，力突然變小會導致主端位置下沉，相應的位置指令傳輸到從端會造成從端對環境造成損害；同理，在20 s處環境剛度突變到4400，突變力的增大會導致主端位置的急劇上升，影響從端末端與環境的交互。

在預測力和主端輸出力的力曲線圖中，當發生參數更新的時候，主端輸出力會根據當前參數進行緩慢增長，增幅相較于直接更新參數明顯更小，有利于保證系統的穩定性。在位置曲線圖中可以看出在20 s、30 s、40 s等時間點參數發生變化時位置均有小幅度的變動，是因為參數更新過程中人手的阻抗作用會引導位置發生變化。

圖12 本文自適應漸進更新參數圖Fig.12 Diagram of adaptive progressive update parameters

從圖12、13可以看出，自適應漸進更新算法在發生參數更新的過程中由于更新速率的調整初始階段會加速更新，在接近目標參數值的范圍內會呈現過渡式的曲線變化。在表1中，可以看到更新完成時間相較于原來的漸進更新時間提高了41%，且更新過程更加平滑，但更新過程中的突變力峰值和平均值相較于原來的漸進更新算法均有所上升。因此自適應漸進更新算法提高了快速迭代更新的效率，同時增大了突變力的閾值，可以通過人手的阻抗作用來補償。

圖13 漸進更新參數圖Fig.13 Diagram of progressive update parameters

表1 更新算法效果對比表

6 結論

1) 對接觸動力學環境模型建模及其參數辨識進行了分析，將其擴展到扭轉接觸動力學模型；針對模型參數修正以及更新問題提出一種新的自適應漸進更新方法來提升模型更新的快速響應特性。

2) 通過搭建基于Unity場景的遙操作試驗平臺進行模型參數更新試驗，進一步驗證了該方法在突變力閾值可接受的范圍內，能使迭代更新的速度大大提高，更新的過程更加平滑。

今后將針對多點、多區域、非同時碰撞接觸等復雜交互場景進行深入研究，提供更加豐富的交互體驗。