基于MATLAB的HP20D機(jī)器人運(yùn)動(dòng)學(xué)分析與仿真

馬國(guó)慶，劉 麗，于正林，曹?chē)?guó)華，范師杰

（長(zhǎng)春理工大學(xué) 機(jī)電學(xué)院，長(zhǎng)春 130022）

0 引言

機(jī)器人的運(yùn)動(dòng)學(xué)分析是機(jī)器人學(xué)的一個(gè)重要組成部分，為機(jī)器人的動(dòng)力學(xué)分析、軌跡規(guī)劃和運(yùn)動(dòng)控制提供重要依據(jù)[1]。本文使用經(jīng)典D-H法對(duì)HP20D型六自由度工業(yè)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析，并用MATLAB進(jìn)行運(yùn)動(dòng)學(xué)的仿真與方程計(jì)算，通過(guò)數(shù)據(jù)對(duì)比驗(yàn)證了其運(yùn)動(dòng)學(xué)模型與運(yùn)動(dòng)學(xué)正逆運(yùn)算的正確性。為機(jī)器人進(jìn)行軌跡規(guī)劃研究與仿真提供了必要的理論基礎(chǔ)和正確的運(yùn)動(dòng)學(xué)模型。

1 機(jī)器人結(jié)構(gòu)分析及數(shù)學(xué)模型建立

1.1 結(jié)構(gòu)分析

工業(yè)機(jī)器人主要由機(jī)身主體、驅(qū)動(dòng)系統(tǒng)和控制系統(tǒng)三部分組成[2]。機(jī)身主體即機(jī)座與執(zhí)行機(jī)構(gòu)，包括臂部、腕部和手部；驅(qū)動(dòng)系統(tǒng)涵蓋動(dòng)力裝置與傳動(dòng)機(jī)構(gòu)，負(fù)責(zé)為機(jī)身主體提供動(dòng)力輸出與傳輸；控制系統(tǒng)負(fù)責(zé)按照輸入的命令及程序?qū)︱?qū)動(dòng)系統(tǒng)與執(zhí)行機(jī)構(gòu)發(fā)出信號(hào)，并進(jìn)行控制[3]。工業(yè)機(jī)器人的有效工作范圍、重復(fù)定位精度、荷載以及允許扭矩是評(píng)判機(jī)器人工作能力的重要參數(shù)。HP20D型工業(yè)機(jī)器人為六自由度垂直多關(guān)節(jié)型機(jī)器人，末端最大負(fù)載為20kg，重復(fù)定位精度為±0.06mm，但由于機(jī)器人制造導(dǎo)致的模型誤差和日常工作產(chǎn)生的磨損，其定位精度在幾毫米左右，可以通過(guò)標(biāo)定技術(shù)來(lái)提高精度[4]。

1.2 機(jī)器人數(shù)學(xué)基礎(chǔ)

機(jī)器人可簡(jiǎn)化成由一系列桿件串聯(lián)而成，將坐標(biāo)系置于每一連桿的關(guān)節(jié)中心處，則可用齊次坐標(biāo)變換對(duì)這些坐標(biāo)系之間的位姿進(jìn)行數(shù)學(xué)表述[5]。

1.2.1 工業(yè)機(jī)器人的位姿描述

1）位置描述

在空間直角坐標(biāo)系{A}中的任意一點(diǎn)P的位置可用一個(gè)（3×1）的位置矢量AP來(lái)表示：

其中，px、py、pz為點(diǎn)P的三個(gè)位置坐標(biāo)分向量，AP稱為位置矢量。

2）姿態(tài)描述

可通過(guò)在物體質(zhì)心或者中心上固定一個(gè)坐標(biāo)系，并給出此坐標(biāo)系相對(duì)于參考坐標(biāo)系的數(shù)學(xué)表達(dá)來(lái)描述物體的姿態(tài)[6]。定義直角坐標(biāo)系{B}，將剛體B的質(zhì)心或者中心定義為坐標(biāo)系{B}的原點(diǎn)，則可以用一個(gè)（3×3）的旋轉(zhuǎn)矩陣ARB來(lái)表示剛體B相對(duì)于坐標(biāo)系{A}的姿態(tài)：

ARB是一個(gè)正交矩陣，稱為旋轉(zhuǎn)矩陣，上標(biāo)A表示參考坐標(biāo)系{A}，下標(biāo)B表示被描述坐標(biāo)系{B}。

3）位姿描述

點(diǎn)的位置信息可用位置矢量來(lái)表述，物體的姿態(tài)可用旋轉(zhuǎn)矩陣來(lái)表述，那么物體的位姿可由坐標(biāo)系{B}來(lái)描述：

其中APB0為坐標(biāo)系{B}的原點(diǎn)位置相對(duì)參考坐標(biāo)系{A}的位置矢量。

1.2.2 坐標(biāo)系映射

在機(jī)器人學(xué)中同一點(diǎn)的位置信息常常需要在不同的參考坐標(biāo)下進(jìn)行描述，因此需要運(yùn)用一種數(shù)學(xué)方法來(lái)表示坐標(biāo)系之間的變換，這就是映射[7]。

設(shè)坐標(biāo)系{B}與坐標(biāo)系{A}的坐標(biāo)原點(diǎn)不重合且姿態(tài)也不相同，用APB0表示坐標(biāo)系{B}的原點(diǎn)OB相對(duì)于坐標(biāo)系{A}的位置矢量，用旋轉(zhuǎn)矩陣ABR表示坐標(biāo)系{B}相對(duì)于坐標(biāo)系{A}的姿態(tài)，若點(diǎn)p在坐標(biāo)系{B}中可用BP描述，則它相對(duì)于另一個(gè)坐標(biāo)系{A}的描述AP可按式(4) 求出：

式(4)為坐標(biāo)系平移與旋轉(zhuǎn)的復(fù)合變換，引入一個(gè)過(guò)渡坐標(biāo)系{C}，令{C}的坐標(biāo)原點(diǎn)OC與坐標(biāo)系{B}的坐標(biāo)原點(diǎn)OB重合，而{C}的姿態(tài)與{A}相同，如圖1所示，則可得復(fù)合變換：

圖1 一般情況下的映射

1.2.3 齊次坐標(biāo)變換

不同的坐標(biāo)系對(duì)空間中任意點(diǎn)p的描述是不同的，利用齊次坐標(biāo)變換可完成點(diǎn)在不同矩陣之間位置關(guān)系的轉(zhuǎn)換，若已知某點(diǎn)在坐標(biāo)系{B}中坐標(biāo)為BP，則該點(diǎn)在坐標(biāo)系{A}中坐標(biāo)AP可表示為：

2 HP20D的運(yùn)動(dòng)學(xué)分析

在機(jī)器人運(yùn)動(dòng)學(xué)求解過(guò)程中，僅研究各桿件之間的運(yùn)動(dòng)關(guān)系并不考慮相互作用力的影響。正運(yùn)動(dòng)學(xué)問(wèn)題是根據(jù)桿件尺寸與各關(guān)節(jié)角，對(duì)機(jī)器人末端執(zhí)行器相對(duì)于基坐標(biāo)系下的位姿進(jìn)行求解，其本質(zhì)是運(yùn)動(dòng)學(xué)方程的建立與求解。而逆運(yùn)動(dòng)學(xué)問(wèn)題正好相反，是根據(jù)末端執(zhí)行器在基坐標(biāo)系下的期望位姿與已知的桿件幾何參數(shù)，來(lái)對(duì)機(jī)器人末端執(zhí)行器運(yùn)行到此位姿時(shí)的各個(gè)關(guān)節(jié)角度值進(jìn)行求解[8]。

2.1 連桿坐標(biāo)系建立

根據(jù)D-H坐標(biāo)變換法對(duì)機(jī)器人進(jìn)行建模，可清晰的展示各連桿間的位姿關(guān)系，建模原則如下：

1）繞Zi-1軸旋轉(zhuǎn)θi角，令Xi-1軸同Xi處于同一平面；

2）沿Zi-1軸平移di，令Xi-1軸同Xi處于同一直線；

3）沿Xi軸平移距離ai，將連桿i-1的坐標(biāo)系移動(dòng)，令其原點(diǎn)同連桿i的坐標(biāo)系原點(diǎn)重合；

4）繞Xi-1軸旋轉(zhuǎn)ai角，令Zi-1軸旋轉(zhuǎn)到同Zi軸處于一條直線上。

HP20D機(jī)器人D-H坐標(biāo)系如圖2所示。

圖2 Motoman-HP20D機(jī)器人D-H坐標(biāo)系

2.2 運(yùn)動(dòng)學(xué)正解

根據(jù)1.2.1中的建模原則即可確定HP20D型機(jī)器人的D-H參數(shù)，如表1所示。HP20D為6R型機(jī)器人，因此只有關(guān)節(jié)角θ是變量，連桿長(zhǎng)度α、關(guān)節(jié)扭轉(zhuǎn)α和連桿距離d均是固定值。

表1 HP20D機(jī)器人的連桿參數(shù)

式(7)中Rot 表示旋轉(zhuǎn)變換矩陣，Trans 表示平移變換矩陣。式(7)的矩陣表示形式如下：

根據(jù)相鄰連桿間的坐標(biāo)變換矩陣，可以得到末端坐標(biāo)系相對(duì)于機(jī)器人基坐標(biāo)系的變換矩陣如下：

將機(jī)器人的每個(gè)坐標(biāo)轉(zhuǎn)換矩陣代入式(9)即可求出HP20D機(jī)器人的正運(yùn)動(dòng)學(xué)方程。

2.3 運(yùn)動(dòng)學(xué)逆解

采用雙變量正切函數(shù)表示關(guān)節(jié)變量，避免出現(xiàn)解丟失的可能性，得到各個(gè)關(guān)節(jié)角如下：

求解θ1：

求解θ2：

求解θ3：

求解θ5：

求解θ4：

求解θ6：

結(jié)合逆運(yùn)動(dòng)學(xué)求解過(guò)程，再考慮到θ5等于0，或者不等于0兩種情況，同一種末端位姿可以求出8組逆解。但由于機(jī)械參數(shù)的約束，部分逆解處于機(jī)器人的不可達(dá)空間，因此應(yīng)根據(jù)機(jī)器人的運(yùn)動(dòng)情況與可達(dá)空間范圍，確定運(yùn)動(dòng)學(xué)逆解。

3 HP20D機(jī)器人運(yùn)動(dòng)學(xué)模型仿真

3.1 建立運(yùn)動(dòng)學(xué)模型

Robotics Toolbox是MATLAB軟件中的一個(gè)專門(mén)針對(duì)工業(yè)機(jī)器人開(kāi)發(fā)的工具箱，它可以為機(jī)器人提供多種運(yùn)動(dòng)學(xué)函數(shù)。根據(jù)表1提供的數(shù)據(jù)，對(duì)HP20D型機(jī)器人進(jìn)行仿真如圖3所示。

圖3 HP20D機(jī)器人仿真三維圖

3.2 正運(yùn)動(dòng)學(xué)驗(yàn)證

為了驗(yàn)證正運(yùn)動(dòng)學(xué)方程求解的正確性，隨機(jī)選取1組關(guān)節(jié)變量值，將從控制器中讀取機(jī)器人末端執(zhí)行器位姿作為真值；將按照2.2節(jié)中的正運(yùn)動(dòng)學(xué)方程計(jì)算所得的位姿作為計(jì)算值。隨機(jī)選取關(guān)節(jié)變量q1=[-0.3744， 2.7564，-1.2282，-0.6109，-1.5281，-0.3743]，將真值與計(jì)算值進(jìn)行數(shù)據(jù)對(duì)比。計(jì)算結(jié)果如表2所示。

表2 真值與MATLAB計(jì)算值比較

表2中數(shù)據(jù)表明真值和計(jì)算值的誤差很小，證明了機(jī)器人正運(yùn)動(dòng)學(xué)方程與運(yùn)動(dòng)學(xué)模型的正確性。

3.3 逆運(yùn)動(dòng)學(xué)驗(yàn)證

定義機(jī)器人末端位姿W=[PxPyPzaxayaz]，將其代入2.3節(jié)機(jī)器人逆運(yùn)動(dòng)學(xué)求解方法可求得機(jī)器人的多組運(yùn)動(dòng)學(xué)逆解Qi=[θ1θ2θ3θ4θ5θ6]。將兩個(gè)末端位姿W1=[-1189.369，531.995，-648.702，0.327， -0.744，-0.583]與W2=[1159.549，-730.090，-745.683， -0.362，0.787，-0.500]代入計(jì)算程序，求得逆解如表3和表4所示。

表3 位姿W1的運(yùn)動(dòng)學(xué)逆解

每個(gè)位姿可以獲取8組解，但由于機(jī)械結(jié)構(gòu)約束與各桿件運(yùn)動(dòng)時(shí)的干涉，僅有部分解為機(jī)器人可以真實(shí)達(dá)到的關(guān)節(jié)角度值，位姿W1的有效解為Q1、Q2，位姿W2的有效解為Q1、Q2、Q5、Q6，通過(guò)數(shù)據(jù)對(duì)比可知，表3中的Q1與3.2節(jié)的q1相等，驗(yàn)證了逆運(yùn)動(dòng)學(xué)的正確性。

表4 位姿W2的運(yùn)動(dòng)學(xué)逆解

4 結(jié)語(yǔ)

本文在機(jī)器人的數(shù)學(xué)基礎(chǔ)和運(yùn)動(dòng)學(xué)基本知識(shí)的基礎(chǔ)上，以HP20D為研究對(duì)象求取其正解和逆解，并在Robotics Toolbox中進(jìn)行了運(yùn)行學(xué)模型仿真，并通過(guò)數(shù)據(jù)對(duì)比驗(yàn)證了其正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)的正確性，為后續(xù)進(jìn)行軌跡規(guī)劃研究與仿真提供了必要的理論基礎(chǔ)和正確的運(yùn)動(dòng)學(xué)模型。