空間雙臂機器人抓捕非合作目標后的協調穩定控制

夏鵬程,羅建軍,王明明,*,譚龍玉

1.西北工業大學 深圳研究院,深圳 518057

2.西北工業大學 航天動力學國家重點實驗室,西安 710072 3.上海航天控制技術研究所,上海 201109

近年來,隨著空間技術與應用的發展,利用空間機器人維修失效衛星、操控非合作目標等在軌服務任務日益引人關注?？臻g多臂機器人相較于空間單臂機器人,具有更強的操作能力和更廣泛的應用,但操作更為復雜。因此,空間多臂機器人捕獲非合作目標的操控技術成為當前研究的熱點和完成任務的關鍵?？臻g多臂機器人捕獲非合作目標的過程可分為抓捕前、抓捕中和抓捕后等三個階段。本文研究空間雙臂機器人抓捕非合作目標后的安全協調穩定控制問題。

針對空間機器人抓捕非合作目標后的穩定控制,相關研究一般將該階段的控制問題拆分為兩個子問題：期望軌跡規劃問題和跟蹤控制問題。為了規劃空間機器人穩定目標運動的期望軌跡,研究者們通常將抓捕后階段的穩定軌跡規劃問題轉化為優化問題進行求解。對于慣性參數無法準確獲得的非合作目標,通常利用其慣性參數的估計值規劃參考期望運動軌跡。Yoshida等從能量分配的角度規劃了空間機器人的運動軌跡。Zappa等則從動量守恒的角度,規劃自由飛行機器人的期望運動軌跡。Wang等利用樣條曲線對空間機器人的關節運動進行參數化處理,提出了針對冗余機械臂的軌跡規劃方法。Yu等利用機械臂的冗余度,提出了一種考慮優先級的空間多臂機器人期望軌跡規劃方法。在抓捕后階段,由于目標慣性參數無法準確地獲得,因而無法有效地控制空間機器人跟蹤期望運動軌跡從而實現目標的穩定。為了避免目標不確定性對基座運動控制的影響,Sharf等提出了基座無擾的穩定控制策略。Abiko等則考慮目標不確定性對期望軌跡跟蹤的影響,根據目標的不確定性設計了自適應穩定控制器。由于非合作目標的慣性參數具有不確定性,加之上述控制方法未考慮控制過程中機械臂末端受到的接觸力與力矩,空間機器人無法在規劃的接觸力范圍內跟蹤參考期望運動,無法保證空間機器人與目標交互的安全。

為了實現抓捕后階段機器人與目標與交互的柔順和安全,降低機械臂末端與目標之間的接觸力/力矩,研究者們將柔順控制方法引入到空間機器人抓捕目標的相關研究中。柔順控制方法通過設計控制律,將機器人與目標的交互過程等效為一個質量-彈簧-阻尼系統,繼而實現交互過程可控的柔順接觸。描述該質量-彈簧-阻尼系統的等式被稱為柔順等式。為了保證機械臂末端的接觸安全,Yoshida等根據柔順等式提出了空間機器人的阻抗控制方法,設計了調整柔順等式參數的自適應律,根據接觸力與力矩調整跟蹤控制律,實現交互過程中的柔順接觸。Wang等利用柔順等式提出了期望運動軌跡的調整方法,通過調整期望軌跡實現了目標穩定過程中的接觸柔順,使得跟蹤控制器的控制性能不受接觸力與力矩的影響。Stolfi等將一般的位置跟蹤控制與柔順控制相結合,提出了力/位混合控制方法,僅在需要實現柔順接觸的方向進行柔順控制。然而上述研究僅針對單臂空間機器人,而且只考慮了目標慣性參數不確定性對接觸力與力矩的影響。當雙臂同時操控被抓捕目標時,雙臂對目標會產生拉扯/擠壓作用,可能對目標會造成損傷,機械臂末端與目標之間可能產生過大的接觸力與力矩,因此,利用空間雙臂機器人穩定和操控非合作目標時,除了需要考慮目標不確定性對接觸力與力矩的影響,還需要進一步地考慮雙臂拉扯/擠壓目標產生的接觸力與力矩影響。

針對空間雙臂機器人抓捕非合作目標后的安全穩定問題,同時考慮非合作目標慣性參數的不確定性以及雙臂操控目標的拉扯/擠壓作用,提出了一種協調穩定控制方法,通過協調地控制雙臂的運動實現雙臂末端與目標接觸柔順,降低穩定過程中產生的接觸力與力矩。首先,利用目標慣性參數的估值規劃穩定目標的運動軌跡。然后,針對目標慣性參數不確定性,對規劃的目標穩定軌跡進行調整；同時,基于抓捕矩陣零空間內存在不影響目標運動的無效運動旋量反應了雙臂對目標的拉扯/擠壓,對雙臂末端的期望運動進行調整；根據運動學約束求解逆運動學得到雙臂運動協調的安全穩定運動軌跡；在設計的控制性能范圍內對安全穩定運動軌跡進行有效跟蹤,協調地控制雙臂運動實現接觸柔順的安全穩定控制。最后,通過仿真算例驗證了本文協調穩定控制方法的有效性。

1 動力學建模

研究對象空間雙臂機器人如圖1所示。該空間機器人由兩個對稱安裝的七關節空間機械臂和可以六自由度運動的基座組成。在空間雙臂機器人捕獲非合作目標的抓捕后階段,空間機械臂的末端與目標固連形成組合體。

圖1 空間雙臂機器人抓捕目標示意圖Fig.1 Snapshot of a dual-arm space robot capturing a target

用表示向量∈R對應的反對稱矩陣。為了便于定義目標的六自由度運動,根據向量,∈R定義運算：

式中：為目標的慣性矩陣；為目標質心到抓捕點的雅可比矩陣即抓捕矩陣。由于抓捕后階段目標與空間機器人雙臂的末端在抓捕點處固定連接,因此目標與空間機器人的運動滿足運動學約束：

2 安全穩定控制問題分析

當空間機器人的雙臂同時作用于非合作目標時,描述末端接觸力與力矩的旋量和作用于目標質心的運動旋量滿足：

式中：運動旋量∈R被稱為控制目標運動的有效運動旋量。與空間單臂機器人不同,當兩個機械臂同時作用于目標時,在雅可比矩陣的零空間內會產生不影響目標運動的無效運動旋量,其表達式為

式中：?表示矩陣的廣義逆；∈R為單位矩陣；∈R為反應拉扯、擠壓程度的旋量。無效運動旋量反映了兩個機械臂與目標之間的拉扯/擠壓,會對機械臂末端的接觸力/力矩產生影響。因此,為了機械臂末端與目標接觸的安全,需要在控制空間機器人的過程中考慮無效運動旋量的影響。

除此之外,由于非合作目標的慣性參數無法準確獲得,通常利用目標慣性參數的估值規劃穩定其運動的參考期望軌跡。根據式(3)中的目標動力學方程可知,參考期望軌跡滿足動力學約束為

由此關系不難看出,因為參數估計值與真值之間存在誤差,跟蹤參考期望軌跡并不一定能夠滿足規劃的接觸力與力矩約束,因此跟蹤參考期望軌跡存在威脅機械臂末端接觸安全的風險。

根據上述分析可知,為了保證空間雙臂機器人穩定非合作目標過程中的末端接觸安全,需要同時考慮無效運動旋量和目標慣性參數不確定性對接觸力/力矩產生的影響,協調地控制雙臂的運動,降低控制過程中產生的接觸力與力矩。

3 安全協調穩定控制

為了同時處理無效運動旋量以及目標不確定性對機械臂末端與目標接觸的影響,基于柔順等式對規劃的期望穩定運動軌跡進行調整,求解能夠實現目標與雙臂末端接觸柔順的安全穩定運動軌跡；通過在設計的控制性能約束范圍內有效地跟蹤安全穩定運動軌跡,保障控制的效率,協調地控制雙臂運動實現接觸的柔順,使得空間雙臂機器人能夠安全地穩定非合作目標的運動。

3.1 目標運動軌跡規劃

為了實現空間雙臂機器人穩定非合作目標的運動,首先,利用目標慣性參數的估值規劃穩定控制過程中目標的參考期望運動軌跡。利用貝塞爾曲線對目標的六自由度運動進行參數化處理,并設計穩定過程中目標的優化運動軌跡。

選取貝塞爾曲線：

式中：下標代表運動的第個自由度；4為貝塞爾曲線的階數；為構造二項式系數的變量；P是構建貝塞爾曲線的控制點；是歸一化時間。根據目標運動穩定時間,時間可定義為。考慮到空間非合作目標運動的不確定性、組合體操控的安全性和時效最優性,本文在參考期望軌跡規劃的過程中采用了時間最優與接觸力/力矩盡量小和安全的聯合優化方式。根據式(3)中的目標動力學方程,可將穩定目標的運動規劃問題轉化為優化問題：

式中：和分別是最優時間和接觸力與力矩的權重因子；和為目標運動約束的下界與上界；為接觸力與力矩的安全約束。由于該優化問題同時考慮了最優穩定時間和接觸力/力矩,不會規劃過大的接觸力/力矩,且可以通過調節權重因子控制最優穩定時間的長短,因此可以根據任務需求靈活地規劃參考期望運動軌跡。

通過求解上述的優化問題,可以規劃穩定過程中目標的參考期望運動軌跡。

3.2 安全穩定運動軌跡求解

鑒于目標運動軌跡是利用目標慣性參數估值規劃的,由3.1節的分析可知,直接跟蹤對應的關節與基座運動軌跡具有一定的風險。提出一種根據接觸力與力矩調整穩定運動軌跡的內外雙環式結構,利用內環與外環分別針對無效運動旋量和目標不確定性對接觸力/力矩的影響,對目標參考期望運動軌跡進行柔順化調整。最終,求解得到安全穩定目標的空間機器人關節與基座期望運動軌跡。

綜上,安全穩定目標的空間機器人關節與基座的期望運動軌跡生成框圖如圖2所示。經過式(14)和式(16)中柔順等式的調整,最終得到的期望運動考慮了無效運動旋量和目標不確定性對接觸力/力矩的影響。跟蹤時,空間雙臂機器人能夠同時考慮無效運動旋量和目標不確定性對機械臂末端與目標接觸產生的影響,在機械臂末端與目標交互過程中實現柔順接觸。

圖2 安全穩定運動軌跡求解框圖Fig.2 Schematic diagram of safe stabilization trajectory generation

3.3 約束跟蹤控制器設計

為了保證空間機器人能夠有效地跟蹤安全穩定目標運動的期望運動,實現目標與雙臂末端交互的柔順,基于障礙李雅普諾夫函數設計了控制性能可約束的跟蹤控制器。

式中：∈R為由正數組成的控制性能約束向量。為了使得機器人跟蹤過程中滿足設計的性能約束,根據動力學方程式(1),設計跟蹤控制律：

其中,障礙李雅普諾夫函數為

對式(25)中的求導可得

障礙李雅普諾夫函數導數為

利用不等式：

將式(19)中的控制律代入李雅普諾夫函數的導數中,可以推得≤0。由此可以推導得到,在跟蹤控制的過程中,0時刻和后續時刻的李雅普諾夫函數滿足：

對不等式兩邊進行指數化處理,整理后可以推得,后續時刻的跟蹤誤差滿足約束關系：

式中： 和 為誤差收斂時的約束下界與上界。根據跟蹤誤差的定義以及式(23)和式(24)可知,該跟蹤控制器能夠滿足式(18)中設計的控制性能。對式(30)求導可知,該約束跟蹤控制器能夠約束跟蹤誤差的收斂速率。利用此約束跟蹤控制器,空間雙臂機器人可以在控制性能約束范圍內有效地跟蹤安全穩定目標的期望運動軌跡,實現空間雙臂機器人抓捕非合作目標后的接觸柔順。

綜上,空間雙臂機器人抓捕非合作目標后的安全協調穩定控制框圖如圖3所示。

圖3 協調穩定控制結構圖Fig.3 Schematic diagram of coordinated stabilization control

4 仿真校驗

設計了利用所提方法穩定非合作目標的仿真算例。空間機器人的雙臂安裝位置對稱,空間機器人的運動學與動力學參數如表1所示。目標慣性參數的估值為真實值的80%。

表1 空間機器人的運動學與動力學參數Table 1 Kinematics and dynamics parameters of space robot

非合作目標被抓捕后的相對速度為零,角速度為[-0.174 5 0 0]rad/s,目標上兩個抓捕點的位置分別為[0 0.5 0]m 和[0 -0.5 0]m。設定目標運動軌跡的接觸力/力矩約束上界分別為20 N 與20 N·m,約束下界為-20 N與-20 N·m,目標運動軌跡的時長為3 s。空間雙臂機器人穩定非合作目標時,為了避免基座運動對機械臂末端與目標接觸產生影響,令基座的位姿固定,期望運動速度為零。

根據式(4)中的運動學約束與目標運動軌跡,通過逆運動學計算可以得到未經內外雙環結構調整的穩定運動軌跡。對于控制性能可設計的跟蹤控制器,選擇控制參數：

為了展示所提協調穩定控制方法的有效性,根據已有研究中的穩定控制方法設計了兩個對照組。在對照組1中,穩定過程中不考慮空間機器人與目標交互的安全,利用PD 控制器直接跟蹤未經調整的穩定運動軌跡。由空間雙臂機器人的動力學方程,可以設計PD 控制器：

在對照組2中,穩定過程中采取已有研究中的阻抗控制方法,根據接觸力與力矩對跟蹤控制律進行調整實現接觸柔順。仿真中利用阻抗控制器跟蹤未經調整的穩定運動軌跡：

式中：、和為對稱的正定矩陣。仿真過程中,對照組2中實現阻抗控制的柔順等式參數以及所提方法求解安全穩定運動軌跡所需的柔順等式參數如表2所示。

表2 柔順等式參數Table 2 Parameters of compliant equations

圖4～圖6給出了所提方法與對照組方法在穩定目標過程時,空間雙臂機器人末端的接觸力/力 矩情況。圖中、、和、、分別為和的 三 軸 分 量；、、和、、分別為和的三軸分量。由于抓捕點處目標的加速度與機械臂末端的加速度之間存在初始偏差,因此在抓捕后開始階段,目標的運動會受到初始接觸的沖擊影響,抓捕點處會產生較大的初始接觸力/力矩。

圖4 協調穩定控制機械臂末端的接觸力與力矩Fig.4 Contact wrenches of robot end-effector under coordinated stabilization control

圖6 阻抗控制機械臂末端的接觸力與力矩Fig.6 Contact wrenches of robot end-effector under impedance control

對比圖4與圖5、圖6可以發現,由于未考慮目標與機械臂末端交互的安全,PD 控制器跟蹤未經調整的期望軌跡時產生了三組中最大的接觸力/力矩,無法保證目標與機械臂末端的安全交互。相較于PD 控制,考慮了末端接觸力/力矩的阻抗控制方法與協調控制方法有效地降低抓捕后階段的接觸力。而協調控制方法同時考慮了目標的不確定性與無效運動旋量對接觸力/力矩的影響,故相較于僅考慮接觸力/力矩大小的阻抗控制,協調控制進一步地降低了機械臂末端的接觸力矩。鑒于協調控制需要根據接觸力/力矩生成安全運動軌跡,受到安全運動軌跡的影響,協調控制穩定目標時產生的接觸力/力矩收斂速度較PD控制慢；而與阻抗控制方法相比,接觸力/力矩能夠根據安全軌跡快速收斂,因而收斂速度較阻抗控制快。因此,所提方法通過協調雙臂期望運動和約束跟蹤控制性能,有效地控制了目標不確定性與無效運動旋量對接觸力/力矩造成的影響,實現了雙臂末端與目標接觸的柔順,降低了穩定過程中產生的接觸力/力矩,保證了穩定過程中雙臂末端與目標交互的安全。

圖5 PD 控制機械臂末端的接觸力與力矩Fig.5 Contact wrenches of robot end-effector under PD control

圖7～圖9給出了利用所提協調控制方法穩定目標和使用對照方法跟蹤軌跡穩定目標時的跟蹤誤差。由仿真結果可知,所提協調控制與PD控制均可以快速地跟蹤上期望運動軌跡。從圖7的仿真結果可以看出,約束跟蹤控制器能夠在設計的跟蹤誤差約束范圍內有效地跟蹤安全穩定運動軌跡。對比圖7和圖8可知,由于安全穩定運動軌跡受到接觸力/力矩的影響,協調控制的跟蹤誤差收斂速率低于PD 控制。此外,由于阻抗控制的控制性能受到接觸力/力矩的影響,阻抗控制的跟蹤誤差收斂速率為三組中最低。相較于阻抗控制,本文所提協調控制方法的跟蹤控制性能不受接觸力/力矩影響且可以設計,能夠有效地跟蹤安全穩定軌跡,保障穩定控制的效率。

圖7 協調穩定控制關節運動跟蹤誤差Fig.7 Tracking errors of joints under coordinated stabilization control

圖8 PD 控制關節運動跟蹤誤差Fig.8 Tracking errors of joints under PD control

圖9 阻抗控制關節運動跟蹤誤差Fig.9 Tracking errors of joints under impedance control

圖10～圖12給出了所提協調控制方法與兩種對照方法穩定目標運動時目標的運動狀態。由于初始時刻的沖擊影響,目標在初始時刻產生了速度。圖中 、 和 為的三軸分量； 、 和 為的三軸分量。從仿真結果可知,三種方法均可以有效地穩定目標運動。由于PD 控制未考慮接觸力/力矩的影響,目標運動參照未經調整的參考期望運動軌跡,因而PD 控制的穩定速度最快。相較于PD 控制,由于協調控制的安全穩定運動軌跡受到接觸力/力矩的影響,因而穩定速率低于PD 控制。而阻抗控制的控制性能受到接觸力/力矩影響,當控制律中的接觸力與力矩收斂時跟蹤誤差才能保證收斂,因此阻抗控制穩定目標運動的速率為三組中最低。

圖10 協調穩定控制目標運動速度Fig.10 Velocities of target under coordinated stabilization control

圖12 阻抗控制目標運動速度Fig.12 Velocities of target under impedance control

圖13～圖15給出了穩定控制過程中,機械臂由于耦合作用對基座產生的反作用力/力矩。圖中 、 和 為的三軸分量； 、 和 為的三軸分量。由仿真結果可知,PD控制產生的反作用力/力矩最大；而阻抗控制與所提協調控制均有效地降低了穩定過程中因耦合作用對基座產生的反作用力/力矩,保證了基座安全。由于阻抗控制根據接觸力/力矩調整了基座和機械臂關節的跟蹤控制律,相比協調控制方法基座受到的最大反作用力/力矩小,但跟蹤期望運動的時間較長。而協調控制穩定目標運動時,約束跟蹤控制器為了保障跟蹤控制性能基座受到的最大反作用力/力矩大于阻抗控制,但仍在安全范圍內,且穩定控制的速率和反作用力/力矩的收斂速率比阻抗控制高,這說明本方法在保證安全的前提下協調控制效率更高。

圖11 PD 控制目標運動速度Fig.11 Velocities of target under PD control

圖13 協調穩定控制對基座的反作用力與力矩Fig.13 Coupling wrenches for base under coordinated stabilization control

圖15 阻抗控制對基座的反作用力與力矩Fig.15 Coupling wrenches for base under impedance control

圖14 PD 控制對基座的反作用力與力矩Fig.14 Coupling wrenches for base under PD control

為了方便對照,表3對3種方法穩定目標的時間、穩定目標過程中的最大接觸力/力矩、接觸力/力矩收斂時間、對基座產生的反作用力/力矩及其收斂時間等進行了匯總和對比。相較于未考慮交互安全的PD 控制,所提協調控制方法實現穩定控制過程中的最大接觸力降低了74.2%,最大接觸力矩降低了84.1%,基座受到的最大反作用力與力矩分別降低了66.7%和66.4%。由于接觸力/力矩對安全穩定運動軌跡的影響,協調控制的穩定時間相較PD 控制增加了21.6%,接觸力/力矩收斂至數量級10內的時間增加了9.1%。此外,由于PD 控制并未根據接觸力/力矩協調雙臂的運動,相較于協調控制,反作用力/力矩收斂至數量級10內的時間短。與考慮了交互安全的阻抗控制相對比,由于本文方法同時考慮了目標不確定性和無效運動旋量對接觸力/力矩的影響,雖然最大接觸力提高了10.9%,但是最大接觸力矩相較阻抗控制降低了81.2%,仍然保證了交互的安全。此外,由于所提方法的跟蹤控制性能不受接觸力/力矩影響,相較于阻抗控制方法,接觸力/力矩收斂至數量級10內的時間減少了68.2%,反作用力/力矩收斂至數量級10內的時間降低了69.6%,穩定目標運動的時間縮短了56.3%,所提方法在穩定目標的過程中更高效地實現了雙臂末端與非合作目標的安全交互。

表3 3種控制方法的性能結果對比Table 3 List of simulation results of three control methods

綜上,相較于無法保證目標與機械臂末端交互安全的PD 控制,本文所提協調控制能夠在實現交互安全的同時保障跟蹤控制性能,便于設計且安全性更高。與阻抗控制相比,協調控制進一步地降低了機械臂末端的接觸力矩；跟蹤控制性能可設計且不受接觸力與力矩的影響,減弱了接觸力/力矩對穩定速率的影響。所提方法通過協調雙臂運動實現了雙臂末端與目標交互柔順的穩定控制,降低了穩定過程中產生的接觸力與力矩,保證了穩定控制過程中目標與機械臂末端交互的安全,提高了柔順交互時穩定控制的效率。

5 結 論

1)所提方法通過跟蹤由內外雙環求解的安全穩定運動軌跡,協調地控制了雙臂的運動,使得雙臂末端與目標交互柔順,降低了穩定過程中產生的接觸力/力矩,實現了機械臂末端與目標交互柔順的安全穩定控制。

2)與已有的阻抗控制方法相比,所提方法同時考慮了雙臂末端對目標拉扯/擠壓以及目標不確定性對接觸力/力矩的影響,進一步地降低了機械臂末端的接觸力矩；跟蹤控制性能可設計,減弱了接觸力/力矩對穩定速率的影響,提高了雙臂末端與目標柔順交互時穩定控制的效率。