孟中杰，黃攀峰

(1.西北工業(yè)大學(xué)航天學(xué)院智能機器人研究中心，西安710072;2.航天飛行動力學(xué)技術(shù)國家級重點實驗室，西安710072)

0 引言

近年來，隨著航天技術(shù)的不斷發(fā)展，各國對于故障衛(wèi)星維修、近距離目標捕獲、軌道垃圾清理等在軌服務(wù)技術(shù)的需求日益迫切［1］。

自主機動飛網(wǎng)機器人(簡稱飛網(wǎng)機器人)是一種由柔性網(wǎng)和多個自主機動單元組成的新型空間機器人。在機動單元的控制下，它可以逼近任意空間目標，由柔性網(wǎng)覆蓋捕獲目標，然后進行操作、變軌等。飛網(wǎng)機器人在安全性、靈活性、可靠性等具有傳統(tǒng)的空間機器人所無法比擬的優(yōu)勢，在軌道垃圾清理、失效衛(wèi)星捕獲等空間任務(wù)中有著非常廣泛的應(yīng)用前景。在操作任務(wù)中，飛網(wǎng)機器人需要保持固定的網(wǎng)型，但是由于重力梯度、科氏力等的作用，柔性飛網(wǎng)保持固定網(wǎng)型十分困難。另外，飛網(wǎng)機器人的機動也大大提高其網(wǎng)型保持的難度。

在與飛網(wǎng)機器人類似的空間柔性網(wǎng)系統(tǒng)動力學(xué)與控制研究方面，G?rdsback M等研究了一種空間自旋網(wǎng)結(jié)構(gòu)的動力學(xué)和最優(yōu)展開控制問題［2－3］。翟光、于洋、李京陽等人的研究對象是一種由平臺、系繩、質(zhì)量塊、網(wǎng)組成空間飛網(wǎng)結(jié)構(gòu)。針對這種飛網(wǎng)，Mankala等研究了動力學(xué)建模與仿真問題［4］，Zhai等研究了結(jié)構(gòu)設(shè)計與動力學(xué)問題，并設(shè)計了一種利用系繩張力和推力的反饋控制器，保證飛網(wǎng)能夠有效地飛向目標［5－7］，但是，在 Mankala和 Zhai的研究中，研究重點放在平臺、系繩、末端執(zhí)行器組成的系統(tǒng)動力學(xué)研究上，飛網(wǎng)結(jié)構(gòu)被假設(shè)為質(zhì)點。于洋等研究了飛網(wǎng)的拋射動力學(xué)問題［8］。李京陽等分析了空間飛網(wǎng)的松弛模型和柔性模型，得出了松弛模型更接近真實飛網(wǎng)的結(jié)論［9］。陳欽等研究了空間飛網(wǎng)的設(shè)計，并利用集中質(zhì)量法，將系繩質(zhì)量集中在兩端節(jié)點處，建立了“節(jié)點+輕質(zhì)彈性桿”的飛網(wǎng)數(shù)學(xué)模型［10－11］。馬駿等利用類似的方法研究了一種帶四個自主機動單元的空間繩網(wǎng)機器人動力學(xué)建模和仿真問題［12］。但是，柔性網(wǎng)的編織特點造成各節(jié)點間的距離較小，而輕質(zhì)彈性桿的剛度極大，“節(jié)點+輕質(zhì)彈性桿”的飛網(wǎng)模型在計算時需要極小的步長。另一方面，上述模型均是針對仿真分析的數(shù)學(xué)模型，利用該模型很難進行針對性的控制器設(shè)計。

針對該問題，本文提出一種面向控制的自主機動飛網(wǎng)機器人的動力學(xué)建模方法，并針對性的設(shè)計其空間任務(wù)過程中的網(wǎng)型保持方法。

1 自主機動飛網(wǎng)機器人動力學(xué)建模

在動力學(xué)建模時，采用如下基本假設(shè):

(1)忽略自主機動單元體積，將其假設(shè)為質(zhì)點;

(2)飛網(wǎng)機器人運行在圓軌道上。

1. 1 飛網(wǎng)機器人的描述

在動力學(xué)建模之前，需要首先研究飛網(wǎng)機器人的表達方式。本文采用無向圖G=(V，E)描述其拓撲結(jié)構(gòu)。其中，V為非空的頂點集，包括自主機動單元、系繩交叉的編織點及人為選擇的系繩節(jié)點，設(shè)n為V中頂點數(shù)。E為V中元素構(gòu)成的二元組的集合，稱為邊集，包含飛網(wǎng)機器人各繩段，描述了各節(jié)點間的連接關(guān)系，設(shè)c為E中所有邊的個數(shù)。在實際建模中，可采用鄰接矩陣表達無向圖。將鄰接矩陣中表示連接的元素1用系繩的自然長度lij代替。則n×n維鄰接矩陣A的元素aij為:

1. 2 空間系繩動力學(xué)建模

系繩的模型由簡單到復(fù)雜可以分為輕質(zhì)剛性桿模型、剛性桿模型、輕質(zhì)彈性桿模型、彈性桿模型、株式模型、有限元線段模型、考慮彈性/彎曲/扭轉(zhuǎn)/剪切的模型等［13－14］。Buckham B 研究了低張力系繩的應(yīng)用，并通過實驗數(shù)據(jù)和仿真數(shù)據(jù)的對比，校驗了將系繩質(zhì)量集中在節(jié)點處的方法適合低張力系繩的數(shù)值仿真［15］。因此，本文采用集中質(zhì)量法建立系繩模型，系繩的非線性本構(gòu)關(guān)系為:

式中:N為系繩張力，E為系繩的楊氏模量，A為系繩的截面積，ε為系繩應(yīng)變，τ為系繩張力的單位方向向量。

空間系繩普遍采用KEVLAR等合成纖維編織而成，楊氏模量E極大，可達130 GPa。以1 mm直徑的KEVLAR系繩段為例，其彈性系數(shù)高達105。而飛網(wǎng)機器人系繩中的張力與網(wǎng)大小、機動單元的推力相關(guān)，一般較小。以10 N張力為例，系繩應(yīng)變僅為10－4，且沿系繩方向具有極高頻的振動。這也是“節(jié)點+輕質(zhì)彈性桿”的飛網(wǎng)模型在計算時需要極小的步長，以保證仿真穩(wěn)定的根本原因。為解決這一問題，假設(shè)EA→+∞，ε→0。此時，式(2)已不能用來計算系繩的張力。分析不可伸長系繩的特點，并借鑒文獻［16］中剛體接觸問題的處理方法，引入間隙函數(shù)和約束反力描述系繩的非線性本構(gòu)關(guān)系。

間隙函數(shù):

約束反力:

式中:Ri，Rj為系繩上兩節(jié)點的位置，l0為兩節(jié)點間系繩的自然長度。約束反力即近似描述了系繩中的張力。

將飛網(wǎng)機器人系統(tǒng)的間隙函數(shù)和約束反力寫成向量形式:

1. 3 飛網(wǎng)機器人的動力學(xué)建模

本節(jié)利用Hamilton原理建立自主機動飛網(wǎng)機器人的動力學(xué)模型。在集中質(zhì)量和系繩不可伸長假設(shè)下，飛網(wǎng)機器人的拉格朗日函數(shù)L僅包括各節(jié)點的動能與勢能。在地心慣性系下:

式中:mi為節(jié)點質(zhì)量，Ri為節(jié)點在地心慣性系下的位置，G表示萬有引力常數(shù)，M表示地球的質(zhì)量。

系統(tǒng)的非保守力包括各節(jié)點的控制力Fi和約束反力。非保守力做的功為:

利用廣義Hamilton原理:

可推導(dǎo)飛網(wǎng)機器人的動力學(xué)模型。

Ri包含飛網(wǎng)機器人的軌道運動和相對運動，很難直接分析機器人的形狀保持情況。在圓軌道假設(shè)下，選取飛網(wǎng)機器人軌道或附近軌道上一點為虛擬目標，建立目標軌道系，將飛網(wǎng)機器人模型轉(zhuǎn)換到目標軌道系下，并利用Hill方程化簡，得:

式中:ω為虛擬目標的平均軌道角速度。

另外，不可伸長的系繩還具有一種獨特的速度跳變現(xiàn)象。以兩節(jié)點、單系繩結(jié)構(gòu)為例，當t－時刻，兩節(jié)點間距離達到標稱長度(即g=0)，但兩節(jié)點相對速度不為0，且有相互遠離的趨勢(即˙g＜0)時，系繩突然繃緊，系繩中張力極大，使得兩個節(jié)點在t+時刻具有了沿系繩方向一致的速度，這就是速度跳變現(xiàn)象。下面對不可伸長系繩的速度跳變進行動力學(xué)建模。

將動力學(xué)方程(9)在時間區(qū)間［t－，t+］積分，得:

時間區(qū)間［t－，t+］為瞬時過程，節(jié)點位置矢量、速度矢量、控制力均為有限值，約束反力為無窮大，式(11)左側(cè)后兩項和右側(cè)第一項均為0，設(shè)約束反力在［t－，t+］的積分為 ΛC，式(11)可化簡為:

式中:ΛC為積分區(qū)間［t－，t+］內(nèi)約束反力產(chǎn)生的動量，未產(chǎn)生速度跳變系繩約束反力產(chǎn)生的動量為零?？坍a(chǎn)生速度跳變現(xiàn)象系繩的間隙函數(shù)。在t+時刻，產(chǎn)生速度跳變現(xiàn)象的系繩間隙函數(shù)為零。即:

聯(lián)立式(12)(13)，可得速度跳變后的系統(tǒng)狀態(tài)。

1. 4 飛網(wǎng)機器人動力學(xué)模型的解算

飛網(wǎng)機器人的動力學(xué)模型(9)中包含未知項約束反力λC，而λC與位置項rN，速度項˙rN，加速度項¨rN隱性相關(guān)，但式(4)僅給出其定性表達式，不能應(yīng)用于動力學(xué)模型解算。

在系統(tǒng)動力學(xué)求解時，位置項、速度項已知，即g，˙g已知。若g＞0∪g=0，˙g＞0，約束反力λ=0;若g=0，˙g=0，需要進一步求解λ。

設(shè)滿足g=0，˙g=0條件的間隙函數(shù)和約束反力為m個，將其寫成向量形式ˉλC，ˉgC。利用間隙函數(shù)定義，對ˉgC求兩次導(dǎo)得:

式中:h(rN，˙rN)為不含二次項的余項，在動力學(xué)解算時為已知項。

將動力學(xué)方程(9)代入式(14)，得:

式中:MR是ˉλC到λC的轉(zhuǎn)換矩陣，為c×m維。

具有不等式約束的隱式方程(15)是一個典型的線性互補問題，最常用的求解算法是Lemke算法。詳細的求解步驟參見文獻［17］。

飛網(wǎng)機器人動力學(xué)模型的解算流程如圖1所示。詳細的解算步驟為:

(1)計算各系繩的間隙函數(shù)，根據(jù)間隙函數(shù)及其微分將間隙函數(shù)ˉgC及約束反力ˉλC分離;

(2)利用Lemke算法求解式(15)，計算當前時刻的約束反力;

(3)利用式(9)，計算當前時刻的加速度項;

(4)利用四階龍格庫塔法進行積分運算，得到下一時刻的系統(tǒng)狀態(tài);

(5)判斷是否存在速度跳變現(xiàn)象;若存在，聯(lián)立式(12)(13)，求解速度跳變后的系統(tǒng)狀態(tài);

(6)轉(zhuǎn)到第一步，循環(huán)求解下一時刻的動力學(xué)。

圖1 自主機動飛網(wǎng)機器人模型解算流程Fig.1 Solution process for the dynamic model of AMNR

2 自主機動飛網(wǎng)機器人網(wǎng)型保持控制

由于重力梯度力、科氏力、系繩張力的作用，飛網(wǎng)機器人不能自動保持其固有形狀，尤其是在軌道機動過程中，控制力僅作用在自主機動單元上，對飛網(wǎng)形狀影響極大，因此需要進行網(wǎng)型保持控制。

系統(tǒng)動力學(xué)模型存在未知的約束反力項λC僅能通過構(gòu)建線性互補問題求解。并且，λC屬于快變項，很難利用神經(jīng)網(wǎng)絡(luò)等方法通過學(xué)習(xí)表達。這給控制系統(tǒng)設(shè)計帶來了極大的困難。速度跳變現(xiàn)象的存在也加劇了控制系統(tǒng)設(shè)計的難度。借鑒單面約束的剛體接觸控制思想，本文提出一種“雙層優(yōu)化偽動態(tài)逆內(nèi)環(huán)+變結(jié)構(gòu)控制外環(huán)”的設(shè)計思想，如圖2所示。

在偽動態(tài)逆控制中，優(yōu)化目標是求取滿足動力學(xué)約束的控制力，使得實際加速度與期望加速度之間的偏差最小。針對約束反力λC的存在，本文提出一種兩層優(yōu)化的策略。首先，設(shè)計虛擬控制量

將式(9)轉(zhuǎn)化為線性方程。在求其偽逆時，需要考慮不等式約束≥0。設(shè)飛網(wǎng)機器人可直接控制的節(jié)點位置為rNC。

然后，利用虛擬控制量求解實際控制量F。以最省能量為目標，重新構(gòu)建優(yōu)化問題。

考慮到偽逆與真實逆的偏差，需要在偽逆的外環(huán)加入控制器。本文選擇變結(jié)構(gòu)控制器。經(jīng)過偽逆控制器后，系統(tǒng)動力學(xué)模型可寫為:

式中:

設(shè)期望指令為XD，跟蹤誤差Xe=XD－X。

設(shè)計積分型切換函數(shù)為:

式中:C為正常數(shù)組成的矩陣，K為狀態(tài)反饋增益矩陣。設(shè)計滑?？刂破鳛?

式中:B+為B的Moore－Penrose逆，f＞0。下面進行穩(wěn)定性證明。

因此，在控制器作用下，系統(tǒng)一致漸近穩(wěn)定。

為減弱控制過程中的抖動，選用飽和函數(shù)sat(s)代替符號函數(shù)sgn(s)，得到最終的變結(jié)構(gòu)控制律。設(shè)Si表示S的元素，sat(S)的各元素表示為:

圖3 自主機動飛網(wǎng)機器人實例Fig.3 An example of MNR

3 仿真算例與分析

以圖3所示飛網(wǎng)機器人為例。該機器人包含5個自主機動單元和12個系繩節(jié)點。將自主機動單元和系繩編織節(jié)點作為無向圖的頂點，鄰接矩陣為:

式中:Ψ是4×8的稀疏矩陣，其元素為:

Φ是8×8的稀疏矩陣，其元素為:

設(shè)系繩直徑1 mm，楊氏模量130 Gpa，線密度4.5 g/m，自主機動單元質(zhì)量 10 kg，軌道高度700 km，飛網(wǎng)機器人初速［1，0，0］m/s，設(shè)初始狀態(tài)為繃緊狀態(tài)，分別仿真無控和節(jié)點1施加［0.1，0，0］N恒推力兩種情況，并與文獻［12］中珠子模型進行對比，誤差如圖4、圖5所示?？梢钥闯?，無控時，本文的數(shù)學(xué)模型與珠子模型仿真誤差較小。施加恒推力時，由于實際系繩存在變形，誤差比無控時大，但與整個系統(tǒng)相比，誤差最大為萬分之八。在時間對比上，珠子模型需要選擇較小的步長(本文選擇為2 ms)，以保證系統(tǒng)收斂，本文建立的模型可以選擇較大步長(本文選擇為500 ms)，時間消耗大大降低。

圖4 無控時，兩種模型對比Fig.4 Comparisons without control

圖5 節(jié)點1施加恒推力時，兩種模型對比Fig.5 Comparisons with constant thrusts on node 1

下面進行控制器的仿真校驗。設(shè)初始狀態(tài)為繃緊狀態(tài)，各自主機動單元具有［1，0，0］m/s初速，其余節(jié)點無初速。無控狀態(tài)的仿真結(jié)果如圖6所示。

可以看出:由于僅自主機動單元具有速度，其他系繩編織節(jié)點通過自主機動單元帶動運行，飛網(wǎng)機器人軌道法向平面視圖由“▽”變?yōu)椤癢”形狀，且整體成收縮趨勢。初始時刻處于軌道面內(nèi)的節(jié)點1與節(jié)點3間距離由141.42 m減小為132.36 m，初始時刻處于軌道面外的節(jié)點2與節(jié)點4間距離由141.42 m減小為65.52 m，由節(jié)點1～節(jié)點4形成的最大開口面積由10000 m2減小為8672 m2。節(jié)點5運動速度大于編織節(jié)點，飛網(wǎng)機器人底部變?yōu)榘夹?。從飛網(wǎng)機器人軌道面看，飛網(wǎng)機器人沿軌道切向運動，由于科氏力的作用，其整體向 －Z方向偏移，且由于速度方向的偏移，沿軌道切向運動變慢。以節(jié)點5為例，在1 m/s的初速下，500 s內(nèi)，其沿軌道 +X向僅運行276.56 m，同時，其沿軌道 －Z向運行198.03 m。因此，在飛網(wǎng)機器人沿切向機動時，需要進行網(wǎng)型保持與整體位置控制。

圖6 自主機動飛網(wǎng)機器人無控飛行Fig.6 Ideal non－control motion of AMNR

以節(jié)點5為例，速度跳變前后的速度差如圖7所示?？梢钥闯?不可伸長系繩所特有的速度跳變現(xiàn)象一直存在于飛行過程。

圖7 節(jié)點5速度跳變現(xiàn)象Fig.7 Velocity jump of node 5

選擇控制器參數(shù) K= － 0.03［I15，5I15］，C=115×30，f=0.1，設(shè)飛網(wǎng)機器人的期望狀態(tài)為沿軌道X 向，以初速［1，0，0］m/s運行，仿真時間為 500 s，仿真結(jié)果如圖8所示?？梢钥闯?

(1)在控制器的作用下，飛網(wǎng)機器人整體沿軌道切向運動，未出現(xiàn)徑向和法向的移動，以節(jié)點5為例，位置由初始［－1000，0，0］m 變?yōu)椋郏?00.07，0.03，－0.02］m。

(2)飛網(wǎng)機器人整體形狀保持良好;處于軌道面內(nèi)的節(jié)點1與節(jié)點3間距離由141.42 m變?yōu)?41.37 m，處于軌道面外的節(jié)點2與節(jié)點4間距離由141.42 m變?yōu)?41.61 m，由節(jié)點1～節(jié)點4形成的開口面積由10000 m2變?yōu)?00010 m2，變化率約為1‰。

圖8 自主機動飛網(wǎng)機器人有控飛行Fig.8 The motion of AMNR under the control

可見，本文設(shè)計的控制器能夠使自主機動飛網(wǎng)機器人沿預(yù)定軌跡運動，并保持其網(wǎng)型。但是，由于僅五個自主機動單元有控制力，飛網(wǎng)機器人形狀并不是嚴格未變化，在各節(jié)點間具有一定的變形。

以節(jié)點1與節(jié)點17間系繩為例，圖9是無控飛行和有控飛行時，該系繩約束反力對比。在無控飛行時，由于僅自主機動單元具有初速，在發(fā)射后，由于系繩的牽拉，系繩具有一定的約束反力(最大約0.02 N)，直至各節(jié)點具備相同的運行速度后，各節(jié)點處于近似自由飛行狀態(tài)、系繩約束反力近似為零。在有控飛行時，飛網(wǎng)機器人形狀保持較好，為克服科氏力等影響，系繩中一直存在約束反力，約束反力最大約0.34 N。

圖9 系繩約束反力對比Fig.9 Comparisons of constraint reaction forces

圖10 自主機動飛網(wǎng)機器人機動飛行Fig.10 The orbit maneuver of AMNR under the control

假設(shè)根據(jù)任務(wù)需要，在任務(wù)初期，飛網(wǎng)機器人在以設(shè)定初速逼近目標的同時，需要在100 s內(nèi)向+Z向機動75 m，設(shè)計機動過程，并利用本文設(shè)計的控制器進行200 s仿真，仿真結(jié)果如圖10所示。可以看出，飛網(wǎng)機器人能夠按設(shè)計的機動軌跡完成機動，且在機動過程中飛網(wǎng)機器人整體構(gòu)型保持良好，驗證了本文設(shè)計的飛網(wǎng)機器人控制及網(wǎng)型保持方法的有效性。

4 結(jié)論

針對自主機動飛網(wǎng)機器人的多節(jié)點編織特點，提出一種基于無向圖的結(jié)構(gòu)描述方法，然后利用間隙函數(shù)和約束反力，基于Hamilton原理建立數(shù)學(xué)模型，并設(shè)計了一種“雙層優(yōu)化偽動態(tài)逆內(nèi)環(huán)+變結(jié)構(gòu)控制外環(huán)”的網(wǎng)型保持方法。仿真結(jié)果表明:本文建立的數(shù)學(xué)模型精度與傳統(tǒng)珠式模型相當，但計算速度快，計算穩(wěn)定，不易發(fā)散;控制器能夠在任務(wù)過程中有效保持飛網(wǎng)機器人的網(wǎng)型，且避免了飛網(wǎng)機器人的軌道徑向移動。

［1］ 敬忠良，袁建平.航天器自主操作的測量與控制［M］.北京:中國宇航出版社，2011:493－518.

［2］ G?rdsback M，Tibert G.Optimal deployment control of spinning space webs and membranes［J］.Journal of Guidance，Control，and Dynamics，2009，32(5):1519 －1530.

［3］ G?rdsback M，Tibert G.Deployment control of spinning space webs［J］.Journal of Guidance，Control，and Dynamics，2009，32(1):40－50.

［4］ Mankala K K，Agrawal SK.Dynamic modeling and simulation of impact in tether net/gripper systems［J］.Multibody System Dynamics，2004，11(3):235 －250.

［5］ Zhai G，Qiu Y，Liang B，et al.System dynamics and feedforward control for tether－net space robot system ［J］.International Journal of Advanced Robotic Systems，2009，6(2):137－144.

［6］ Zhai G，Qiu Y，Liang B，et al.Research and design of orbital net－capture robot system ［C］.The 36th COSPAR Scientific Assembly，Beijing，China，July 16 －23，2006.

［7］ Zhai G，Qiu Y，Liang B，et al.Research of capture error and error compensate for space net－capture robot ［C］. IEEE International Conference on Robotics and Biomimetics，Sanya，China，December 15－18，2007.

［8］ 于洋，寶音賀西，李俊峰.空間柔性網(wǎng)拋射展開過程動力學(xué)建模與仿真［J］.宇航學(xué)報，2010，31(5):1289－1296.［Yu Yang，Baoyin He－xi，Li Jun－feng.Modeling and simulation of space web projecting dynamics ［J］.Journal of Astronautics，2010，31(5):1289 －1296.］

［9］ 李京陽，于洋，寶音賀西，等.空間柔性網(wǎng)兩種動力學(xué)模型的比較［J］.力學(xué)學(xué)報，2011，43(3):542－550.［Li Jingyang，Yu Yang，Baoyin He－xi，et al.Simulation and comparison of different dynamical models of space webs［J］.Chinese Journal of Theoretical and Applied Mechanics，2011，43(3):542 －550.］

［10］ 陳欽，楊樂平.空間繩網(wǎng)系統(tǒng)發(fā)射動力學(xué)問題研究［J］.宇航學(xué)報，2009，30(5):1829 － 1833.［Chen Qin，Yang Le－ping.Research on casting dynamics of orbital net systems［J］.Journal of Astronautics，2009，30(5):1829 －1833.］

［11］ 陳欽.空間繩網(wǎng)系統(tǒng)設(shè)計與動力學(xué)研究［D］.長沙:國防科學(xué)技術(shù)大學(xué)，2010.［Chen Qin.Design and dynamics of an orbital net－capture system ［D］.Changsha:National University of Defense Technology，2010.］

［12］ 馬駿，黃攀峰，孟中杰，等.自主機動空間繩網(wǎng)機器人設(shè)計與動力學(xué)建模［J］.宇航學(xué)報，2013，34(10):1316－1322.［Ma Jun，Huang Pan－feng，Meng Zhong－jie，et al.Design and dynamics modeling of autonomous maneuvering tethered－net space robot system［J］.Journal of Astronautics，2013，34(10):1316－1322.］

［13］ Kumar K D. Review on dynamics and control of nonelectrodynamic tethered satellite systems［J］.Journal of Spacecraft and Rockets，2006，43(4):705 －720.

［14］ Kristiansen K U， Palmer P L， Roberts R M. Numerical modelling of elastic space tethers［J］.Celestial Mechanics and Dynamical Astronomy，2012，113(2):235－254.

［15］ Buckham B，Nahon M，Cote G.Validation of a finite element model for slack ROV tethers［C］.OCEANS 2000 MTS/IEEE Conference and Exhibition，Providence，USA，September 11－14，2000.

［16］ F?rg M， Pfeiffer F， Ulbrich H. Simulation of unilateral constrained systems with many bodies［J］.Multibody System Dynamics，2005，14(2):137－154.

［17］ 陳寶林.最優(yōu)化理論與算法(第二版)［M］.北京:清華大學(xué)出版社，2005:422－426.