受限空間下雙冗余臂機(jī)器人最優(yōu)軌跡規(guī)劃方法

李新宇，鄒紉秋，董 愨，王運(yùn)來，孫羽佳

（1.中國空間技術(shù)研究院，北京 100094；2.北京航天長征飛行器研究所，北京 100076；3.北京理工大學(xué) 機(jī)電學(xué)院，北京 100081）

0 引言

隨著航天器研制技術(shù)的快速發(fā)展，航天器的結(jié)構(gòu)和功能更加復(fù)雜，特殊裝配操作需求日益增多。在航天器總裝工作中，采用與人手臂自由度配置一致的雙冗余臂機(jī)器人替代工人完成傳統(tǒng)總裝工作中的部分任務(wù)，可在保持原有生產(chǎn)模式、裝配流程不變的同時提高裝配質(zhì)量和效率。然而，航天器裝配場景具有非結(jié)構(gòu)環(huán)境、艙內(nèi)空間狹小等特點(diǎn)，受限空間下雙臂機(jī)器人協(xié)同操作面臨著非結(jié)構(gòu)化環(huán)境難以建模等問題，無法保證雙臂機(jī)器人在自主完成協(xié)同操作任務(wù)時的安全性[1]。因此，如何利用機(jī)器人的冗余自由度，針對裝配環(huán)境限制，尋求機(jī)器人的最優(yōu)軌跡規(guī)劃是亟需解決的關(guān)鍵問題。同時，由于雙冗余臂機(jī)器人的構(gòu)型空間維度更高，協(xié)同操作任務(wù)約束復(fù)雜，具有較強(qiáng)的非線性，也給雙臂機(jī)器人最優(yōu)軌跡規(guī)劃帶來挑戰(zhàn)[2-3]。

王美玲[4]將雙臂協(xié)調(diào)操作分為雙臂獨(dú)立、雙臂部分約束和雙臂全約束3 種情況，分別給出了雙臂之間、雙臂與被操作物體之間的相對位姿約束關(guān)系和動力學(xué)約束關(guān)系。段晉軍等[5]將任務(wù)空間正交解耦為外環(huán)的位置自由子空間和內(nèi)環(huán)的力約束子空間，有效解決了雙臂協(xié)調(diào)搬運(yùn)過程中同時存在外部干擾和內(nèi)力約束的問題。蒙磊等[6-7]利用改進(jìn)后的粒子群算法進(jìn)行冗余機(jī)械臂位置控制部分逆運(yùn)動學(xué)計(jì)算，并基于靈活性能指標(biāo)對機(jī)械臂進(jìn)行參數(shù)優(yōu)化，實(shí)現(xiàn)了大型板材安裝。顧丹寧等[8]提出面向雙臂機(jī)器人的改進(jìn)RRT 方法，在關(guān)節(jié)空間中對機(jī)械臂進(jìn)行路徑規(guī)劃，加入引力勢場，提高規(guī)劃效率。張子潤[9]針對空間機(jī)器人在軌捕獲和搬運(yùn)組裝等任務(wù)，采用高階多項(xiàng)式插值和靈敏系數(shù)實(shí)現(xiàn)分別對左臂和右臂避碰規(guī)劃。段嘉琪[10]針對在軌裝配過程中的旋擰問題，提出了一種空間雙臂機(jī)器人協(xié)調(diào)旋擰柔順控制方法。楊志強(qiáng)[11]針對雙臂機(jī)器人協(xié)調(diào)操作過程中給定路徑的時間最優(yōu)軌跡規(guī)劃問題，采用基于雜交的粒子群算法對機(jī)械臂進(jìn)行軌跡規(guī)劃。張溯[12]提出一種以時間、能量、沖擊為目標(biāo)的多目標(biāo)機(jī)械臂軌跡優(yōu)化算法，實(shí)現(xiàn)了雙臂動態(tài)協(xié)調(diào)軸孔裝配。

目前對受限空間下的雙冗余自由度機(jī)器人高效最優(yōu)軌跡規(guī)劃的研究相對較少。本文以雙七自由度臂機(jī)器人的裝配操作規(guī)劃為目標(biāo)，提出一種基于非線性約束優(yōu)化方法的雙臂機(jī)器人自主避碰與靈巧操作的最優(yōu)軌跡規(guī)劃方法，基于三維點(diǎn)云場景重建法建立受限作業(yè)環(huán)境的安全包絡(luò)模型，構(gòu)建雙臂機(jī)器人協(xié)同操作的軌跡規(guī)劃優(yōu)化模型，以實(shí)現(xiàn)受限空間下雙臂機(jī)器人協(xié)同操作優(yōu)化模型的高效求解。

非結(jié)構(gòu)化復(fù)雜環(huán)境下雙冗余臂機(jī)器人與環(huán)境易發(fā)生碰撞等問題，嚴(yán)重威脅到機(jī)器人自身安全。本節(jié)將通過對作業(yè)場景的建模和表征，構(gòu)建作業(yè)環(huán)境的安全包絡(luò)，實(shí)現(xiàn)機(jī)器人主動避碰。研究所采用的機(jī)器人配備雙七自由度機(jī)械臂，如圖1 所示，為了實(shí)現(xiàn)對受限作業(yè)環(huán)境的感知，機(jī)器人須實(shí)時獲取機(jī)械臂當(dāng)前狀態(tài)（雙臂的2×7 個關(guān)節(jié)的角度和角速度）和末端相機(jī)的安裝位姿，并根據(jù)坐標(biāo)映射關(guān)系將環(huán)境模型轉(zhuǎn)換至機(jī)器人世界坐標(biāo)系，即

圖1 雙臂機(jī)器人系統(tǒng)中坐標(biāo)系的變換關(guān)系Fig.1 Transformation relation of coordinate system in dualarm robot system

1 受限作業(yè)環(huán)境建模

1.1 雙冗余臂機(jī)器人環(huán)境感知

1.2 基于三維點(diǎn)云數(shù)據(jù)的安全包絡(luò)模型

首先利用安裝在雙臂機(jī)器人右臂末端的RGBD相機(jī)，按照預(yù)設(shè)軌跡在安全移動范圍內(nèi)實(shí)現(xiàn)對未知作業(yè)場景的完整掃描，獲得多幀受限作業(yè)場景的局部原始三維點(diǎn)云信息，根據(jù)點(diǎn)云數(shù)據(jù)構(gòu)造局部幾何特征；然后利用局部幾何特征找到旋轉(zhuǎn)和平移參數(shù)，實(shí)現(xiàn)多幀點(diǎn)云數(shù)據(jù)配準(zhǔn)；最終通過多幀點(diǎn)云信息對作業(yè)環(huán)境進(jìn)行三維重建，獲得末端相機(jī)坐標(biāo)系下完整作業(yè)場景的三維點(diǎn)云。

為了表征復(fù)雜環(huán)境中的安全工作區(qū)域的包絡(luò)，采用基于掃掠球體法的環(huán)境障礙表征模型，對三維重建場景進(jìn)行抽象表征。考慮到機(jī)械臂的連桿構(gòu)型以及作業(yè)場景中三維點(diǎn)云的聚類結(jié)果，將雙臂機(jī)器人的機(jī)械臂各連桿以及三維點(diǎn)云聚類出來的障礙特征抽象為線段掃掠體。線段掃掠球體是線段和球體的閔可夫斯基和，幾何體A和幾何體B的閔可夫斯基和可以定義為

其幾何上的直觀體現(xiàn)為，閔可夫斯基和是幾何體A沿幾何體B的邊際連續(xù)運(yùn)動1 周掃過的區(qū)域與幾何體B本身的并集。如果A為球體，則生成的掃掠球體(A+B)的邊界至幾何體B的距離為球體A的半徑r。具體地，考慮到機(jī)械臂的連桿構(gòu)型，將機(jī)器人各連桿抽象為線段掃掠球體，即為線段和球體的閔可夫斯基和；基于受限空間中安全區(qū)域邊界的結(jié)合重構(gòu)參數(shù)，構(gòu)建平面點(diǎn)云的三角形掃掠球體，為三角形和球體的閔可夫斯基和，即受限環(huán)境下機(jī)器人協(xié)作空間的安全包絡(luò)，如圖2 所示。

1.3 安全包絡(luò)間距離計(jì)算

基于掃掠球體法建立約束環(huán)境中的機(jī)器人協(xié)同操作的安全包絡(luò)，采用計(jì)算圖形學(xué)算法獲得安全包絡(luò)間的距離，即線段與線段間的空間距離。雙臂機(jī)器人協(xié)同操作的安全性條件為包絡(luò)模型間的距離大于0。包絡(luò)模型間的距離可以通過平面幾何體間的距離與各自球體半徑的差值求得。

線段上任一點(diǎn)的幾何參數(shù)表征公式為

式中：P0是線段的起點(diǎn)；d0為線段起點(diǎn)至終點(diǎn)的單位矢量，即||d0||=1；s∈[0, 1]，為線段位置參數(shù)，如圖3 所示。

圖3 線段與線段間空間距離計(jì)算示意Fig.3 Schematic diagram for calculating the spatial distance between line segments

線段與線段間的任一矢量可表示為d=L0(t)?L1(t) = (P0–P1)+sd0?td1，其中s∈[0, 1]、t∈[0, 1]，分別為端點(diǎn)在兩條線段上的位置參數(shù)，因此矢量d的長度的平方為

考慮到參數(shù)s、t的取值范圍，線段間的最近距離為一個含有邊界約束的優(yōu)化問題：

式(5)將線段間最近距離轉(zhuǎn)化為一個含有邊界約束的最優(yōu)化問題，可以計(jì)算出不同條件下該問題最優(yōu)解的解析形式[13]。

通過計(jì)算獲得線段與線段間的最近距離d，若第k個掃掠體所采用的球體半徑為rk，則掃掠球體i和掃掠體j之間的安全距離定義為Di j=d?ri?rj，則受限環(huán)境中雙冗余臂機(jī)器人協(xié)同操作的安全約束數(shù)學(xué)模型為

2 雙臂機(jī)器人最優(yōu)安全軌跡規(guī)劃

針對非結(jié)構(gòu)化約束環(huán)境中的雙臂機(jī)器人靈巧協(xié)同操作任務(wù)，在第1 章對受限作業(yè)環(huán)境建模和線段掃掠球體包絡(luò)間安全距離計(jì)算的基礎(chǔ)上，提出一種基于最優(yōu)化方法的雙臂機(jī)器人安全軌跡快速規(guī)劃方法，主要步驟包括：建立雙臂機(jī)器人的安全約束和任務(wù)約束；構(gòu)建和求解軌跡規(guī)劃的最優(yōu)化問題。

2.1 安全約束

雙臂機(jī)器人安全避碰地完成操作任務(wù)的條件為：1）雙臂機(jī)器人自身各線段掃掠球體間的距離不小于0；2）雙臂機(jī)器人與受限作業(yè)空間的所有線段掃掠球體間的距離不小于0；3）關(guān)節(jié)角度和角速度在安全限制范圍內(nèi)。

雙臂機(jī)器人自身各線段掃掠球體間的距離與機(jī)器人的雙臂狀態(tài)有關(guān)：

其中x=[q;q˙]。

假設(shè)受限空間中的障礙是固定不變的，那么安全避碰條件也僅與機(jī)器人雙臂狀態(tài)有關(guān)：

機(jī)械臂各關(guān)節(jié)角度和角速度應(yīng)當(dāng)滿足相應(yīng)的物理限制：

即xmin≤x≤xmax。

綜上，受限作業(yè)空間下雙臂機(jī)器人協(xié)同操作的安全約束可以統(tǒng)一寫為

2.2 任務(wù)約束

雙臂機(jī)器人協(xié)同操作任務(wù)主要包括笛卡兒空間下的雙臂操作任務(wù)和關(guān)節(jié)空間下的雙臂操作任務(wù)。笛卡兒空間下的雙臂協(xié)同操作任務(wù)約束主要是針對末端執(zhí)行器的約束，包括完整末端路徑約束、部分末端位姿約束以及雙臂協(xié)同操作任務(wù)約束。完整末端路徑約束完全確定了末端位姿，但對于七自由度冗余機(jī)械臂，仍有冗余自由度實(shí)現(xiàn)關(guān)節(jié)空間內(nèi)的狀態(tài)轉(zhuǎn)移。為了計(jì)算機(jī)械臂末端位姿Tee(t)至末端約束路徑的距離，使用距離當(dāng)前末端位置pee(t)最近的路徑約束點(diǎn)的位姿作為目標(biāo)約束位姿，即機(jī)械臂的末端路徑約束的數(shù)學(xué)模型為：

部分末端位姿約束與完整末端路徑約束的數(shù)學(xué)模型相似，僅是對部分軸的方向進(jìn)行約束，約束條件會相對更少，比如要求末端的第i個軸的方向在運(yùn)動過程中保持不變，即

以雙臂搬運(yùn)任務(wù)為例的雙臂協(xié)同操作的任務(wù)約束則需要考慮左右兩機(jī)械臂末端間的相對位姿，在夾持物體后該相對位姿應(yīng)當(dāng)保持不變，因此有約束條件

關(guān)節(jié)空間任務(wù)約束主要為約束最終關(guān)節(jié)狀態(tài)qterminal和terminal至目標(biāo)值。針對機(jī)械臂關(guān)節(jié)運(yùn)動至指定狀態(tài)的任務(wù)要求，比如移動至目標(biāo)關(guān)節(jié)角度qtarget，需要滿足關(guān)節(jié)軌跡終端約束條件：

以上所有的任務(wù)約束條件都是等式約束的形式，因此，可以通過調(diào)整等式兩側(cè)的表達(dá)式，寫成更為一般的任務(wù)約束模型

2.3 受限空間下雙臂機(jī)器人最優(yōu)軌跡規(guī)劃數(shù)學(xué)模型

考慮到安全約束和雙臂協(xié)同操作等約束條件，綜合評價雙臂機(jī)器人輸入能量、軌跡長度和末端累積誤差等因素，構(gòu)建非結(jié)構(gòu)化環(huán)境中雙臂機(jī)器人協(xié)同操作的多約束非線性軌跡優(yōu)化模型，如圖4 所示。

圖4 受限空間下機(jī)械臂最優(yōu)軌跡規(guī)劃數(shù)學(xué)模型Fig.4 Mathematical model for optimal trajectory planning of robotic arms in constrained space

2.4 基于約束差分動態(tài)規(guī)劃的雙臂機(jī)器人軌跡規(guī)劃

2.3 節(jié)構(gòu)建的受限空間下雙臂機(jī)器人最優(yōu)軌跡規(guī)劃數(shù)學(xué)模型具有高維、多約束和非線性等特點(diǎn)，因此優(yōu)化問題的求解會比較困難且耗時較長，影響雙臂機(jī)器人開展操作任務(wù)的效率。為了提高軌跡規(guī)劃效率，且保證軌跡的安全性以及最優(yōu)性，本研究提出采用基于增廣拉格朗日法的差分動態(tài)規(guī)劃方法[14]：將多約束問題轉(zhuǎn)化為無約束問題，并利用差分動態(tài)規(guī)劃法實(shí)現(xiàn)高維規(guī)劃問題的高效求解，獲得滿足安全避碰和任務(wù)約束的雙臂操作最優(yōu)軌跡。基于增廣拉格朗日法的約束差分動態(tài)規(guī)劃方法框架如圖5 所示。

圖5 基于增廣拉格朗日法的約束差分動態(tài)規(guī)劃法框架Fig.5 Framework of constrained differential dynamic programming method based on augmented Lagrange method

3 衛(wèi)星配件裝配仿真和實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證本文提出的受限空間下雙冗余臂機(jī)器人最優(yōu)軌跡規(guī)劃方法的可行性，選取典型的雙臂機(jī)器人完成航天器裝配的任務(wù)場景進(jìn)行仿真和實(shí)驗(yàn)驗(yàn)證。軌跡規(guī)劃方法的安全性和機(jī)器人的實(shí)際裝配能力將在后續(xù)的裝配任務(wù)實(shí)驗(yàn)中進(jìn)行驗(yàn)證，因此本文仿真中進(jìn)行了必要的簡化，僅顯示了機(jī)器人本體和雙冗余度機(jī)械臂，便于觀察機(jī)械臂的運(yùn)動軌跡，隱藏復(fù)雜的末端執(zhí)行器。

3.1 衛(wèi)星配件裝配仿真驗(yàn)證

仿真中采用編程語言Python 實(shí)現(xiàn)了相關(guān)規(guī)劃算法的設(shè)計(jì)，采用MeshCat 作為機(jī)器人仿真的前端顯示軟件。衛(wèi)星配件裝配仿真中的軌跡規(guī)劃長度（采樣點(diǎn)數(shù)）均為N=500，采樣間隔為dt=0.02 s，即軌跡總時長為10 s。雙臂規(guī)劃優(yōu)化模型中的系數(shù)對角矩陣為

約束差分動態(tài)規(guī)劃法中的初始懲罰因子μ=100，懲罰因子更新倍數(shù)為10。在衛(wèi)星配件裝配仿真中，不等式約束包括機(jī)械臂各臂的自避碰、雙臂各連桿間的安全避碰以及雙臂與目標(biāo)衛(wèi)星的安全避碰，等式約束主要為雙臂末端間的閉鏈約束和雙臂末端沿直線裝配路徑運(yùn)動的路徑約束。

具體地，雙臂衛(wèi)星配件裝配的任務(wù)形式如圖6所示：雙臂共同抓取同一結(jié)構(gòu)件，并通過結(jié)構(gòu)件形成閉鏈；雙臂同時按照裝配件與衛(wèi)星上的銷孔中心線移動，實(shí)施衛(wèi)星配件的裝配任務(wù)。在雙臂軌跡規(guī)劃中，同時具有雙臂間形成的閉鏈約束和沿銷孔中心線移動形成的路徑約束，任務(wù)約束比較復(fù)雜。采用本文提出的最優(yōu)軌跡規(guī)劃方法所規(guī)劃軌跡上的約束誤差如圖7 所示，雙臂最大距離誤差范數(shù)都小于7×10-7m，滿足實(shí)際裝配所需的精度，驗(yàn)證了本文最優(yōu)軌跡規(guī)劃方法的可行性。

圖6 雙臂機(jī)器人衛(wèi)星配件裝配任務(wù)仿真Fig.6 Simulation of satellite accessory assembly task for dual-arm robot

圖7 雙臂機(jī)器人末端執(zhí)行器路徑約束誤差Fig.7 Path constraint error of end-effector of dual-arm robot

3.2 衛(wèi)星配件裝配實(shí)驗(yàn)驗(yàn)證

在3.1 節(jié)仿真驗(yàn)證結(jié)果的基礎(chǔ)上，考慮實(shí)際的衛(wèi)星和衛(wèi)星配件的幾何形狀和抓取裝配方式，設(shè)計(jì)基于本文雙臂機(jī)器人最優(yōu)軌跡規(guī)劃方法的衛(wèi)星配件裝配實(shí)驗(yàn)驗(yàn)證。所采用的衛(wèi)星模型和裝配件如圖8 所示：在衛(wèi)星模型的正面有3 個裝配銷；衛(wèi)星裝配件包括抓握點(diǎn)和裝配面板，面板上有與衛(wèi)星模型上裝配銷對應(yīng)的裝配孔。

圖8 衛(wèi)星模型與衛(wèi)星裝配件Fig.8 Satellite model and satellite assembly

先利用本文提出的雙臂機(jī)器人最優(yōu)軌跡規(guī)劃方法生成滿足雙臂末端裝配路徑約束和雙臂閉鏈約束的最優(yōu)裝配軌跡；然后將該軌跡應(yīng)用于雙臂機(jī)器人實(shí)施衛(wèi)星配件裝配。裝配過程如圖9 所示，可以看到雙臂機(jī)器人順利完成了衛(wèi)星裝配件的抓取、移動和銷孔對準(zhǔn)裝配，驗(yàn)證了本文最優(yōu)軌跡規(guī)劃方法的可行性。

圖9 雙臂機(jī)器人衛(wèi)星配件裝配過程Fig.9 Assembly process of dual-arm robot satellite accessories

4 結(jié)束語

本文針對受限空間下雙臂機(jī)器人協(xié)同操作軌跡規(guī)劃面臨的非結(jié)構(gòu)化環(huán)境復(fù)雜、作業(yè)空間小、易發(fā)生碰撞等問題，提出基于最優(yōu)化方法的雙臂機(jī)器人自主避碰與靈巧操作的軌跡規(guī)劃方法：通過三維點(diǎn)云重建方法建立復(fù)雜約束環(huán)境的安全包絡(luò)；通過將安全作業(yè)和靈巧操作任務(wù)抽象為數(shù)學(xué)約束形式，將雙臂協(xié)同規(guī)劃問題轉(zhuǎn)化為約束最優(yōu)化問題，并利用差分動態(tài)規(guī)劃方法求解雙臂機(jī)器人在受限環(huán)境下自主避碰的最優(yōu)軌跡。最后通過雙七自由度機(jī)械臂機(jī)器人進(jìn)行衛(wèi)星配件裝配任務(wù)的仿真和實(shí)驗(yàn)，驗(yàn)證了本文最優(yōu)軌跡規(guī)劃方法的可行性。