六自由度工業(yè)機(jī)器人標(biāo)定

本科生畢業(yè)設(shè)計(jì) 畢 業(yè) 設(shè) 計(jì) 題 目 工業(yè)機(jī)器人標(biāo)定 學(xué) 生 姓 名 專 業(yè) 機(jī) 械 設(shè) 計(jì) 制 造 及 其 自 動 化 班 級 指 導(dǎo) 教 師 完 成 日 期 2 0 1 4 年 05 月 30 日 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 摘 要 運(yùn)動學(xué)標(biāo)定是提高機(jī)器人精度的關(guān)鍵技術(shù)，也是機(jī)器人學(xué)的重要內(nèi)容，在機(jī)器人空前發(fā)展的今天有十分重要的理論和現(xiàn)實(shí)意義。機(jī)器人 運(yùn)動學(xué)標(biāo)定以運(yùn)動學(xué)建模為基礎(chǔ)，幾何誤差參數(shù)辨識為目的，為機(jī)器人的誤差補(bǔ)償提供依據(jù)。 現(xiàn)今機(jī)器人廠家生產(chǎn)的機(jī)器人其重復(fù)定位精度比較高，而絕對定位精度卻很低。伴隨著機(jī)器人越來越廣泛的運(yùn)用，提高機(jī)器人絕對定位精度已成為其中一關(guān)鍵技術(shù)問題。本文采用一種運(yùn)動學(xué)標(biāo)定方法，應(yīng)用先進(jìn)的激光跟蹤測量系統(tǒng)和基于模型的參數(shù)辨識方法識別出一種 6R 機(jī)器人模型的準(zhǔn)確參數(shù)，提高了該機(jī)器人的絕對定位精度。 針對工業(yè)機(jī)器人標(biāo)定問題，首先結(jié)合機(jī)器人的實(shí)際機(jī)構(gòu)特點(diǎn)，運(yùn)用 D-H 方法建立了機(jī)器人的連桿坐標(biāo)系，在此基礎(chǔ)上進(jìn)行了機(jī)器人 運(yùn)動學(xué)正逆解和雅可比矩陣的詳細(xì)推導(dǎo)及求解，并運(yùn)用 Matlab 語言進(jìn)行運(yùn)動學(xué)模型的編程求解，通過與機(jī)器人控制器中位姿數(shù)據(jù)對比，驗(yàn)證了所建立的連桿坐標(biāo)系統(tǒng)的正確性。 針對工業(yè)機(jī)器人的機(jī)構(gòu)特點(diǎn)，分析了影響機(jī)器人末端絕對定位精度的誤差來源，采用修正的運(yùn)動學(xué)連桿參數(shù)模型，基于微分變換法推導(dǎo)了用于機(jī)器人標(biāo)定的誤差模型，并基于 Matlab 軟件系統(tǒng)編制了機(jī)器人運(yùn)動學(xué)誤差模型的最小二乘算法，通過對誤差模型進(jìn)行模擬求解，驗(yàn)證了機(jī)器人標(biāo)定誤差模型的可行性。 關(guān)鍵詞：工業(yè)機(jī)器人 ; 運(yùn)動學(xué) ; 定位精度 ; 標(biāo)定 ; 誤差模型 ;連桿參數(shù)。 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 II Abstract Kinematic calibration is the key technology to improve the accuracy of robot, is also the important content of robotics, an unprecedented development in robot today have very important theoretical and practical significance. The robot kinematics calibration modeling based on kinematics, geometric error parameter identification for the purpose, to provide basis for error compensation of robot. The robot manufacturers robot its repetitive positioning precision is higher, but the absolute positioning accuracy is very low. With the use of robots are more and more widely, improving the robot absolute positioning accuracy has become a key technology problem which. This paper uses a kinematic calibration method, the application of advanced laser tracking measurement system based on parameter identification method and identification model of accurate parameters of a 6R robot model, improves the accuracy of the robot absolute positioning. Aiming at the industrial robot calibration, the actual mechanism firstly with the robot, the robot is established by D-H method of pole coordinates, based on the detailed derivation and solution of robot kinematics and Jacobi matrix, programming and kinematics model using Matlab language, with the attitude data comparison of robot controller, verified the correctness of the established link coordinate system. According to the mechanism of industrial robot, analyzes the impact of absolute location error precision of the robot, the kinematics model, based on differential transform method is derived for the error model calibration of robots, and based on the Matlab software system of least square algorithm for robot kinematics error model, through the simulation to solve the error model, validation the feasibility of robot calibration error model. Keywords: industrial robot; kinematics; positioning accuracy; calibration; error model; link parameters. 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 III 目錄 摘 要 . 2 Abstract . II 第一章 緒論 . 1 1.1 引言 . 1 1.2 工業(yè)機(jī)器人運(yùn)動學(xué)標(biāo)定技術(shù)的背景和意義 . 1 1.3 機(jī)器人標(biāo)定技術(shù)的研究現(xiàn)狀 . 3 第二章 機(jī)器人運(yùn)動學(xué) . 5 2.1 機(jī)器 人運(yùn)動學(xué)模型的建立 . 5 2.2 正向運(yùn)動學(xué)求解 . 9 2.3 逆向運(yùn)動學(xué)求解 . 10 2.4 微分運(yùn)動學(xué)模型 . 13 2.5 本章小 結(jié) . 17 第三章 SR06 型機(jī)器人的標(biāo)定技術(shù) . 18 3.1 標(biāo)定用運(yùn)動學(xué)模型的建立 . 18 3.1.1 直線的無極點(diǎn)表示法 . 19 3.1.2 CPC 模型的建立 . 20 3.2 機(jī)器人的標(biāo)定方法 . 24 3.2.1 幾何誤差的來源 . 24 3.2.2 連桿參數(shù)的線性求解方法 . 25 3.3 本章小結(jié) . 30 第四章 標(biāo)定實(shí)驗(yàn)及結(jié)論 . 32 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 IV 4.1 原始數(shù)據(jù)采集 . 32 4.2 數(shù)據(jù)處理 . 33 4.2.1 齊次坐標(biāo)變換矩陣與繞任意軸的旋轉(zhuǎn)矩陣之間的關(guān)系 33 4.2.2 方程 RA Rx= RxRb的求解 . 36 4.3 標(biāo)定結(jié)果 . 錯(cuò)誤 !未定義書簽。 4.4 本章小結(jié) . 38 第五章 總結(jié)與展望 . 39 致謝 . 41 參考文獻(xiàn) . 42 附錄 . 46 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 1 第一章 緒論 1.1 引言 工業(yè)機(jī)器人是一種具有自動控制的操作和移動功能，能夠完成各種作業(yè)的可編程操作機(jī)器，工業(yè)機(jī)器人一般是由機(jī)械執(zhí)行機(jī)構(gòu)、伺服驅(qū)動裝置、檢測傳感裝置和控制系統(tǒng)等部分組成。自從 1959 年美國發(fā)明家喬治德沃爾和約瑟夫英格伯格制造出世界上第一臺工業(yè)機(jī)器人以來，工業(yè)機(jī)器人作為機(jī)器人的一個(gè)主要分支，其五十多年來的發(fā)展歷程使得工業(yè)機(jī)器人技術(shù)及產(chǎn)品廣泛應(yīng)用于各類工業(yè)生產(chǎn)中。工業(yè)機(jī)器人技術(shù)及其產(chǎn)品和相關(guān)配套技術(shù)發(fā)展迅速，已經(jīng)成為自動化工廠（ Factory Automation, FA）和柔性制造系統(tǒng)（ Flexible Manufacturing System, FMS）實(shí)現(xiàn)自動化的重要工具。 工業(yè)機(jī)器人延伸了人的手足和大腦功能，可以代替人從事危險(xiǎn)、有害、有毒、低溫和高熱等惡劣環(huán)境中的工作，代替人完成繁重、簡單重復(fù)的枯燥勞動，提高勞動生產(chǎn)率，保證產(chǎn)品的生產(chǎn)質(zhì)量。與計(jì)算機(jī)技術(shù)、網(wǎng)絡(luò)技術(shù)對世界的生活生產(chǎn)方式產(chǎn)生巨大改變類似，工業(yè)機(jī)器人在自動化生產(chǎn)線上的廣泛應(yīng)用正在逐漸對全世界的生產(chǎn)方式產(chǎn)生變革。 1.2 工業(yè)機(jī)器人運(yùn)動學(xué)標(biāo)定技術(shù)的背景和意義 伴隨著工業(yè)自動 化的需要和發(fā)展，工業(yè)機(jī)器人技術(shù)及產(chǎn)業(yè)規(guī)模都飛速發(fā)展起來。在工業(yè)生產(chǎn)中，弧焊機(jī)器人、點(diǎn)焊機(jī)器人、分配機(jī)器人、裝配機(jī)器人、噴漆機(jī)器人及搬運(yùn)機(jī)器人等工業(yè)機(jī)器人都已被大量采用。預(yù)計(jì)至 2015年，中國工業(yè)機(jī)器人年銷售將達(dá)到 3.5 萬臺，工業(yè)機(jī)器人被廣泛應(yīng)用于汽車及汽車零部件制造業(yè)、機(jī)械加工行業(yè)、電子電氣行業(yè)、橡膠及塑料工業(yè)、食品工業(yè)、木材與家具制造業(yè)等領(lǐng)域中，尤其是汽車行業(yè)、機(jī)械制造業(yè)。 技術(shù)融合是機(jī)器人發(fā)展的主要趨勢。從近幾年世界機(jī)器人推出的產(chǎn)品來看，工業(yè)機(jī)器人技術(shù)正在向智能機(jī)器和智能系統(tǒng)的方向發(fā)展，其發(fā)展趨勢主要為 結(jié)構(gòu)的模塊化和可重構(gòu)化，控制技術(shù)的開放化、 PC 化和網(wǎng)絡(luò)化，伺服驅(qū)動技術(shù)的數(shù)字化和分散化，多傳感器融合技術(shù)的實(shí)用化，工作環(huán)境設(shè)計(jì)的優(yōu)化和作業(yè)的柔性化，以及系統(tǒng)的網(wǎng)絡(luò)化和智能化等方面。機(jī)器人系統(tǒng)由硬件系統(tǒng)和軟件系統(tǒng)組成，在硬件系統(tǒng)條件一定的情況下，機(jī)器人實(shí)用功能的靈活性 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 2 和智能程度在很大程度上取決于機(jī)器人的編程能力。機(jī)器人編程有在線編程(On-Line Programming)和離線編程 (Off-Line Programming) 兩種形式。在機(jī)器人所要完成的作業(yè)不很復(fù)雜，以及示教時(shí)間相對工作時(shí)間比較短的情況下， 在線示教編程是切實(shí)可行的。隨著企業(yè)對柔性加工要求的提高和計(jì)算機(jī)的發(fā)展，出現(xiàn)了機(jī)器人離線編程技術(shù)。機(jī)器人離線編程系統(tǒng)是機(jī)器人編程語言的推廣，它利用計(jì)算機(jī)圖形學(xué)的成果，建立機(jī)器人及其工作環(huán)境的模型，再利用一些規(guī)劃算法，通過對圖形的控制和操作，在不使用實(shí)際機(jī)器人的情況下進(jìn)行軌跡規(guī)劃，進(jìn)而產(chǎn)生機(jī)器人程序。離線編程系統(tǒng)中的仿真模型 (理想模型 )和實(shí)際機(jī)器人模型存在有誤差，產(chǎn)生誤差的因素主要有機(jī)器人本體、工作環(huán)境以及離線編程系統(tǒng)等因素。因此如何有效地消除誤差，提高機(jī)器人的精度成為離線編程系統(tǒng)實(shí)用化的關(guān)鍵技術(shù)之一。一般地， 影響機(jī)器人精度的因素可以歸結(jié)為以下 4 類： (1) 參數(shù)因素：機(jī)器人運(yùn)動學(xué)模型參數(shù)，包括桿件長度、相鄰軸線的傾斜、關(guān)節(jié)的零位偏差等，機(jī)器人基礎(chǔ)坐標(biāo)系相對與參考坐標(biāo)系的坐標(biāo)轉(zhuǎn)換關(guān)系誤差等。 (2) 負(fù)載因素：機(jī)器人桿件彈性變形，齒輪傳動誤差，關(guān)節(jié)間隙等。 (3) 數(shù)字誤差：包括機(jī)器人關(guān)節(jié)碼盤讀數(shù)精度，機(jī)器人控制器的控制分辨能力，編程分辨能力，計(jì)算機(jī)舍入誤差等。 (4) 環(huán)境因索：溫度影響，濕度影響，電噪聲以及隨機(jī)誤差等。 對于不同結(jié)構(gòu)、不同用途的機(jī)器人，以上各因素的影響不盡相同，一般來講，在參 數(shù)因素中幾何結(jié)構(gòu)參數(shù)偏差引起的誤差占機(jī)器人總誤差的 80%左右。 結(jié)合機(jī)器人設(shè)計(jì)、加工、裝配、編程和控制，一般認(rèn)為提高機(jī)器人精度主要有兩種方法： (1)誤差預(yù)防法。即減少誤差源，這種方法是通過改進(jìn)設(shè)計(jì)方法和制造工藝，最大限度地提高機(jī)器人零部件的設(shè)計(jì)、加工精度，以降低加工誤差；盡可能的提高裝配精度，以降低裝配過程造成的誤差；另外還有采用高性能控制器提高機(jī)器人的控制精度等。但是這種方法本身具有較大的局限性。該方法的實(shí)現(xiàn)需要高精度的加工技術(shù)與條件，價(jià)格昂貴，而且這種方法對于機(jī)器人機(jī)械磨損、元件性能降低以及構(gòu)件自 身動態(tài)特性等因素帶來的誤差無能為力。 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 3 (2)誤差補(bǔ)償法，即運(yùn)動學(xué)標(biāo)定，所謂標(biāo)定就是應(yīng)用先進(jìn)的測量手段和基于模型的參數(shù)識別方法辨識出機(jī)器人模型的準(zhǔn)確參數(shù)，通過采用附加控制算法或修改原控制算法來補(bǔ)償機(jī)器人誤差，從而提高機(jī)器人絕對精度的過程。該方法采用軟件方法提高機(jī)器人精度，由于誤差源與機(jī)器人誤差之間的函數(shù)規(guī)律復(fù)雜，該方法只能補(bǔ)償某些誤差源 (主要是桿件幾何偏差和桿件靜態(tài)變形 )引起的誤差。但是標(biāo)定仍然是機(jī)器人誤差補(bǔ)償?shù)牧己猛緩?，并且具有成本低的?yōu)點(diǎn)。 機(jī)器人標(biāo)定在自動化制造、計(jì)算機(jī)集成制造裝配系統(tǒng)中起 到非常重要的作用。機(jī)器人標(biāo)定不僅可以提高精度，而且可以大幅降低制造成本。下面為機(jī)器人運(yùn)動學(xué)標(biāo)定的典型應(yīng)用： （ 1）離線編程和機(jī)器人任務(wù)仿真的基礎(chǔ)。機(jī)器人標(biāo)定在計(jì)算機(jī)集成制造 (CIM)系統(tǒng)中的必要性已經(jīng)被廣泛接受。通過標(biāo)定，機(jī)器人作業(yè)可以不通過示教來編程，而通過仿真進(jìn)行離線編程，這樣可以大幅降低成本。為了使離線生成的程序能夠在機(jī)器人控制系統(tǒng)中有效執(zhí)行，真實(shí)模型和設(shè)計(jì)模型要良好匹配，否則，位姿誤差將會增大，使正常作業(yè)無法進(jìn)行。通常運(yùn)動學(xué)標(biāo)定可以將絕對精度提高 10 倍。 （ 2）機(jī)器人制造的評價(jià)手段。許多物理參數(shù) 在機(jī)器人制造及裝配后無法直接測量，而標(biāo)定則是一種行之有效的間接測量方法。準(zhǔn)確測量一定數(shù)目的機(jī)器人位姿，就可以較為準(zhǔn)確地識別機(jī)器人的物理參數(shù)。 （ 3）機(jī)器人輔助手術(shù)中的應(yīng)用。 Goswami、 Kinzle 以及 Tull 等把機(jī)器人系統(tǒng)應(yīng)用到手術(shù)中，以提高手術(shù)質(zhì)量。手術(shù)通常對絕對精度要求嚴(yán)格，故研究有效的補(bǔ)償方法以保證絕對精度非常必要。 1.3 機(jī)器人標(biāo)定技術(shù)的研究現(xiàn)狀 在機(jī)器人應(yīng)用范圍擴(kuò)大、所完成任務(wù)的復(fù)雜程度提高的情況下，特別是在 CAD/CAM/機(jī)器人 /數(shù)控設(shè)備的一體化環(huán)境中，示教再現(xiàn)型機(jī)器人難以提供環(huán)境所要求 的靈活性。機(jī)器人離線編程系統(tǒng)的迅速發(fā)展，成為解決實(shí)際生產(chǎn)問題的智能化手段。對于采用離線編程方式工作的機(jī)器人而言，機(jī)器人的絕對精度成為關(guān)鍵指標(biāo)。而一般來說絕對精度只有厘米數(shù)量級，精度較低。于是需要進(jìn)行機(jī)器人運(yùn)動學(xué)標(biāo)定的研究來提高機(jī)器人的絕對精度。機(jī)器人標(biāo)定 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 4 的結(jié)果是一系列被識別的機(jī)器人參數(shù)，這些參數(shù)可以提供給制造廠家作為產(chǎn)品質(zhì)量檢驗(yàn)指標(biāo)，也可以提供給用戶來提高機(jī)器人的絕對精度，是離線編程和機(jī)器人任務(wù)仿真的基礎(chǔ)。 在很多應(yīng)用場合，機(jī)器人負(fù)載較小，此時(shí)造成位姿誤差的主要原因在于機(jī)器人控制器中的機(jī) 器人理論運(yùn)動學(xué)模型與機(jī)器人的實(shí)際物理模型之間存在的微小偏差，這個(gè)偏差主要是在機(jī)器人的制造及安裝過程中產(chǎn)生的。 機(jī)器人運(yùn)動學(xué)標(biāo)定是離線編程技術(shù)實(shí)用化的關(guān)鍵技術(shù)之一，它是一個(gè)集運(yùn)動學(xué)建模以及運(yùn)動學(xué)參數(shù)辨識于一體的過程。機(jī)器人運(yùn)動學(xué)標(biāo)定根據(jù)標(biāo)定方法的不同可以分成以下兩類：基于運(yùn)動學(xué)模型的參數(shù)標(biāo)定以及基于神經(jīng)網(wǎng)絡(luò)的參數(shù)標(biāo)定。本文主要討論基于運(yùn)動學(xué)模型的參數(shù)標(biāo)定方法，它一般包括四個(gè)步驟： （ 1） 建立準(zhǔn)確的運(yùn)動學(xué)模型； （ 2） 用已知精度的測量裝置測量出機(jī)器人末端操作器的位姿； （ 3） 引入算法辨識幾何參數(shù)； （ 4） 對原有機(jī)器人運(yùn)動學(xué)模型進(jìn)行修 正。其中，選擇合適的運(yùn)動學(xué)模型和測量方法是標(biāo)定的基礎(chǔ)，辨識出幾何參數(shù)的真實(shí)值并校正是標(biāo)定的目的。 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 5 第二章 機(jī)器人運(yùn)動學(xué) 2.1 機(jī)器人運(yùn)動學(xué)模型的建立 SR06 型機(jī)器人是一臺串聯(lián) 6 自由度的旋轉(zhuǎn)機(jī)器人，其控制柜中內(nèi)建的運(yùn)動模型為 D-H 模型，該模型是由 Denavit 和 Hartenberg 兩人在 1956 年提出的一種方法，以列表的形式完成變換矩陣 A 的填寫，使用起來十分簡便，因此獲得了廣泛應(yīng)用。 在建立機(jī) 器人運(yùn)動學(xué)模型之前先要對各個(gè)關(guān)節(jié)和連桿進(jìn)行編號：基座為桿 0，從基座起依次向上為桿 1、桿 2；關(guān)節(jié) i 連接桿 i-1 和 i，即桿 i 離基座近的一端（簡稱近端）有關(guān)節(jié) i，而離基座遠(yuǎn)的一端（簡稱遠(yuǎn)端）有關(guān)節(jié) i+1。 D-H 方法嚴(yán)格定義了桿件坐標(biāo)系，遵循的基本原則如下： 第一步：確定各坐標(biāo)系的 z 軸。 基本原則是：選取 Zi軸沿關(guān)節(jié) i+1 的軸向（指向可以任選，但通常都將各平行的 z 軸均取為相同的指向）。這里需要說明的是： 1. 當(dāng)關(guān)節(jié) i+1 是移動關(guān)節(jié) 時(shí)，其軸線指向已知，但位置不確定，這時(shí)選取 Zi軸與 Zi+1軸相交（若還有移動關(guān)節(jié) i+2，則取 Zi軸和 Zi+1軸都與 Zi+2軸相交）。 2. 機(jī)器人桿 n 遠(yuǎn)端沒有關(guān)節(jié) n+1，這時(shí)可選取 Zn軸與 Zn-1軸重合。 第二步：確定各坐標(biāo)系的原點(diǎn)。 基本原則是：選取原點(diǎn) Oi在過 Zi-1軸與 Zi軸的公法線上（即 Oi為此公法線與 Zi軸的交點(diǎn)）。這里需要說明的是： 1. 當(dāng) Zi-1 軸與 Zi 軸平行時(shí)，經(jīng)過兩軸的公法線不唯一。確定方法是：若 Zi-1軸與 Zi軸重合，取 Oi=Oi-1，若 Zi-1軸與 Zi軸平行且不重合，過 Oi-1點(diǎn)作 Zi-1軸和 Zi軸的公法線，取此公法線與 Zi軸的交點(diǎn)為 Oi。 2. 由于沒有 Zi-1軸，故無法按上述基本原則選取 O0，這時(shí)確定 O0的方法是：若 Z0與 Z1相交時(shí)，取 O0= O1，若 Z0與 Z1不相交時(shí)， O0在 Z0與 Z1的公法線上。 第三步：確定坐標(biāo)系的 X 軸。 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 6 基本原則是選取 Xi軸沿過 Zi-1軸和 Zi軸的公法線，方向從 Zi-1軸指向 Zi軸。 這里要說明的是： 1.當(dāng) Zi-1軸與 Zi軸重合時(shí)，（這時(shí) Oi=Oi-1）選取 Xi軸滿足在初始條件位置時(shí)， Xi軸與 Xi-1軸重合。 2.當(dāng) Zi-1軸與 Zi軸相交且不重合時(shí)，選擇 Xi =（ Zi-1 Zi，通常使所有平行的 X 軸均有相同的指向。 3.當(dāng) i=0 時(shí)，由上所述知，這時(shí) O0=0，或 O0在 Z0軸與 Z1軸的公法線上，選取在初始位置時(shí) X0軸與 X1軸重合。 第四步：確定各坐標(biāo)系的 y 軸 基本原則是：使 yi= zi xi，即構(gòu)成右手坐標(biāo)系。 這 樣 ， 便 能 建 立 起 如 圖 2-1 所 示 的 坐 標(biāo) 系 分 布 。 圖 2-1 D-H 模型坐標(biāo)系分布示意圖 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 7 一旦對全部連桿規(guī)定坐標(biāo)系之后，就能夠確定用于指示系 i-1 和系 i 間相對位置和方位的 4 個(gè)參數(shù)。 桿件長度 ai：定義為從 zi-1軸到 zi軸的距離。沿 xi軸指向?yàn)檎?桿件扭角 i ：定義為從 zi-1軸到 zi軸的轉(zhuǎn)角。繞 xi軸正向轉(zhuǎn)動為正，且規(guī)定 i , )。 關(guān)節(jié)距離 di: 定義為從 xi-1軸到 xi軸的距離。沿 zi-1軸指向?yàn)檎?關(guān)節(jié)轉(zhuǎn)角 i ：定義為從 xi-1 軸到 xi 軸的轉(zhuǎn)角。繞 zi-1 軸正向轉(zhuǎn)向?yàn)檎乙?guī)定 i , )。 參數(shù) ai， i ， di和 i 的意義如圖 2-2。這些參數(shù)被稱為 D-H 參數(shù)，又常被稱為機(jī)器人運(yùn)動參數(shù)或幾何參數(shù)。這里要說明的是： 1. 桿 i 的兩端分別有 zi-1 軸和 zi 軸， ai 和 i 分別描述了從 zi-1 軸到 zi軸的距離和轉(zhuǎn)角，關(guān)節(jié)轉(zhuǎn)角 i 的軸向 zi-1是 xi-1 軸和 xi 軸的公法線；di和 i 分別描述了從 xi-1軸到 xi 軸的距離和轉(zhuǎn)角。 2. ai和 i 由桿的結(jié)構(gòu)確定，是常數(shù)。而 di和 i 與關(guān)節(jié) i 的類型有關(guān)，其中一個(gè)是常數(shù)，另一個(gè)是變量。當(dāng)關(guān)節(jié) i 是轉(zhuǎn)動關(guān)節(jié)時(shí)， di是常數(shù)， i 是變量；當(dāng)關(guān)節(jié) i 是移動關(guān)節(jié)時(shí)， di是變量， i 是常數(shù)。 圖 2-2 D-H 模型參數(shù)示意圖 所以，機(jī) 器人的每個(gè)桿件都有 4個(gè)參數(shù) ai， i ， di 和 i 。它們決定 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 8 了機(jī)器人手臂每一個(gè)桿件的運(yùn)動學(xué)形態(tài)。由此可以確定聯(lián)系 i-1坐標(biāo)系和 i坐標(biāo)系之間的齊次變換矩陣，即從 i-1坐標(biāo)系變換到 i 坐標(biāo)系，只需完成下述變換： 1 沿 zi-1 軸平移距離 di，使 xi-1 軸和 xi 軸相交； 2 將 xi-1 軸繞 zi-1 軸轉(zhuǎn) i 角，使它同 xi 軸對準(zhǔn)； 3 沿 xi 軸移動距離 ai，使 i 坐標(biāo)系和 i-1坐標(biāo)系的原點(diǎn)及 x 軸重合； 4 繞 xi 軸轉(zhuǎn) i 角，使 i 坐標(biāo)系和 i-1坐標(biāo)系完全重合。 表 2-1 D-H 模型連桿參數(shù)名義值 序號 i ( ) di（ mm） ai（ mm） i ( ) 1 0 0 0 0 2 -90 0 180 -90 3 0 54 635 0 4 -90 515.7 0 0 5 90 0 0 180 6 -90 0 0 0 將這四步過程用基本的齊次轉(zhuǎn)動和齊次平移矩陣表示并連乘，可得相鄰坐標(biāo)系 i-1 和 i 之間的合成齊次變換矩陣 Aii-1，即 Aii-1=Transz（ di） Rotz（ i） Transz（ ai） Rotz（ i） =100010000100001id 1000010000c o ss in00s inc o siiii1000100001001 ia10000c o ss in00s inc o s00001iiii = 1000c o ss i n0s i nc o ss i nc o sc o ss i nc o ss i ns i ns i nc o sc o siiiiiiiiiiiiiiiiidaa（ 2-1） 由上式，要充分表示相鄰連桿坐標(biāo)系之間的位置關(guān)系，每個(gè)連桿至少由 4 個(gè)參數(shù)進(jìn)行描述，其中連桿長度和扭角描述連桿本身，另外兩個(gè)描述連桿和相鄰連桿的連接關(guān)系。對于 6 自由度機(jī)器人，用 18 個(gè)參數(shù)可完全描述 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 9 它的運(yùn)動學(xué)的固定參數(shù)部分，而其它 6 個(gè)關(guān)節(jié)變量則是機(jī)器人運(yùn)動方程中的變量部分。本文所研究的機(jī)器人各關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，故 1 到 6 為關(guān)節(jié)變量。 2.2 正向運(yùn)動學(xué)求解 當(dāng)機(jī)器人的結(jié)構(gòu)參數(shù)已確定，并給出各個(gè)關(guān)節(jié)角度，就可以計(jì)算機(jī)器人末端執(zhí)行器在基坐標(biāo)系中所處的位置和姿態(tài)，這就是求解機(jī)器人運(yùn)動學(xué)的正問題。 對于 SR06 型機(jī)器人，在根據(jù) D-H 原則建立起坐標(biāo)系之 后，就可以根據(jù)式 (2-1)得到 6 個(gè)轉(zhuǎn)換矩陣： A1=1000010s inc o s0s inc o ss in0c o s111111111daa， A2=10000100c o s0s inc o ss in0c o ss in22222222aaA3=10000010s inc o s0s inc o ss in0c o s33333333aa， A4=10000100c o s0s in0s in0c o s44444dA5=100000100c o s0s in0s in0c o s5555， A6= 100010000c o ss in00s inc o s66666d令 T=A1A2A3A4A5A6 (2-2) 則 T 表示了機(jī)器人末端法蘭盤坐標(biāo)系相對于機(jī)器人基坐標(biāo)系的位置與姿態(tài)，所以 T 矩陣就是機(jī)器人正向運(yùn)動學(xué)的解。 需要注意的是，為了增強(qiáng)機(jī)械強(qiáng)度， SR06 型機(jī)器人包含有一個(gè)四桿機(jī)構(gòu)在內(nèi)的閉環(huán)結(jié)構(gòu)，這將導(dǎo)致 z2 軸與 z1 軸的聯(lián)動現(xiàn)象。一旦 z1 軸相對零位發(fā) 生旋轉(zhuǎn)，即使沒有令 z2 軸單獨(dú)轉(zhuǎn)動，此時(shí) z2 軸也會發(fā)生旋轉(zhuǎn)，所以在計(jì)算時(shí)要考慮到這種影響，即從第三根軸的轉(zhuǎn)角中減去第二根軸的轉(zhuǎn)角。反過來的情況卻不同， z2 軸的轉(zhuǎn)動不會給 z1 軸產(chǎn)生任何影響。 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 10 正向運(yùn)動學(xué)求解： 圖 2-3 顯示了機(jī)器人正向運(yùn)動學(xué)求解的計(jì)算結(jié)果。 2.3 逆向運(yùn)動學(xué)求解 當(dāng)機(jī)器人末端執(zhí)行器的位置和姿態(tài)給定時(shí)，去求解相應(yīng)的關(guān)節(jié)轉(zhuǎn)角是逆向運(yùn)動學(xué)問題。由于求解逆問題的非線性方程組不一定總有封閉解、可能存在多個(gè)解、解與機(jī)器人實(shí)際結(jié)構(gòu)相違背等可能，使得求解起來較為復(fù)雜。 根據(jù)機(jī)器人運(yùn)動學(xué)理論，如果一臺 6 自由度機(jī)器人的 3 個(gè)連續(xù)關(guān)節(jié)的軸線相交于一點(diǎn)，那么該機(jī)器人就有逆向運(yùn)動的封閉解。對于 SR06 型機(jī)器人，它 的第 4、 5、 6 關(guān)節(jié)軸線相交于一點(diǎn)，因此就可以運(yùn)用代數(shù)法得到封閉解。 在這里采用遞推逆變換法求解一組非線性超越方程組。將一組逆矩陣A-11， A-12， A-13 連續(xù)左乘式 (2-2)兩端，可得到若干矩陣方程，每個(gè)矩陣有 12 個(gè)方程式，在這些關(guān)系式中可選擇只包含一個(gè)或不多于兩個(gè)待求運(yùn)動參數(shù)的關(guān)系式。然后遞推求解，一般遞推過程不一定全部作完，就可利用等式兩端矩陣中所包含對應(yīng)元素相等的關(guān)系式，求得所需的全部待求運(yùn)動參數(shù)。 設(shè) T=1000zzzzyyyyxxxxpaonpaonpaon(2-3) 則 T 中 12 個(gè)有效元素為： nx=c1s23(s4s6-c4c5c6)-c23s5c6-s1(s4c5c6+c4s6) ny=s1s23(s4s6-c4c5c6)-c23s5c6+c1(s4c5c6+c4s6) 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 11 nz=-c23(c4c5c6-s4s6)+s23s5c6 Ox=c1s23(c4c5s6+s4c6)+c23s5s6+s1(s4c5c6-c4c6) Oy=s1s23(c4c5s6+s4c6)+c23s5s6-c1(s4c5s6-c4c6) Oz=c23(c4c5s6+s4c6)-s23s5s6 ax=-c1(c4s23s5+c23c5)-s1s4s5 ay=-s1(c4s23s5-c23c5)+c1s4s5 az=-c4c23s5-s23c5 Px=-c1d6(c4s23s5-c23c5)-d4c23-a3s23-a2s2-a1-s1s4s5d6 Py=-s1d6(c4s23s5-c23c5)-d4c23-a3s23-a2s2-a1+c1s4s5d6 pz=d6(-c4c23s5-s23c5)-d4s23+a3c23+a2c2+d1 其中， si = sin i， ci = cos i， s23= sin( 2 + 3),c23 = cos( 2 + 3) 將式 (2-2)做簡單變換，得到 A -12A-11T61A-16=A3A4A5 (2-4) 令該式左右兩邊的 (3,4)元素相等，化簡得到 s1(axd6-px)=c1(ayd6-py),當(dāng) cos 0，即 k + /2（ k 為整數(shù)）時(shí)，有 tan 1=xxyy pda pda 66 由此可求得 1=arctan（xxyy pda pda 66 ）。 令式 (2-4)兩邊的 (1,4)和 (2,4)對應(yīng)元素分別相等，得 -s3d4+a3c3=-d6c2(c1ax+s1ay)-s2az+c2(c1px+s1py-a1)-s2(pz-d1)-a2 (2-5) C3d4+a3s3=-d6-s2(c1ax+s1ay)-c2az-s2(c1px+s1py-a1)-s2(pz-d1) (2-6) 先將式 (2-5)中右邊的 a2項(xiàng)移到左邊，再將該方程組兩邊平方相加，得： 左邊 =d24+a23+a22-2d4a2s3+2a3a2s3 右邊 =-2d6f1f2+f3az+d26f21+azz+f22+f33 這里 f1=c1ax+s1ay ,f2=c1px+s1py-a1 ,f3=pz-d1 左式 =右式，那么 2d4a2s3-2a3a2c3=d24+a23+a22+2d6f1f2+f3az-d26f21+a2z-f22-f23 , 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 12 再令 p1 = 2 d4a2， p2 = 2 a3a2， P3=d24+a23+a22+2d6f1f2+f3az-d26f21+a2z-f22-f23 經(jīng)過三角變換可以的得到 3=arctan(2322213pppp)-arctan(12pp ),上姿態(tài)時(shí)取正號，下姿態(tài)時(shí)取負(fù)號。 令 g1=d6( c1ax+s1ay)-c1px-s1py+a1 ,g2=d6az-pz+d1 ,g3 = c3d4+a3s3 ,那么式 (2-6)可以寫成 s2g1+c2g2=g3，經(jīng)過三角變換，得到 2=arctan(2322213gggg)-arctan(12gg ),上姿態(tài)時(shí)取正號，下姿態(tài)時(shí)取負(fù)號。 令式（ 2-4）兩邊（ 1,3）和（ 2,3）元素分別對應(yīng)相等，有C3c4s5+s3c5=c2（ c1ax+s1ay） -s2az （ 2-7） S3c4s5-c3c5=-s2（ c1ax+s1ay） -c2az （ 2-8） 式（ 2-7)x s3-式（ 2-8） x c3 ，整理后得： C5=s32（ c1ax+s1ay） +c32az ,代入 1 、 2 、 3 ，即可求得 5 。 令式 (2-4)兩邊的 (3,3)元素相等，得 -s4s5=-s1ax+c1ay ,代入 5 即可求得4 。令式 (2-4)兩邊的 (3,2)元素相等，得 C4 = s6s1nx+c1ny+c6s1ox+c1oy，做三角變換，并代入 1 、 4 ，可以求得 6 。 按照上述方法求解關(guān)節(jié)變量時(shí)，都存在多解的問題，這時(shí)可根據(jù) robtarget 數(shù)據(jù)類型的 robconf 來確定到底取哪個(gè)解。另外，當(dāng)末端執(zhí)行器由前一個(gè)點(diǎn)位向后一個(gè)點(diǎn)位運(yùn)動時(shí)，達(dá)到后一點(diǎn)位的位姿有多解時(shí)，可選擇最“接近”前一點(diǎn)位的解，即選擇關(guān)節(jié)變量解最靠近前一點(diǎn)的關(guān)節(jié)變量值。 需要指出的是， SR06 型機(jī)器人的姿態(tài)并不是用 3 3 的旋轉(zhuǎn)矩陣表示的，而是用一個(gè)四維向量 q 來表示的，它們之間 的關(guān)系為： q1=2 1 zyx aon, q2=2 1 zyx aon, signq2=sign(oz-ay), 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 13 q3=2 1 zxy ano, signq3=sign(ax-nz), q4=2 1 yxz ona,signq4=sign(ny-ox). 所以在求逆向解時(shí)要先將此四維向量轉(zhuǎn)換成旋轉(zhuǎn)矩陣。 運(yùn)動學(xué)逆解： 圖 2-4 顯示了機(jī)器人逆向運(yùn)動學(xué)求解的計(jì)算結(jié)果。 2.4 微分運(yùn)動學(xué)模型 機(jī)器人的微分運(yùn)動是指當(dāng)關(guān)節(jié)坐標(biāo)變量（包括關(guān)節(jié)變量和連桿參數(shù)）產(chǎn)生微小變化而引起臂端（手部）位姿的某一微小變化。 設(shè)機(jī)器人運(yùn)動鏈中某一桿件對于固定坐標(biāo)系的位姿為 T，經(jīng)過微運(yùn)動后該桿件對固定系的位姿變?yōu)?T+dT，若這個(gè)微運(yùn)動是相對于固定系進(jìn)行的，總可以用微小的平移和旋轉(zhuǎn)來表示，即 T + dT=Trans( dx,dy,dz)Rot(k,d )T dT = Trans(dx,dy,dz)Rot(k,d )TT= Trans(dx,dy,dz)Rot(k ,d )IT (2-9) 根據(jù)齊 次變換的相對性，求微分運(yùn)動是對于某個(gè)桿系 i 進(jìn)行的，則 T+dT 可以表示為： T + dT=TTrans( dx,dy,dz)Rot(k,d ) dT=TTrans(dx,dy,dz)Rot(k,d )T= TTrans (dx,dy,dz)Rot(k,d )I (2-10) 其中 =Trans ( dx,dy,dz)Rot(k ,d )I 稱為微分變換矩陣。 于是，式 (2-9)變?yōu)?dT = 0T，式 (2-10)變?yōu)?dT = T i，此處 的下標(biāo) 六自由度工業(yè)機(jī)器人標(biāo)定 蔣濤 14 不同是相對不同的坐標(biāo)系，若變換 T 是若干變量 qi (i=1,2, ,n)的函數(shù)，則 dT= niii dqqT1(2-11) 微分平移與一般平移變換一樣，其變換矩陣為： Trans(dx ,dy ,dz )=1000100010001zyxddd(2-12) 從幾何意義上講， 旋轉(zhuǎn)矩陣可以表示為遵循右手法則繞軸 K= Tzyx k k k 做角的旋轉(zhuǎn)，展開后如式 (2-13)所示： Rot(k, )=c o ss i ns i ns i nc o ss i ns i ns i nc o sv e r skkkv e r skkkv e r skkkv e r skkv e r skkkv e r skkkv e r skkkv e r skkv e r skkzzxyzyzxxyxyyzyxyzxzyxx