沈孝龍，王吉芳，郭子昇

(北京信息科技大學(xué)機(jī)電工程學(xué)院，北京 100192)

0 引言

隨著機(jī)器人在生產(chǎn)制造領(lǐng)域的大范圍應(yīng)用，工業(yè)機(jī)械臂在生產(chǎn)線的裝機(jī)量逐年上升，稠密布置的機(jī)械臂在有限的工作空間內(nèi)暴露出機(jī)器人與現(xiàn)場障礙物物理接觸時產(chǎn)生的安全問題。在即將碰撞瞬間前能否對碰撞進(jìn)行檢測避免設(shè)備撞擊損壞與安全事故發(fā)生，使機(jī)械臂可以安全避過潛在障礙物是該類機(jī)器人能否被進(jìn)一步推廣應(yīng)用與提高單位空間裝機(jī)量的關(guān)鍵所在，因此，研究一種使機(jī)械臂可在三維空間避障的路徑規(guī)劃算法具有極高的工程應(yīng)用與學(xué)術(shù)研究價值。

目前，避碰研究分全局避碰與局部避碰兩種[1]。前者需獲知大量周圍環(huán)境信息后才能對其進(jìn)行規(guī)劃，有A*搜索、馬爾科夫決策法與隨機(jī)樹形法[2]等。此類方法缺點(diǎn)是：無法約束關(guān)節(jié)位姿、易陷入局部災(zāi)難、耗時等。局部避碰使用碰撞檢測，能根據(jù)物體當(dāng)前運(yùn)動狀態(tài)與障礙物關(guān)系實(shí)時給出避碰方案，受到廣泛研究。曹博等[3]利用改進(jìn)人工勢場法解決冗余機(jī)械臂無碰運(yùn)動。季曉明等[4]把無碰規(guī)劃問題變?yōu)樽顑?yōu)化問題，設(shè)計(jì)天牛須搜索機(jī)制的循環(huán)神經(jīng)網(wǎng)絡(luò)解決避碰問題。劉建春等[5]對機(jī)械臂建立球體和膠囊體包圍盒模型，使用二次投影法完成對雙機(jī)械臂的快速碰撞檢測。王剛等[6]利用人工勢場法修改蟻群算法的啟發(fā)值參數(shù)并提出吸引素修改信息素參數(shù)更新規(guī)則，實(shí)現(xiàn)了機(jī)器人三維路徑規(guī)劃。王猛等[7]設(shè)計(jì)指引函數(shù)改進(jìn)蟻群算法初期搜索的盲目性與隨機(jī)性，通過增加閥值和路徑權(quán)重改造信息素更新規(guī)則，改進(jìn)算法性能。朱晨曦等[8]改造蟻群算法啟發(fā)值和信息素更新規(guī)則，保留前代最優(yōu)路徑，提高了人機(jī)象棋實(shí)用性與效率。

目前蟻群算法在機(jī)械臂路徑規(guī)劃研究較多，改進(jìn)的策略基本上是對啟發(fā)值、信息素及選擇概率方面改進(jìn)，或者融合人工勢場算法，但此類方法耗時較長，在求解最優(yōu)路徑時較易陷入局部最優(yōu)，對機(jī)械臂三維避碰路徑規(guī)劃研究少。實(shí)際應(yīng)用的機(jī)械臂，工作場景多變復(fù)雜，同時存在障礙物，給路徑規(guī)劃增加了難度。所以，針對復(fù)雜三維空間的避障路徑規(guī)劃有重要探索價值。

1 機(jī)械臂運(yùn)動學(xué)分析

1.1 正運(yùn)動學(xué)分析

仿真實(shí)驗(yàn)以6-DOF串聯(lián)機(jī)械臂為對象，進(jìn)行算法的驗(yàn)證。機(jī)械臂的連桿坐標(biāo)系如圖1所示。

圖1 機(jī)械臂連桿坐標(biāo)系

由D-H法，連桿坐標(biāo)系i-1到i的變換由關(guān)節(jié)角θi、連桿扭角αi、連桿偏距di、連桿長ai這4個參數(shù)描述，結(jié)合產(chǎn)品手冊可得其D-H參數(shù)如表1所示。

表1 機(jī)械臂D-H參數(shù)表

相鄰兩連桿位姿變換可用i-1Ti表示，如式(1)所示。

(1)

式中，cαi-1=cosαi-1；sαi-1=sinαi-1；cθi=cosθi；sθi=sinθi；i=1,2,…,n。

則末端執(zhí)行器相對基坐標(biāo)系位姿矩陣0T6如下：

(2)

式中，R=[n,o,a]T系末端工具的姿態(tài)信息；P=[px,py,pz]T系末端工具的位置信息。把表1各參數(shù)帶入式(1)和式(2)，即可求得各位姿參數(shù)方程，從而得出機(jī)械臂正運(yùn)動學(xué)描述。

1.2 逆運(yùn)動學(xué)分析

由圖1知，機(jī)械臂符合Pieper準(zhǔn)則,可根據(jù)其正運(yùn)動學(xué)方程，通過多次左乘逆矩陣將其進(jìn)行逆變換，最終可以求到8組逆解，具體計(jì)算過程不在本文論述。對于軌跡規(guī)劃這些逆解并不都滿足，因各個關(guān)節(jié)的轉(zhuǎn)動范圍有約束，自身構(gòu)型問題也會存在奇異解。另外，我們常常希望在滿足軌跡規(guī)劃的前提下，使機(jī)械臂的運(yùn)動具有優(yōu)異的第二特性，如最佳柔順或最低能量消耗等。我們需要在滿足機(jī)械臂工作要求前提下，去除非法解，然后根據(jù)某準(zhǔn)則得出一組最優(yōu)解，本文期望各相鄰路徑節(jié)點(diǎn)上的關(guān)節(jié)轉(zhuǎn)角差的絕對值和最小以滿足較低能量消耗。

2 碰撞檢測分析

由于障礙物是已知且靜止的，故碰撞檢測時只需保證臂自身在運(yùn)動時避免與障礙物碰撞即可，處理手段是利用層次包圍盒法[9]，即用規(guī)則幾何體包裹形狀復(fù)雜不規(guī)則障礙物。

2.1 包絡(luò)方法

使用長方體包裹空間中的障礙物并滿足其各棱邊均與坐標(biāo)軸平行。數(shù)學(xué)描述為：

R={(x,y,z)|lx≤x≤mx,ly≤y≤my,lz≤z≤mz}

(3)

式中，(lx,ly,lz)表示長方體棱邊沿坐標(biāo)軸X、Y、Z方向的最小值；(mx,my,mz)表示長方體棱邊沿坐標(biāo)軸X、Y、Z方向的最大值。

本文研究的是多自由度串聯(lián)機(jī)械臂，因其本體各個連桿的形狀與圓柱體類似，使用圓柱體包絡(luò)法能夠?qū)C(jī)身簡化，同時較好留存機(jī)械臂的結(jié)構(gòu)特征。這里底座在其運(yùn)動中位姿并不變化，只需對余下的連桿包絡(luò)簡化。

在使用圓柱體簡化機(jī)械臂，長方體簡化障礙物后，要想在空間中判斷兩者間的位置狀態(tài)依舊不易，為進(jìn)一步簡化計(jì)算，將機(jī)械臂連桿抽象為直線，把原圓柱體半徑加到障礙物模型中進(jìn)行擴(kuò)大[10]，最終變?yōu)闄z測直線與平面相交的問題，就可判斷機(jī)械臂與障礙物是否碰撞。簡易化模型如圖2所示。

圖2 機(jī)械臂與障礙物簡化模型

假設(shè)連桿i直線方程為Ri，障礙物模型Ii，膨脹后的障礙物模型用符號Oi表示。我們假定Ii的8個頂點(diǎn)坐標(biāo)依次為(x1,y1,z1)…(x8,y8,z8)。膨脹厚度設(shè)為r，頂點(diǎn)1沿x軸的單位向量Px為：

(4)

頂點(diǎn)1沿y軸的單位向量Py為：

(5)

頂點(diǎn)1沿z軸的單位向量Pz為：

(6)

綜上所述，求得膨脹后第一個頂點(diǎn)的坐標(biāo)：

O1=[x1,y1,z1]+Px+Py+Pz

(7)

重復(fù)上述過程，易求得Oi的其余7個頂點(diǎn)坐標(biāo)。

2.2 碰撞檢測

方法如下：假設(shè)連桿i兩端關(guān)節(jié)i和i+1的坐標(biāo)為p1(x1,y1,z1)和p2(x2,y2,z2)，則連桿i所在直線方向H=(x2-x1,y2-y1,z2-z1)。假設(shè)某空間障礙物的某一平面S其4個端點(diǎn)為s1(xs1,ys1,zs1)、s2(xs2,ys2,zs2)、s3(xs3,ys3,zs3)、s4(xs4,ys4,zs4)，法向量為F(f1,f2,f3)。由空間幾何原理知，當(dāng)H·F=0時，線與面平行，無交點(diǎn)；反之，線與面相交，有某交點(diǎn)p0(x0,y0,z0)。由幾何關(guān)系知，交點(diǎn)為碰撞點(diǎn)需要滿足：①交點(diǎn)在連桿線段上，②交點(diǎn)在空間平面內(nèi)。障礙物抽象為長方體，其表面方向有3種：分別平行于x軸上下表面、y軸左右表面、z軸前后表面。下面以平行于x軸上表面平面S舉例，其4個端點(diǎn)為s1(xs1,ys1,zs1)、s2(xs2,ys1,zs1)、s3(xs2,ys3,zs1)、s4(xs1,ys3,zs1)，連桿i若與平面S相交，需滿足如下約束：

min(z1,z2)min(xs1,xs2)min(ys1,ys3)

(8)

在檢測完連桿i與面S是否發(fā)生碰撞后，同樣的方法檢測連桿i與長方體障礙物下面是否碰撞，以及其余連桿是否與其碰撞。

3 三維空間建模

與二維平面的機(jī)械臂避障研究比，三維空間的避障研究較少，下面重點(diǎn)研究如何實(shí)現(xiàn)在三維空間做避障軌跡規(guī)劃，不可避免的涉及到三維空間的建模。具體方法為：創(chuàng)建一個包含機(jī)械臂與障礙物及起終點(diǎn)的三維立體區(qū)域，首先我們沿X軸方向?qū)⒖臻g進(jìn)行等距劃分，得到n個平面，然后沿Y軸方向?qū)@n個平面進(jìn)行n等分，最后沿Z軸方向進(jìn)行n等分，把這些交點(diǎn)提取出來就可以把空間離散成(n+1)*(n+1)*(n+1)的一系列三維空間點(diǎn)集合，同時也可以得到n*n*n個立方體，用0表示安全，1表示障礙物。柵格化空間路徑節(jié)點(diǎn)如圖3所示。

圖3 三維空間點(diǎn)圖

在有了三維軌跡規(guī)劃所需的路徑節(jié)點(diǎn)后，我們把機(jī)械臂末端執(zhí)行器的移動軌跡看作是依次路過規(guī)劃路徑點(diǎn)的集合，由運(yùn)動學(xué)逆解求解可能的解，經(jīng)過逆解選擇與碰撞檢測無碰后的逆解，得出對應(yīng)關(guān)節(jié)角，求得系列路徑節(jié)點(diǎn)，再用高次樣條曲線平滑路徑點(diǎn)實(shí)現(xiàn)路徑規(guī)劃任務(wù)。

4 基于改進(jìn)麻雀搜索的機(jī)械臂避障路徑規(guī)劃

麻雀搜索(sparrow search algorithm，SSA)是一種基于群體社會化特征優(yōu)化的群智能算法[11]。相比傳統(tǒng)群智能算法，結(jié)構(gòu)簡單易實(shí)現(xiàn)、控制參數(shù)較少，局部尋優(yōu)本領(lǐng)較強(qiáng)。在單、多峰等基準(zhǔn)函數(shù)測試中表現(xiàn)性能勝于粒子群、蟻群等算法。原作者將算法運(yùn)用在三維無人機(jī)航跡優(yōu)化中，效果顯著，與本文探討的機(jī)械臂避障路徑規(guī)劃類似。對基本麻雀算法，通過融合柯西變異和反向?qū)W習(xí)策略對其控制參數(shù)進(jìn)行優(yōu)化改進(jìn)[12]，最終實(shí)現(xiàn)了機(jī)械臂避障路徑規(guī)劃。

4.1 基本麻雀算法

在麻雀搜索算法里，一般把種群內(nèi)的個體分為發(fā)現(xiàn)者、跟隨者和警戒者3種，每個個體位置代表一個解。根據(jù)作者設(shè)定，警戒者一般占種群的10%～20%，發(fā)現(xiàn)者與跟隨者為動態(tài)變化量，所以當(dāng)有一個體變?yōu)榘l(fā)現(xiàn)者代表著另一個體將轉(zhuǎn)變?yōu)楦S者。根據(jù)分工，發(fā)現(xiàn)者為種群搜集覓食方位和區(qū)域，跟隨者跟隨發(fā)現(xiàn)者進(jìn)行覓食，警戒者負(fù)責(zé)對覓食區(qū)域的安全監(jiān)視與預(yù)警。在一個完整的覓食過程中，三者位置動態(tài)更新，完成對食物資源的獲取。限于篇幅，有關(guān)基本麻雀算法的具體理論與數(shù)學(xué)模型可以閱讀文獻(xiàn)[13]。

基本麻雀搜索在處理復(fù)雜組合優(yōu)化問題時，易陷入局部災(zāi)難、收斂速度減慢、尋優(yōu)結(jié)果變差或搜索不全等。因機(jī)械臂是應(yīng)用于實(shí)際生產(chǎn)的，工作場合復(fù)雜多變，希望在較短時間內(nèi)尋到最優(yōu)解，便于實(shí)時控制，故需對基本麻雀算法進(jìn)行改進(jìn)。

4.2 融合柯西變異和反向?qū)W習(xí)的麻雀算法

4.2.1 Sin混沌初始化種群

混沌運(yùn)動的隨機(jī)性和遍歷不重復(fù)性有助于建構(gòu)多樣的種群個體，使群落內(nèi)個體均勻分布于搜索空間。Sin混沌是一種映射折疊次數(shù)無窮的模型，楊海東等[14]研究證明Sin混沌比其它混沌有更優(yōu)的混沌特性，為提升最優(yōu)解質(zhì)量和保持群落里個體多樣性，采用Sin混沌映射對麻雀搜索算法的初始種群進(jìn)行改造,以提高算法尋優(yōu)能力。其一維自映射表達(dá)式為：

(9)

4.2.2 動態(tài)自適應(yīng)權(quán)重

基本麻雀搜索的發(fā)現(xiàn)者在迭代初期就向全局最優(yōu)解逼近，會導(dǎo)致搜索范圍不足，陷入局部最優(yōu)，使搜索失敗。為克服這一弊端，在原發(fā)現(xiàn)者位置更新公式中加入上一代全局最優(yōu)解，使當(dāng)前發(fā)現(xiàn)者位置同時受上代發(fā)現(xiàn)者位置與最優(yōu)解影響，有利于降低算法陷入局部最優(yōu)解概率。此外，再在其位置更新公式中加入動態(tài)權(quán)重系數(shù)ω[15]，讓它在迭代初期具有較大值，促進(jìn)全局搜索，迭代后期自適應(yīng)變小，促進(jìn)局部搜索并加快收斂速度。權(quán)重因子ω與新的發(fā)現(xiàn)者位置更新公式為：

(10)

(11)

4.2.3 改進(jìn)的警戒者公式：

(12)

該式表示：若該麻雀是最優(yōu)位置麻雀，它會逃到最優(yōu)與最差位置間的任一隨機(jī)位置，反之，它會在當(dāng)前位置與最優(yōu)位置間的任一隨機(jī)位置。Xbest為當(dāng)前全局最優(yōu)位置；β為步長參數(shù)，是服從均值為0，方差為1的正態(tài)分布隨機(jī)數(shù)；fg為當(dāng)前全局最優(yōu)適應(yīng)值；fi為當(dāng)前種群某個體適應(yīng)值。

4.2.4 融合柯西變異和反向?qū)W習(xí)機(jī)制

反向?qū)W習(xí)的目的是以當(dāng)前解為基礎(chǔ)，經(jīng)反向?qū)W習(xí)找到對應(yīng)的反向解，再經(jīng)過判斷比較保留更好的解。為確保個體尋到的解是最優(yōu)解，將反向?qū)W習(xí)機(jī)制融入麻雀搜索中，其數(shù)學(xué)表達(dá)式為：

(13)

(14)

(15)

一維柯西分布密度函數(shù)如下：

(16)

當(dāng)a取1時，式(16)變?yōu)闃?biāo)準(zhǔn)柯西分布。把式(16)加入目標(biāo)位置更新公式里，展現(xiàn)柯西算子的干擾能力，有利于算法全局尋優(yōu)性能的提高。

(17)

式中，cauchy(0,1)為標(biāo)準(zhǔn)柯西分布?？挛鞣植茧S機(jī)變量生成的函數(shù)為η=tan[(ξ-0.5)π]。

為找到最優(yōu)性能解，這里設(shè)計(jì)一種分段選擇策略來更新目標(biāo)位置，把反向?qū)W習(xí)與柯西擾動依某概率分段執(zhí)行，使目標(biāo)位置動態(tài)更新。由反向?qū)W習(xí)求得反向解，增加算法搜索區(qū)域。通過柯西變異策略在最優(yōu)解位置產(chǎn)生干擾得出新解，降低算法陷入局部最優(yōu)解。其選擇概率表達(dá)式[16]如下：

(18)

式中，θ為調(diào)節(jié)系數(shù)。選擇原理如下：若rand

(19)

式中，f(x)表示x位置的適應(yīng)值。

4.2.5 算法步驟

步驟1：初始化參數(shù)設(shè)置，如種群量NP，最高迭代數(shù)itermax，發(fā)現(xiàn)者比率PD，安全值ST等，并用Sin混沌映射產(chǎn)生初始麻雀種群；

步驟2：求各個體適應(yīng)值，選出當(dāng)前最優(yōu)與最差適應(yīng)值個體及相對應(yīng)的位置；

步驟3：從較優(yōu)適應(yīng)度值的麻雀中選取PD比例作為發(fā)現(xiàn)者，按照式(11)更新位置。其余麻雀為跟隨者，按照基本麻雀搜索位置更新公式更新；

步驟4：從種群隨機(jī)選取一些麻雀為警戒者，按式(12)更新位置；

步驟5：由概率Ps判斷是否用柯西擾動或反向?qū)W習(xí)對當(dāng)前最優(yōu)解干擾，產(chǎn)生新解；

步驟6：根據(jù)式(19)確定是否進(jìn)行位置更新；

步驟7：判斷是否到達(dá)結(jié)束條件，如是執(zhí)行下一步，反之跳轉(zhuǎn)到步驟2；

步驟8：程序執(zhí)行完，輸出結(jié)果。

5 仿真結(jié)果與分析

5.1 算法實(shí)現(xiàn)流程

機(jī)械臂無碰路徑規(guī)劃是在已知障礙物信息的工作空間中，給定起終兩個位置點(diǎn)使其完成兩點(diǎn)間運(yùn)動，以最短路徑為評價原則構(gòu)建適應(yīng)度函數(shù)，并要求軌跡運(yùn)行連續(xù)，各關(guān)節(jié)角度變化連續(xù)平緩無沖擊且能避開所有障礙物的最優(yōu)無碰路徑。軌跡規(guī)劃仿真是在笛卡爾空間中對末端執(zhí)行器的移動路徑及自身位姿狀態(tài)進(jìn)行規(guī)劃，以防機(jī)身與障礙物碰撞。讓機(jī)械臂做往復(fù)運(yùn)動，觀察全過程有無與障礙物發(fā)生碰撞。算法實(shí)現(xiàn)流程如圖4所示。

圖4 算法實(shí)現(xiàn)流程圖

5.2 仿真結(jié)果分析

仿真環(huán)境為MATLAB 2020b，計(jì)算機(jī)配置為：AMD5800H、3.2 GHz、16 G，分別對改進(jìn)前后的麻雀搜索進(jìn)行路徑優(yōu)化效果對比。另外，為了體現(xiàn)新穎的麻雀搜索算法的出色優(yōu)化性能，選取文獻(xiàn)[10]的改進(jìn)蟻群算法與之對比，算法所需的參數(shù)如表2所示，α為信息素啟發(fā)值，ρ為揮發(fā)值，β1為適應(yīng)度啟發(fā)值，為保證對比公平性，種群數(shù)量與迭代次數(shù)設(shè)置相同。離散點(diǎn)劃分量為21*21*21，起位置(1,10,4)，終位置(21,8,6)，設(shè)置5個障礙物，仿真結(jié)果對比如圖5～圖7所示。

表2 算法相關(guān)參數(shù)值

(a) 三維路徑規(guī)劃效果 (b) X-Z方向路徑規(guī)劃效果

(c) 適應(yīng)值隨迭代次數(shù)變化圖圖5 改進(jìn)麻雀搜索的路徑規(guī)劃

(c) 適應(yīng)值隨迭代次數(shù)變化圖圖6 基本麻雀搜索的路徑規(guī)劃

(a) 三維路徑規(guī)劃效果 (b) X-Z方向路徑規(guī)劃效果

(c) 適應(yīng)值隨迭代次數(shù)變化圖圖7 改進(jìn)蟻群算法的路徑規(guī)劃

可以看出，在相等參數(shù)值下，改進(jìn)麻雀算法搜索到的路徑長度更短，迭代更快。改進(jìn)與基本麻雀算法的路徑長度分別為31.075 7和83.563 4，迭代數(shù)分別為6次和47次，尋優(yōu)時間分別為1.746 s和2.932 s。蟻群搜索迭代次數(shù)為30次時的路徑長度為89.516 9，尋優(yōu)時間2.479 s。

選擇改進(jìn)麻雀搜索算法對機(jī)械臂進(jìn)行避障軌跡規(guī)劃，起位置(35,210,525)，終位置(735,175,175)。選取機(jī)械臂往復(fù)運(yùn)動的一些路徑節(jié)點(diǎn)位置坐標(biāo)求出對應(yīng)關(guān)節(jié)角如表3所示。機(jī)械臂運(yùn)行的軌跡顯示在三維空間中，其各關(guān)節(jié)角位移與角速度隨時間變化曲線如圖9所示，截取部分機(jī)械臂往復(fù)運(yùn)動圖如圖8所示。

表3 路徑節(jié)點(diǎn)的位置坐標(biāo)和關(guān)節(jié)轉(zhuǎn)角

(a) 起點(diǎn) (b) 中間點(diǎn)1 (c) 中間點(diǎn)2 (d) 中間點(diǎn)3

(e) 終點(diǎn) (f) 中間點(diǎn)4 (g) 中間點(diǎn)5(h) 起點(diǎn)圖8 機(jī)械臂往復(fù)運(yùn)行圖

圖9 關(guān)節(jié)角位移與速度曲線圖

仿真結(jié)果表明：各關(guān)節(jié)轉(zhuǎn)角變化平滑連續(xù)，未發(fā)生突變，滿足機(jī)械臂運(yùn)動學(xué)規(guī)律；基于五次多項(xiàng)式的路徑點(diǎn)規(guī)劃，軌跡連續(xù)平滑；機(jī)械臂在運(yùn)動全程能夠根據(jù)障礙物位置信息調(diào)整路徑點(diǎn)，使其未與障礙物碰撞，表明算法規(guī)劃有效。

6 結(jié)束語

針對串聯(lián)機(jī)械臂在三維空間的無碰路徑規(guī)劃問題，建立機(jī)械臂與障礙物及三維空間模型，把碰撞檢測抽象為空間內(nèi)判斷直線與平面的相交問題，再把問題轉(zhuǎn)化為多元函數(shù)優(yōu)化問題，利用新穎的麻雀搜索算法，融合柯西變異與反向?qū)W習(xí)機(jī)制對其改進(jìn)，最終運(yùn)用在串聯(lián)機(jī)械臂的避障軌跡規(guī)劃仿真實(shí)驗(yàn)中，得出以下結(jié)論：

(1)改進(jìn)麻雀搜索可以提高原算法尋優(yōu)性能，跳出局部最優(yōu)。由Sin混沌產(chǎn)生的個體位置豐富了種群內(nèi)個體多樣性；發(fā)現(xiàn)者位置更新公式引入自適應(yīng)權(quán)重因子，有效平衡全局搜索與局部搜索矛盾；融合柯西變異和反向?qū)W習(xí)機(jī)制，增加解的多樣性，降低了算法陷入局部最優(yōu)概率，提升了全局尋優(yōu)性能。

(2)機(jī)械臂無碰軌跡規(guī)劃仿真實(shí)驗(yàn)表明：所提算法在收斂速度，收斂精度及最優(yōu)解方面表現(xiàn)出色，能夠滿足避障要求，使軌跡運(yùn)行連續(xù)平緩，有利保證機(jī)械臂運(yùn)動中不會產(chǎn)生振動與剛性沖擊，關(guān)節(jié)角位移與速度變化曲線連續(xù)，其算法正確性得到驗(yàn)證。