機(jī)器人傳送帶動(dòng)態(tài)跟蹤

1、廣東工業(yè)大學(xué)碩士學(xué)位論文廣東工業(yè)大學(xué)碩士學(xué)位論文（工學(xué)碩士）并聯(lián) delta 機(jī)器人的傳送帶動(dòng)態(tài)抓取系統(tǒng)設(shè)計(jì)鄧明星二一四 年 六月二一四 年 六分類號(hào)： 學(xué)校代號(hào)：11845udc： 密級(jí)： 學(xué) 號(hào)：2111101011廣東工業(yè)大學(xué)碩士學(xué)位論文廣東工業(yè)大學(xué)碩士學(xué)位論文（工學(xué)碩士）并聯(lián) delta 機(jī)器人的傳送帶動(dòng)態(tài)抓取系統(tǒng)設(shè)計(jì)鄧明星 指導(dǎo)教師姓名、職稱： 劉冠峰 教授 學(xué)科(專業(yè))或領(lǐng)域名稱： 智能工業(yè)機(jī)器人 學(xué) 生 所 屬 學(xué) 院： 機(jī)電工程學(xué)院 論 文 答 辯 日 期： 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 機(jī)器人是最典型的空間三自由度移動(dòng)并聯(lián)機(jī)構(gòu)，具有運(yùn)動(dòng)速度快、定位精確、效率高、重量輕等特點(diǎn)。其在 3c 電子產(chǎn)品、食品包裝、生物制藥、3d 打印行業(yè)得到了廣泛應(yīng)用。隨著人民生活水平的提高，促使產(chǎn)品質(zhì)量的提升和需求數(shù)量的增加，間接的促使機(jī)器人技術(shù)的發(fā)展。本文主要以并聯(lián) delta 機(jī)器人為研究對(duì)象。首先介紹國(guó)內(nèi)外工業(yè)機(jī)器人的發(fā)展?fàn)顩r；接著分析了 delta 機(jī)器人的運(yùn)動(dòng)學(xué)正逆解，傳送帶標(biāo)定、相機(jī)標(biāo)定；然后提出了pid 跟蹤算法和改進(jìn)的跟蹤算法；再者給出了 conveyor administor 的軟件設(shè)計(jì)流程圖，并解決了其中幾個(gè)關(guān)鍵問題：目標(biāo)物體的篩選、排序、位置狀態(tài)

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

5、實(shí)際應(yīng)用要求，為后面的項(xiàng)目應(yīng)用現(xiàn)提供了技術(shù)支持，也為多機(jī)器人協(xié)調(diào)作業(yè)提供了一種方法。關(guān)鍵詞：并聯(lián) delta 機(jī)器人、傳送帶跟蹤、pid 算法、多機(jī)器人協(xié)同作業(yè)、數(shù)據(jù)流廣東工業(yè)大學(xué)碩士學(xué)位論文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è)大學(xué)碩士學(xué)位論文iv目錄摘要 .iabstract.ii目錄 .ivcontents.vi第一章 緒論.11.1 什么是工業(yè)機(jī)器人.11.2 工業(yè)機(jī)器人在國(guó)內(nèi)外發(fā)展?fàn)顩r.31.3 delta機(jī)器人簡(jiǎn)介.51.4 本課題研究?jī)?nèi)容和研究意義.7第二章 delta 機(jī)器人軟、硬件系統(tǒng).82.1 系統(tǒng)硬件介紹.82.2 系統(tǒng)軟件介紹.9第三章 delta 機(jī)器人

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

15、veyor administor軟件架構(gòu).385.2 目標(biāo)物體的篩選和遺漏問題.395.3 目標(biāo)物體的排序.415.4 更新目標(biāo)物體的操作狀態(tài)和坐標(biāo)狀態(tài).435.5 conveyor administor的輸出和多機(jī)器人協(xié)同.465.6 conveyor administor的交互.485.7 軟件流程圖.50結(jié)論與展望.52參考文獻(xiàn).54學(xué)位期間發(fā)表的論文.58學(xué)位論文獨(dú)創(chuàng)性聲明.59致謝.60廣東工業(yè)大學(xué)碩士學(xué)位論文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è)機(jī)器人工業(yè)機(jī)器人是一種面向工業(yè)領(lǐng)域的具有多個(gè)自由度或多個(gè)關(guān)節(jié)的特殊機(jī)器人1；一般以機(jī)械手的形式呈現(xiàn)，包括串聯(lián)的工業(yè)機(jī)器人和并聯(lián)的工業(yè)機(jī)器人，它一般由本體、控制系統(tǒng)、動(dòng)力系統(tǒng)三個(gè)基本部分。工業(yè)機(jī)器人是一種自動(dòng)完成工作的智能機(jī)器裝置，是依靠控制系統(tǒng)來(lái)控制、動(dòng)力系統(tǒng)來(lái)執(zhí)行任務(wù)的一種機(jī)器。它可以與使用者交互，通過示教的方式來(lái)學(xué)習(xí)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

48、角；r 為靜平臺(tái)所在圓的半徑。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 為輸入桿的長(zhǎng)度，、分別為輸入桿的輸入角8-9。123, 圖 3-7 delta robot 結(jié)構(gòu)簡(jiǎn)化圖figure 3-7 the structure of delta robot (3.2.5)1111222

49、23333obobb boba aocobb coba aodobb doba a 第三章 delta 機(jī)器人的運(yùn)動(dòng)學(xué)求解和標(biāo)定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 三棱錐簡(jiǎn)圖figure 3-8 the structure of triangular pyramid由此，可以求得 b、c、d 在基坐標(biāo)系下面的坐標(biāo)。要求得靜平臺(tái)中心 a 的坐標(biāo)，可以通過三菱錐 a-bcd 求得，因 b、c、d 坐標(biāo)已求得，而 ab、ac、ad 的邊長(zhǎng)也是已知的，且 ab=ac=ad。設(shè) f 為三角形的外心，e 為 bc 的中點(diǎn)，那么；bcdaebc。根據(jù)立體幾何知識(shí)可知，；同理可以得到、efbcafbcafcd。所以，f 既是的外心，又是 a 在的投影。afdbbcdbcd廣東工業(yè)大學(xué)碩士學(xué)位論文18(3.2.8)

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

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

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

54、lorlsino 其中， 為從動(dòng)桿的長(zhǎng)度。聯(lián)立 =1，2，3 三個(gè)方程：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）表達(dá)式為：,iiii j ki第三章 delta 機(jī)器人的運(yùn)動(dòng)學(xué)求解和標(biāo)定21(3.3.9)222222coscoscos(sinsin)sin2(coscos)(sinsin)ixiiiyiiixiiyiiziloraraaoraraajilo zkioraraoraraoll最終可以得到：(3.3.10)2222taniiiiiiiijjikaki而公式 3.3.9 便是 delta 機(jī)器人的逆運(yùn)動(dòng)學(xué)的解，知道動(dòng)平臺(tái)的位置，反求機(jī)器人的輸入角。3.4 傳送帶標(biāo)定3.4.1 轉(zhuǎn)換矩陣和比例因子傳動(dòng)帶標(biāo)定（conveyor calibration）就是確定傳動(dòng)帶坐標(biāo)系相

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

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

58、帶標(biāo)定簡(jiǎn)圖figure 3-10 conveyor belt calibration diagram通過以上步驟之后，可以得到編碼器比因子：(3.4.2)222212121()(yy )(zz )rlxx(3.4.3)21ceelvv(3.4.4)rcclfactorl還可以求得變化矩陣。在這里，規(guī)定傳送帶移動(dòng)的方向?yàn)閭魉蛶ё鴺?biāo)系的 x 軸crt第三章 delta 機(jī)器人的運(yùn)動(dòng)學(xué)求解和標(biāo)定23方向，通過空間立體的幾何建立直線方程組，再用 matlab 來(lái)計(jì)算，可以得到傳送帶坐標(biāo)系原點(diǎn) o在機(jī)器人坐標(biāo)系下面的坐標(biā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機(jī)器人坐標(biāo)系到傳送帶坐標(biāo)系的旋轉(zhuǎn)矩陣為：(3.4.6)12212112()_rrrrppppxnewpppp (3.4.7)33y_o pnewo p (3.4.8)z_newxnewynew(3.4.