一種空間機器人動力學參數辨識的激勵軌跡優化方法

一種空間機器人動力學參數辨識的激勵軌跡優化方法
【專利摘要】本發明公開了一種空間機器人動力學參數辨識的激勵軌跡優化方法，可以實現空間機器人動力學參數辨識的激勵軌跡優化，同時滿足參數辨識的PE條件和機械臂關節運動約束，提高空間機器人參數辨識的收斂速度和準確性。自由漂浮空間機器人的非完整特性決定了參數辨識模型回歸矩陣A(k)不僅含有與機械臂關節運動軌跡相關的關節位置和關節速度，還含有與待辨識動力學參數間接相關的基座位置、姿態、速度和角速度，而這些量必須根據系統的動力學模型進行求解，因此在激勵軌跡離線優化時需要用到待辨識動力學參數的先驗信息。
【專利說明】一種空間機器人動力學參數辨識的激勵軌跡優化方法 【技術領域】
[0001 ]本發明屬于空間機器人領域，涉及一種空間機器人動力學參數辨識的激勵軌跡優 化方法。 【【背景技術】】
[0002] 空間機器人完成在軌服務任務時一般采用基于模型的控制方法，這類方法的控制 精度與空間機器人動力學參數的準確程度緊密相關。在進行空間機器人設計和加工時能夠 通過CAD軟件計算其動力學參數，也能夠通過地面測試得到其各個部件精確的動力學參數。 然而空間機器人在軌捕獲目標后整個系統的動力學參數會發生相應改變，為了保證后續操 作中控制系統的穩定性并提高控制精度，需要對捕獲目標后的空間機器人動力學參數進行 辨識并利用辨識得到的動力學參數更新控制器的相關參數。
[0003] 動力學參數辨識的準確性和快速性不僅和選用的參數辨識模型以及估計方法有 關，也與參數辨識的激勵軌跡有很大關系。為了加快參數辨識的收斂速度并提高參數估計 的準確性，需要合理選擇空間機器人參數辨識的激勵軌跡，以保證用于辨識的測量數據滿 足持續激勵(PE)條件。
[0004] 地面工業機器人一般是通過離線設計激勵軌跡來滿足動力學參數辨識所需的PE 條件，但自由漂浮空間機器人由于其非完整約束特性，導致動力學參數辨識模型的回歸矩 陣中含有和待辨識動力學參數相關的狀態量(基座的位置、姿態、速度和角速度），這些狀態 量與關節運動軌跡相關，無法預先單獨設計，需要根據系統的動力學模型進行解算。因此不 同于地面工業機器人，空間機器人的激勵軌跡優化需要用到待辨識參數的先驗信息。
[0005] 雖然一些空間機器人動力學參數辨識方面的文獻提到了激勵軌跡需要滿足PE條 件，但多數文獻僅是在完成參數辨識后驗證參數辨識模型回歸矩陣的條件數是否足夠小， 目前已發表的文獻中只有很少幾篇涉及空間機器人動力學參數辨識的激勵軌跡優化問題。 文獻"Modeling and experimental design for the on-orbit inertial parameter identification of free-flying space robots"中采用機械臂運動對空間機器人基座的 擾動幅度來衡量激勵軌跡的激勵程度并根據該指標優化激勵軌跡，然而該指標的提出更多 是出于一種直觀感覺，缺乏準確的理論支撐。文獻"Parameter identification methods for free-floating space robots with direct torque sensing"米用B樣條參數化表不 空間機器人的機械臂運動軌跡并根據設計的優化準則優化激勵軌跡，然而其軌跡優化的目 的是為了更好地激勵空間機器人的柔性模態來研究空間機器人的柔性附件和燃料晃動對 參數辨識的影響，而并非通過優化使激勵軌跡滿足參數辨識所需的PE條件。 【
【發明內容】
】
[0006] 本發明的目的在于針對自由漂浮空間機器人動力學參數辨識問題，提供一種空間 機器人動力學參數辨識的激勵軌跡優化方法，用于在機械臂關節運動約束范圍內提高空間 機器人動力學參數辨識的收斂速度和準確性，保證用于參數辨識的測量信息滿足持續激勵 (PE)條件且不違背機械臂的關節運動約束。
[0007] 為達到上述目的，本發明采用以下技術方案予以實現：
[0008] 一種空間機器人動力學參數辨識的激勵軌跡優化方法，包括以下步驟：
[0009] 1)建立自由漂浮空間機器人的參數辨識模型
[0010] 空間機器人由一個串聯機械臂和一個作為基座的航天器平臺組成，其中機械臂由 η個旋轉關節連接而成，所有構件均視為剛體;基座航天器上安裝有測量基座位置、姿態、線 速度和角速度的敏感器，機械臂各關節都安裝有測量關節位置和角速度的敏感器；
[0011] 空間機器人末端執行器的線速度和角速度用矩陣形式統一表示為：
[0016]空間機器人的線動量Ρ和角動量L用矩陣形式統一表示為：
[0022] jTi= [ki X (ri_pi)，k2 X (ri_p2)，…，ki X (ri_pi)，0,…，0] (8)
[0023] Jri = [ki,k2, ··· ,ki,0, ··· ,0] (9)
[0024] r〇g = rg-r〇, rgi = r i-rg, r〇i = n-r〇 (10)
[0025] 設系統初始動量為零，從式(4)中分離出待辨識的動力學參數，化簡整理得到：
[0027] 式中：
[0028] in= [ Ixx, _Ixy , _Ixz , Iyy , _Iyz , Izz]T
[0032] 構造一個以x=[l/mnnanT inT]T為未知量的線性回歸方程組進行求解;假設在每 個采樣點獲取一組測量值，當完成第k次采樣后，線性回歸方程組能夠表示如下：
[0033] A(k)x = Y(k) (12)
[0034] 式中：
[0038]待辨識的動力學參數X可采用最小二乘算法進行估計：
[0040] 2)確定激勵軌跡優化準則
[0041]選用回歸矩陣A(k)的譜條件數作為激勵軌跡的優化準則，如下所示：
[0043] 式中：〇max(A(k))表不矩陣A(k)的最大奇異值，〇min(A(k))表不矩陣A(k)的最小奇 異值；
[0044] 3)空間機器人關節軌跡參數化
[0045] 空間機器人機械臂第i個關節的關節位置使用有限傅里葉級數表示如下：
[0047]式中：cof為傅立葉級數的基頻，φι()為關節位置偏移量;機械臂各關節采用相同的 基頻來保證激勵軌跡的周期性，空間機器人每個關節的參數化運動軌跡含有2Ν+1個待定系 數；
[0048]對式（18)關于時間求一階導數和二階導數能夠得到關節i的角速度和角加速度如 下所示：
[0051 ] 4)激勵軌跡優化問題求解
[0052] 激勵軌跡優化問題描述為如下形式：
[0053] <})*(t)=argmin(J) (21)
[0054] 約束條件為：
[0056]式（21)和式（22)為含多約束的非線性優化問題，采用MATLAB優化工具箱中的 fmincon函數求得該非線性優化問題的解，即步驟3)中待定系數《丨，踔和Φ1()的值。
[0057]與現有技術相比，本發明具有以下有益效果：
[0058] 本發明可以實現空間機器人動力學參數辨識的激勵軌跡優化，同時滿足參數辨識 的ΡΕ條件和機械臂關節運動約束，提高空間機器人參數辨識的收斂速度和準確性。自由漂 浮空間機器人的非完整特性決定了參數辨識模型回歸矩陣A(k)不僅含有與機械臂關節運 動軌跡相關的關節位置和關節速度，還含有與待辨識動力學參數間接相關的基座位置、姿 態、速度和角速度，而這些量必須根據系統的動力學模型進行求解，因此在激勵軌跡離線優 化時需要用到待辨識動力學參數的先驗信息。但仿真結果表明該方法對先驗誤差具有較強 的魯棒性，在先驗值和真實值具有50 %誤差的情況下，通過優化方法得到的激勵軌跡仍可 以滿足PE條件，因此該方法具有很好的工程適用性。 【【附圖說明】】
[0059] 圖1為空間機器人模型圖；
[0060]圖2為最優激勵軌跡的關節角度變化曲線圖；
[0061] 圖3為最優激勵軌跡的關節角速度變化曲線圖；
[0062] 圖4為對照組激勵軌跡的關節角度變化曲線圖；
[0063] 圖5為對照組激勵軌跡的關節角速度變化曲線圖；
[0064] 圖6為質量辨識結果圖；
[0065]圖7為轉動慣量辨識結果圖；
[0066]圖8為質心位置辨識結果圖；
[0067]圖9為回歸矩陣條件數圖；
[0068]圖10為捕獲目標后的空間機器人系統圖；
[0069]圖11為激勵軌跡優化求解流程圖。
[0070] 其中：1-連桿1; 2-連桿2; 3-連桿3;4-連桿4; 5-連桿5;6-連桿6;7-連桿7;8-基座。 【【具體實施方式】】
[0071]下面結合附圖對本發明做進一步詳細描述：
[0072] 參見圖1-圖11，本發明空間機器人動力學參數辨識的激勵軌跡優化方法，其特征 在于，包括以下步驟：
[0073] 步驟1:建立自由漂浮空間機器人的參數辨識模型
[0074] 空間機器人由一個串聯機械臂和一個作為基座的航天器平臺組成，其中機械臂由 η個旋轉關節連接而成，所有構件均視為剛體。基座航天器上安裝有測量基座位置、姿態、線 速度和角速度的敏感器，機械臂各關節都安裝有測量關節位置和角速度的敏感器。
[0075] 空間機器人末端執行器的線速度和角速度用矩陣形式統一表示為：
[0080]空間機器人的線動量ρ和角動量L用矩陣形式統一表示為：
[0086] jTi= [ki X (ri_pi)，k2 X (ri_p2)，…，ki X (ri_pi)，0,…，0] (8)
[0087] Jri= [ki,k2, ··· ,ki,0, ··· ,0] (9)
[0088] r〇g = rg-r〇, rgi = r i-rg, r〇i = n-r〇 (10)
[0089] 設系統初始動量為零，從式(4)中分離出待辨識的動力學參數，化簡整理可得：
[0096] (11)是一個含有10個未知數，6個獨立方程的線性方程組，因此僅靠一組測量數據 無法得到確定的解，需要利用多組測量數據，構造一個以x=[l/m nnanT inT]T為未知量的線 性回歸方程組進行求解。假設在每個采樣點獲取一組測量值，當完成第k次采樣后，線性回 歸方程組可以表示如下：
[0097] A(k)x = Y(k) (12)
[0098] 式中：
[0102]待辨識的動力學參數X可采用最小二乘算法進行估計：
[0104]步驟2:確定激勵軌跡優化準則
[0105]線性回歸方程組A(k)x = Y(k)中系數矩陣A(k)的條件數可以度量方程組的解X對 于誤差的敏感性。如果A(k)的條件數較大，Y(k)的微小改變就會引起X較大的改變，從而降 低數值穩定性。
[0106]在參數辨識中，經常采用辨識模型的回歸矩陣條件數來衡量參數辨識的激勵水 平，為了保證參數辨識的準確性和收斂速度，需要盡量減小回歸矩陣的條件數。本發明選用 回歸矩陣A(k)的譜條件數作為激勵軌跡的優化準則，如下所示：
[0108] 式中：〇max(A(k))表不矩陣A(k)的最大奇異值，〇min(A(k))表不矩陣A(k)的最小奇 異值。
[0109] 步驟3:空間機器人關節軌跡參數化
[0110] 空間機器人機械臂第i個關節的關節位置使用有限傅里葉級數表示如下：
[0112] 式中：cof為傅立葉級數的基頻，φι()為關節位置偏移量。機械臂各關節采用相同的 基頻來保證激勵軌跡的周期性，空間機器人每個關節的參數化運動軌跡含有2Ν+1個待定系 數，根據步驟4中的方法確定盡和Φ 1()的值能夠保證空間機器人的運動滿足參數辨識的 ΡΕ條件。
[0113] 對式（18)關于時間求一階導數和二階導數可以得到關節i的角速度和角加速度如 下所示：
[0116] 步驟4:激勵軌跡優化問題求解
[0117] 空間機器人機械臂的關節運動不僅要滿足參數辨識的激勵要求，還需要滿足關節 運動范圍、關節角速度、關節角加速度的約束。因此激勵軌跡優化問題可以描述為如下形 式：
[0118] <})*(t)=argmin(J) (21)
[0119] 約束條件為：
[0121] 式（21)和式（22)為含多約束的非線性優化問題，采用MATLAB優化工具箱中的 fmincon函數可求得該非線性優化問題的解，即步驟3中待定系數α丨，6；和φ ι()的值。
[0122] 本發明的原理：
[0123] 針對圖1所示的空間機器人模型，采用本發明得到的最優激勵軌跡如圖2和圖3所 示。空間機器人(含目標）的動力學參數如表1所示，7自由度機械臂的Denavit-Hatenberg (DH)參數如表2所示，關節運動約束如表3所示。
[0124] 表1空間機器人動力學參數
[0127] 表2機械臂DH參數
[0128]
[0129]表3機械臂關節運動約束
[0131] 圖2和圖3所示的最優激勵軌跡根據動力學參數先驗值計算的回歸矩陣條件數為 1.6912。為了說明本發明的效果，選取圖4和圖5所示的對照組激勵軌跡，該激勵軌跡由頻率 為0.1Hz和0.5Hz的兩種正弦諧波組成，滿足機械臂關節角度和關節角速度的初始及終端約 束條件，對照組激勵軌跡根據參數先驗值計算的回歸矩陣條件數為510.6878。
[0132] 控制空間機器人分別跟蹤最優激勵軌跡和對照組激勵軌跡，根據兩組采樣數據分 別完成對空間機器人動力學參數的辨識，辨識結果如圖6至圖8所示。從仿真結果可以看出， 最優激勵軌跡對應的參數辨識結果收斂速度更快、準確性更高。雖然仿真中的最優激勵軌 跡是根據待辨識參數的先驗值求得的，但從圖9所示的回歸矩陣條件數仿真結果可以看出， 本發明的激勵軌跡設計方法對于先驗誤差具有較強的魯棒性，在參數先驗值僅為真實值一 半的情況下，設計的激勵軌跡仍具有較小的回歸矩陣條件數，能夠滿足PE條件，從而保證了 參數辨識的快速性和準確性。
[0133] 圖10是捕獲目標后空間機器人系統的結構模型，其中：
[0134] ΣΙ:慣性坐標系，原點為〇1;
[0135] Σ β :空間機器人基座的體坐標系，原點(^位于基座質心；
[0136] m〇，πη :分別表示基座和連桿i的質量，
[0137] I。，Ii e R3X3:分別表示基座和連桿i繞各自質心的慣性張量；
[0138] ki e R3:表示關節i旋轉方向的單位向量；
[0139] riGR3:連桿i質心的位置向量；
[0140] rg e R3:空間機器人系統質心〇g的位置向量；
[0141] r〇GR3:空間機器人基座質心〇b的位置向量；
[0142] PleR3:關節i的位置向量；
[0143] pe e R3:機械臂末端執行器的位置向量；
[0144] b〇eR3:從基座質心仏指向關節1的位置矢量；
[0145] ai，bieR3:分別為從關節i指向連桿i質心，從連桿i質心指向關節i+Ι的位置矢量；
[0146] ViGR3:連桿i質心的線速度；
[0147] ?1^護:連桿丨的角速度；
[0148] R/ e/?3x3:表示從坐標系Σ^Ι」坐標系Σ」的旋轉矩陣；
[0149] VQeR3:基座的線速度；
[0150] co〇eR3:基座的角速度；
[0151] veeR3:機械臂末端執行器的線速度；
[0152] ω e e R3:機械臂末端執行器的角速度；
[0?53] Φ eRn:機械臂關節角向量；
[0154] E，0:分別為單位矩陣和零矩陣；
[0155]此外，定義向量1=|^，7,2]1'的叉乘算子？為：
[0157] 圖11為激勵軌跡優化求解流程，其中虛線框內的部分體現了自由漂浮空間機器人 激勵軌跡優化設計的特殊之處。
[0158] 不同于固定基座機器人，自由漂浮空間機器人動力學參數辨識模型的回歸矩陣中 不僅包含由機械臂關節運動軌跡直接決定的關節角度和關節角速度，還包含與待辨識動力 學參數相關的基座位置、姿態、線速度和角速度。其中，基座的線速度和角速度需要將關節 角速度和空間機器人的動力學參數(包括待辨識動力學參數的先驗值)代入式(4)進行求 解，基座的位置和姿態則需要對基座線速度和角速度進行數值積分獲得，因此在激勵軌跡 優化設計過程中不可避免地要用到待辨識動力學參數的先驗信息。
[0159] 圖11中JdPJ1+1分別為經過i次和i+Ι次迭代優化后得到的激勵軌跡所對應的目標 函數值，即回歸矩陣A(k)的譜條件數。
[0160]以上內容僅為說明本發明的技術思想，不能以此限定本發明的保護范圍，凡是按 照本發明提出的技術思想，在技術方案基礎上所做的任何改動，均落入本發明權利要求書 的保護范圍之內。
【主權項】
1. 一種空間機器人動力學參數辨識的激勵軌跡優化方法，其特征在于，包括以下步驟： 1)建立自由漂浮空間機器人的參數辨識模型 空間機器人由一個串聯機械臂和一個作為基座的航天器平臺組成，其中機械臂由η個 旋轉關節連接而成，所有構件均視為剛體;基座航天器上安裝有測量基座位置、姿態、線速 度和角速度的敏感器，機械臂各關節都安裝有測量關節位置和角速度的敏感器； 空間機器人末端執行器的線速度和角速度用矩陣形式統一表示為： e =/s0 +/』 （1) l_oe」 [_兩」 式中： JS = 〇 E ，m (2) = ΓΑ,χ：(^-/?1) k2x(pe-p2) -· Λ"χ( W1 < h … < 」 空間機器人的線動量P和角動量L用矩陣形式統一表示為： 細: 卜]+卜1 [ ο ] 式中： ^^(L+m^ + I^R^ (5) Μ ~= Σ(，Λ,· +冊 ΛΑ)Μ3χη (6) Μ JTco = 0) />1 jTi = [ki X (ri_pi)，k2 X (ri_p2)，…，ki X (ri_pi)，0，.··，0] (8) jRi=[ki，k2，."，ki，0，.·_，0] (9) r〇g=rg-r〇, rgi = r i-rg, r〇i = r i~r〇 (10) 設系統初始動量為零，從式(4)中分離出待辨識的動力學參數，化簡整理得到：式中： ?η [ IXX , - Ixy , - Ixz , Iyy , - Iyz , Izz ] ? 〇 ο ' 〇 Η, 〇 "?m. ?: 〇 _ 〇 ο Ο 、"ω"ζ_ ^Γ~\ η -1 . Η = ΣΙ-?ην? ^=Σ,-Ιο (?ω·+Γ/χ ηι^ )+^χρ" 構造一個以X = [l/msΒ?/ i/；f為未知量的線性回歸方程組進行求解;假設在每個 采樣點獲取一組測量值，當完成第k次采樣后，線性回歸方程組能夠表示如下： A(k)x = Y(k) (12) 式中：待辨識的動力學參數X可采用最小二乘算法進行估計： x = (ai (k)A(k)) 'a1 {k)\(k) (16) 2) 確定激勵軌跡優化準則 選用回歸矩陣A(k)的譜條件數作為激勵軌跡的優化準則，如下所示：(17) 式中：〇max(A(k))表不矩陣A(k)的最大奇異值，〇min(A(k))表不矩陣A(k)的最小奇異值； 3) 空間機器人關節軌跡參數化 空間機器人機械臂第i個關節的關節位置使用有限傅里葉級數表示如下：(18) 式中：C0f為傅立葉級數的基頻，φι()為關節位置偏移量;機械臂各關節采用相同的基頻 來保證激勵軌跡的周期性，空間機器人每個關節的參數化運動軌跡含有2Ν+1個待定系數； 對式（18)關于時間求一階導數和二階導數能夠得到關節i的角速度和角加速度如下所 示： 4(，) = cos(r"/") + ,，; sin(%")) (19) /=1 x ' φ： (/) = 〇jt y^i^/co^(〇Jt^-(A]lsin {(〇. //j) (20) 1=1 、 4)激勵軌跡優化問題求解 激勵軌跡優化問題描述為如下形式： <i>*(t)=argmin(J) (21) 約束條件為： Mr:、=i' 4 (，，卜 〇 於（，") = 〇, 》:(，,) = 0 Vi < ' _ (22) ΦΓη<Φ,{?<ΦΓ Vi e[v tf]j <Γ<φ,{?)<φ- Vie[v tf],i 式（21)和式（22)為含多約束的非線性優化問題，采用MATLAB優化工具箱中的fmincon 函數求得該非線性優化問題的解，即步驟3)中待定系數α丨，貧和Φ1()的值。
【文檔編號】B25J9/16GK106064377SQ201610389042
【公開日】2016年11月2日
【申請日】2016年6月2日 公開號201610389042.8, CN 106064377 A, CN 106064377A, CN 201610389042, CN-A-106064377, CN106064377 A, CN106064377A, CN201610389042, CN201610389042.8
【發明人】羅建軍, 薛爽爽, 袁建平, 朱戰霞, 馬衛華, 王明明, 方群, 唐歌實, 胡松杰, 王保豐
【申請人】西北工業大學