[發(fā)明專利]一種空間五自由度自由飛行機(jī)械臂的軌跡跟蹤控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910975424.2
• 申請(qǐng)日: 2019-10-15
• 公開（公告）號(hào): CN110641738B
• 公開（公告）日: 2022-08-09
• 發(fā)明（設(shè)計(jì)）人: 汪銳;吳楨;孫希明;呂宗陽(yáng)
• 申請(qǐng)（專利權(quán)）人: 大連理工大學(xué)
• 主分類號(hào): B64G1/24
• 分類號(hào): B64G1/24
• 代理公司: 大連理工大學(xué)專利中心 21200
• 代理人: 溫福雪;侯明遠(yuǎn)

1.一種空間五自由度自由飛行機(jī)械臂的軌跡跟蹤控制方法，

1)應(yīng)用D-H方法建立機(jī)械臂的正運(yùn)動(dòng)學(xué)模型，構(gòu)建機(jī)械臂末端位置與自由度之間的聯(lián)系；2)建立拉格朗日方程，導(dǎo)出系統(tǒng)質(zhì)量矩陣和非線性速度項(xiàng)矢量；3)結(jié)合雅可比轉(zhuǎn)置矩陣與PD控制器，計(jì)算系統(tǒng)廣義力矢量；4)構(gòu)造動(dòng)力學(xué)微分方程，實(shí)現(xiàn)軌跡跟蹤控制；其特征在于，步驟如下：

(1)應(yīng)用D-H方法建立機(jī)械臂的正運(yùn)動(dòng)學(xué)模型：

分別將基座坐標(biāo)系、關(guān)節(jié)1處坐標(biāo)系、關(guān)節(jié)2處坐標(biāo)系編號(hào)為坐標(biāo)系{0}、{1}、{2}，基座質(zhì)心在慣性坐標(biāo)系下的位置向量R_C0＝[x₀ y₀ 0]^T，由于基座位于慣性坐標(biāo)系的X-Y平面，故R_C0的Z軸分量為0；令廣義坐標(biāo)向量其中θ₀、θ₁、θ₂分別表示基座旋轉(zhuǎn)的歐拉角、關(guān)節(jié)1處的旋轉(zhuǎn)角和關(guān)節(jié)2處的旋轉(zhuǎn)角；

機(jī)械臂運(yùn)動(dòng)時(shí)坐標(biāo)系均繞z軸轉(zhuǎn)動(dòng)；則：

坐標(biāo)系{0}到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣

坐標(biāo)系{1}到坐標(biāo)系{0}的旋轉(zhuǎn)矩陣

坐標(biāo)系{2}到坐標(biāo)系{1}的旋轉(zhuǎn)矩陣

坐標(biāo)系{1}到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣

；

坐標(biāo)系{2}到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣

關(guān)節(jié)1與慣性坐標(biāo)系原點(diǎn)之間的距離為r₀，關(guān)節(jié)1與連桿1質(zhì)心之間的距離為l₁，關(guān)節(jié)2與連桿1之間的距離為r₁，關(guān)節(jié)2與連桿2之間的距離為l₂，末端與連桿2之間的距離為r₂；

由此，分別計(jì)算出關(guān)節(jié)1與慣性坐標(biāo)系原點(diǎn)之間的距離為r₀，關(guān)節(jié)2與關(guān)節(jié)1之間的距離為r₁+l₁，末端與關(guān)節(jié)2之間的距離為r₂+l₂；

于是得到機(jī)械臂末端位置坐標(biāo)在慣性坐標(biāo)系中表示為

將每一項(xiàng)分別對(duì)x₀、y₀、θ₀、θ₁和θ₂進(jìn)行求偏導(dǎo)運(yùn)算，得到J_x矩陣；

(2)建立拉格朗日方程，導(dǎo)出系統(tǒng)質(zhì)量矩陣和非線性速度項(xiàng)矢量：

由于SFFR的典型機(jī)動(dòng)長(zhǎng)度和持續(xù)時(shí)間相對(duì)較短，因此與控制力相比，由軌道力學(xué)引起的微重力和動(dòng)態(tài)效應(yīng)忽略不計(jì)；因此，在慣性坐標(biāo)系中考慮系統(tǒng)運(yùn)動(dòng)，并且將系統(tǒng)勢(shì)能取為零；這種系統(tǒng)的拉格朗日方程為

其中，N是機(jī)械臂的自由度數(shù)，T是系統(tǒng)動(dòng)能，q_i、和Q_i分別是第i個(gè)元素的廣義坐標(biāo)向量、廣義速度和廣義力；系統(tǒng)動(dòng)能T的計(jì)算公式如下：

其中，M是系統(tǒng)總質(zhì)量，是機(jī)械臂上任意點(diǎn)P的速度；

系統(tǒng)動(dòng)能由以下三部分組成：

其中，ω表示旋轉(zhuǎn)角速度矢量，r表示系統(tǒng)中兩點(diǎn)之間的位置向量，I表示機(jī)械臂各部分的轉(zhuǎn)動(dòng)慣量矢量；對(duì)于空間五自由度自由飛行機(jī)械臂系統(tǒng)，得到：基座繞z軸旋轉(zhuǎn)的角速度在基座坐標(biāo)系{0}中的矢量表示為

連桿1的角速度在坐標(biāo)系{1}中的矢量表示為

連桿2的角速度在坐標(biāo)系{2}中的矢量表示為

關(guān)節(jié)1相對(duì)于慣性坐標(biāo)系原點(diǎn)的位置向量為

r₀＝[r₀cosθ₀ r₀sinθ₀ 0]^T (9)

第一個(gè)關(guān)節(jié)相對(duì)于連桿1質(zhì)心的位置向量為

l₁＝[-l₁cos(θ₀+θ₁) -l₁sin(θ₀+θ₁) 0]^T (10)

第二個(gè)關(guān)節(jié)相對(duì)于連桿1質(zhì)心的位置向量為

r₁＝[r₁cos(θ₀+θ₁) r₁sin(θ₀+θ₁) 0]^T (11)

第二個(gè)關(guān)節(jié)相對(duì)于連桿2質(zhì)心的位置向量為

l₂＝[-l₂cos(θ₀+θ₁+θ₂) -l₂sin(θ₀+θ₁+θ₂) 0]^T (12)

末端相對(duì)于連桿2質(zhì)心的位置向量為

r₂＝[r₂cos(θ₀+θ₁+θ₂) r₂sin(θ₀+θ₁+θ₂) 0]^T (13)

將式(3)-(5)分別代入拉格朗日方程，得到

其中，N為機(jī)械臂的自由度數(shù)，m_k為第k個(gè)連桿的質(zhì)量，與q分別表示基座質(zhì)心在慣性坐標(biāo)系下的位置向量和廣義坐標(biāo)矢量；

提取式(14)-(16)中的加速度項(xiàng)系數(shù)，得到系統(tǒng)質(zhì)量矩陣

提取式(14)-(16)中的速度項(xiàng)系數(shù)，得到系統(tǒng)非線性速度矢量

其中

和

其中，m＝1...n，n表示機(jī)械臂個(gè)數(shù)，本發(fā)明只考慮一臺(tái)機(jī)械臂，故n為1；k＝1...N_m，N_m表示連桿個(gè)數(shù)，空間五自由度自由飛行機(jī)械臂有兩條連桿，故N_m為2；I₀為基座的轉(zhuǎn)動(dòng)慣量，表示第m臺(tái)機(jī)械臂中第k條連桿的轉(zhuǎn)動(dòng)慣量；表示在慣性坐標(biāo)系下的第k個(gè)連桿的質(zhì)心位置矢量，其計(jì)算公式為

其中，m為機(jī)械臂個(gè)數(shù)，N_m為第m個(gè)機(jī)械臂的自由度數(shù)；

將式(9)-(13)代入式(21)，得到在慣性系中連桿1和連桿2的質(zhì)心位置矢量分別為

在計(jì)算系統(tǒng)質(zhì)量矩陣和系統(tǒng)非線性速度項(xiàng)矢量之前，對(duì)式(17)、(19)和(20)進(jìn)行簡(jiǎn)化；

對(duì)式(17)進(jìn)行簡(jiǎn)化結(jié)果為

在式(19)中，有

則式(19)的簡(jiǎn)化結(jié)果為

式(20)中的偏微分項(xiàng)結(jié)果均為0，則C2＝0；

將式(6)-(8)、式(22)、式(23)分別代入式(24)與式(25)，計(jì)算出系統(tǒng)質(zhì)量矩陣H和系統(tǒng)非線性速度項(xiàng)矢量C中的每一項(xiàng)，結(jié)果如下所示：

其中，

H₁₁＝m₀+m₁+m₂；

H₁₂＝H₂₁＝0；

H₁₃＝H₃₁＝-m₁(r₀sin(θ₀)+l₁sin(θ₀+θ₁))-m₂(r₀sin(θ₀)+(r₁+l₁)sin(θ₀+θ₁)+l₂sin(θ₀+θ₁+θ₂))；

H₁₄＝H₄₁＝-m₁l₁sin(θ₀+θ₁)-m₂((r₁+l₁)sin(θ₀+θ₁)+l₂sin(θ₀+θ₁+θ₂))；

H₁₅＝H₅₁＝-m₂l₂sin(θ₀+θ₁+θ₂)；

H₂₂＝m₀+m₁+m₂；

H₂₃＝H₃₂＝m₁(r₀cos(θ₀)+l₁cos(θ₀+θ₁))+m₂(r₀cos(θ₀)+(r₁+l₁)cos(θ₀+θ₁)+l₂cos(θ₀+θ₁+θ₂))；

H₂₄＝H₄₂＝m₁l₁cos(θ₀+θ₁)+m₂((r₁+l₁)cos(θ₀+θ₁)+l₂cos(θ₀+θ₁+θ₂))；

H₂₅＝H₅₂＝m₂l₂cos(θ₀+θ₁+θ₂)；

其中，

C₁₁＝C₁₂＝0；

C₂₁＝C₂₂＝0；

C₃₁＝C₃₂＝0；

C₄₁＝C₄₂＝0；

C₅₁＝C₅₂＝C₅₅＝0；

(3)結(jié)合雅可比轉(zhuǎn)置矩陣與PD控制器，計(jì)算廣義力矢量；

雅可比轉(zhuǎn)置矩陣為

PD控制器的設(shè)計(jì)如下所示：

廣義力矢量表示為雅可比轉(zhuǎn)置矩陣與PD控制器輸出的乘積Q＝J^Tu；

(4)構(gòu)造動(dòng)力學(xué)微分方程

其中C2＝0，則

由式(1)和式(26)，即搭建出仿真模型；

最后，根據(jù)空間五自由度自由飛行機(jī)械臂的動(dòng)力學(xué)微分方程，應(yīng)用Simulink軟件對(duì)模型進(jìn)行仿真。