陸偉，趙雅琪，褚云勝，黃學(xué)務(wù)

(安徽安利合成革有限公司，安徽 合肥 230601)

0 引言

由于社會(huì)發(fā)展的需要，對(duì)工業(yè)機(jī)器人使用的數(shù)量將會(huì)越來(lái)越多，這就促使工業(yè)機(jī)器人向經(jīng)濟(jì)實(shí)惠、高效、低損耗的方向發(fā)展，未來(lái)工業(yè)機(jī)器人學(xué)的重大挑戰(zhàn)就是經(jīng)濟(jì)的可行性［1］，而在工業(yè)機(jī)器人領(lǐng)域，由于機(jī)器人控制的復(fù)雜性，需要進(jìn)行大量的數(shù)學(xué)運(yùn)算以及在運(yùn)動(dòng)控制過(guò)程中需要與驅(qū)動(dòng)系統(tǒng)進(jìn)行實(shí)時(shí)的數(shù)據(jù)交換等原因，為了保證其穩(wěn)定性，這就對(duì)工業(yè)機(jī)器人控制系統(tǒng)軟件環(huán)境的操作系統(tǒng)提出了很高的要求。目前主流的工業(yè)機(jī)器人都是采用的專門定制的運(yùn)動(dòng)控制卡，加上實(shí)時(shí)操作系統(tǒng)，這樣既保證了數(shù)據(jù)的實(shí)時(shí)傳輸同時(shí)又能保證運(yùn)動(dòng)控制的精確執(zhí)行，大大提升了整個(gè)系統(tǒng)的穩(wěn)定性，從而提升機(jī)器人的性能。如ABB機(jī)器人，采用的是VxWorks+.NetFrameWork高性能穩(wěn)定的操作系統(tǒng)來(lái)保證其精確的軌跡規(guī)劃和定位控制，KUKA機(jī)器人控制軟件運(yùn)行于WindowsXP+VxWorks平臺(tái)，既可以提供良好的人機(jī)交互界面，又能提供精確的實(shí)時(shí)控制。Keba與ABB和庫(kù)卡不同，它不是機(jī)器人生產(chǎn)商，他的產(chǎn)品是工業(yè)級(jí)伺服控制系統(tǒng)，能夠?qū)崿F(xiàn)多自由度機(jī)器人的控制，其控制系統(tǒng)中通過(guò)VxWorks平臺(tái)或者Windows+RTX實(shí)時(shí)擴(kuò)展平臺(tái)保證軟件運(yùn)行環(huán)境的實(shí)時(shí)性，通過(guò)運(yùn)動(dòng)規(guī)劃和運(yùn)動(dòng)控制單元可以實(shí)現(xiàn)對(duì)總線式伺服驅(qū)動(dòng)器的控制，從而達(dá)到對(duì)機(jī)器人的精確控制。采用VxWorks實(shí)時(shí)操作系統(tǒng)來(lái)搭建機(jī)器人控制系統(tǒng)是一個(gè)很好的解決方案，然而，其代價(jià)也是昂貴的，由于實(shí)時(shí)操作系統(tǒng)的成本高，很大程度限制了國(guó)內(nèi)工業(yè)機(jī)器人產(chǎn)業(yè)化發(fā)展。

采用通用的操作系統(tǒng)由于消息處理機(jī)制的缺陷，不能滿足工業(yè)機(jī)器人在運(yùn)行的過(guò)程中高穩(wěn)定性和響應(yīng)快速性的要求，控制系統(tǒng)的上下位機(jī)之間進(jìn)行頻繁地通信的實(shí)時(shí)必然不能滿足運(yùn)動(dòng)控制的要求，從而降低了工業(yè)機(jī)器人的產(chǎn)業(yè)化的可能。一些學(xué)者從控制算法以及控制系統(tǒng)的結(jié)構(gòu)做了很大的改進(jìn)。J.S.Gu和C.W.de Silva［2］提出，工業(yè)機(jī)器人系統(tǒng)必須具備具有實(shí)時(shí)操作功能的在線控制和高級(jí)智能決策，并設(shè)計(jì)了一種實(shí)時(shí)的開(kāi)放架構(gòu)的工業(yè)機(jī)器人控制系統(tǒng)。張良安，梅江平等［3］采用Petri網(wǎng)絡(luò)建立軟件模塊調(diào)用規(guī)則模型和控制邏輯模型，避免機(jī)器人在碼垛過(guò)程中的邏輯錯(cuò)誤，從而提升機(jī)器人的碼垛效率，并通過(guò)實(shí)際的工業(yè)現(xiàn)場(chǎng)碼垛驗(yàn)證了該方法的有效性。張廣立、談世哲等［4］介紹了一個(gè)在Windows NT平臺(tái)上開(kāi)發(fā)的工業(yè)機(jī)器人控制系統(tǒng)，該系統(tǒng)可在通用的工業(yè)計(jì)算機(jī)上運(yùn)行，系統(tǒng)運(yùn)行環(huán)境為Windows加RTX實(shí)時(shí)擴(kuò)展，可保證良好的實(shí)時(shí)性;該系統(tǒng)硬件平臺(tái)為標(biāo)準(zhǔn)的PC硬件平臺(tái)，控制軟件采用模塊化設(shè)計(jì)具有良好的開(kāi)放性和可擴(kuò)展性。但是控制系統(tǒng)的軟件算法并不能脫離對(duì)實(shí)時(shí)性的要求。張廣立、付瑩等［5］開(kāi)發(fā)了可在Windows NT平臺(tái)下運(yùn)行的開(kāi)放式工業(yè)機(jī)器人控制系統(tǒng)。該系統(tǒng)采用單處理器結(jié)構(gòu)，可在通用的工業(yè)計(jì)算機(jī)和Windows NT加RTX實(shí)時(shí)擴(kuò)展環(huán)境下運(yùn)行，具有良好的實(shí)時(shí)性和友好的圖形用戶接口。雖然提供了解決控制系統(tǒng)的可擴(kuò)展性，但是不能解決系統(tǒng)對(duì)實(shí)時(shí)性的要求。田茂勝，唐小琦等［6］以嵌入式工業(yè)PC為硬件平臺(tái)，Linux搭載實(shí)時(shí)RT核的操作系統(tǒng)為軟件平臺(tái)，采用模塊化的軟件設(shè)計(jì)方法，設(shè)計(jì)了工業(yè)機(jī)器人開(kāi)放式控制系統(tǒng)來(lái)實(shí)現(xiàn)信號(hào)交互。Hang Thu Tran等［7］提出了一種實(shí)時(shí)的PC平臺(tái)的實(shí)時(shí)機(jī)器人學(xué)控制器，該控制器能夠方便的被用戶使用，并實(shí)現(xiàn)了對(duì)二自由度拾放(pick-and-place)機(jī)器人的控制，該控制器能夠使用戶方便的實(shí)現(xiàn)其合理復(fù)雜度的控制算法。這些工作從一定程度上能夠解決工業(yè)機(jī)器人在實(shí)際應(yīng)用過(guò)程中的操作系統(tǒng)問(wèn)題，但是不能解決對(duì)實(shí)時(shí)操作系統(tǒng)的依賴性。由于采用實(shí)時(shí)操作系統(tǒng)來(lái)搭建機(jī)器人控制系統(tǒng)的代價(jià)是昂貴的，如Vx-Works是Wind River System公司的一款實(shí)時(shí)操作系統(tǒng)，其價(jià)格達(dá)到數(shù)十萬(wàn)元甚至幾十萬(wàn)元［8］。所以提出一種脫離主機(jī)，減少機(jī)器人控制對(duì)操作系統(tǒng)依賴性的控制策略將會(huì)對(duì)工業(yè)機(jī)器人產(chǎn)業(yè)化進(jìn)程產(chǎn)生積極的意義。而泛型編程(Generic Programming)理論的發(fā)展使得跨語(yǔ)言編程成為可能，它支持不同數(shù)據(jù)類型的快速編程［9］，這是使得字符特征的提取成為可能，通過(guò)對(duì)語(yǔ)法規(guī)則的構(gòu)建，利用提取的語(yǔ)句特征進(jìn)行程序合成，實(shí)現(xiàn)跨語(yǔ)言自動(dòng)編程。

本文提出的自動(dòng)編程技術(shù)解決工業(yè)機(jī)器人控制系統(tǒng)軟件系統(tǒng)對(duì)操作系統(tǒng)的依賴性的策略正是以這種以泛型編程理論為基礎(chǔ)的自動(dòng)編程技術(shù)。通過(guò)自動(dòng)編程技術(shù)，示教系統(tǒng)可以動(dòng)態(tài)加載運(yùn)動(dòng)控制程序至下位運(yùn)動(dòng)控制器，使得下位運(yùn)動(dòng)控制器來(lái)管理機(jī)器人運(yùn)動(dòng)控制，在機(jī)器人運(yùn)動(dòng)的過(guò)程中，主機(jī)可以和控制器進(jìn)行動(dòng)態(tài)的通信，從而實(shí)現(xiàn)運(yùn)動(dòng)控制系統(tǒng)既不受限于主機(jī)的運(yùn)動(dòng)數(shù)據(jù)，也能響應(yīng)主機(jī)的基本控制指令，如停止，參數(shù)監(jiān)測(cè)等。

1 分層控制系統(tǒng)(Layered Control System)

分層控制系統(tǒng)是指工業(yè)機(jī)器人控制系統(tǒng)采用分層控制的思想，分成決策層和物理層兩個(gè)獨(dú)立的系統(tǒng)，決策層完成運(yùn)動(dòng)學(xué)計(jì)算以及動(dòng)力學(xué)模型估計(jì)，物理層作為運(yùn)動(dòng)的執(zhí)行部分，負(fù)責(zé)執(zhí)行決策層的決策結(jié)果［10］。采用分層控制可以降低系統(tǒng)功能模塊之間的耦合性，提高控制系統(tǒng)穩(wěn)定性和可修改性。該系統(tǒng)分層控制體現(xiàn)在采用自動(dòng)編程技術(shù)的示教系統(tǒng)和帶有可編程運(yùn)動(dòng)控制器的運(yùn)動(dòng)控制系統(tǒng)。

1.1 采用自動(dòng)編程技術(shù)的示教系統(tǒng)

示教系統(tǒng)采用的是工業(yè)嵌入式PC加載Windows操作系統(tǒng)，和底層運(yùn)動(dòng)控制器采用Ethernet高速通信，

并且提供友好的示教界面。該部分主要實(shí)現(xiàn)兩個(gè)功能，機(jī)器人末端位置坐標(biāo)記錄和自動(dòng)生成運(yùn)動(dòng)控制程序。

1.1.1 機(jī)器人末端位置記錄

機(jī)器人的末端位置記錄是通過(guò)電機(jī)位置編碼器反饋回來(lái)機(jī)器人各個(gè)軸當(dāng)前角度來(lái)進(jìn)行正運(yùn)動(dòng)學(xué)計(jì)算獲得機(jī)器人末端位置的空間坐標(biāo)和姿態(tài)，并以機(jī)器人語(yǔ)言的方式進(jìn)行記錄，產(chǎn)生標(biāo)準(zhǔn)化的機(jī)器人語(yǔ)言的運(yùn)動(dòng)程序。

末端位置記錄是為了方便機(jī)器人示教重現(xiàn)的過(guò)程，根據(jù)示教記錄的空間點(diǎn)坐標(biāo)，運(yùn)用正逆運(yùn)動(dòng)學(xué)進(jìn)行軌跡規(guī)劃和速度規(guī)劃，產(chǎn)生各個(gè)軸轉(zhuǎn)動(dòng)的角度，進(jìn)而驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)。

1.1.2 自動(dòng)編程技術(shù)

自動(dòng)編程技術(shù)主要是通過(guò)軟件算法來(lái)實(shí)現(xiàn)對(duì)機(jī)器人語(yǔ)言的運(yùn)動(dòng)學(xué)程序自動(dòng)翻譯生成運(yùn)動(dòng)控制卡語(yǔ)言的運(yùn)動(dòng)程序，自動(dòng)生成的程序嚴(yán)格的符合運(yùn)動(dòng)控制器的編程語(yǔ)法。在控制的過(guò)程中，我們不采用實(shí)時(shí)操作系統(tǒng)，基于windowsXP的示教系統(tǒng)必須具有動(dòng)態(tài)加載運(yùn)動(dòng)程序的能力，才能保證機(jī)器人運(yùn)動(dòng)正確運(yùn)行。自動(dòng)編程技術(shù)是實(shí)現(xiàn)工業(yè)機(jī)器人示教過(guò)程重現(xiàn)的關(guān)鍵技術(shù)，采用自動(dòng)編程技術(shù)，可以對(duì)機(jī)器人示教系統(tǒng)產(chǎn)生的點(diǎn)進(jìn)行程序合成，動(dòng)態(tài)下載至運(yùn)動(dòng)控制卡中，從而避免了傳統(tǒng)工業(yè)機(jī)器人實(shí)時(shí)加載運(yùn)動(dòng)數(shù)據(jù)的過(guò)程，即機(jī)器人的運(yùn)動(dòng)控制由底層的運(yùn)動(dòng)控制器來(lái)管理，實(shí)現(xiàn)運(yùn)動(dòng)控制脫離主機(jī)而獨(dú)立運(yùn)行。圖1所示為自動(dòng)編程技術(shù)算法結(jié)構(gòu)流程。從算法的結(jié)構(gòu)圖可以看出，核心部分是機(jī)器人語(yǔ)言程序語(yǔ)句處理部分，采用泛型編程理論和機(jī)器人語(yǔ)言語(yǔ)法規(guī)則，提取出相關(guān)參數(shù)進(jìn)行存儲(chǔ)并進(jìn)行分類。具體做法是根據(jù)泛型變成理論，定義特征標(biāo)志結(jié)構(gòu)體和容器變量，將語(yǔ)句解析過(guò)程中所獲的特征標(biāo)志賦值給特征結(jié)構(gòu)體變量，將結(jié)構(gòu)體存入容器變量。

圖1 基于泛型編程理論的自動(dòng)編程技術(shù)框圖

其語(yǔ)句合成部分則按照運(yùn)動(dòng)控制卡語(yǔ)法進(jìn)行合成，通過(guò)前面提取的運(yùn)動(dòng)學(xué)參數(shù)，包括坐標(biāo)，插補(bǔ)方式，端口號(hào)和端口操作類型，根據(jù)事先定制好的運(yùn)動(dòng)控制卡運(yùn)動(dòng)學(xué)語(yǔ)法規(guī)則，對(duì)獲得的參數(shù)進(jìn)行語(yǔ)句合成，生成可執(zhí)行文件，直接加載到運(yùn)動(dòng)控制卡中，使得機(jī)器人脫離主機(jī)，在只有運(yùn)動(dòng)控制卡參與控制的情況下進(jìn)行示教重現(xiàn)運(yùn)動(dòng)。

1.2 運(yùn)動(dòng)控制系統(tǒng)

運(yùn)動(dòng)控制系統(tǒng)采用的是運(yùn)動(dòng)控制卡搭載伺服系統(tǒng)來(lái)構(gòu)建，由于運(yùn)動(dòng)控制卡是一個(gè)獨(dú)立的運(yùn)動(dòng)控制器，它在工作的過(guò)程中不需要主機(jī)的參與，同時(shí)能實(shí)時(shí)的與主機(jī)進(jìn)行通信，接收和響應(yīng)主機(jī)的命令，從而在運(yùn)動(dòng)的同時(shí)，能夠執(zhí)行主機(jī)的命令操作。該部分的運(yùn)動(dòng)控制卡采用支持加載運(yùn)動(dòng)程序的卡，即能通過(guò)上位機(jī)動(dòng)態(tài)加載運(yùn)動(dòng)控制程序。

2 測(cè)試平臺(tái)(Testing Platform)

測(cè)試平臺(tái)是采用上述設(shè)計(jì)的機(jī)器人控制系統(tǒng)搭建工業(yè)現(xiàn)場(chǎng)使用的碼垛機(jī)器人控制系統(tǒng)，根據(jù)實(shí)際生產(chǎn)的需要，編寫了機(jī)器人示教器軟件界面。在使用運(yùn)動(dòng)控制卡之前，先要將我們實(shí)現(xiàn)設(shè)計(jì)好的運(yùn)動(dòng)控制算法寫入卡里，包括正逆運(yùn)動(dòng)學(xué)求解，并且使得運(yùn)動(dòng)控制卡工作在獨(dú)立的坐標(biāo)系模式下。

PMAC(Programmable Multi-Axis Controller)是美國(guó) Delta Tau Data System公司推出的PC機(jī)平臺(tái)上的運(yùn)動(dòng)控制器，是一個(gè)完全開(kāi)放的系統(tǒng)。采用PMAC運(yùn)動(dòng)控制卡搭建機(jī)器人運(yùn)動(dòng)控制系統(tǒng)具有一定的優(yōu)勢(shì)，能夠?qū)崿F(xiàn)運(yùn)動(dòng)程序的自動(dòng)加載與運(yùn)行，通過(guò)VC++開(kāi)發(fā)環(huán)境中MFC基礎(chǔ)類開(kāi)發(fā)可視化人機(jī)界面，并且根據(jù)運(yùn)動(dòng)控制卡提供接口函數(shù)對(duì)功能函數(shù)的封裝。

圖2所示的是采用PMAC運(yùn)動(dòng)控制卡搭建機(jī)器人控制系統(tǒng)的硬件結(jié)構(gòu)圖，整個(gè)系統(tǒng)分三層，即會(huì)話層，決策層和物理層，通過(guò)分層將運(yùn)動(dòng)控制系統(tǒng)進(jìn)行模塊化設(shè)計(jì)，回話層主要是指示教系統(tǒng)，采用嵌入式PC搭載工業(yè)觸摸屏，構(gòu)成嵌入式示教系統(tǒng)，決策層主要是指PMAC運(yùn)動(dòng)控制器，負(fù)責(zé)信息采集和命令決策，物理層采用安川伺服系統(tǒng)構(gòu)建運(yùn)動(dòng)控制執(zhí)行部分。

圖2 PMAC運(yùn)動(dòng)控制系統(tǒng)結(jié)構(gòu)

由PMAC運(yùn)動(dòng)控制卡和電機(jī)伺服驅(qū)動(dòng)搭建的運(yùn)動(dòng)控制系統(tǒng)既能獨(dú)立的運(yùn)行，也能實(shí)時(shí)的和主機(jī)進(jìn)行通信，并支持多線程操作，保證了機(jī)器人在獨(dú)立運(yùn)行的同時(shí)，能夠?qū)崟r(shí)的對(duì)主機(jī)的命令進(jìn)行響應(yīng)。根據(jù)合成的運(yùn)動(dòng)控制程序，PMAC管理運(yùn)動(dòng)學(xué)參數(shù)，包括正逆解，以及路徑規(guī)劃。由于PMAC運(yùn)動(dòng)控制器和主機(jī)之間的通信方式采用Ethernet高速通信模式，從而更加保證了整個(gè)系統(tǒng)的可靠性。

示教操作是上位示教系統(tǒng)的核心，采用逐點(diǎn)輸入模式加載運(yùn)動(dòng)控制數(shù)據(jù)，支持多坐標(biāo)系切換。在手動(dòng)運(yùn)行情況下，移動(dòng)機(jī)器人末端，通過(guò)記錄按鈕，記錄當(dāng)前機(jī)器人末端坐標(biāo)。通過(guò)示教器界面的編譯按鈕，可以對(duì)示教生成的程序進(jìn)行編譯生成PMAC運(yùn)動(dòng)控制語(yǔ)言。并生成control.pmc文件。圖3所示的自動(dòng)編程技術(shù)翻譯的程序過(guò)程。

圖3 自動(dòng)編程技術(shù)程序解析

由于在初始化的過(guò)程中，將運(yùn)動(dòng)控制算法下載到卡里，這就使得機(jī)器人在運(yùn)動(dòng)控制時(shí)，按照自動(dòng)編譯成的運(yùn)控制程序進(jìn)行運(yùn)行，不需要進(jìn)行上位進(jìn)行實(shí)時(shí)數(shù)據(jù)交換，使得運(yùn)動(dòng)控制部分脫離主機(jī)，將機(jī)器人放置工業(yè)現(xiàn)場(chǎng)，對(duì)其進(jìn)行碼垛測(cè)試，根據(jù)機(jī)構(gòu)設(shè)計(jì)和控制系統(tǒng)分析的結(jié)果，采用NDI三維動(dòng)態(tài)位移測(cè)量系統(tǒng)，標(biāo)定機(jī)器人重復(fù)定位精度，并測(cè)試機(jī)器人D-H參數(shù)，進(jìn)行機(jī)器人定位精度校核，根據(jù)生產(chǎn)搬運(yùn)實(shí)際作業(yè)要求，示教產(chǎn)生實(shí)際的運(yùn)動(dòng)路徑，進(jìn)行機(jī)器人實(shí)際搬運(yùn)作業(yè)性能試驗(yàn)，以獲得樣機(jī)性能測(cè)試數(shù)據(jù)。

經(jīng)過(guò)碼垛測(cè)試，從機(jī)器人碼垛的負(fù)重，工作半徑，工作節(jié)拍以及重復(fù)定位精度來(lái)與國(guó)內(nèi)外工業(yè)機(jī)器人進(jìn)行比較。表1所示的是該機(jī)器人與國(guó)內(nèi)外先進(jìn)機(jī)器人性能之間的比較，通過(guò)比較可得，在性能上與國(guó)外的機(jī)器人(ABB，OTC)達(dá)到同等水平，重復(fù)精度達(dá)到±0.3 mm，工作節(jié)拍能力相當(dāng)，搬運(yùn)速度較快。

表1 機(jī)器人實(shí)驗(yàn)數(shù)據(jù)對(duì)比

3 結(jié)束語(yǔ)

采用自動(dòng)編程技術(shù)工業(yè)機(jī)器人示教系統(tǒng)可以自動(dòng)進(jìn)行跨語(yǔ)言平臺(tái)進(jìn)行自動(dòng)編程，有效的實(shí)現(xiàn)運(yùn)動(dòng)控制程序的動(dòng)態(tài)加載與執(zhí)行，從而減少底層運(yùn)動(dòng)控制器和主機(jī)之間進(jìn)行數(shù)據(jù)交換。降低控制系統(tǒng)對(duì)操作系統(tǒng)實(shí)時(shí)性的要求，從而降低工業(yè)機(jī)器人生產(chǎn)成本。

采用分層控制思想，將工業(yè)機(jī)器人的示教系統(tǒng)和運(yùn)動(dòng)控制進(jìn)行分層設(shè)計(jì)，降低示教系統(tǒng)和底層運(yùn)動(dòng)控制之間的耦合，增強(qiáng)系統(tǒng)軟件的可修改性和可擴(kuò)展性，同時(shí)提高系統(tǒng)的穩(wěn)定性。

［1］TORGNY B，Present and future robot control development—An industrial perspective［J］.Annual Reviews in Control，2007(31)31:69-79.

［2］GU J S，SILVA C W，Development and implementation of a real-time open-architecture control system for industrial robot systems［J］.Engineering Applications of Artificial Intelligence，2004，17:469-483.

［3］張良安，梅江平，黃田.基于 Petri網(wǎng)絡(luò)的碼垛機(jī)器人生產(chǎn)線控制軟件設(shè)計(jì)方法［J］.天津大學(xué)學(xué)報(bào)，2011，44(1):23-28.

［4］張廣立，談世哲，楊汝清.基于WindowsNT的開(kāi)放式機(jī)器人控制系統(tǒng)［J］.機(jī)器人，2002，24(5):443-446.

［5］張廣立，付瑩，楊汝清，等.基于Windows NT的開(kāi)放式機(jī)器人實(shí)時(shí)控制系統(tǒng)［J］.機(jī)器人，2003(5):724-728.

［6］田茂勝，唐小琦，孟國(guó)軍，等.基于嵌入式PC的工業(yè)機(jī)器人開(kāi)放式控制系統(tǒng)交互控制的實(shí)現(xiàn)［J］.計(jì)算機(jī)應(yīng)用，2010，11(30):3087-3090.

［7］HANG T T，AMIR KHAJEPOUR，KAAN ERKORKMAZ.Development and analysis of a PC-based object-oriented real-time robotics controller［C］//［CD］Proceedings of the 2005 IEEE Conference on Control Applications.Toronto，Canada，CCA.2005:1379-1384.

［8］SHAO B B，Open resources of RTOS for embedded systems［OL］//Conference of Nuclear Electronics＆Detection2002-12-02/2002-12-04.http://wenku.baidu.com/view/2c25a1f49e31433239689352.html.

［9］奧斯騰.泛型編程和 STL(侯捷譯)［M］.北京:中國(guó)電力出版社，2003.

［10］FANG J，ZHAO J H，HE F，et al，Design and research of three-layers open architecture model for industrial robot software system，proceedings of 2013 IEEE International conference on mechatronics and automation，104-109［C］，Takamatsu，IEEE:2013.