本發(fā)明涉及一種數(shù)值迭代算法，具體涉及一種含有柔性運動副欠驅(qū)動機構(gòu)運動學、動力學方程的求解迭代算法。

背景技術(shù)：

欠驅(qū)動機構(gòu)由于驅(qū)動器的減少而具有能耗低、質(zhì)量輕、成本小等優(yōu)點，但其運動學和動力學約束不完整，機構(gòu)運動具有不確定性，而在含有柔性運動副的欠驅(qū)動機構(gòu)中，柔性運動副的彈性反力可以起到驅(qū)動力的作用，一定程度上彌補欠驅(qū)動機構(gòu)動力約束的不完整，但柔性運動副的加入也使機構(gòu)的運動學、動力學問題變得更加復雜而難以求解，但機構(gòu)的運動學和動力學方程求解問題是機構(gòu)深入研究開發(fā)的基礎，因此這一類問題的研究十分必要。

含有柔性運動副的欠驅(qū)動機構(gòu)由于同時具有欠驅(qū)動和柔性兩方面的特性，因此其動力學模型是高度復雜的非線性微分方程組，要得到其動態(tài)響應的解析解是非常困難的。目前，全驅(qū)動柔性機構(gòu)的動力學模型一般采用數(shù)值解法，用積分迭代的方法逐步逼近，然而對于含有柔性運動副的欠驅(qū)動機構(gòu)而言，被動關節(jié)的加速度存在二階非完整約束，具有不可積性，因此，傳統(tǒng)的數(shù)值解法不再適用。目前，針對此問題，主要有兩種解決途徑：一是從控制的角度出發(fā)，利用主被動關節(jié)之間的動力學耦合作用，通過恰當?shù)目刂撇呗允箼C構(gòu)實現(xiàn)運動，但是，主被動關節(jié)之間的內(nèi)在本質(zhì)規(guī)律并不是固定不變的，它是隨著機構(gòu)位形和驅(qū)動器的位置而變化的，因此，該類方法的缺點是求解結(jié)果準確度不高[1]；另一種方法是通過模型降階的思想將欠驅(qū)動二階非完整系統(tǒng)降階為幾個具有完整約束的子系統(tǒng)，然后基于各子系統(tǒng)的積分特性進行求解，該類方法的缺點是需要針對不同的機構(gòu)設置不同的約束進行降階，方法不具有通用性[2]。因此，針對含有柔性運動副欠驅(qū)動機構(gòu)的運動學、動力學問題，目前還未見較為完善的數(shù)值求解方法。

[1]Arai H,Tachi S.Position control ofmanipulator with passive joints using dynamic coupling[J].IEEE Transactions on Robotics&Automation,1991,7(4):528-534.

[2]Katake A B,Mahindrakar A D,Banavar R N.Study of underactuated mechanisms in the presence ofholonomic constraints:the constrained Acrobot[C]//IEEE International Conference on Industrial Technology.2000:703-706vol.2.

技術(shù)實現(xiàn)要素：

針對現(xiàn)有技術(shù)的不足，本發(fā)明擬解決的技術(shù)問題是，提供一種含有柔性運動副欠驅(qū)動機構(gòu)運動學、動力學求解迭代算法，該算法基于時間無限細分時兩個時刻區(qū)間內(nèi)速度和加速度不變的原理進行迭代和求解，即直接認為不同時刻區(qū)間內(nèi)速度和加速度是階躍變化的，不用通過積分方法求解速度和加速度，克服了目前欠驅(qū)動機構(gòu)中由于存在二階非完整約束而無法進行積分迭代的缺陷。

本發(fā)明解決所述技術(shù)問題采用的技術(shù)方案是：

一種含有柔性運動副欠驅(qū)動機構(gòu)運動學、動力學求解迭代算法，該算法的步驟是：

1)建立機構(gòu)坐標系，含有柔性運動副欠驅(qū)動機構(gòu)的固定端為A點，與固定端連接的構(gòu) 件的另一端為B點，柔性運動副連接一端為D點，D點為滑塊所在位置，柔性運動副另一端連接固定，以A點為原點，AD連線為x軸，指向D的方向為x軸的正方向，垂直x軸向上為y軸正方向；

2)定義機構(gòu)參數(shù)，建立欠驅(qū)動機構(gòu)的運動學、動力學模型，建模過程中不考慮各運動副之間的摩擦，并設機構(gòu)處于水平面上，欠驅(qū)動機構(gòu)的桿件數(shù)量為e，其中，原動件記為i，其余各桿件記為o，各桿件的桿長記為l_m，各桿件與x軸的夾角記為θ_m，其中m＝1、2、…、e，各構(gòu)件的重力勢能為零，得到機構(gòu)的運動學位置方程、運動學速度方程、運動學加速度方程和動力學方程，定義迭代次數(shù)n，定義迭代步長δt；

3)給定各構(gòu)件初始條件及原動件運動規(guī)律，求解t₀時刻機構(gòu)的運動學和動力學方程，得到此時各構(gòu)件的運動規(guī)律及原動件的驅(qū)動力；所述的求解t₀時刻機構(gòu)的運動學和動力學方程按如下步驟進行：

(3a)機構(gòu)原動件的位置輸入已知，記為θ_i(t₀)；滑塊的初始位置已知，記為x(t₀)，利用運動學位置方程求出θ_o(t₀)；

(3b)初始時刻，原動件角速度滑塊速度x以步驟(3a)為基礎，利用運動學速度方程求得

(3c)機構(gòu)原動件的角加速度輸入已知，記為在步驟(3a)和步驟(3b)的基礎上利用動力學方程求得滑塊的加速度，記為同時求得原動件轉(zhuǎn)矩，記為τ_i(t₀)；

(3d)在步驟(3a)-(3c)的基礎上將和帶入運動學加速度方程，求得

4)設從t_j-1時刻到t_j時刻的時間間隔無限小，記為δt，其中j＝1,2,3...，認為滑塊的速度和加速度在某一時間間隔無限小的區(qū)間內(nèi)不變，等于區(qū)間內(nèi)前一時刻的速度和加速度，而不同時刻的區(qū)間速度和加速度是階躍變化的，求解t_j時刻機構(gòu)的運動學和動力學方程，得到此時各構(gòu)件的運動規(guī)律及原動件的驅(qū)動力；所述求解t_j時刻機構(gòu)的運動學和動力學方程按如下步驟進行：

(4a)在t_j時刻機構(gòu)原動件的位置輸入已知，記為θ_i(t_j)；滑塊的位置記為x(t_j)，x(t_j)由如下等式求得，然后利用運動學位置方程求得θ_o(t_j)；

(4b)機構(gòu)原動件的角速度輸入已知，記為滑塊的速度記為以t_j-1時刻所求結(jié)果為基礎進行迭代，然后利用運動學速度方程求出

(4c)機構(gòu)原動件的角加速度輸入已知，記為利用動力學方程求得滑塊的加速度及原動件轉(zhuǎn)矩，滑塊的加速度記為原動件轉(zhuǎn)矩記為τ_i(t_j)；

(4d)將和帶入運動學加速度方程求得

5)將上述步驟4)重復進行n次，即可得到該機構(gòu)所有構(gòu)件在整個運動周期內(nèi)的運動規(guī)律及原動件的驅(qū)動力，利用MATLAB逐點繪圖，得到各構(gòu)件運動規(guī)律曲線圖及原動件驅(qū)動力變化曲線圖。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：

本發(fā)明構(gòu)思巧妙，提出一種含有柔性運動副欠驅(qū)動機構(gòu)運動學、動力學求解的數(shù)值迭代算法，直接認為滑塊的速度和加速度在某一時間間隔無限小的區(qū)間內(nèi)不變，等于區(qū)間內(nèi)前一時刻的速度和加速度，省去積分求解速度和加速度的環(huán)節(jié)，克服了欠驅(qū)動機構(gòu)中由于加速度不可積而無法得到數(shù)值解的缺陷，實現(xiàn)了對含有柔性運動副欠驅(qū)動機構(gòu)的求解。

附圖說明

圖1為含有柔性運動副欠驅(qū)動機構(gòu)二自由度的機構(gòu)簡圖。

圖2是滑塊位移隨時間變化圖。

圖3是滑塊速度隨時間變化圖。

圖4是滑塊加速度隨時間變化圖。

圖5是原動件驅(qū)動力矩隨時間變化圖。

圖6為含有柔性運動副欠驅(qū)動機構(gòu)三自由度的機構(gòu)簡圖。

具體實施方式

下面結(jié)合附圖和具體的實施例對本發(fā)明作進一步說明，本發(fā)明的實施方式包括但不限于下列實施例。

本發(fā)明含有柔性運動副欠驅(qū)動機構(gòu)(以下簡稱機構(gòu)或欠驅(qū)動機構(gòu))運動學、動力學求解迭代算法，該算法的步驟是：

1)建立機構(gòu)坐標系，含有柔性運動副欠驅(qū)動機構(gòu)(以下簡稱機構(gòu)或欠驅(qū)動機構(gòu))的固定端為A點，與固定端連接的構(gòu)件的另一端為B點，柔性運動副連接一端為D點，D點為滑塊所在位置，柔性運動副另一端連接固定，以A點為原點，AD連線為x軸，指向D的方向為x軸的正方向，垂直x軸向上為y軸正方向；

2)定義機構(gòu)參數(shù)，建立欠驅(qū)動機構(gòu)的運動學、動力學模型，建模過程中不考慮各運動副之間的摩擦，并設機構(gòu)處于水平面上，欠驅(qū)動機構(gòu)的桿件數(shù)量為e，其中，原動件記為i，其余各桿件記為o，各桿件的桿長記為l_m，各桿件與x軸的夾角記為θ_m，其中m＝1、2、…、e，各構(gòu)件的重力勢能為零，得到機構(gòu)的運動學位置方程、運動學速度方程、運動學加速度方程和動力學方程，定義迭代次數(shù)n，定義迭代步長δt；

3)給定各構(gòu)件初始條件及原動件運動規(guī)律，求解t₀時刻機構(gòu)的運動學和動力學方程，得到此時各構(gòu)件的運動規(guī)律及原動件的驅(qū)動力(力矩)；所述的求解t₀時刻機構(gòu)的運動學和動力學方程按如下步驟進行：

(3a)機構(gòu)原動件的位置輸入已知，記為θ_i(t₀)；滑塊的初始位置已知，記為x(t₀)，利用運動學位置方程求出θ_o(t₀)；

(3b)初始時刻，原動件角速度滑塊速度以步驟(3a)為基礎，利用運動學速度方程求得

(3c)機構(gòu)原動件的角加速度輸入已知，記為在步驟(3a)和步驟(3b)的基礎上利用動力學方程求得滑塊的加速度，記為同時求得原動件轉(zhuǎn)矩，記為τ_i(t₀)；

(3d)在步驟(3a)-(3c)的基礎上將和帶入運動學加速度方程，求得

4)設從t_j-1時刻到t_j時刻的時間間隔無限小，記為δt，認為滑塊的速度和加速度在某一時間間隔無限小的區(qū)間內(nèi)不變，等于區(qū)間內(nèi)前一時刻的速度和加速度，而不同時刻的區(qū)間速度和加速度是階躍變化的，求解t_j(j＝1,2,3...)時刻機構(gòu)的運動學和動力學方程，得到此時各構(gòu)件的運動規(guī)律及原動件的驅(qū)動力(力矩)；所述求解t_j時刻機構(gòu)的運動學和動力學方程按如下步驟進行：

(4a)在t_j(j＝1,2,3...)時刻機構(gòu)原動件的位置輸入已知，記為θ_i(t_j)；滑塊的位置記為x(t_j)，x(t_j)由如下等式求得，然后利用運動學位置方程求得θo(t_j)；

(4b)機構(gòu)原動件的角速度輸入已知，記為滑塊的速度記為以t_j-1時刻所求結(jié)果為基礎進行迭代，然后利用運動學速度方程求出

(4c)機構(gòu)原動件的角加速度輸入已知，記為利用動力學方程求得滑塊的加速度及原動件轉(zhuǎn)矩，滑塊的加速度記為原動件轉(zhuǎn)矩記為τ_i(t_j)；

(4d)將和帶入運動學加速度方程求得

5)將上述步驟4)重復進行n次，即可得到該機構(gòu)所有構(gòu)件在整個運動周期內(nèi)的運動規(guī)律及原動件的驅(qū)動力(力矩)，利用MATLAB逐點繪圖，得到各構(gòu)件運動規(guī)律曲線圖及原動件驅(qū)動力(力矩)變化曲線圖。

本發(fā)明中的構(gòu)件是指桿件和滑塊。

本發(fā)明的進一步特征在于步驟2)中所述的建立欠驅(qū)動機構(gòu)的運動學、動力學模型，以一種含有柔性移動副的平面二自由度欠驅(qū)動機構(gòu)為研究對象，該欠驅(qū)動機構(gòu)(參見圖1)有五個構(gòu)件，固定端為A點，柔性連接端為D點，在D點處的滑塊連接線性彈簧的一端，線性彈簧的另一端固定在E點，第一個構(gòu)件與第二個構(gòu)件的連接點記為B點，第二個構(gòu)件與第三個構(gòu)件的連接點記為C點，其中，桿件1為機構(gòu)的原動件，桿件2和桿件3為機構(gòu)的其余桿件，三個桿件的桿長分別記為l₁,l₂,l₃,各桿件與x軸的夾角分別記為θ₁,θ₂,θ₃,A點到E點的距離為l₀，A點到D點的距離(即滑塊距原點的距離)為x，線性彈簧的自由長度為a，線性彈簧的剛度系數(shù)為k,各桿件質(zhì)量分別為m₁,m₂,m₃，滑塊的質(zhì)量為m₄，

具體建模步驟如下：

(2a)采用封閉矢量法建立機構(gòu)的運動學位置方程：

(2b)對步驟(2a)中的運動學位置方程求導，得到機構(gòu)的運動學速度方程：

(2c)對步驟(2b)中的運動學速度方程求導，得到機構(gòu)的運動學加速度方程：

(2d)采用拉格朗日方程法得到機構(gòu)的動力學方程：

其中，q₁,q₂為選定的廣義坐標，J₁₁,J₁₂,J₂₂是關于q₁,q₂的函數(shù)。

上述步驟3)中所述的“求解t₀時刻機構(gòu)的運動學和動力學方程”按如下步驟進行：

(3a)機構(gòu)原動件的位置輸入已知，記為θ₁(t₀)；滑塊的初始位置已知，記為x(t₀)。利用運動學位置方程求出θ₂(t₀),θ₃(t₀)；

(3b)初始時刻，原動件角速度滑塊速度以步驟(3a)為基礎，利用運動學速度方程求得參見下式；

(3c)機構(gòu)原動件的角加速度輸入已知，記為在步驟(3a)和步驟(3b)的基礎上利用動力學方程求得滑塊的加速度，記為參見下式

同時將求得的結(jié)果帶入動力學方程，求得原動件轉(zhuǎn)矩，記為τ₁(t₀)，參見下式

(3d)在步驟(3a)-(3c)的基礎上將和帶入運動學加速度方程求得

采用如下等式：

其中

步驟4)中的步驟(4a)～(4d)求解未知數(shù)的方法與步驟(3a)～(3d)類似。

本發(fā)明以一種含有柔性移動副的平面二自由度欠驅(qū)動機構(gòu)為研究對象進行說明，顯然，本發(fā)明同樣適用于具有該類機構(gòu)特征的其他含有柔性移動副的欠驅(qū)動機構(gòu)。

實施例1

本實施例以含有柔性移動副的平面二自由度欠驅(qū)動機構(gòu)為研究對象，如圖1所示，建立機構(gòu)的坐標系，取A點為坐標原點，以AD連線為x軸，指向D的方向為x軸的正方向，垂直x軸向上為y軸正方向，機構(gòu)各構(gòu)件參數(shù)見表1。

表1含柔性運動副欠驅(qū)動機構(gòu)的構(gòu)件參數(shù)

定義機構(gòu)參數(shù)，建立欠驅(qū)動機構(gòu)的運動學、動力學模型，選取迭代時間步長為0.05s，迭代次數(shù)為40次。具體建模步驟如下：

(2a)采用封閉矢量法建立機構(gòu)的運動學位置方程：

(2b)對步驟(2a)中的運動學位置方程求導，得到機構(gòu)的運動學速度方程：

(2c)對步驟(2b)中的運動學速度方程求導，得到機構(gòu)的運動學加速度方程：

(2d)采用拉格朗日方程法得到機構(gòu)的動力學方程：

其中，q₁,q₂為選定的廣義坐標，J₁₁,J₁₂,J₂₂是關于q₁,q₂的函數(shù)。

對于t₀時刻機構(gòu)的運動學和動力學方程的求解步驟如下：

(3a)給定原動件的運動規(guī)律是初始位置為π/12，初始速度為0rad/s，加速度為πrad/s²，的勻加速圓周運動，滑塊的初始位置為400mm，l₀為400mm；將初始位置輸入帶入運動學位置方程利用加減消元法求得θ₂(t₀),θ₃(t₀)；

(3b)初始時刻，原動件角速度滑塊速度將初始速度輸入及步驟

(3a)中位置已知量與位置所求量帶入方程利用加減消元法求得

(3c)機構(gòu)原動件的角加速度輸入已知，記為將初始加速度輸入及步驟(3a)和步驟(3b)中位置已知量與位置所求量，速度已知量與速度所求量帶入動力學方程

求得并將其結(jié)果帶入方程求得原動件轉(zhuǎn)矩，即驅(qū)動轉(zhuǎn)矩τ₁(t₀)；

(3d)將以上已知量及所得結(jié)果帶入運動學加速度方程

利用加減消元法求得

對于t_j時刻機構(gòu)的運動學和動力學方程的求解步驟如下：

(4a)在t_j(j＝1,2,3...)時刻機構(gòu)原動件的位置輸入已知，記為θ₁(t_j)；滑塊的位置記為x(t_j)，以t_j-1時刻所求結(jié)果為基礎進行迭代，x(t_j)由如下等式求得，然后計算θ₂(t_j),θ₃(t_j)，計算方法與求θ₂(t₀),θ₃(t₀)類似；

(4b)機構(gòu)原動件的角速度輸入已知，記為滑塊的速度記為以t_j-1時刻所求結(jié)果為基礎進行迭代，由如下等式求得，然后計算計算方法與求類似；

(4c)機構(gòu)原動件的角加速度輸入已知，記為將初始加速度輸入及步驟(4a)-(4b)中的位置和速度的已知量與所求量帶入方程

求得并將其帶入方程求得驅(qū)動轉(zhuǎn)矩τ₁(t_j)。

(4d)計算計算方法與求類似。

(5)重復執(zhí)行上述步驟(4a)～(4d)39次，即可得到整個運動周期內(nèi)的各構(gòu)件的運動規(guī)律及原動件的驅(qū)動力矩，利用matlab編寫M文件，得到滑塊運動規(guī)律變化曲線圖如圖2～4所示，原動件驅(qū)動力矩變化曲線圖如圖5所示。由圖可知，滑塊的位置、速度、加速度、驅(qū)動力矩曲線都較為平緩，顯然，本發(fā)明解決了含柔性運動副欠驅(qū)動機構(gòu)由于存在非完整約束 而求解困難的難題，且整個求解過程共用時132s，求解速度較快。

實施例2

本實施例以三自由度含有柔性移動副欠驅(qū)動機構(gòu)為研究對象，如圖6所示，建立機構(gòu)的坐標系，取A點為坐標原點，以AD連線為x軸，指向D的方向為x軸的正方向，垂直x軸向上為y軸正方向，AB桿件和BC桿件均為原動件。求解過程同實施例1。

本發(fā)明未述及之處適用于現(xiàn)有技術(shù)。