機器人傳送帶動態(tài)跟蹤

1、廣東工業(yè)大學碩士學位論文廣東工業(yè)大學碩士學位論文（工學碩士）并聯(lián) delta 機器人的傳送帶動態(tài)抓取系統(tǒng)設計鄧明星二一四 年 六月二一四 年 六分類號： 學校代號：11845udc： 密級： 學 號：2111101011廣東工業(yè)大學碩士學位論文廣東工業(yè)大學碩士學位論文（工學碩士）并聯(lián) delta 機器人的傳送帶動態(tài)抓取系統(tǒng)設計鄧明星 指導教師姓名、職稱： 劉冠峰 教授 學科(專業(yè))或領域名稱： 智能工業(yè)機器人 學 生 所 屬 學 院： 機電工程學院 論 文 答 辯 日 期： 2014 年 6 月 1 日 a dissertation submitted to guangdong univers

2、ity of technology for the degree of master of science（master of engineering science）the dynamic conveyor tracking of delta robotcandidate: deng mingxingsupervisor: prof. liu guangfengjune 2014school of electromechanical engineeringguangdong university of technologyguangzhou, guangdong, p. r. china,

3、5100摘要i摘要delta 機器人是最典型的空間三自由度移動并聯(lián)機構，具有運動速度快、定位精確、效率高、重量輕等特點。其在 3c 電子產(chǎn)品、食品包裝、生物制藥、3d 打印行業(yè)得到了廣泛應用。隨著人民生活水平的提高，促使產(chǎn)品質(zhì)量的提升和需求數(shù)量的增加，間接的促使機器人技術的發(fā)展。本文主要以并聯(lián) delta 機器人為研究對象。首先介紹國內(nèi)外工業(yè)機器人的發(fā)展狀況；接著分析了 delta 機器人的運動學正逆解，傳送帶標定、相機標定；然后提出了pid 跟蹤算法和改進的跟蹤算法；再者給出了 conveyor administor 的軟件設計流程圖，并解決了其中幾個關鍵問題：目標物體的篩選、排序、位置狀態(tài)

4、更新、多機器人協(xié)同作業(yè)。本文的核心內(nèi)容之一是機器人對運動在傳送帶上的物體的動態(tài)跟蹤，通過視覺檢測系統(tǒng)定位物體的位姿，再結合傳送帶編碼器反饋的位置數(shù)據(jù)，計算出物體在機器人坐標系下面的實時位姿。機器人在位置模式下，通過改進后的跟蹤算法，能預測傳送帶上物體的位姿，并實時調(diào)整自身移動速度和末端位姿來跟蹤傳送帶上移動的物體，最終實現(xiàn)對目標物體的動態(tài)跟蹤抓取。本文的核心內(nèi)容之二是處理傳送帶上連續(xù)不斷的物體，在實際應用中，要求物體盡量少被遺漏甚者不能漏，這就要求機器人的移動速度要快，其次是抓取順序必須規(guī)劃好，本文的最后給出了解決方法和設計的數(shù)據(jù)管理軟件。通過大量的實驗數(shù)據(jù)和分析驗證，該動態(tài)跟蹤方法基本滿足了

5、實際應用要求，為后面的項目應用現(xiàn)提供了技術支持，也為多機器人協(xié)調(diào)作業(yè)提供了一種方法。關鍵詞：并聯(lián) delta 機器人、傳送帶跟蹤、pid 算法、多機器人協(xié)同作業(yè)、數(shù)據(jù)流廣東工業(yè)大學碩士學位論文iiabstractthe 3 dof delta robot is one of the most typical parallel robot. it has many advantages, such as high movement speed, accurate location, high efficiency, light weight etc. it has been widely use

6、d in the electronic products industry, food packaging industry, biological pharmaceutical industry, 3d printing. with the development of robot technology and the improvement of peoples living standard, it is improvement to improve the quality of product and the number of product.this article mainly

7、took delta robot as the research object. firstly, it introduced the development of industrial robots at home and abroad; then analyzed the forward kinematics and inverse kinematics of delta robot，the calibration of conveyor belt and camera; then introduced a pid tracking algorithm and a improved tra

8、cking algorithm; moreover display the flow chart of software design of conveyor administor, and resolved the problem of several joint data filtering, sorting, update of data status, coordination work of multi-robot; one of the core content of this article is the dynamic tracing of robot to the movin

9、g objects on the conveyor belt, coupled with the position of the conveyor belt encoder feedback data. visual system locates the objects positions then gives them to the delta robot. conveyor administor calculates the position of object in the robot coordinate system with the real-time system. robot

10、can predict the objects position on the conveyor belt, and adjust their movement speed and real-time posture to track objects on the conveyor belt in position mode and through the improved tracking algorithm, which finally realizes the dynamic tracing to moving target object. the another core conten

11、t of this article is the operation for continuous object on a conveyor belt, in actual application, there should no missed object, so, that requires that the robot move faster, objects are followed by fetching order must plan well. in the end, the paper puts forward the solution and design data mana

12、gement software.abastractiiithrough the analysis of the results and the experimental verification, the basic dynamic tracing method satisfied the requirements of practical application. it provides the technical guidance for the back of the project application, and realize the coordination working of

13、 multi-robot.keywords: delta robot, conveyor belt tracking, pid algorithm, multi-robot coordination work, data stream.廣東工業(yè)大學碩士學位論文iv目錄摘要 .iabstract.ii目錄 .ivcontents.vi第一章 緒論.11.1 什么是工業(yè)機器人.11.2 工業(yè)機器人在國內(nèi)外發(fā)展狀況.31.3 delta機器人簡介.51.4 本課題研究內(nèi)容和研究意義.7第二章 delta 機器人軟、硬件系統(tǒng).82.1 系統(tǒng)硬件介紹.82.2 系統(tǒng)軟件介紹.9第三章 delta 機器人

14、的運動學求解和標定.123.1 空間點、向量、坐標系的表示.123.2 delta機器人運動學正解（幾何法）.143.3 delta機器人運動學逆解.183.4 傳送帶標定.213.4.1 轉(zhuǎn)換矩陣和比例因子.213.4.2 傳送帶的標定方法.213.5 視覺標定.243.5.1 視覺標定原理.243.5.2 基于機器人的相機標定.273.5.3 基于傳送帶的視覺標定.28第四章 動態(tài)跟蹤算法.30目錄v4.1 物體位姿的相互轉(zhuǎn)換.304.2 pid 算法的概念.314.3 pid 形式的跟蹤.324.4 兩種改進形式的 pid 跟蹤.36第五章 目標物體跟蹤序列軟件的設計.385.1 con

15、veyor administor軟件架構.385.2 目標物體的篩選和遺漏問題.395.3 目標物體的排序.415.4 更新目標物體的操作狀態(tài)和坐標狀態(tài).435.5 conveyor administor的輸出和多機器人協(xié)同.465.6 conveyor administor的交互.485.7 軟件流程圖.50結論與展望.52參考文獻.54學位期間發(fā)表的論文.58學位論文獨創(chuàng)性聲明.59致謝.60廣東工業(yè)大學碩士學位論文vicontentsabstract(chinese).iabstract(english).iicontents(chinese) .ivcontents(chinese).

16、vichapter1 introduction .11.1 industrial robot.11.2 the development of industrial robot .31.3 delta robot.51.4 main works of this paper.7chapter2 the hardware & software of delta robot.82.1 hardware system .82.2 software system.9chapter3 the kinematics & calibration of delta robot.123.1 the

17、representation of point，vector，coordinate.123.2 the forward kinematics of delta robot.143.3 the inverse kinematics of delta robot.183.4 calibrate conveyor.213.4.1 transition matrix & scale factor.213.4.2 calibrate conveyor.213.5 calibrate vision.243.5.1 the theory of visual calibration.243.5.2 c

18、alibrate vision based on robot .273.5.3 calibrate vision based on conveyor.28chapter4 the algorithm of tracking.304.1 transfer position of different parts.30contentsvii4.2 the concept of pid.314.3 the form of pid tracking .324.4 two kinds of improved pid tracking.36chapter5 conveyor administor.385.1

19、 the software architecture of conveyor administor.385.2 design filter for target parts.395.3 sort the target parts.415.4 update the operational state & position state .435.5 the export & multi robot.465.6 the user interface of conveyor administor.485.7 software flow pattern .50conclusion &am

20、p; prospect.52referances .54published papers during studying degree .58originality declaration .59acknowledgements.60第一章 緒論1第一章 緒論1.1 什么是工業(yè)機器人工業(yè)機器人是一種面向工業(yè)領域的具有多個自由度或多個關節(jié)的特殊機器人1；一般以機械手的形式呈現(xiàn)，包括串聯(lián)的工業(yè)機器人和并聯(lián)的工業(yè)機器人，它一般由本體、控制系統(tǒng)、動力系統(tǒng)三個基本部分。工業(yè)機器人是一種自動完成工作的智能機器裝置，是依靠控制系統(tǒng)來控制、動力系統(tǒng)來執(zhí)行任務的一種機器。它可以與使用者交互，通過示教的方式來學習

21、新的功能，豐富自身的知識數(shù)據(jù)庫、工業(yè)機器人可以依靠視覺圖像處理技術、運動控制技術、人工智能學、聽覺模糊識別技術、計算機技術、網(wǎng)絡通信技術等來增加自身的功能。工業(yè)機器人可以替代人類來完成一些危險、枯燥、重復、單調(diào)、環(huán)境惡劣的工作。它們通常應用于焊接、搬運、鍛造、熱處理、化學物品制造、高精度定位以及原子工業(yè)部門中。工業(yè)機器人具有一定的通用性和普遍性，因種類繁多、形態(tài)各異，所以它們多應用在各種生產(chǎn)線上，是柔性制造單元系統(tǒng)中不可或缺的組成部分。圖 1-1 機器人工作流水線figure1-1 the working line of robot工業(yè)機器人具有智能化、可編程化、通用性等特點。它涵蓋的科學技術

22、相當廣泛，總結下來，工業(yè)機器人就是機械技術和電子技術的結合，即機電一體化發(fā)展的高級產(chǎn)物。當前的工業(yè)機器人已經(jīng)相當智能了，不僅具備了獲得外部環(huán)境數(shù)據(jù)的各種傳感器，還具有記憶存儲、示教學習、語音識別、圖像處理、邏輯推理判斷功能。所以，在很廣東工業(yè)大學碩士學位論文2大程度上，機器人技術的發(fā)展勢必帶動其他領域?qū)W科的發(fā)展。機器人技術的發(fā)展和應用程度也是一個國家科學技術和工業(yè)技術發(fā)展水平的衡量標準。 圖 1-2 常見的工業(yè)機器人figure1-2 the common industrial robot如圖 1-2，市場上常見的工業(yè)機器人：delta robot,5、6 軸串聯(lián)機械手, dual-arm r

23、obot, scara robot, youbot。 第一章 緒論31.2 工業(yè)機器人在國內(nèi)外發(fā)展狀況工業(yè)機器人技術的發(fā)展和成熟，即將掀起新一輪的產(chǎn)業(yè)革命。大多數(shù)發(fā)達國家：美國、德國、意大利、瑞士、日本、法國等早已普及工業(yè)機器人。電子產(chǎn)品、汽車制造、機械工程、食品包裝、生物制藥等行業(yè)的自動化生產(chǎn)線已經(jīng)大量投入工業(yè)機器人。這些機器人的普及提升了產(chǎn)品質(zhì)量、提高了勞動生成率、節(jié)約了用人成本、解放了生產(chǎn)力、減少了工傷事故。自動化設備的發(fā)展方向正逐漸轉(zhuǎn)向工業(yè)機器人成套設備的發(fā)展，正是此發(fā)展轉(zhuǎn)變，促使政府、企業(yè)、高校投入更多的精力和經(jīng)費來研究工業(yè)機器人技術，也推動了生產(chǎn)力的發(fā)展。根據(jù)國際機器人聯(lián)合會(if

24、r)和聯(lián)合國歐洲經(jīng)濟委員會(unece)對各國使用工業(yè)機器人情況的統(tǒng)計，工業(yè)機器人的發(fā)展一片光明。從上世紀中葉，工業(yè)機器人產(chǎn)值一直保持著穩(wěn)定增長的良好形勢。到九十年代，工業(yè)機器人發(fā)展形勢瞬間變得猛烈，平均年增長率在 10上下。而到了 2004 年，平均增長率更是達到了前所未有的 21。其中，在亞洲機地區(qū)，機器人的增長率尤為明顯，高達 44%。2004 年全球地區(qū)至少使用了 10 萬臺新的工業(yè)機器人。其中：歐洲地區(qū) 3.1 萬臺，比上一年多增加 15；北美地區(qū)大約 1.6 萬臺，該數(shù)目比上一年增加 27；亞洲地區(qū)大約 5.1 萬臺，日本使用最多，但中國市場也不容小覷，增長異常迅速，比上一年多增長

25、 242。根據(jù)美國電氣和電子工程師協(xié)會對機器人使用情況的統(tǒng)計，至 08 年底，全球地區(qū)已經(jīng)布局了大約 1000000 臺工業(yè)機器人，而日本是世界上使用工業(yè)機器人最多、普及最廣的國家。圖 1-3 每 10000 工人占有機器人的個數(shù)廣東工業(yè)大學碩士學位論文4figure 1-3 the number of robot per 10000 workers圖 1-3 是依照每 10000 個工人使用工業(yè)機器人數(shù)量的比例繪制的，從圖中可以清晰的知道，日本工人的工業(yè)機器人分配密度達到了世界平均水平的 9 倍，幾乎是第二位新加坡的 2 倍。排在前 5 的國家分別是：日本每一萬工人擁有 295 臺工業(yè)機器人

26、、新加坡 169 臺、韓國 164 臺、德國 163 臺、瑞典 126 臺。雖然在這前五位國家中，有三位屬于亞洲地區(qū)，但歐洲的工業(yè)機器人普及程度是全世界最廣的。歐洲國家工業(yè)機器人密度為每萬名工人 50 臺、北美洲為 31 臺、亞洲 27 臺。國外的工業(yè)機器人制造廠商：瑞典的 abb robotics；瑞士的 staubli; 日本的fanuc,yaskawa,nachi,otc,mitsubishi;德國的 kuka roboter;美國的adept technology,american robot,emerson industrial automation,s-t robotics;意大利

27、comau;英國的 autotech robotics;加拿大的 jcd international robotics;以色列的robogroup tek。與國外相比，我國工業(yè)機器人基礎研究啟動比較晚，從上世紀七十年代初期才開始發(fā)展。截止 2010 年底，國內(nèi)正在工作和使用的工業(yè)機器人數(shù)量超過 50000 臺，普及程度仍不及歐美等發(fā)達國家，這個數(shù)量大致是德國的 1/4、日本的 1/10，由此可知，中國工業(yè)機器人行業(yè)發(fā)展空間巨大。到 2014 年底，中國購買機器人的數(shù)量將位居全球首位。預計到 2015 年底，我國機器人需要數(shù)量大約為 35000 臺，占全球總量的16.9%，而中國對機器人的需求量

28、在全球仍將保存首位。分析表示，未來五年，中國工業(yè)機器人市場需求增長率可達 35%，呈井噴式增長。到 2016 年或成為全球最大的機器人市場。目前“十二五”規(guī)劃中，中國開始實行經(jīng)濟轉(zhuǎn)型。中國的制造業(yè)面臨著向智能化、高效率、低成本、更綠色方向轉(zhuǎn)變。加速工業(yè)機器人基礎研究即是中國的一個歷史機遇又是一種挑戰(zhàn)，因此，大力發(fā)展工業(yè)機器人產(chǎn)業(yè)的工作要進一步落實。因為：第一，工業(yè)機器人技術是十二五期間經(jīng)濟轉(zhuǎn)型的主要手段和途徑，政府要對國產(chǎn)工業(yè)機器人有更多的政策與經(jīng)濟支持，高校和科研機構要學習國外先進技術，拓展更多的機器人應用領域；第二，在國家的發(fā)展規(guī)劃中，應繼續(xù)對機器人基礎研究和應用給予大力支持，力爭形成產(chǎn)品

29、和自動化制造裝備同步協(xié)調(diào)的新局面；第三，部分自主研發(fā)工業(yè)機器人已具備質(zhì)量高、功能全、高度智能化、低成本等特點，國內(nèi)企業(yè)購買工業(yè)機器人時不要盲目崇洋媚外，應全面評估，支持國產(chǎn)。中國國內(nèi)也投入了相當多的經(jīng)費對工業(yè)機器人技術做研究。國內(nèi)的工業(yè)機器人公第一章 緒論5司也雨后春筍般發(fā)展。沈陽新松、眾為興、埃斯頓、啟帆、李群科技、廣州數(shù)控、樂佰特，雖然起步晚，但是勁頭足。1.3 delta 機器人簡介delta 機器人是一種并聯(lián)的、高速的、輕載的、智能的機器人。由于機械結構的牽扯和限制，它一般只有 3 到 4 個自由度，故只能在工作空間內(nèi)實現(xiàn)三個方向的平動和繞 z 軸的旋轉(zhuǎn)運動。它是由法國人 clavel

30、 博士于 1985 年發(fā)明的，隨后又提出三種機構變異形式，以適應不同空間需求。delta 機器人具有重量輕、體積小、運動速度快、定位精確、效率高等特點。delta 機器人在近幾年得到了飛速發(fā)展，無論國內(nèi)還是國外，各大機器人制造廠商分別制造出各自的機器人。表 1-1 是各機器人制造商的 delta robot，圖 1-4 是形態(tài)各異的 delta robot.表 1-1 各機器人制造商的 delta robottable 1-1 the delta robots of each manufacturercompanydelta robotabbflexpickeradeptquattroyask

31、awahigh speed robotfanucm-3iaboschpalomaqkmapollo 廣東工業(yè)大學碩士學位論文6圖 1-4 形態(tài)各異的 delta robotfigure 1-4 different kinds of delta robots在中國，隨著市場經(jīng)濟的發(fā)展和生活水平的提高，國民對食品、藥物、電子產(chǎn)品的需求越來越大，對這些行業(yè)來說，既是一種機遇又是一種挑戰(zhàn)，而其中的產(chǎn)品包裝工序尤為值得一提。隨著產(chǎn)業(yè)結構升級，勞動資本提高，越來越多的生產(chǎn)廠家不愿依靠雇傭更多的員工而投入大量的財力去提高生產(chǎn)率，而是希望尋求更高效、更安全的方法來提高產(chǎn)品的質(zhì)量和生產(chǎn)效率。delta robo

32、t 完全可以滿足上述需要：高精度、高速、高質(zhì)量、高效率。因此對 delta 機器人的研究變得尤為重要。delta 機器人的技術研究包括：前向運動學，后向運動學（forward kinematics、inverse kinematics） 、動力學（dynamics） 、機器視覺技術（machine vision） 、精確標定技術、傳動帶跟蹤、機械設計、機械優(yōu)化、計算機科學、網(wǎng)絡通信。本論文中主要研究傳送帶跟蹤、運動學求解、機器人標定。1.4 本課題研究內(nèi)容和研究意義由于delta機器人具有速度快、負載輕、定位準特點，因此，它常常被運用在包裝流水線上，待包裝物品在一條傳送帶上，包裝盒在另外一條傳

33、送帶上。在實際應用中，傳送帶是移動的，物體的位置信息可以通過傳感器、視覺系統(tǒng)獲取，但是這些位置信息只是某一時刻的。因此，機器人不得不實時計算物體的位置，并且不斷調(diào)整自身末端的位置和移動速度，讓它不斷的接近目標物體，最終動態(tài)抓取。抓取完目標物體之后，機器人仍需要定位包裝盒，并且實時跟蹤它，然后第一章 緒論7把目標物體放進包裝盒里面。除了動態(tài)跟蹤物體以外，實際項目中還要求傳送帶上的物體不能被遺漏。如何解決這個問題呢？可以降低傳送帶的移動速度，增加機器人的移動速度，但是這樣會影響到機器人的生成效率。機器人的抓取順序也非常重要，如果機器人不先抓快走出工作空間的物體，而去抓剛進工作空間的物體。那么肯定會

34、造成物體被遺漏。如果傳送帶上的待撿物體確實太多，一個機器人根本解決不了遺漏問題，那么應該考慮多個機器人協(xié)同作業(yè)。上一個機器人遺漏的物體傳遞給下一個機器人，下一個機器人繼續(xù)作業(yè)，依次傳遞下去。這樣既可以提高生成效率、解決遺漏問題，又可以節(jié)約成本。如果采用視覺系統(tǒng)，只需要一個視覺系統(tǒng)即可，不再需要為每一個機器人都配一個視覺系統(tǒng)。也許實際應用中是上一個機器人分揀，下一個機器人貼標簽，再下一個機器人抓取到包裝盒?？傊?，如果實現(xiàn)了多機器人的協(xié)調(diào)作業(yè)，這些問題便迎刃而解了。而且還可以通過它建立一個龐大的workspace.本文的研究內(nèi)容大致有以下幾個方面：delta機器人的前向運動學、逆向運動學、傳動帶標

35、定、相機的標定、目標物體動態(tài)跟蹤、目標物體重復篩選、目標物體排序、多機器人標定、多機器人協(xié)同。本文的研究意義不但要解決上述的實際應用問題，而且還提出了一種新的跟蹤算法，編寫了一個數(shù)據(jù)管理軟件。廣東工業(yè)大學碩士學位論文8第二章 delta 機器人軟、硬件系統(tǒng)2.1 系統(tǒng)硬件介紹本論文的硬件平臺是基于 qkm 公司的 apollo 機器人，如圖 2-1。在這個硬件平臺上，研究 delta 機器人的相關技術。圖 2-1 apollo 機器人 figure 2-1 apollo delta robot如圖 2-1，機器人硬件系統(tǒng)由機械本體、伺服電機、傳送帶、工業(yè)攝像機、工控電腦、pa 運動控制卡和示教

36、盒組成3。工業(yè)相機通過 usb 數(shù)據(jù)線與工控電腦連接，攝像機把圖像傳遞給工控電腦，電腦對圖像進行特征值提取，分析出物體的位姿，再將物體的位姿信息通過以太網(wǎng)傳遞給 pa 控制卡。用戶可以在示教盒上編寫簡單的控制程序，也可以通過示教盒移動機器人，類似于一個遙控器，可以很方便地控制機器人。示教盒與控制卡是通過 485 串口來通訊。除此之外，圖 2-2 中有兩條傳送帶，一條傳送帶上是待分揀的物體，另外一條上是托盤。一般情況下，視覺系統(tǒng)被安裝在機器人的工作空間之外，這樣的好處保證了機器人在工作空間內(nèi)正常作業(yè)時，視覺系統(tǒng)不會受干擾。第二章 delta 機器人軟、硬系統(tǒng)9圖 2-2 delta robot

37、控制系統(tǒng)figure 2-2 the delta robots control system在圖 2-2 中，控制系統(tǒng)的核心為控制卡，本系統(tǒng)中的運動控制卡為 presice automation controller，簡稱 pa 卡。pa 運動控制卡是由美國 precise automation公司所研發(fā)生產(chǎn)的，是一款新型的驅(qū)控一體、支持高級編程、嵌入多種運動學模型的支持串口、以太網(wǎng)通信的控制卡。該控制卡不需要額外的伺服電機驅(qū)動器（driver） ，它可以直接和電機相連。 圖 2-3 pa 控制卡figure 2-3 pa controller2.2 系統(tǒng)軟件介紹如圖 2-4，pv 是圖像處理

38、軟件，它會提取出目標物體，分析出目標物體的位置信廣東工業(yè)大學碩士學位論文10息。而得到的坐標是像素坐標，通過轉(zhuǎn)換矩陣（內(nèi)參和外參） ，可以將像素坐標轉(zhuǎn)換成機器人坐標。最終通過以太網(wǎng)把物體的位姿數(shù)據(jù)傳遞給 pa 控制卡，pa 控制卡控制機器人電機，電機的轉(zhuǎn)動反映在機械本體的末端，最終實現(xiàn)對目標物體的抓取。圖 2-4 precise vision：機器視覺軟件figure 2-4 precise vision：machine vision software圖 2-4 是機器人控制軟件 gde，用戶可以在該軟件上編寫復雜的控制程序，機器人語言是高級語言 c#，用戶可以很方便的進行二次開發(fā)。除此之外，

39、gde 還提供了大量關于機器人運動，以太網(wǎng)通訊，文件處理的 api 函數(shù)。圖 2-5 gde：程序開發(fā)軟件figure 2-5 gde：develop software第二章 delta 機器人軟、硬系統(tǒng)11圖 2-6 是機器人數(shù)據(jù)分析軟件 datalogger。在對電機進行 pid 參數(shù)設置時：該軟件可以很好的輔助用戶分析電流環(huán)、速度環(huán)、位置環(huán)數(shù)據(jù)。圖 2-6 datalogger：機器人數(shù)據(jù)分析軟件figure 2-6 datalogger：data analysis software廣東工業(yè)大學碩士學位論文12第三章 delta 機器人的運動學求解和標定3.1 空間點、向量、坐標系的表示

40、如圖 3-1 所示，空間點 p 可以用它的相對于參考坐標系的三個坐標來表示： (3.1.1)xyzpa ib jc k其中， 是參考坐標系中表示該點的坐標。,xyza b c圖 3-1 空間點的表示figure 3-1 the representation of a space point向量可以由一個起始點和終止點來表示4-5。如果一個向量起始于點 a，終止于點b，那么它可以表示為。特殊情況下，如果一()()()abxxyyzzpba ibajba k個向量起始于原點，如圖 3-1 所示，則有：(3.1.2)xyzpa ib jc k其中是該向量在參考坐標系中的三個分量。實際上，點 p 就是

41、用原點連,xyza b c接到該點的向量來表示的，具體地說，也就是用該向量的三個坐標來表示。圖 3-2 空間向量的表示figure 3-2 the representation of a space vector第三章 delta 機器人的運動學求解和標定13向量的三個分量也可以寫成矩陣的形式，如式（3.1.2）所示。 (3.1.3)xyzapbc這種表示法也可以稍做變化：加入一個比例因子 w,如果 x, y, z 各除以 w,則得到。于是，這時向量可以寫為：,xyza b c(3.1.4) xypzw 變量 w 可以為任意數(shù)，而且隨著它的變化，向量的大小也會發(fā)生變化，這與在計算機圖形學中縮放

42、一張圖片十分類似。隨著 w 值的改變，向量的大小也相應地變化。如果 w 大于 1，向量的所有分量都變大；如果 w 小于 1，向量的所有分量都變小。這種方法也用于計算機圖形學中改變圖形與畫片的大小。如果 w 是 1，各分量的大小保持不變。但是，如果 w=0，則為無窮大。在,xyza b c這種情況下，x，y 和 z（以及）表示一個長度為無窮大的向量，它的方向即,xyza b c為該向量所表示的方向。這就意味著方向向量可以由比例因子 w=0 的向量來表示，這里向量的長度并不重要，而其方向由該向量的三個分量來表示。一個中心位于參考坐標系原點的坐標系由三個向量表示，通常這三個向量相互垂直，稱為單位向量

43、，分別表示法線（normal） 、指向（orientation）和接近, ,n o a（approach）向量，如圖 3-3 所示。正如公式 3.1.2，每一個單位向量都由它們所在參考坐標系著的三個分量表示。這樣，坐標系 f 可以由三個向量以矩陣的形式表示為：圖 3-3 移動坐標系原點在參考坐標系下面的表示figure 3-3 the representation of a moving coordinate system origin in the reference frame廣東工業(yè)大學碩士學位論文14(3.1.5)xxxyyyzzznoafnoanoa如果一個坐標系不在固定參考坐標系的

44、原點，那么該坐標系的原點相對于參考坐標系的位置也必須表示出來。因此，在該坐標系原點與參考坐標系原點之間做一個向量來表示該坐標系的位置，如圖 3-4 所示。這個向量由相對于參考坐標系的三個向量來表示。這樣，這個坐標系就可以由三個表示方向的單位向量以及第四個位置向量來表示6。圖 3-4 一個坐標系在另一個坐標系中的表示figure 3-4 the representation of a moving coordinate in the reference frame(3.1.6)0001xxxxyyyyzzzznoapnoapfnoap如式（3.1.5）所示，前三個向量是 w=0 的方向向量，表示

45、該坐標系的三個單位向量的方向，而第四個 w=1 的向量表示該坐標系原點相對于參考坐標系的位置。, ,n o a與單位向量不同，向量 p 的長度十分重要，因而使用比例因子為 16。3.2 delta 機器人運動學正解（幾何法）到目前為止，并聯(lián) delta 機器人運動學正解的封閉解問題仍沒有得到全面解決。 常用通過代數(shù)方程組的數(shù)值解法來解決這個問題, 該方法缺點在于推導過程極其復雜, 最終有多組解，不得不尋找更多的約束來對這些解做取舍。因而，本論文中采用更直觀的解法，運用矢量代數(shù)和空間幾何學來建立三自由度 delta 機器人的簡化運動學模型, 求解并聯(lián)機器人運動學正解。相比于基于代數(shù)方程組的求解方

46、法，幾何法推導過第三章 delta 機器人的運動學求解和標定15程更簡單、直觀，并且回避了并聯(lián)機器人運動學正解多解取舍的問題, 可直接獲得工作空間內(nèi)滿足運動連續(xù)性的合理解7。圖 3-5 是 delta 機器人的結構圖。一個靜平臺、三個主動臂、三個平行四邊形的從動臂、一個運動平臺構成了該機器人的框架。可以看出三組平行四邊形結構的應用消除了運動平臺繞的轉(zhuǎn)動自由度，但向各個方向的平動自由度沒有被消除。即運動平臺只能向 x、y、z 軸平動，不能向 x、y、z 軸做旋轉(zhuǎn)運動。圖 3-5 delta robot 結構圖figure 3-5 the structure of delta robot 將上述結

47、構簡化一下，如圖 3-6。圖 3-6 delta robot 結構簡化圖figure 3-6 the structure of delta robot 首先，將模型改造，分別構造三個平行四邊形的中線：、，這三條11ab22a b33a b中線的運動分別與平行四邊形兩邊的運動一樣。動平臺的中心為，沿著平移a1a a，交于，向量、分別與、相等?？梢院苋菀浊骾iaba1a a2a a 3a a 1b b2b c 3b d 廣東工業(yè)大學碩士學位論文16得 b、c、d 坐標。如下公式：， =1，2，3(3.2.1)cos( )sin( )0iiiraocra i其中，、分別是三個輸入桿與 x 坐標軸的夾

48、角；r 為靜平臺所在圓的半徑。1a2a3a， =1，2，3(3.2.2)iiiiobocc b i， =1，2，3 (3.2.3)cos( )cos( )cos( )sin( )sin( )iiiiiiilac blal i， =1，2，3 (3.2.4)cos( )cos( )cos( )sin( )cos( )sin( )sin( )iiiiiiiiralaobralali其中 l 為輸入桿的長度，、分別為輸入桿的輸入角8-9。123, 圖 3-7 delta robot 結構簡化圖figure 3-7 the structure of delta robot (3.2.5)1111222

49、23333obobb boba aocobb coba aodobb doba a 第三章 delta 機器人的運動學求解和標定17， =1，2，3 (3.2.6)rcos( )rsin( )0iiiaaaai(3.2.7)1111111112222222223cos()cos( )cos()rcos()sin()cos( )sin()rsin()sin( )cos()cos()cos()rcos()sin()cos()sin()rsin()sin()cos()cralaaobralaalralaaocralaalralod 33333333os()cos()rcos()sin()cos()s

50、in()rsin()sin()aaralaal圖 3-8 三棱錐簡圖figure 3-8 the structure of triangular pyramid由此，可以求得 b、c、d 在基坐標系下面的坐標。要求得靜平臺中心 a 的坐標，可以通過三菱錐 a-bcd 求得，因 b、c、d 坐標已求得，而 ab、ac、ad 的邊長也是已知的，且 ab=ac=ad。設 f 為三角形的外心，e 為 bc 的中點，那么；bcdaebc。根據(jù)立體幾何知識可知，；同理可以得到、efbcafbcafcd。所以，f 既是的外心，又是 a 在的投影。afdbbcdbcd廣東工業(yè)大學碩士學位論文18(3.2.8)

51、oaoffa (3.2.9)22244*2efefofoeefocoboeefef nbccdbcnbccdbcefbfbeabcabcbfsp papbpcabcp (3.2.10)22*fafafafa nfaabbfbccdnbccd 通過上述這些公式可以求得運動平臺中心 a 在機器人坐標系下面的坐標，這便是delta 機器人的正解。3.3 delta 機器人運動學逆解將 delta 的幾何結構再次簡化，如圖 3-9第三章 delta 機器人的運動學求解和標定19圖 3-9 delta robot 結構簡化圖figure 3-9 the structure of delta robot其

52、中 o 為靜平臺的中心，也是機器人坐標系的原點；為機器人的主動桿；iipa為平行四邊形從動桿中線；為靜平臺；為動平臺。固定平臺 3 個iiab123pp po123b b b o鉸鏈點的坐標為10：ip， =1，2，3(3.3.1) cossin()0iiiropri其中 r 為靜平臺的平面的圓半徑；為三個輸入桿與與 x 軸的夾角。輸入桿末端i點的坐標可以通過向量表示：iaioa(3.3.2)iiiioaoppa 向量可以求得：iipa ， =1，2，3.(3.3.3) iiios( ) coscos( )*sin()( )iiiilcpallsin i廣東工業(yè)大學碩士學位論文20， =1，2

53、，3(3.3.4) iiicos()os( ) cossin()cos( )*sin()( )iiiiirlcoarllsini其中 l 為輸入桿的長度，分別為三個輸入桿的輸入角。i假設動平臺中心的坐標為，那么動平臺鉸鏈點的坐標為10：o( , , )xyzoooib， =1，2，3.(3.3.5) cossin()xiiyizoroboroi其中 r 為動平臺的平面的圓半徑；為三個從動桿與與 x 軸的夾角。通過上述公i式，可以得到：(3.3.6) iiicos()os( ) coscos()sin()cos( )*sin()sin()( )iixiiiiiiiyizrlcorlaboboar

54、lorlsino 其中， 為從動桿的長度。聯(lián)立 =1，2，3 三個方程：li(3.3.7) 21111222111112222222222cos()os( ) coscos()sin()cos( )*sin()sin()( )cos()os() coscos()sin()cos()*sin()sin()()xyzixyzrlcorrlorlsinolrlcorrlorlsino 222333322233333cos()os() coscos()sin()cos()*sin()sin()()xyzlrlcorrlorlsinol由公式 3.3.7 得：(3.3.8)cossin0iiiiiijk

55、其中：（ =1，2，3）表達式為：,iiii j ki第三章 delta 機器人的運動學求解和標定21(3.3.9)222222coscoscos(sinsin)sin2(coscos)(sinsin)ixiiiyiiixiiyiiziloraraaoraraajilo zkioraraoraraoll最終可以得到：(3.3.10)2222taniiiiiiiijjikaki而公式 3.3.9 便是 delta 機器人的逆運動學的解，知道動平臺的位置，反求機器人的輸入角。3.4 傳送帶標定3.4.1 轉(zhuǎn)換矩陣和比例因子傳動帶標定（conveyor calibration）就是確定傳動帶坐標系相

56、對于機器人坐標系的位姿。設機器人坐標系為 r，傳送帶坐標系為 c，那么傳動帶坐標系相對于機器人坐標系的關系可通過轉(zhuǎn)換矩陣來表示。由平移向量和旋轉(zhuǎn)矩陣組成11。如果crtcrt知道這個轉(zhuǎn)換矩陣和物體在傳送帶坐標系下面的位置，那么就能確定該物體在機cp器人坐標系下面的位置。(3.4.1)rrccpt p傳動帶的標定實質(zhì)就是確定這個轉(zhuǎn)換矩陣和傳送帶編碼器的比例因子（encoder crtfactor） 。所謂的傳送帶編碼器比例因子就是傳送帶移動一段距離，這段距離分別在機器人坐標系和編碼器上對應了一個變化值，而這兩個變化值的比例關系即為比例因子。廣東工業(yè)大學碩士學位論文223.4.2 傳送帶的標定方法

57、1）在傳送帶上確定一點 p，移動 delta 機器人，將其末端觸碰到該點 p，記錄傳送帶的編碼器值為，機器人此時末端的位姿為（1ev1rp） 。111111,yaw ,pitch ,rollx y z2）移動傳送帶，在保證機器人可以再次觸碰到 p 的條件下，讓這段移動距離越大越好。再次移動機器人，使末端觸碰到 p 點。記錄編碼器的值為，2ev機器人末端的位姿為（） 。2rp222222,yaw ,pitch ,rollxyz3）讓機器人沿著傳送帶垂直的方向移動一段距離，記錄機器人末端的位姿（）123rp333333,yaw ,pitch ,rollxy z如圖 3-10 所示圖 3-10 傳送

58、帶標定簡圖figure 3-10 conveyor belt calibration diagram通過以上步驟之后，可以得到編碼器比因子：(3.4.2)222212121()(yy )(zz )rlxx(3.4.3)21ceelvv(3.4.4)rcclfactorl還可以求得變化矩陣。在這里，規(guī)定傳送帶移動的方向為傳送帶坐標系的 x 軸crt第三章 delta 機器人的運動學求解和標定23方向，通過空間立體的幾何建立直線方程組，再用 matlab 來計算，可以得到傳送帶坐標系原點 o在機器人坐標系下面的坐標。(3.4.5)2213123112113121231 1 21 1 322221

59、21 2323212122132232 12222 1 22 1 32231212122121211221 311 32(2)/() () () )y(o xx xx x xx y yx y yx yx y yx z zx z zx zx z zx xx yx y yx y yx y yx zx z zx z zx z zxxyyzzox yx x yx x yx x yx x yx2212312322222131231 1 21 1 31 21 23232 12222 1 22 1 3223121212221212 11221 3 11 321 3 11 3221223 1232122)/(

60、) () () )z(yx x yx x yy yy y yy z zy z zy zy z zy yy zy z zy z zy z zxxyyzzox zx x zx x zx x zx x zx x zx x zx zx x zx x zy zy212 112213 11322122222232131 23231212122)/() () () )y zy y zy y zy y zy zy y zz zz z zz zxxyyzz機器人坐標系到傳送帶坐標系的旋轉(zhuǎn)矩陣為：(3.4.6)12212112()_rrrrppppxnewpppp (3.4.7)33y_o pnewo p (3.4.8)z_newxnewynew(3.4.