本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，特別是涉及一種基于ros和orocos的機(jī)器人控制的通信方法和系統(tǒng)。

背景技術(shù)：

ros(robotoperatingsystem)是開源的機(jī)器人操作系統(tǒng)，可以為機(jī)器人開發(fā)者提供一個標(biāo)準(zhǔn)化的、開源的編程框架。但是ros目前不支持實時線程操作。orocos(openrobotcontrolsoftware)也是一種開源的機(jī)器人控制軟件編程框架，它的特點是支持實時的線程操作，但是它的開放性，通用性沒有ros好。

因此，目前在一些應(yīng)用方案中，在ros上采用orocos，但現(xiàn)有技術(shù)方案在搭建的架構(gòu)上，不能構(gòu)成一個完整的機(jī)器人控制器軟件，無法充分利用ros和orocos的特性，系統(tǒng)開發(fā)成本高，控制效果差。

技術(shù)實現(xiàn)要素：

基于此，有必要針對上述技術(shù)問題，提供一種基于ros和orocos的機(jī)器人控制的通信方法和系統(tǒng)，降低系統(tǒng)開發(fā)成本，提高控制效果。

一種基于ros和orocos的機(jī)器人控制的通信方法，包括：

通過第二接口接收算法處理程序發(fā)送的控制指令數(shù)據(jù)；

將所述控制指令數(shù)據(jù)保存到電機(jī)指令緩沖隊列中；

根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量，從電機(jī)指令緩沖隊列中選擇控制指令數(shù)據(jù)輸出至所述控制主站；

其中，所述控制指令數(shù)據(jù)是算法處理程序的機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)根據(jù)控制指令計算得到，所述控制指令由控制器管理程序從接收端進(jìn)行接收，所述控制器管理程序的控制指令觸發(fā)函數(shù)通過調(diào)用算法處理的所述響應(yīng)函數(shù)，所述第二接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口，用于與通信管理程序進(jìn)行通信連接。

一種基于ros和orocos的機(jī)器人控制的通信系統(tǒng)，包括：

接收單元，用于通過第二接口接收算法處理程序發(fā)送的控制指令數(shù)據(jù)；

緩存單元，用于將所述控制指令數(shù)據(jù)保存到電機(jī)指令緩沖隊列中；

發(fā)送單元，用于根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量，從電機(jī)指令緩沖隊列中選擇控制指令數(shù)據(jù)輸出至所述控制主站；

其中，所述控制指令數(shù)據(jù)是算法處理程序的機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)根據(jù)控制指令計算得到，所述控制指令由控制器管理程序從接收端進(jìn)行接收，所述控制器管理程序的控制指令觸發(fā)函數(shù)通過調(diào)用算法處理的所述響應(yīng)函數(shù)，所述第二接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口，用于與通信管理程序進(jìn)行通信連接。

上述基于ros和orocos的機(jī)器人控制的通信方法和系統(tǒng)，利用orocos的實時輸入/輸出接口與控制器管理程序和算法處理程序進(jìn)行通信，由控制器管理程序接收控制指令，通過控制指令觸發(fā)函數(shù)，調(diào)用算法處理程序的機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)計算控制指令數(shù)據(jù)，對控制指令數(shù)據(jù)進(jìn)行緩存后轉(zhuǎn)發(fā)至機(jī)器人，實現(xiàn)控制指令的接收分發(fā)、解析和傳輸，可以構(gòu)成一個完整的機(jī)器人控制器軟件，降低機(jī)器人控制系統(tǒng)開發(fā)成本，提高控制效果。

附圖說明

圖1是本發(fā)明實施例的基于ros和orocos的機(jī)器人控制的通信方法流程圖；

圖2是一種機(jī)器人控制系統(tǒng)的硬件結(jié)構(gòu)模型；

圖3是基于ros和orocos的機(jī)器人控制方法搭建的軟件架構(gòu)圖；

圖4是控制器狀態(tài)機(jī)的狀態(tài)變化示意圖；

圖5是設(shè)備狀態(tài)機(jī)的狀態(tài)變化示意圖；

圖6是本發(fā)明實施例的基于ros和orocos的機(jī)器人控制的通信結(jié)構(gòu)示意圖。

具體實施方式

下面結(jié)合附圖闡述本發(fā)明的基于ros和orocos的機(jī)器人控制的通信方法和系統(tǒng)的實施例。

參考圖1所示，圖1是本發(fā)明實施例的基于ros和orocos的機(jī)器人控制的通信方法流程圖，包括：

s101，通過第二接口接收算法處理程序發(fā)送的控制指令數(shù)據(jù)；其中，所述控制指令數(shù)據(jù)是算法處理程序的機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)根據(jù)控制指令計算得到，所述控制指令由控制器管理程序從接收端進(jìn)行接收，所述控制器管理程序的控制指令觸發(fā)函數(shù)通過調(diào)用算法處理的所述響應(yīng)函數(shù)；

對于控制器管理程序，可以通過ice開發(fā)的通信協(xié)議建立與控制端的通信連接，通過調(diào)用所述通信協(xié)議的動態(tài)異步遠(yuǎn)程過程調(diào)用方法輸入的控制指令；

在一個實施例中，所述控制器管理程序的控制指令觸發(fā)函數(shù)可以根據(jù)控制指令的類型，通過第一接口調(diào)用算法處理程序相應(yīng)類型的響應(yīng)函數(shù)；每種機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)對應(yīng)控制器管理程序的一個控制指令觸發(fā)函數(shù)；所述第一接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口，用于算法處理程序與控制器管理程序進(jìn)行通信連接。

所述第二接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口，用于與通信管理程序進(jìn)行通信連接。。

s102，將所述控制指令數(shù)據(jù)保存到電機(jī)指令緩沖隊列中；在此，可以按照順序?qū)⒖刂浦噶顢?shù)據(jù)存儲到隊列中。

s103，根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量，從電機(jī)指令緩沖隊列中選擇控制指令數(shù)據(jù)輸出至所述控制主站；

在此，控制主站可以是canopen主站，canopen是一種架構(gòu)在控制局域網(wǎng)路(controllerareanetwork，can)上的高層通信協(xié)協(xié)議，包括通信子協(xié)議及設(shè)備子協(xié)議。

上述實施例的技術(shù)方案，利用orocos的實時輸入/輸出接口與控制器管理程序和算法處理程序進(jìn)行通信，由控制器管理程序接收控制指令，通過控制指令觸發(fā)函數(shù)，調(diào)用算法處理程序的機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)計算控制指令數(shù)據(jù)，對控制指令數(shù)據(jù)進(jìn)行緩存后轉(zhuǎn)發(fā)至機(jī)器人，實現(xiàn)控制指令的接收分發(fā)、解析和傳輸，可以構(gòu)成一個完整的機(jī)器人控制器軟件，降低機(jī)器人控制系統(tǒng)開發(fā)成本，提高控制效果。

在一個實施例中，所述通信方法，還可以通過第二接口向算法處理程序發(fā)送電機(jī)運行數(shù)據(jù)，通過所述第三接口向控制器管理程序發(fā)送反饋信息；其中，所述反饋信息包括事件報告信息、機(jī)器人電機(jī)運行的診斷數(shù)據(jù)；所述第三接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口，用于與控制器管理程序進(jìn)行通信連接。

在一個實施例中，在接收算法處理程序發(fā)送的控制指令數(shù)據(jù)前，還可以進(jìn)行初始化處理，具體可以如下：

(1)檢測歷史運行所記錄的日志報告，若日志報告正常，將相關(guān)信息通過事件報告接口傳遞給控制器管理程序；若日志報告異常，則退出。

(2)電機(jī)驅(qū)動的初始化：

建立與電機(jī)驅(qū)動器的通信連接，將電機(jī)指令緩沖隊列清空；

使能電機(jī)，若使能成功，讀取電機(jī)的位置信息，根據(jù)位置信息計算機(jī)器人的機(jī)械臂的狀態(tài)信息；其中，所述狀態(tài)信息包括關(guān)節(jié)角度和機(jī)械臂的末端位姿；

(3)機(jī)械臂的初始化：

若機(jī)械臂任一關(guān)節(jié)角度與零度相差大于設(shè)定值，則調(diào)用點到點運動規(guī)劃，根據(jù)所述末端位姿對機(jī)械臂進(jìn)行回零運動規(guī)劃；

具體的，可以依據(jù)電機(jī)的位置信息，計算機(jī)械臂任一關(guān)節(jié)角度與零度相差是否大于0.01度，若是，則執(zhí)行回零運動，可以調(diào)用點到點運動規(guī)劃，對回零運動進(jìn)行規(guī)劃。

作為實施例，在本發(fā)明實施例的基于ros和orocos的機(jī)器人控制的通信方法，可以采用設(shè)備狀態(tài)機(jī)來進(jìn)行相關(guān)狀態(tài)的控制，其中，所述設(shè)備狀態(tài)機(jī)設(shè)有初始化、中斷、暫停、恢復(fù)、手動示教和運轉(zhuǎn)六種狀態(tài)；所述使能為暫停、恢復(fù)、手動示教和運轉(zhuǎn)四種狀態(tài)的合集狀態(tài)；使能狀態(tài)的轉(zhuǎn)移規(guī)則，對四個子狀態(tài)均有效。

基于設(shè)備狀態(tài)機(jī)，所述方法可以如下：

(1)根據(jù)用戶設(shè)定的頻率(如1khz)讀取設(shè)備狀態(tài)機(jī)的狀態(tài)信息；

(2)根據(jù)所述狀態(tài)信息執(zhí)行控制指令數(shù)據(jù)的發(fā)送操作；具體可以包括如下幾種情況：

a)若設(shè)備狀態(tài)機(jī)是初始化狀態(tài)，執(zhí)行機(jī)械臂的回零運動規(guī)劃；

根據(jù)回零運動規(guī)劃的軌跡，計算電機(jī)的控制指令數(shù)據(jù)，并將該控制指令數(shù)據(jù)發(fā)送給控制主站；

在回零成功，向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件，將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運轉(zhuǎn)狀態(tài)，并將該事件報告反饋至控制器管理程序。

b)若設(shè)備狀態(tài)機(jī)是運轉(zhuǎn)狀態(tài)，從控制指令數(shù)據(jù)的輸入通道中，讀取控制指令數(shù)據(jù)，并存儲到電機(jī)指令緩沖隊列中；

讀取控制主站的指令緩沖區(qū)的現(xiàn)有控制指令數(shù)據(jù)的數(shù)量，根據(jù)該數(shù)量大小從電機(jī)指令緩沖隊列中取出指定數(shù)量的控制指令數(shù)據(jù)發(fā)送至控制主站。

c)若設(shè)備狀態(tài)機(jī)是恢復(fù)狀態(tài)，向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件，將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運轉(zhuǎn)狀態(tài)，并將該事件報告反饋至控制器管理程序；若恢復(fù)不成功，則向設(shè)備狀態(tài)機(jī)發(fā)送中斷事件，狀態(tài)機(jī)轉(zhuǎn)換為中斷狀態(tài)，將該事件報告反饋至控制器管理程序，并退出；

d)若設(shè)備狀態(tài)機(jī)是暫停狀態(tài)，檢測控制指令數(shù)據(jù)的輸入通道中是否有新的控制指令數(shù)據(jù)，若是，則讀取控制指令數(shù)據(jù)并存儲到電機(jī)指令緩沖隊列中；

e)若設(shè)備狀態(tài)機(jī)是手動示教狀態(tài)，根據(jù)手動示教的運動規(guī)劃軌跡，計算該時刻電機(jī)的控制指令數(shù)據(jù)，并將該單條控制指令數(shù)據(jù)發(fā)送至控制主站。

(3)讀取電機(jī)狀態(tài)，計算機(jī)械臂關(guān)節(jié)以及末端運動狀態(tài)信息，并反饋至算法處理程序與控制器管理程序；

(4)檢測控制主站的診斷信息，將該診斷信息反饋至算法處理程序與控制器管理程序，將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運轉(zhuǎn)狀態(tài)，并將該事件報告發(fā)送至控制器管理程序。

另外，如果機(jī)器人的機(jī)械臂需要執(zhí)行回零運動規(guī)劃，則控制設(shè)備狀態(tài)機(jī)保持初始化狀態(tài)不變；否則，向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件，將設(shè)備狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)換為運轉(zhuǎn)狀態(tài)。

本發(fā)明實施例中，所述機(jī)器人運動規(guī)劃可以包括：點到點運動，直線運動，圓弧運動，手動示教，回到原點、暫停、恢復(fù)、急停等算法。

對于第二接口和第三接口，是指分別用于與控制器管理程序和用于與算法處理程序進(jìn)行通信的接口；按照輸入/輸出接口方式劃分，第二接口包括向算法處理程序的輸入/輸出接口，第三接口包括向控制器管理程序的輸入/輸出接口。

可以使用ros的orocreate-catkin-pkg方法創(chuàng)建為ros的package，并在package中繼承orocos的rtt：：taskcontext類，利用orocos的rtt：：input與rtt：：output方法定義實時輸入/輸出接口。

為了更加清晰本發(fā)明的實施例的技術(shù)方案，下面闡述采用本發(fā)明的方法實現(xiàn)的實例。

本實例中，硬件和軟件環(huán)境可以如下：

參考圖2所示，圖2是一種機(jī)器人控制系統(tǒng)的硬件結(jié)構(gòu)模型，在機(jī)器人控制器的搭建軟件架構(gòu)，運行于linux操作系統(tǒng)，該linux主機(jī)可以是x86架構(gòu)的pc機(jī)，或者arm芯片嵌入式架構(gòu)的開發(fā)板，控制器管理程序接入控制端的人機(jī)交互界面的控制指令。

linux主機(jī)安裝如下軟件：安裝有xenomai或者rtai的實時內(nèi)核補丁；然后安裝ros，orocos，rfsm等軟件。

參考圖3所示，圖3是基于ros和orocos的機(jī)器人控制方法搭建的軟件架構(gòu)圖；在控制過程中，操作系統(tǒng)上運行控制器管理程序、算法處理程序和通信管理程序；通信管理程序?qū)?yīng)運行本發(fā)明的基于ros和orocos的機(jī)器人控制的通信方法。

1、對于控制器管理程序：

(1)控制器管理程序使用ros的orocreate-catkin-pkg方法創(chuàng)建ros的package，記為ec_control_system，然后在package中，通過繼承orocos的rtt：：taskcontext類，記為ec_control_system_component。

在ec_control_system_component類的構(gòu)造函數(shù)中，設(shè)置為執(zhí)行如下操作：

a)利用orocos的rtt：：input與rtt：：output方法，對輸入/輸出接口進(jìn)行定義。

其中輸入接口包括：

①通信管理程序傳入的診斷數(shù)據(jù)；

②通信管理程序傳入的狀態(tài)反饋信息：包括電機(jī)運行狀態(tài)等等；

③控制器狀態(tài)機(jī)的狀態(tài)；

輸出接口包括：

①控制器狀態(tài)機(jī)事件觸發(fā)，輸出給控制器狀態(tài)機(jī)。

b)利用orocos的operationalcaller方法設(shè)置函數(shù)調(diào)用接口。

第一，設(shè)置事件報告的回調(diào)函數(shù)：對事件報告處理請求進(jìn)行響應(yīng)，包括生成錯誤的時間戳，事件級別等信息，并將事件信息，發(fā)送至控制端的人機(jī)交互界面顯示。

第二，設(shè)置警報設(shè)置的回調(diào)函數(shù)：依據(jù)診斷信息，判斷是否生成警報；例如機(jī)械臂的位置、速度、加速度是否超限等等。

第三，設(shè)置各種運動規(guī)劃的控制指令觸發(fā)函數(shù)，這些控制指令觸發(fā)函數(shù)對算法處理程序的相應(yīng)響應(yīng)函數(shù)進(jìn)行調(diào)用，包括：

①點到點運動；

②回到原點；

③直線運動；

④圓弧運動；

⑤暫停；

⑥恢復(fù)；

⑦急停；

⑧手動示教。

c)調(diào)用orocos的properties方法定義控制器管理程序的屬性，將控制器管理程序定義一個機(jī)械臂關(guān)節(jié)個數(shù)的屬性。

(2)初始化處理函數(shù)：在ec_control_system_component的starthook()成員函數(shù)中，設(shè)置執(zhí)行如下操作：

a)檢查日志報告是否正常，若異常直接退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序處理；

b)通過ice開發(fā)的通信協(xié)議建立與人機(jī)交互界面的通信連接，并調(diào)用通信協(xié)議提供的動態(tài)異步遠(yuǎn)程過程調(diào)用(rpc)方法，對人機(jī)交互界面發(fā)起的控制指令進(jìn)行響應(yīng)的回調(diào)函數(shù)進(jìn)行綁定。該回調(diào)函數(shù)，首先根據(jù)ice(theinternetcommunicationsengine，互聯(lián)網(wǎng)通信引擎)提供的遠(yuǎn)程過程異步調(diào)用方法傳入的第一個參數(shù)，判斷調(diào)用類型，然后依據(jù)此類型選擇調(diào)用相應(yīng)運動規(guī)劃的控制指令觸發(fā)函數(shù)，包括：恢復(fù)，暫停，手動示教，回到原點，點到點運動，直線運動，圓弧運動，急停。

(3)清除處理函數(shù)：對于ec_control_system_component的cleanuphook()成員函數(shù)，為了使得該函數(shù)在控制器管理程序結(jié)束運行時，實現(xiàn)自動調(diào)用，還可以設(shè)置為執(zhí)行如下操作：

a)調(diào)用ice開發(fā)的通信協(xié)議接口，關(guān)閉與人機(jī)交互界面的通信連接。

(4)對于控制器狀態(tài)機(jī)，參考圖4所示，圖4是控制器狀態(tài)機(jī)的狀態(tài)變化示意圖；可以設(shè)置init、ready、fault、active.recovery、active.halt、active.hands、active.tozero、active.ptp、active.line、active.circle、active.stop共十一個狀態(tài)，分別代表初始化、等待指令輸入、恢復(fù)、暫停、手動示教、回到原點、點到點運動、直線運動、圓弧運動、急停狀態(tài)。其中，active.recovery、active.halt、active.hands、active.tozero、active.ptp、active.line、active.circle、active.stop這八個狀態(tài)組成一個active狀態(tài)的集合，active的狀態(tài)轉(zhuǎn)移規(guī)則，對八個子狀態(tài)均有效。例如，給八個狀態(tài)中的任一個，寫入“e_ready”事件，將控制器狀態(tài)機(jī)的狀態(tài)從當(dāng)前狀態(tài)轉(zhuǎn)移到ready狀態(tài)(即等待指令輸入狀態(tài))。

另外，還可以使用lua語言，編寫控制器管理程序的啟動文件，該啟動文件設(shè)置為執(zhí)行如下動作：

a)通過orocos的import方法，加載模塊進(jìn)行運行；

b)定義模塊的刷新頻率，線程的優(yōu)先級別；

c)對模塊的屬性進(jìn)行賦值；

d)通過orocos的connect方法，將控制器管理程序的輸入、輸出接口和算法處理程序和通信管理程序的接口建立連接。

e)通過orocos的start方法，運行控制器管理程序，控制器管理程序?qū)⑾日{(diào)用starthook()函數(shù)，然后按預(yù)設(shè)刷新頻率，實時地周期性調(diào)用updatehook()函數(shù)。

2、對于算法處理程序：

算法處理程序使用ros的orocreate-catkin-pkg方法創(chuàng)建ros的package，記為ec_control_loop，然后在package中，通過繼承orocos的rtt：：taskcontext類，記為ec_control_loop_component。

(1)在ec_control_loop_component類的構(gòu)造函數(shù)中，設(shè)置為執(zhí)行如下操作：

a)算法處理程序利用orocos的rtt：：input與rtt：：output方法，對輸入，輸出接口進(jìn)行定義。

其中輸入接口包括：

①通信管理程序傳入的電機(jī)運行數(shù)據(jù)；

②通信管理程序傳入的診斷數(shù)據(jù)；

③設(shè)備狀態(tài)機(jī)的狀態(tài)；

④控制器狀態(tài)機(jī)的狀態(tài)；

輸出接口包括：

①電機(jī)控制指令數(shù)據(jù)，輸出給設(shè)備通信模塊；

②電機(jī)運行數(shù)據(jù)，輸出給控制算法處理程序；

③設(shè)備狀態(tài)機(jī)事件觸發(fā)，輸出給設(shè)備狀態(tài)機(jī)；

④控制器狀態(tài)機(jī)事件觸發(fā)，輸出給控制器狀態(tài)機(jī)。

b)利用orocos的operationalcaller方法設(shè)置函數(shù)調(diào)用接口，設(shè)置事件報告的接口：該接口將觸發(fā)控制器管理程序的事件報告處理函數(shù)設(shè)置各種運動規(guī)劃的控制指令響應(yīng)函數(shù)，包括：

①點到點運動；

②回到原點；

③直線運動；

④圓弧運動；

⑤暫停；

⑥恢復(fù)；

⑦急停；

⑧手動示教。

c)調(diào)用orocos的properties方法定義算法處理程序的屬性，算法處理程序定義一個機(jī)械臂關(guān)節(jié)個數(shù)的屬性。

(2)初始化處理函數(shù)：在ec_control_loop_component的starthook()成員函數(shù)中，設(shè)置為執(zhí)行如下操作：

a)檢查日志報告是否正常，若異常直接退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序處理；

b)檢查電機(jī)運行數(shù)據(jù)通道是否有數(shù)據(jù)，若無數(shù)據(jù)直接退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序處理。

(3)實時更新函數(shù)：對于ec_control_loop_component類的updatehook()成員函數(shù)，設(shè)置該函數(shù)在算法處理程序運行時，按照用戶設(shè)定的頻率實時運行(如設(shè)為100hz)，可以設(shè)置為執(zhí)行如下操作：

a)讀取控制器狀態(tài)機(jī)狀態(tài)；

b)根據(jù)控制器狀態(tài)機(jī)的不同狀態(tài)，執(zhí)行不同操作：

ⅰ、如果是點到點運動，直線運動，圓弧運動，手動示教，急停，回到原點狀態(tài)。此時，可以執(zhí)行如下操作：

如果指令緩沖區(qū)的控制指令個數(shù)小于20個，則將所有指令一起發(fā)送給通信管理程序，并將控制器狀態(tài)機(jī)的狀態(tài)改變?yōu)榈却噶钶斎霠顟B(tài)；

如果指令緩沖區(qū)的控制指令個數(shù)大于20個，則取指令隊列末尾的20個，發(fā)送給通信管理程序；

ⅱ、如果是暫停狀態(tài)，則停止動作。

(4)對于ec_control_loop_component類，定義點到點運動，直線運動，圓弧運動，手動示教，急停，回到原點，暫停，恢復(fù)等函數(shù)調(diào)用接口，實現(xiàn)如下：

a)點到點運動，直線運動，圓弧運動，手動示教，回到原點的函數(shù)，內(nèi)部實現(xiàn)如下：

檢查控制器狀態(tài)機(jī)是否處于等待指令輸入狀態(tài)。如果不是則退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序處理；

讀取電機(jī)的當(dāng)前的狀態(tài)信息；

依據(jù)電機(jī)的當(dāng)前狀態(tài)，分別調(diào)用點到點運動，直線運動，圓弧運動，手動示教，回到原點的運動規(guī)劃，并將生成的電機(jī)控制指令保存到指令緩沖區(qū)；

將控制器狀態(tài)機(jī)設(shè)為相應(yīng)的狀態(tài)。比如點到點運動回調(diào)函數(shù)，則將控制器狀態(tài)機(jī)設(shè)為點到點運動狀態(tài)。

b)暫停函數(shù)，內(nèi)部實現(xiàn)如下：

檢查控制器狀態(tài)機(jī)是否是點到點運動，直線運動，圓弧運動，手動示教，回到原點等狀態(tài)。如果不是則退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序進(jìn)行處理；

記錄當(dāng)前控制器狀態(tài)機(jī)的當(dāng)前狀態(tài)，并將控制器狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)變?yōu)闀和顟B(tài)。

c)恢復(fù)函數(shù)，內(nèi)部實現(xiàn)如下：

檢查控制器狀態(tài)機(jī)是否是暫停狀態(tài)。如果不是則退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序處理；

將控制器狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)變?yōu)闀和Ｇ暗臓顟B(tài)。

d)急停函數(shù)，內(nèi)部實現(xiàn)如下：

ⅰ、檢查控制器狀態(tài)機(jī)是否是點到點運動，直線運動，圓弧運動，手動示教，回到原點等狀態(tài)。如果不是則退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序處理；

ⅱ、讀取電機(jī)的當(dāng)前的狀態(tài)信息；

ⅲ、將電機(jī)控制指令緩沖區(qū)清零；

ⅳ、調(diào)用速度規(guī)劃運動規(guī)劃，讓電機(jī)以最短時間，速度降為0，并將生成的電機(jī)控制指令保存到指令緩沖區(qū)。

(5)使用lua語言，編寫算法處理程序的啟動文件，設(shè)置為執(zhí)行如下動作：

a)通過orocos的import方法，加載算法處理程序；

d)定義算法處理程序的刷新頻率，線程的優(yōu)先級別；

c)對算法處理程序的屬性進(jìn)行賦值；

d)通過orocos的connect方法，將算法處理程序的輸入，輸出接口和控制器管理程序和通信管理程序的接口建立連接。

e)通過orocos的start方法，運行算法處理程序，算法處理程序先調(diào)用starthook()函數(shù)，然后按設(shè)置的刷新頻率，實時地周期性調(diào)用updatehook()函數(shù)。

3、對于通信管理程序：

通信管理程序可以通過linux主機(jī)minicom中的ttyacm0與arm開發(fā)板通信，可以在該arm開發(fā)板上運行一個控制主站協(xié)議，該主站協(xié)議可以設(shè)置一個指令緩存區(qū)，最多可以存儲25個指令。

通信管理程序可以利用orocos的rtt：：input與rtt：：output方法與機(jī)器人算法處理程序和控制器管理程序進(jìn)行通信。

利用rfsm軟件建立設(shè)備狀態(tài)機(jī)，對通信管理程序的業(yè)務(wù)邏輯進(jìn)行控制。

通信管理程序利用orocos的rtt：：input與rtt：：output方法與設(shè)備狀態(tài)機(jī)連接，可改變設(shè)備狀態(tài)機(jī)的狀態(tài)，并讀取狀態(tài)。

(1)通信模塊使用ros的orocreate-catkin-pkg方法創(chuàng)建為ros的package，然后在package中，通過繼承orocos的rtt：：taskcontext類，創(chuàng)建一個orocos的實時模塊，記為ec_component。

在ec_component類的構(gòu)造函數(shù)中，設(shè)置為執(zhí)行如下操作：

a)通信管理程序利用orocos的rtt：：input與rtt：：output方法，對輸入，輸出接口進(jìn)行定義。

其中輸入接口包括：

①算法處理程序傳入的控制指令數(shù)據(jù)；

②設(shè)備狀態(tài)機(jī)的狀態(tài)；

輸出接口包括：

①診斷數(shù)據(jù)，輸出至算法處理程序和控制器管理程序；

②電機(jī)運行數(shù)據(jù)，輸出給算法處理程序；

③狀態(tài)機(jī)事件觸發(fā)，輸出給設(shè)備狀態(tài)機(jī)。

b)利用orocos的operationalcaller方法定義函數(shù)調(diào)用接口，通信管理程序定義事件報告的接口，通過該接口觸發(fā)控制器管理程序的事件報告處理函數(shù)。

c)調(diào)用orocos的properties方法定義通信管理程序的屬性，通信管理程序定義一個機(jī)械臂關(guān)節(jié)個數(shù)的屬性。

(2)初始化處理函數(shù)：在ec_component的starthook()成員函數(shù)中，設(shè)置為執(zhí)行如下操作：

a)檢查日志報告是否正常，若異常直接退出，并將相關(guān)信息通過事件報告接口傳遞給控制器管理程序進(jìn)行處理；

b)電機(jī)驅(qū)動初始化：

ⅰ、通過ttyacm0與電機(jī)驅(qū)動器建立通信；

ⅱ、電機(jī)指令緩沖隊列清空；

ⅲ、電機(jī)使能，如果使能成功，則進(jìn)行下一步，否則退出；

ⅳ、讀取電機(jī)的位置，計算機(jī)器人的機(jī)械臂當(dāng)前狀態(tài)，包括關(guān)節(jié)角度，機(jī)械臂末端位姿；

c)機(jī)械臂狀態(tài)初始化：

依據(jù)電機(jī)位置，判斷機(jī)械臂是否需要執(zhí)行回零運動，如果機(jī)械臂任一關(guān)節(jié)角度與零度相差大于0.01度，則執(zhí)行回零運動，調(diào)用點到點運動規(guī)劃，對回零運動進(jìn)行規(guī)劃。

d)改變設(shè)備狀態(tài)機(jī)的狀態(tài)：

如果機(jī)器人的機(jī)械臂需要執(zhí)行回零運動，則控制設(shè)備狀態(tài)機(jī)保持init狀態(tài)不變；否則，向設(shè)備狀態(tài)機(jī)發(fā)送“e_nominal”事件，將設(shè)備狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)換為active.nominal。

(3)實時更新函數(shù)：對于ec_component的updatehook()成員函數(shù)，設(shè)置該函數(shù)在通信管理程序運行時，以用戶設(shè)定的頻率實時運行(如設(shè)為1khz)，設(shè)置為執(zhí)行如下操作：

a)讀取設(shè)備狀態(tài)機(jī)狀態(tài)；

b)根據(jù)設(shè)備狀態(tài)機(jī)的不同狀態(tài)，執(zhí)行不同操作：

ⅰ、如果是init狀態(tài)，執(zhí)行機(jī)械臂的回零運動。此時，執(zhí)行如下操作：

讀取系統(tǒng)的時鐘，依據(jù)回零運動軌跡規(guī)劃結(jié)果，計算該時刻電機(jī)的運動指令，并將該單條運動指令發(fā)送給控制主站。

如果運動已回零成功，向設(shè)備狀態(tài)機(jī)發(fā)送“e_nominal”事件，將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為active.nominal，并將該事件報告給控制器管理程序。

ⅱ、如果是active.nominal狀態(tài)。此時，執(zhí)行如下操作：

從控制指令數(shù)據(jù)的輸入通道中，讀取控制指令，并存儲到電機(jī)指令緩沖隊列中。

讀取控制主站指令緩沖區(qū)的現(xiàn)有指令個數(shù)，如果小于10個，則一次性從電機(jī)指令緩沖隊列中取出15個運動指令發(fā)送給控制主站。如果電機(jī)指令緩沖隊列的指令個數(shù)小于15個，則一次性全部發(fā)送給控制主站。

ⅲ、如果是active.recovery狀態(tài)。此時，通信管理程序處于恢復(fù)狀態(tài)。

此時，依據(jù)診斷信息，對系統(tǒng)狀態(tài)進(jìn)行恢復(fù)，若恢復(fù)成功，向設(shè)備狀態(tài)機(jī)發(fā)送“e_nominal”事件，將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為active.nominal。并將該事件報告給控制器管理程序。

若恢復(fù)不成功，給狀態(tài)機(jī)發(fā)送“e_fault”事件，狀態(tài)機(jī)轉(zhuǎn)換為fault狀態(tài)，將該事件報告給控制器管理程序，并直接退出updatehook()。

ⅳ、如果是active.halt狀態(tài)。此時，模塊處于暫停狀態(tài)，執(zhí)行如下操作：檢查控制指令數(shù)據(jù)的輸入通道中是否有新的指令，若有新指令，則讀取控制指令，并存儲到電機(jī)指令緩沖隊列中。

ⅴ、如果是active.hands狀態(tài)。此時，模塊處于手控模式，執(zhí)行如下操作：

讀取系統(tǒng)的時鐘，依據(jù)運動軌跡規(guī)劃結(jié)果，計算該時刻電機(jī)的運動指令，并將該單條運動指令發(fā)送給控制主站。

ⅵ、如果是fault狀態(tài)，則直接退出updatehook()。

c)讀取電機(jī)狀態(tài)，計算機(jī)械臂關(guān)節(jié)以及末端運動狀態(tài)信息，并通過輸出數(shù)據(jù)通道，傳遞給算法處理程序與控制器管理程序；

d)檢查控制主站是否有錯誤報告信息，如果有錯誤信息，則將診斷信息傳遞給算法處理程序與控制器管理程序。向設(shè)備狀態(tài)機(jī)發(fā)送“e_recovery”事件，將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為active.recovery狀態(tài)、并將該事件報告給控制器管理程序。

(4)對于ec_component的cleanuphook()成員函數(shù)，該函數(shù)在模塊結(jié)束運行時，自動調(diào)用，設(shè)置執(zhí)行如下操作：

a)關(guān)閉電機(jī)驅(qū)動使能；

b)關(guān)閉電機(jī)驅(qū)動連接。

(5)參考圖5所示，圖5是設(shè)備狀態(tài)機(jī)的狀態(tài)變化示意圖。共有init(初始化)、fault(中斷)，active.recovery(恢復(fù))、active.hands(手動示教)、active.halt(暫停)、active.nominal(運轉(zhuǎn))六個狀態(tài)。active.recovery、active.hands、active.halt、active.nominal四個狀態(tài)組成一個active(使能)的狀態(tài)集合，active的狀態(tài)轉(zhuǎn)移規(guī)則，對四個子狀態(tài)均有效。

(6)使用lua語言，編寫該模塊的啟動文件，該啟動文件設(shè)置執(zhí)行如下動作：

a)通過orocos的import方法，加載通信管理程序；

b)定義通信管理程序的刷新頻率，線程的優(yōu)先級別；

c)對通信管理程序的屬性進(jìn)行賦值；

d)通過orocos的connect方法，將通信管理程序的輸入，輸出接口和控制器管理程序和算法模等的接口建立連接。

e)通過orocos的start方法，運行通信管理程序，通信管理程序先調(diào)用starthook()函數(shù)，然后按定義好的刷新頻率，實時地周期性調(diào)用updatehook()函數(shù)。

對于上述控制器管理程序、算法處理程序和通信管理程序，設(shè)置為當(dāng)運行后，如果用戶需要中途停止該模塊，同時按鍵盤的ctrl鍵與d鍵。

綜上實施例，基于ros和orocos，利用orocos的實時性特點，保證軟件程序的實時性能；充分利用ros的開放性，基于ros、orocos開發(fā)的控制器管理程序、算法處理程序和通信管理程序進(jìn)行實時通信，共同構(gòu)成一個完整的機(jī)器人控制器軟件；進(jìn)一步建立了控制器狀態(tài)機(jī)和設(shè)備狀態(tài)機(jī)，從而實現(xiàn)對控制器管理程序、通信管理程序的業(yè)務(wù)邏輯進(jìn)行有效管理。

通過orocos的rtt：：input,rtt：：output方法建立控制器管理程序、算法處理程序和通信管理程序的數(shù)據(jù)輸入、輸出通道，通過orocos的operationalcaller方法定義函數(shù)調(diào)用接口，通過orocos的properties方法定義控制器管理程序、算法處理程序和通信管理程序的屬性。因此，保證了控制器管理程序、算法處理程序和通信管理程序之間的獨立性與解耦性。

參考圖6所示，圖6是本發(fā)明實施例的基于ros和orocos的機(jī)器人控制的通信系統(tǒng)結(jié)構(gòu)示意圖，包括：

接收單元101，用于通過第二接口接收算法處理程序發(fā)送的控制指令數(shù)據(jù)；

緩存單元102，用于將所述控制指令數(shù)據(jù)保存到電機(jī)指令緩沖隊列中；

發(fā)送單元103，用于根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量，從電機(jī)指令緩沖隊列中選擇控制指令數(shù)據(jù)輸出至所述控制主站；

其中，所述控制指令數(shù)據(jù)是算法處理程序的機(jī)器人運動規(guī)劃的響應(yīng)函數(shù)根據(jù)控制指令計算得到，所述控制指令由控制器管理程序從接收端進(jìn)行接收，所述控制器管理程序的控制指令觸發(fā)函數(shù)通過調(diào)用算法處理的所述響應(yīng)函數(shù)，所述第二接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口，用于與通信管理程序進(jìn)行通信連接。

本發(fā)明的基于ros和orocos的機(jī)器人控制的通信系統(tǒng)與本發(fā)明的基于ros和orocos的機(jī)器人控制的通信方法一一對應(yīng)，在上述基于ros和orocos的機(jī)器人控制的通信方法的實施例闡述的技術(shù)特征及其有益效果均適用于基于ros和orocos的機(jī)器人控制的通信系統(tǒng)的實施例中，特此聲明。

以上所述實施例的各技術(shù)特征可以進(jìn)行任意的組合，為使描述簡潔，未對上述實施例中的各個技術(shù)特征所有可能的組合都進(jìn)行描述，然而，只要這些技術(shù)特征的組合不存在矛盾，都應(yīng)當(dāng)認(rèn)為是本說明書記載的范圍。

以上所述實施例僅表達(dá)了本發(fā)明的幾種實施方式，其描述較為具體和詳細(xì)，但并不能因此而理解為對發(fā)明專利范圍的限制。應(yīng)當(dāng)指出的是，對于本領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變形和改進(jìn)，這些都屬于本發(fā)明的保護(hù)范圍。因此，本發(fā)明專利的保護(hù)范圍應(yīng)以所附權(quán)利要求為準(zhǔn)。