本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域,特別是涉及一種基于ros和orocos的機(jī)器人控制系統(tǒng)。
背景技術(shù):
robotoperatingsystem(ros)是開(kāi)源的機(jī)器人操作系統(tǒng),可以為機(jī)器人開(kāi)發(fā)者提供一個(gè)標(biāo)準(zhǔn)化的、開(kāi)源的編程框架。但是ros目前不支持實(shí)時(shí)線程操作。openrobotcontrolsoftware(orocos)也是一種開(kāi)源的機(jī)器人控制軟件編程框架,它的特點(diǎn)是支持實(shí)時(shí)的線程操作,但是它的開(kāi)放性,通用性沒(méi)有ros好。
因此,目前在一些應(yīng)用方案中,在ros上采用orocos,但現(xiàn)有技術(shù)方案在搭建的架構(gòu)上,不能構(gòu)成一個(gè)完整的機(jī)器人控制器軟件,無(wú)法充分利用ros和orocos的特性,系統(tǒng)開(kāi)發(fā)成本高,控制效果差。
技術(shù)實(shí)現(xiàn)要素:
基于此,有必要針對(duì)上述技術(shù)問(wèn)題,提供一種基于ros和orocos的機(jī)器人控制系統(tǒng),降低系統(tǒng)開(kāi)發(fā)成本,提高控制效果。
一種基于ros和orocos的機(jī)器人控制系統(tǒng),包括:總控模塊、算法模塊和通信管理模塊;
所述總控模塊,用于根據(jù)預(yù)先配置的通信協(xié)議接口接收控制端輸入的控制指令;根據(jù)所述控制指令選擇調(diào)用機(jī)器人運(yùn)動(dòng)規(guī)劃的控制指令觸發(fā)函數(shù);通過(guò)所述控制指令觸發(fā)函數(shù)通過(guò)第一接口調(diào)用所述算法模塊中相應(yīng)的機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù);
所述算法模塊,用于通過(guò)響應(yīng)所述調(diào)用指令,從預(yù)存的機(jī)器人運(yùn)動(dòng)規(guī)劃中選擇相應(yīng)的響應(yīng)函數(shù);根據(jù)選擇的響應(yīng)函數(shù)計(jì)算控制指令數(shù)據(jù);通過(guò)第二接口將控制指令數(shù)據(jù)發(fā)送至所述通信管理模塊;
所述總控模塊,用于根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量,選擇控制指令數(shù)據(jù)輸出至所述控制主站;
其中,每個(gè)控制指令觸發(fā)函數(shù)對(duì)應(yīng)算法模塊中的一種機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù)對(duì)應(yīng);所述第一接口、第二接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口,第一接口用于總控模塊與算法模塊進(jìn)行通信連接,第二接口用于算法模塊與通信管理模塊進(jìn)行通信連接。
上述基于ros和orocos的機(jī)器人控制系統(tǒng),利用orocos的實(shí)時(shí)輸入/輸出接口,實(shí)現(xiàn)總控模塊、算法模塊和通信管理模塊之間的通信,總控模塊通過(guò)預(yù)先配置的通信協(xié)議接口接收控制指令,通過(guò)控制指令觸發(fā)函數(shù),調(diào)用算法模塊的機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù),計(jì)算控制指令數(shù)據(jù),并通過(guò)通信管理模塊發(fā)送至機(jī)器人,實(shí)現(xiàn)控制指令的接收分發(fā)、解析和傳輸,可以構(gòu)成一個(gè)完整的機(jī)器人控制器軟件,降低機(jī)器人控制系統(tǒng)開(kāi)發(fā)成本,提高控制效果。
附圖說(shuō)明
圖1是本發(fā)明實(shí)施例的基于ros和orocos的機(jī)器人控制系統(tǒng)結(jié)構(gòu)示意圖;
圖2是一種機(jī)器人控制系統(tǒng)的硬件結(jié)構(gòu)模型;
圖3是基于ros和orocos的機(jī)器人控制系統(tǒng)搭建的軟件架構(gòu)圖;
圖4是控制器狀態(tài)機(jī)的狀態(tài)變化示意圖;
圖5是設(shè)備狀態(tài)機(jī)的狀態(tài)變化示意圖。
具體實(shí)施方式
下面結(jié)合附圖闡述本發(fā)明的基于ros和orocos的機(jī)器人控制系統(tǒng)的實(shí)施例。
參考圖1所示,圖1是本發(fā)明實(shí)施例的基于ros和orocos的機(jī)器人控制系統(tǒng)結(jié)構(gòu)示意圖,該系統(tǒng)包括:總控模塊、算法模塊和通信管理模塊;
所述總控模塊,用于根據(jù)預(yù)先配置的通信協(xié)議接口接收控制端輸入的控制指令;根據(jù)所述控制指令選擇調(diào)用機(jī)器人運(yùn)動(dòng)規(guī)劃的控制指令觸發(fā)函數(shù);通過(guò)所述控制指令觸發(fā)函數(shù)通過(guò)第一接口調(diào)用所述算法模塊中相應(yīng)的機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù);可以通過(guò)ice(theinternetcommunicationsengine,互聯(lián)網(wǎng)通信引擎)開(kāi)發(fā)的通信協(xié)議建立與控制端的通信連接,通過(guò)調(diào)用所述通信協(xié)議的動(dòng)態(tài)異步遠(yuǎn)程過(guò)程調(diào)用方法輸入的控制指令。
所述算法模塊,用于通過(guò)響應(yīng)所述調(diào)用指令,從預(yù)存的機(jī)器人運(yùn)動(dòng)規(guī)劃中選擇相應(yīng)的響應(yīng)函數(shù);根據(jù)選擇的響應(yīng)函數(shù)計(jì)算控制指令數(shù)據(jù);通過(guò)第二接口將指令緩存區(qū)中的控制指令數(shù)據(jù)發(fā)送至所述通信管理模塊。
所述通信管理模塊,用于根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量,選擇控制指令數(shù)據(jù)輸出至所述控制主站。
其中,每個(gè)控制指令觸發(fā)函數(shù)對(duì)應(yīng)算法模塊中的一種機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù)對(duì)應(yīng);所述第一接口、第二接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口,第一接口用于總控模塊與算法模塊進(jìn)行通信連接,第二接口用于算法模塊與通信管理模塊進(jìn)行通信連接。
上述基于ros和orocos的機(jī)器人控制系統(tǒng),利用orocos的實(shí)時(shí)輸入/輸出接口,實(shí)現(xiàn)總控模塊、算法模塊和通信管理模塊之間的通信,總控模塊通過(guò)預(yù)先配置的通信協(xié)議接口接收控制指令,通過(guò)控制指令觸發(fā)函數(shù),調(diào)用算法模塊的機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù),計(jì)算控制指令數(shù)據(jù),并通過(guò)通信管理模塊發(fā)送至機(jī)器人,實(shí)現(xiàn)控制指令的接收分發(fā)、解析和傳輸,可以構(gòu)成一個(gè)完整的機(jī)器人控制器軟件,降低機(jī)器人控制系統(tǒng)開(kāi)發(fā)成本,提高控制效果。
在一個(gè)實(shí)施例中,所述算法模塊,還可以用于根據(jù)選擇的響應(yīng)函數(shù)計(jì)算控制指令數(shù)據(jù),并保持在指令緩存區(qū);通過(guò)第二接口將指令緩存區(qū)中的控制指令數(shù)據(jù)發(fā)送至所述通信管理模塊;所述通信管理模塊,還可以用于將所述控制指令數(shù)據(jù)保存到電機(jī)指令緩沖隊(duì)列中;根據(jù)機(jī)器人的控制主站的指令緩存區(qū)的控制指令數(shù)據(jù)數(shù)量,從電機(jī)指令緩沖隊(duì)列中選擇控制指令數(shù)據(jù)輸出至所述控制主站。
如上所述,通過(guò)指令數(shù)據(jù)的緩存,便于控制指令數(shù)據(jù)的轉(zhuǎn)發(fā)處理,增強(qiáng)控制效果。
在一個(gè)實(shí)施例中,所述總控模塊,還用于通過(guò)回調(diào)函數(shù)方式從所述第一接口接收算法模塊傳入的反饋信息,通過(guò)所述第三接口接收通信管理模塊傳入的反饋信息,并對(duì)所述反饋信息進(jìn)行處理;
所述算法模塊,還用于利用所述第二接口接收通信管理模塊傳入的機(jī)器人的電機(jī)運(yùn)行數(shù)據(jù)和診斷數(shù)據(jù);并通過(guò)所述第一接口將所述電機(jī)運(yùn)行數(shù)據(jù)和診斷數(shù)據(jù)上報(bào)至總控模塊;
其中,所述反饋信息包括事件報(bào)告信息、機(jī)器人電機(jī)運(yùn)行的診斷數(shù)據(jù);所述第三接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口,用于總控模塊與通信管理模塊進(jìn)行通信連接。
在本實(shí)施例中,所述總控模塊可以獲取算法模塊和通信管理模塊的相關(guān)信息并進(jìn)行處理。
(1)總控模塊可以通過(guò)回調(diào)函數(shù)方式,從所述第一接口接收算法模塊傳入的反饋信息;其中,所述反饋信息包括事件報(bào)告信息、機(jī)器人電機(jī)運(yùn)行的診斷數(shù)據(jù);包括生成錯(cuò)誤的時(shí)間戳,事件級(jí)別等信息,并將事件信息,這些信息可以發(fā)送給控制端的人機(jī)交互界面顯示;
(2)總控模塊可以通過(guò)所述第三接口接收通信管理模塊傳入的反饋信息;其中,所述第三接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口,用于與通信管理模塊進(jìn)行通信連接;所述反饋信息包括事件報(bào)告信息、機(jī)器人電機(jī)運(yùn)行的診斷數(shù)據(jù);例如,位置、速度及加速度是否超限等等。
在一個(gè)實(shí)施例中,所述總控模塊,用于通過(guò)ice開(kāi)發(fā)的通信協(xié)議建立與控制端的通信連接,通過(guò)調(diào)用所述通信協(xié)議的動(dòng)態(tài)異步遠(yuǎn)程過(guò)程調(diào)用方法輸入的控制指令;在接收控制端輸入的控制指令前,將控制端發(fā)起的控制指令與所述算法模塊中相應(yīng)的機(jī)器人運(yùn)動(dòng)規(guī)劃的響應(yīng)函數(shù)進(jìn)行綁定;在接收控制端輸入的控制指令后,獲取所述控制指令的第一個(gè)參數(shù),根據(jù)所述參數(shù)判斷當(dāng)前調(diào)用的運(yùn)動(dòng)規(guī)劃類型,根據(jù)所述運(yùn)動(dòng)規(guī)劃類型選擇調(diào)用相應(yīng)的運(yùn)動(dòng)規(guī)劃的控制指令觸發(fā)函數(shù),調(diào)用算法模塊中相應(yīng)的響應(yīng)函數(shù)計(jì)算控制指令參數(shù)。
在一個(gè)實(shí)施例中,在系統(tǒng)進(jìn)行初始化時(shí):
所述總控模塊,還用于檢測(cè)歷史運(yùn)行所記錄的日志報(bào)告,若日志報(bào)告正常,通過(guò)事件報(bào)告接口接收相關(guān)信息并進(jìn)行處理;若日志報(bào)告異常,則退出;
所述算法模塊,還用于檢測(cè)歷史運(yùn)行所記錄的日志報(bào)告,若日志報(bào)告正常,將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;若日志報(bào)告異常,則退出;以及檢測(cè)電機(jī)運(yùn)行數(shù)據(jù)的通道,若通道存在電機(jī)運(yùn)行數(shù)據(jù),將所述電機(jī)運(yùn)行數(shù)據(jù)通過(guò)事件報(bào)告接口傳遞給總控模塊;若日志報(bào)告異常,則退出;
所述通信管理模塊,還用于檢測(cè)歷史運(yùn)行所記錄的日志報(bào)告,若日志報(bào)告正常,將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;若日志報(bào)告異常,則退出。
在一個(gè)實(shí)施例中,所述運(yùn)動(dòng)規(guī)劃包括:點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,回到原點(diǎn)、暫停、恢復(fù)或急停。
基于上述實(shí)施例的運(yùn)動(dòng)規(guī)劃,所述總控模塊,還可以用于根據(jù)接收到的控制指令的類型,向控制器狀態(tài)機(jī)寫(xiě)入狀態(tài)事件;根據(jù)所述狀態(tài)事件控制所述算法模塊和通信管理模塊的運(yùn)行狀態(tài);
所述算法模塊,還可以用于根據(jù)用戶設(shè)定的頻率,實(shí)時(shí)讀取控制器狀態(tài)機(jī)的狀態(tài)信息;根據(jù)所述狀態(tài)信息選擇的響應(yīng)函數(shù),利用所述響應(yīng)函數(shù)計(jì)算控制指令數(shù)據(jù),并控制指令數(shù)據(jù)保持在指令緩存區(qū)的指令隊(duì)列中;根據(jù)指令隊(duì)列中的控制指令數(shù)據(jù)的數(shù)量,依據(jù)隊(duì)列的順序?qū)⒖刂浦噶顢?shù)據(jù)發(fā)送至通信管理模塊,并更新控制器狀態(tài)機(jī)的狀態(tài);
所述通信管理模塊,還可以用于根據(jù)用戶設(shè)定的頻率讀取設(shè)備狀態(tài)機(jī)的狀態(tài)信息,根據(jù)所述狀態(tài)信息執(zhí)行控制指令數(shù)據(jù)的發(fā)送操作;讀取電機(jī)狀態(tài),計(jì)算機(jī)械臂關(guān)節(jié)以及末端運(yùn)動(dòng)狀態(tài)信息,并反饋至算法模塊與總控模塊;檢測(cè)控制主站的診斷信息,將該診斷信息反饋至算法模塊與總控模塊,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運(yùn)轉(zhuǎn)狀態(tài),并將該事件報(bào)告發(fā)送至總控模塊;
其中,控制器狀態(tài)機(jī)設(shè)有初始化、指令等待、指令執(zhí)行、中斷和使能對(duì)應(yīng)的狀態(tài),所述使能狀態(tài)為分別與每種運(yùn)動(dòng)規(guī)劃對(duì)應(yīng)的子狀態(tài)的合集;所述設(shè)備狀態(tài)機(jī)設(shè)有初始化、中斷、暫停、恢復(fù)、手動(dòng)示教和運(yùn)轉(zhuǎn)六種狀態(tài);所述使能為暫停、恢復(fù)、手動(dòng)示教和運(yùn)轉(zhuǎn)四種狀態(tài)的合集狀態(tài)。
在一個(gè)實(shí)施例中,所述算法模塊,進(jìn)一步用于若控制器狀態(tài)機(jī)處于等待指令輸入狀態(tài),并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;根據(jù)電機(jī)運(yùn)行數(shù)據(jù),分別調(diào)用點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教或回到原點(diǎn)的運(yùn)動(dòng)規(guī)劃計(jì)算電機(jī)控制指令數(shù)據(jù),并將其保存到指令緩沖區(qū);將控制器狀態(tài)機(jī)設(shè)為直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教或回到原點(diǎn)的運(yùn)動(dòng)規(guī)劃相應(yīng)的狀態(tài);其中,所述運(yùn)動(dòng)規(guī)劃為點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、手動(dòng)示教或回到原點(diǎn);
在一個(gè)實(shí)施例中,所述算法模塊,進(jìn)一步用于檢測(cè)控制器狀態(tài)機(jī)是否為點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、手動(dòng)示教或回到原點(diǎn)對(duì)應(yīng)的狀態(tài);若是,記錄控制器狀態(tài)機(jī)當(dāng)前的狀態(tài),并將控制器狀態(tài)機(jī)的狀態(tài)設(shè)為暫停對(duì)應(yīng)的狀態(tài),否則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;其中,所述運(yùn)動(dòng)規(guī)劃為暫停;
在一個(gè)實(shí)施例中,所述算法模塊,進(jìn)一步用于檢測(cè)控制器狀態(tài)機(jī)是否為暫停狀態(tài);若是,將控制器狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)變?yōu)闀和G八涗浀臓顟B(tài),否則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;其中,所述運(yùn)動(dòng)規(guī)劃為恢復(fù)。
在一個(gè)實(shí)施例中,所述算法模塊,進(jìn)一步用于根據(jù)選擇的響應(yīng)函數(shù)計(jì)算控制指令數(shù)據(jù)的步驟包括:檢測(cè)控制器狀態(tài)機(jī)是否為點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、手動(dòng)示教或回到原點(diǎn)對(duì)應(yīng)的狀態(tài);若否,退出并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;若是,讀取電機(jī)的當(dāng)前的指令狀態(tài)信息,將通信管理模塊的電機(jī)控制指令緩沖區(qū)清零;將控制器狀態(tài)機(jī)設(shè)為急停對(duì)應(yīng)的狀態(tài),并調(diào)用速度規(guī)劃運(yùn)動(dòng)算法計(jì)算電機(jī)以最短時(shí)間速度降為0的控制指令數(shù)據(jù),將該控制指令數(shù)據(jù)保存到指令緩沖區(qū);其中,所述運(yùn)動(dòng)規(guī)劃為急停。
在一個(gè)實(shí)施例中,所述通信管理模塊,進(jìn)一步用于若設(shè)備狀態(tài)機(jī)是初始化狀態(tài),執(zhí)行機(jī)械臂的回零運(yùn)動(dòng)規(guī)劃;根據(jù)回零運(yùn)動(dòng)規(guī)劃的軌跡,計(jì)算電機(jī)的控制指令數(shù)據(jù),并將該控制指令數(shù)據(jù)發(fā)送給控制主站;在回零成功,向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運(yùn)轉(zhuǎn)狀態(tài),并將該事件報(bào)告反饋至總控模塊;
在一個(gè)實(shí)施例中,所述通信管理模塊,進(jìn)一步用于若設(shè)備狀態(tài)機(jī)是運(yùn)轉(zhuǎn)狀態(tài),從控制指令數(shù)據(jù)的輸入通道中,讀取控制指令數(shù)據(jù),并存儲(chǔ)到電機(jī)指令緩沖隊(duì)列中;讀取控制主站的指令緩沖區(qū)的現(xiàn)有控制指令數(shù)據(jù)的數(shù)量,根據(jù)該數(shù)量大小從電機(jī)指令緩沖隊(duì)列中取出指定數(shù)量的控制指令數(shù)據(jù)發(fā)送至控制主站;
在一個(gè)實(shí)施例中,所述通信管理模塊,進(jìn)一步用于若設(shè)備狀態(tài)機(jī)是恢復(fù)狀態(tài),向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運(yùn)轉(zhuǎn)狀態(tài),并將該事件報(bào)告反饋至總控模塊;若恢復(fù)不成功,則向設(shè)備狀態(tài)機(jī)發(fā)送中斷事件,狀態(tài)機(jī)轉(zhuǎn)換為中斷狀態(tài),將該事件報(bào)告反饋至總控模塊,并退出;
在一個(gè)實(shí)施例中,所述通信管理模塊,進(jìn)一步用于若設(shè)備狀態(tài)機(jī)是暫停狀態(tài),檢測(cè)控制指令數(shù)據(jù)的輸入通道中是否有新的控制指令數(shù)據(jù),若是,則讀取控制指令數(shù)據(jù)并存儲(chǔ)到電機(jī)指令緩沖隊(duì)列中;
在一個(gè)實(shí)施例中,所述通信管理模塊,進(jìn)一步用于若設(shè)備狀態(tài)機(jī)是手動(dòng)示教狀態(tài),根據(jù)手動(dòng)示教的運(yùn)動(dòng)規(guī)劃軌跡,計(jì)算該時(shí)刻電機(jī)的控制指令數(shù)據(jù),并將該單條控制指令數(shù)據(jù)發(fā)送至控制主站。
在一個(gè)實(shí)施例中,所述通信管理模塊,進(jìn)一步用于讀取機(jī)器人電機(jī)的狀態(tài)信息,根據(jù)機(jī)器人運(yùn)動(dòng)模型計(jì)算機(jī)器人機(jī)械臂的狀態(tài)信息,將機(jī)器人的狀態(tài)信息通過(guò)第三接口反饋給總控模塊,將機(jī)器人的狀態(tài)信息通過(guò)第二接口反饋給算法模塊;
其中,所述電機(jī)的狀態(tài)信息包括位置、速度和力矩;所述機(jī)械臂的狀態(tài)信息包括關(guān)節(jié)角度、關(guān)節(jié)角速度、關(guān)節(jié)角加速度、末端位姿、末端線速度、末端角速度、末端線加速度和末端線加速度。
對(duì)于算法模塊計(jì)算控制指令,以點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,回到原點(diǎn)、暫停、恢復(fù)、急停為例,其實(shí)現(xiàn)方法可以如下:
根據(jù)選擇的響應(yīng)函數(shù)計(jì)算控制指令數(shù)據(jù)的步驟包括:
(1)所述運(yùn)動(dòng)規(guī)劃為點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、手動(dòng)示教或回到原點(diǎn);計(jì)算方法如下:
若控制器狀態(tài)機(jī)處于等待指令輸入狀態(tài),并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;
根據(jù)電機(jī)運(yùn)行數(shù)據(jù),分別調(diào)用點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教或回到原點(diǎn)的運(yùn)動(dòng)規(guī)劃計(jì)算電機(jī)控制指令數(shù)據(jù),并將其保存到指令緩沖區(qū);
將控制器狀態(tài)機(jī)設(shè)為直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教或回到原點(diǎn)的運(yùn)動(dòng)規(guī)劃相應(yīng)的狀態(tài)。
(2)所述運(yùn)動(dòng)規(guī)劃為暫停和恢復(fù);計(jì)算方法如下:
對(duì)于暫停:
檢測(cè)控制器狀態(tài)機(jī)是否為點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、手動(dòng)示教或回到原點(diǎn)對(duì)應(yīng)的狀態(tài);若是,記錄控制器狀態(tài)機(jī)當(dāng)前的狀態(tài),并將控制器狀態(tài)機(jī)的狀態(tài)設(shè)為暫停對(duì)應(yīng)的狀態(tài),否則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;
對(duì)于恢復(fù):
檢測(cè)控制器狀態(tài)機(jī)是否為暫停狀態(tài);若是,將控制器狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)變?yōu)闀和G八涗浀臓顟B(tài),否則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊。
(3)所述運(yùn)動(dòng)規(guī)劃為急停;計(jì)算方法如下:
檢測(cè)控制器狀態(tài)機(jī)是否為點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、手動(dòng)示教或回到原點(diǎn)對(duì)應(yīng)的狀態(tài);
若否,退出并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;
若是,讀取電機(jī)的當(dāng)前的指令狀態(tài)信息,將通信管理模塊的電機(jī)控制指令緩沖區(qū)清零;
將控制器狀態(tài)機(jī)設(shè)為急停對(duì)應(yīng)的狀態(tài),并調(diào)用速度規(guī)劃運(yùn)動(dòng)算法計(jì)算電機(jī)以最短時(shí)間速度降為0的控制指令數(shù)據(jù),將該控制指令數(shù)據(jù)保存到指令緩沖區(qū)。
作為實(shí)施例,通信管理模塊的進(jìn)行初始化處理,具體可以包括如下:
(1)檢測(cè)歷史運(yùn)行所記錄的日志報(bào)告,若日志報(bào)告正常,將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊;若日志報(bào)告異常,則退出。
(2)電機(jī)驅(qū)動(dòng)的初始化:
建立與電機(jī)驅(qū)動(dòng)器的通信連接,將電機(jī)指令緩沖隊(duì)列清空;
使能電機(jī),若使能成功,讀取電機(jī)的位置信息,根據(jù)位置信息計(jì)算機(jī)器人的機(jī)械臂的狀態(tài)信息;其中,所述狀態(tài)信息包括關(guān)節(jié)角度和機(jī)械臂的末端位姿;
(3)機(jī)械臂的初始化:
若機(jī)械臂任一關(guān)節(jié)角度與零度相差大于設(shè)定值,則調(diào)用點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃,根據(jù)所述末端位姿對(duì)機(jī)械臂進(jìn)行回零運(yùn)動(dòng)規(guī)劃;
具體的,可以依據(jù)電機(jī)的位置信息,計(jì)算機(jī)械臂任一關(guān)節(jié)角度與零度相差是否大于0.01度,若是,則執(zhí)行回零運(yùn)動(dòng),可以調(diào)用點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃,對(duì)回零運(yùn)動(dòng)進(jìn)行規(guī)劃。
基于設(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ī)械臂的回零運(yùn)動(dòng)規(guī)劃;
根據(jù)回零運(yùn)動(dòng)規(guī)劃的軌跡,計(jì)算電機(jī)的控制指令數(shù)據(jù),并將該控制指令數(shù)據(jù)發(fā)送給控制主站;
在回零成功,向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運(yùn)轉(zhuǎn)狀態(tài),并將該事件報(bào)告反饋至總控模塊。
b)若設(shè)備狀態(tài)機(jī)是運(yùn)轉(zhuǎn)狀態(tài),從控制指令數(shù)據(jù)的輸入通道中,讀取控制指令數(shù)據(jù),并存儲(chǔ)到電機(jī)指令緩沖隊(duì)列中;
讀取控制主站的指令緩沖區(qū)的現(xiàn)有控制指令數(shù)據(jù)的數(shù)量,根據(jù)該數(shù)量大小從電機(jī)指令緩沖隊(duì)列中取出指定數(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)換為運(yùn)轉(zhuǎn)狀態(tài),并將該事件報(bào)告反饋至總控模塊;若恢復(fù)不成功,則向設(shè)備狀態(tài)機(jī)發(fā)送中斷事件,狀態(tài)機(jī)轉(zhuǎn)換為中斷狀態(tài),將該事件報(bào)告反饋至總控模塊,并退出;
d)若設(shè)備狀態(tài)機(jī)是暫停狀態(tài),檢測(cè)控制指令數(shù)據(jù)的輸入通道中是否有新的控制指令數(shù)據(jù),若是,則讀取控制指令數(shù)據(jù)并存儲(chǔ)到電機(jī)指令緩沖隊(duì)列中;
e)若設(shè)備狀態(tài)機(jī)是手動(dòng)示教狀態(tài),根據(jù)手動(dòng)示教的運(yùn)動(dòng)規(guī)劃軌跡,計(jì)算該時(shí)刻電機(jī)的控制指令數(shù)據(jù),并將該單條控制指令數(shù)據(jù)發(fā)送至控制主站。
(3)讀取電機(jī)狀態(tài),計(jì)算機(jī)械臂關(guān)節(jié)以及末端運(yùn)動(dòng)狀態(tài)信息,并反饋至算法模塊與總控模塊;
(4)檢測(cè)控制主站的診斷信息,將該診斷信息反饋至算法模塊與總控模塊,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為運(yùn)轉(zhuǎn)狀態(tài),并將該事件報(bào)告發(fā)送至總控模塊。
另外,如果機(jī)器人的機(jī)械臂需要執(zhí)行回零運(yùn)動(dòng)規(guī)劃,則控制設(shè)備狀態(tài)機(jī)保持初始化狀態(tài)不變;否則,向設(shè)備狀態(tài)機(jī)發(fā)送狀態(tài)事件,將設(shè)備狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)換為運(yùn)轉(zhuǎn)狀態(tài)。
對(duì)于第一接口、第二接口和第三接口,第一接口是用于總控模塊與算法模塊進(jìn)行通信連接的接口,第二接口是用于算法模塊和通信管理模塊進(jìn)行通信連接的接口,第三接口是用于總控模塊與通信管理模塊進(jìn)行通信的接口;按照輸入/輸出接口方式劃分,第一接口包括向算法模塊的輸入/輸出接口,第二接口包括向通信管理模塊的輸入/輸出接口。
作為實(shí)施例,可以使用ros的orocreate-catkin-pkg方法創(chuàng)建為ros的package,并在package中繼承orocos的rtt::taskcontext類,利用orocos的rtt::input與rtt::output方法定義實(shí)時(shí)輸入/輸出接口。
為了更加清晰本發(fā)明的實(shí)施例的技術(shù)方案,下面闡述采用本發(fā)明的方法實(shí)現(xiàn)的實(shí)例。
本實(shí)例中,硬件和軟件環(huán)境可以如下:
參考圖2所示,圖2是一種機(jī)器人控制系統(tǒng)的硬件結(jié)構(gòu)模型,在機(jī)器人控制器的搭建軟件架構(gòu),運(yùn)行于linux操作系統(tǒng),該linux主機(jī)可以是x86架構(gòu)的pc機(jī),或者arm芯片嵌入式架構(gòu)的開(kāi)發(fā)板,總控模塊接入控制端的人機(jī)交互界面的控制指令。
linux主機(jī)可以安裝如下軟件:安裝xenomai或者rtai或者rtpreempt的實(shí)時(shí)內(nèi)核補(bǔ)??;安裝ros,orocos,rfsm等軟件。
參考圖3所示,圖3是基于ros和orocos的機(jī)器人控制系統(tǒng)搭建的軟件架構(gòu)圖;在控制過(guò)程中,操作系統(tǒng)上運(yùn)行總控模塊、算法模塊和通信管理模塊。
1、對(duì)于總控模塊:
(1)總控模塊使用ros的orocreate-catkin-pkg方法創(chuàng)建ros的package,記為ec_control_system,然后在package中,通過(guò)繼承orocos的rtt::taskcontext類,記為ec_control_system_component。
在ec_control_system_component類的構(gòu)造函數(shù)中,設(shè)置為執(zhí)行如下操作:
a)利用orocos的rtt::input與rtt::output方法,對(duì)模塊的輸入,輸出接口進(jìn)行定義。
其中輸入的接口包括:
①通信管理模塊傳入的診斷數(shù)據(jù);
②通信管理模塊傳入的狀態(tài)反饋信息:包括電機(jī)運(yùn)行狀態(tài)等等;
③控制器狀態(tài)機(jī)的狀態(tài);
輸出的接口包括:
①控制器狀態(tài)機(jī)事件觸發(fā),輸出給控制器狀態(tài)機(jī)。
b)利用orocos的operationalcaller方法設(shè)置函數(shù)調(diào)用接口。
第一,設(shè)置事件報(bào)告的回調(diào)函數(shù):對(duì)事件報(bào)告處理請(qǐng)求進(jìn)行響應(yīng),包括生成錯(cuò)誤的時(shí)間戳,事件級(jí)別等信息,并將事件信息,發(fā)送給人機(jī)交互界面顯示。
第二,設(shè)置警報(bào)設(shè)置的回調(diào)函數(shù):依據(jù)診斷信息,判斷是否生成警報(bào)。比如,位置,速度,加速度是否超限等等。
第三,設(shè)置各種運(yùn)動(dòng)規(guī)劃的控制指令觸發(fā)函數(shù),這些函數(shù)將對(duì)算法模塊的相應(yīng)響應(yīng)函數(shù)進(jìn)行調(diào)用,包括:
①點(diǎn)到點(diǎn)運(yùn)動(dòng);
②回到原點(diǎn);
③直線運(yùn)動(dòng);
④圓弧運(yùn)動(dòng);
⑤暫停;
⑥恢復(fù);
⑦急停;
⑧手動(dòng)示教。
c)調(diào)用orocos的properties方法定義總控模塊的屬性,將總控模塊定義一個(gè)機(jī)械臂關(guān)節(jié)個(gè)數(shù)的屬性。
(2)在ec_control_system_component的starthook()成員函數(shù)中,設(shè)置執(zhí)行如下操作:
a)檢查日志報(bào)告是否正常,若異常直接退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊處理;
b)通過(guò)ice開(kāi)發(fā)的通信協(xié)議建立與人機(jī)交互界面的通信連接,并調(diào)用通信協(xié)議提供的動(dòng)態(tài)異步遠(yuǎn)程過(guò)程調(diào)用(rpc)方法,對(duì)人機(jī)交互界面發(fā)起的控制指令進(jìn)行響應(yīng)的回調(diào)函數(shù)進(jìn)行綁定。該回調(diào)函數(shù),首先根據(jù)ice(theinternetcommunicationsengine,互聯(lián)網(wǎng)通信引擎)提供的遠(yuǎn)程過(guò)程異步調(diào)用方法傳入的第一個(gè)參數(shù),判斷調(diào)用類型,然后依據(jù)此類型選擇調(diào)用相應(yīng)運(yùn)動(dòng)規(guī)劃的控制指令觸發(fā)函數(shù),包括:恢復(fù),暫停,手動(dòng)示教,回到原點(diǎn),點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),急停。
(3)對(duì)于ec_control_system_component的cleanuphook()成員函數(shù),為了使得該函數(shù)在總控模塊結(jié)束運(yùn)行時(shí),實(shí)現(xiàn)自動(dòng)調(diào)用,還可以設(shè)置為執(zhí)行如下操作:
a)調(diào)用ice開(kāi)發(fā)的通信協(xié)議接口,關(guān)閉與人機(jī)交互界面的通信連接。
(4)對(duì)于控制器狀態(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共十一個(gè)狀態(tài),分別代表初始化、等待指令輸入、恢復(fù)、暫停、手動(dòng)示教、回到原點(diǎn)、點(diǎn)到點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、急停狀態(tài)。其中,active.recovery、active.halt、active.hands、active.tozero、active.ptp、active.line、active.circle、active.stop這八個(gè)狀態(tài)組成一個(gè)active狀態(tài)的集合,active的狀態(tài)轉(zhuǎn)移規(guī)則,對(duì)八個(gè)子狀態(tài)均有效。例如,給八個(gè)狀態(tài)中的任一個(gè),寫(xiě)入“e_ready”事件,將控制器狀態(tài)機(jī)的狀態(tài)從當(dāng)前狀態(tài)轉(zhuǎn)移到ready狀態(tài)(即等待指令輸入狀態(tài))。
另外,還可以使用lua語(yǔ)言,編寫(xiě)總控模塊的啟動(dòng)文件,該啟動(dòng)文件設(shè)置為執(zhí)行如下動(dòng)作:
a)通過(guò)orocos的import方法,加載模塊進(jìn)行運(yùn)行;
b)定義模塊的刷新頻率,線程的優(yōu)先級(jí)別;
c)對(duì)模塊的屬性進(jìn)行賦值;
d)通過(guò)orocos的connect方法,將總控模塊的輸入、輸出接口和算法模塊和通信管理模塊的接口建立連接。
e)通過(guò)orocos的start方法,運(yùn)行總控模塊,總控模塊將先調(diào)用starthook()函數(shù),然后按預(yù)設(shè)刷新頻率,實(shí)時(shí)地周期性調(diào)用updatehook()函數(shù)。
2、對(duì)于算法模塊:
算法模塊使用ros的orocreate-catkin-pkg方法創(chuàng)建ros的package,記為ec_control_loop,然后在package中,通過(guò)繼承orocos的rtt::taskcontext類,記為ec_control_loop_component。
(1)在ec_control_loop_component類的構(gòu)造函數(shù)中,設(shè)置為執(zhí)行如下操作:
a)算法模塊利用orocos的rtt::input與rtt::output方法,對(duì)輸入,輸出接口進(jìn)行定義。
其中輸入的接口包括:
①通信管理模塊傳入的電機(jī)運(yùn)行數(shù)據(jù);
②通信管理模塊傳入的診斷數(shù)據(jù);
③設(shè)備狀態(tài)機(jī)的狀態(tài);
④控制器狀態(tài)機(jī)的狀態(tài);
輸出的接口包括:
①電機(jī)控制指令數(shù)據(jù),輸出給設(shè)備通信模塊;
②設(shè)備狀態(tài)機(jī)事件觸發(fā),輸出給設(shè)備狀態(tài)機(jī);
③控制器狀態(tài)機(jī)事件觸發(fā),輸出給控制器狀態(tài)機(jī)。
b)利用orocos的operationalcaller方法設(shè)置函數(shù)調(diào)用接口,設(shè)置事件報(bào)告的接口:該接口將觸發(fā)總控模塊的事件報(bào)告處理函數(shù)設(shè)置各種運(yùn)動(dòng)規(guī)劃的控制指令響應(yīng)函數(shù),包括:
①點(diǎn)到點(diǎn)運(yùn)動(dòng);
②回到原點(diǎn);
③直線運(yùn)動(dòng);
④圓弧運(yùn)動(dòng);
⑤暫停;
⑥恢復(fù);
⑦急停;
⑧手動(dòng)示教。
c)調(diào)用orocos的properties方法定義算法模塊的屬性,算法模塊定義一個(gè)機(jī)械臂關(guān)節(jié)個(gè)數(shù)的屬性。
(2)在ec_control_loop_component的starthook()成員函數(shù)中,設(shè)置為執(zhí)行如下操作:
a)檢查日志報(bào)告是否正常,若異常直接退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊處理;
b)檢查電機(jī)運(yùn)行數(shù)據(jù)通道是否有數(shù)據(jù),若無(wú)數(shù)據(jù)直接退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊處理。
(3)對(duì)于ec_control_loop_component類的updatehook()成員函數(shù),設(shè)置該函數(shù)在算法模塊運(yùn)行時(shí),按照用戶設(shè)定的頻率實(shí)時(shí)運(yùn)行(如設(shè)為100hz),可以設(shè)置為執(zhí)行如下操作:
a)讀取控制器狀態(tài)機(jī)狀態(tài);
b)根據(jù)控制器狀態(tài)機(jī)的不同狀態(tài),執(zhí)行不同操作:
ⅰ、如果是點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,急停,回到原點(diǎn)狀態(tài)。此時(shí),執(zhí)行如下操作:
如果指令緩沖區(qū)的控制指令個(gè)數(shù)小于20個(gè),則將所有指令一起發(fā)送給通信管理模塊,并將控制器狀態(tài)機(jī)的狀態(tài)改變?yōu)榈却噶钶斎霠顟B(tài);
如果指令緩沖區(qū)的控制指令個(gè)數(shù)大于20個(gè),則取指令隊(duì)列末尾的20個(gè),發(fā)送給通信管理模塊;
ⅱ、如果是暫停狀態(tài),則什么也不做。
(4)對(duì)于ec_control_loop_component類,定義點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,急停,回到原點(diǎn),暫停,恢復(fù)等函數(shù)調(diào)用接口,實(shí)現(xiàn)如下:
a)點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,回到原點(diǎn)的函數(shù),內(nèi)部實(shí)現(xiàn)如下:
檢查控制器狀態(tài)機(jī)是否處于等待指令輸入狀態(tài)。如果不是則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊處理;
讀取電機(jī)的當(dāng)前的狀態(tài)信息;
依據(jù)電機(jī)的當(dāng)前狀態(tài),分別調(diào)用點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,回到原點(diǎn)的運(yùn)動(dòng)規(guī)劃,并將生成的電機(jī)控制指令保存到指令緩沖區(qū);
將控制器狀態(tài)機(jī)設(shè)為相應(yīng)的狀態(tài)。比如點(diǎn)到點(diǎn)運(yùn)動(dòng)回調(diào)函數(shù),則將控制器狀態(tài)機(jī)設(shè)為點(diǎn)到點(diǎn)運(yùn)動(dòng)狀態(tài)。
b)暫停函數(shù),內(nèi)部實(shí)現(xiàn)如下:
檢查控制器狀態(tài)機(jī)是否是點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,回到原點(diǎn)等狀態(tài)。如果不是則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊進(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)部實(shí)現(xiàn)如下:
檢查控制器狀態(tài)機(jī)是否是暫停狀態(tài)。如果不是則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊處理;
將控制器狀態(tài)機(jī)的狀態(tài)轉(zhuǎn)變?yōu)闀和G暗臓顟B(tài)。
d)急停函數(shù),內(nèi)部實(shí)現(xiàn)如下:
ⅰ、檢查控制器狀態(tài)機(jī)是否是點(diǎn)到點(diǎn)運(yùn)動(dòng),直線運(yùn)動(dòng),圓弧運(yùn)動(dòng),手動(dòng)示教,回到原點(diǎn)等狀態(tài)。如果不是則退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊處理;
ⅱ、讀取電機(jī)的當(dāng)前的狀態(tài)信息;
ⅲ、將電機(jī)控制指令緩沖區(qū)清零;
ⅳ、調(diào)用速度規(guī)劃運(yùn)動(dòng)規(guī)劃,讓電機(jī)以最短時(shí)間,速度降為0,并將生成的電機(jī)控制指令保存到指令緩沖區(qū)。
(5)使用lua語(yǔ)言,編寫(xiě)算法模塊的啟動(dòng)文件,設(shè)置為執(zhí)行如下動(dòng)作:
a)通過(guò)orocos的import方法,加載算法模塊;
d)定義算法模塊的刷新頻率,線程的優(yōu)先級(jí)別;
c)對(duì)算法模塊的屬性進(jìn)行賦值;
d)通過(guò)orocos的connect方法,將算法模塊的輸入,輸出接口和總控模塊和通信管理模塊的接口建立連接。
e)通過(guò)orocos的start方法,運(yùn)行算法模塊,算法模塊先調(diào)用starthook()函數(shù),然后按設(shè)置的刷新頻率,實(shí)時(shí)地周期性調(diào)用updatehook()函數(shù)。
3、對(duì)于通信管理模塊:
通信管理模塊可以通過(guò)linux主機(jī)minicom中的ttyacm0與arm開(kāi)發(fā)板通信,可以在該arm開(kāi)發(fā)板上運(yùn)行一個(gè)canopen主站協(xié)議,該主站協(xié)議可以設(shè)置一個(gè)指令緩存區(qū),最多可以存儲(chǔ)25個(gè)指令。
通信管理模塊可以利用orocos的rtt::input與rtt::output方法與機(jī)器人算法模塊和總控模塊進(jìn)行通信。
利用rfsm軟件建立設(shè)備狀態(tài)機(jī),對(duì)通信管理模塊的業(yè)務(wù)邏輯進(jìn)行控制。
通信管理模塊利用orocos的rtt::input與rtt::output方法與設(shè)備狀態(tài)機(jī)連接,可改變?cè)O(shè)備狀態(tài)機(jī)的狀態(tài),并讀取狀態(tài)。
(1)通信模塊使用ros的orocreate-catkin-pkg方法創(chuàng)建為ros的package,然后在package中,通過(guò)繼承orocos的rtt::taskcontext類,創(chuàng)建一個(gè)orocos的實(shí)時(shí)模塊,記為ec_component。
在ec_component類的構(gòu)造函數(shù)中,設(shè)置為執(zhí)行如下操作:
a)通信管理模塊利用orocos的rtt::input與rtt::output方法,對(duì)輸入,輸出接口進(jìn)行定義。
其中輸入的接口包括:
①算法模塊傳入的控制指令數(shù)據(jù);
②設(shè)備狀態(tài)機(jī)的狀態(tài);
輸出的接口包括:
①診斷數(shù)據(jù),輸出至算法模塊和總控模塊;
②電機(jī)運(yùn)行數(shù)據(jù)和機(jī)械臂狀態(tài)數(shù)據(jù),輸出給算法模塊;
③狀態(tài)機(jī)事件觸發(fā),輸出給設(shè)備狀態(tài)機(jī)。
b)利用orocos的operationalcaller方法定義函數(shù)調(diào)用接口,通信管理模塊定義事件報(bào)告的接口,通過(guò)該接口觸發(fā)總控模塊的事件報(bào)告處理函數(shù)。
c)調(diào)用orocos的properties方法定義通信管理模塊的屬性,通信管理模塊定義一個(gè)機(jī)械臂關(guān)節(jié)個(gè)數(shù)的屬性。
(2)在ec_component的starthook()成員函數(shù)中,設(shè)置為執(zhí)行如下操作:
a)檢查日志報(bào)告是否正常,若異常直接退出,并將相關(guān)信息通過(guò)事件報(bào)告接口傳遞給總控模塊進(jìn)行處理;
b)電機(jī)驅(qū)動(dòng)初始化:
ⅰ、通過(guò)ttyacm0與電機(jī)驅(qū)動(dòng)器建立通信;
ⅱ、電機(jī)指令緩沖隊(duì)列清空;
ⅲ、電機(jī)使能,如果使能成功,則進(jìn)行下一步,否則退出;
ⅳ、讀取電機(jī)的位置,計(jì)算機(jī)器人的機(jī)械臂當(dāng)前狀態(tài),包括關(guān)節(jié)角度,機(jī)械臂末端位姿;
c)機(jī)械臂狀態(tài)初始化:
依據(jù)電機(jī)位置,判斷機(jī)械臂是否需要執(zhí)行回零運(yùn)動(dòng)。如果機(jī)械臂任一關(guān)節(jié)角度與零度相差大于0.01度,則執(zhí)行回零運(yùn)動(dòng),調(diào)用點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃,對(duì)回零運(yùn)動(dòng)進(jìn)行規(guī)劃。
d)改變?cè)O(shè)備狀態(tài)機(jī)的狀態(tài):
如果機(jī)器人的機(jī)械臂需要執(zhí)行回零運(yùn)動(dòng),則控制設(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)對(duì)于ec_component的updatehook()成員函數(shù),設(shè)置該函數(shù)在通信管理模塊運(yùn)行時(shí),以用戶設(shè)定的頻率實(shí)時(shí)運(yùn)行(如設(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ī)械臂的回零運(yùn)動(dòng)。此時(shí),執(zhí)行如下操作:
讀取系統(tǒng)的時(shí)鐘,依據(jù)回零運(yùn)動(dòng)軌跡規(guī)劃結(jié)果,計(jì)算該時(shí)刻電機(jī)的運(yùn)動(dòng)指令,并將該單條運(yùn)動(dòng)指令發(fā)送給canopen主站。
如果運(yùn)動(dòng)已回零成功,向設(shè)備狀態(tài)機(jī)發(fā)送“e_nominal”事件,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為active.nominal,并將該事件報(bào)告給總控模塊。
ⅱ、如果是active.nominal狀態(tài)。此時(shí),執(zhí)行如下操作:
從控制指令數(shù)據(jù)的輸入通道中,讀取控制指令,并存儲(chǔ)到電機(jī)指令緩沖隊(duì)列中。
讀取canopen主站指令緩沖區(qū)的現(xiàn)有指令個(gè)數(shù),如果小于10個(gè),則一次性從電機(jī)指令緩沖隊(duì)列中取出15個(gè)運(yùn)動(dòng)指令發(fā)送給canopen主站。如果電機(jī)指令緩沖隊(duì)列的指令個(gè)數(shù)小于15個(gè),則一次性全部發(fā)送給canopen主站。
ⅲ、如果是active.recovery狀態(tài)。此時(shí),通信管理模塊處于恢復(fù)狀態(tài)。
此時(shí),依據(jù)診斷信息,對(duì)系統(tǒng)狀態(tài)進(jìn)行恢復(fù),若恢復(fù)成功,向設(shè)備狀態(tài)機(jī)發(fā)送“e_nominal”事件,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為active.nominal。并將該事件報(bào)告給總控模塊。
若恢復(fù)不成功,給狀態(tài)機(jī)發(fā)送“e_fault”事件,狀態(tài)機(jī)轉(zhuǎn)換為fault狀態(tài),將該事件報(bào)告給總控模塊,并直接退出updatehook()。
ⅳ、如果是active.halt狀態(tài)。此時(shí),模塊處于暫停狀態(tài),執(zhí)行如下操作:檢查控制指令數(shù)據(jù)的輸入通道中是否有新的指令,若有新指令,則讀取控制指令,并存儲(chǔ)到電機(jī)指令緩沖隊(duì)列中。
ⅴ、如果是active.hands狀態(tài)。此時(shí),模塊處于手控模式,執(zhí)行如下操作:
讀取系統(tǒng)的時(shí)鐘,依據(jù)運(yùn)動(dòng)軌跡規(guī)劃結(jié)果,計(jì)算該時(shí)刻電機(jī)的運(yùn)動(dòng)指令,并將該單條運(yùn)動(dòng)指令發(fā)送給canopen主站。
ⅵ、如果是fault狀態(tài),則直接退出updatehook()。
c)讀取電機(jī)狀態(tài),根據(jù)機(jī)械臂的模型計(jì)算機(jī)械臂關(guān)節(jié)以及末端運(yùn)動(dòng)狀態(tài)信息,并通過(guò)輸出數(shù)據(jù)通道,傳遞給算法模塊與總控模塊;
d)檢查控制主站是否有錯(cuò)誤報(bào)告信息,如果有錯(cuò)誤信息,則將診斷信息傳遞給算法模塊與總控模塊。向設(shè)備狀態(tài)機(jī)發(fā)送“e_recovery”事件,將設(shè)備狀態(tài)機(jī)轉(zhuǎn)換為active.recovery狀態(tài)、并將該事件報(bào)告給總控模塊。
(4)對(duì)于ec_component的cleanuphook()成員函數(shù),該函數(shù)在模塊結(jié)束運(yùn)行時(shí),自動(dòng)調(diào)用,設(shè)置執(zhí)行如下操作:
a)關(guān)閉電機(jī)驅(qū)動(dòng)使能;
b)關(guān)閉電機(jī)驅(qū)動(dòng)連接。
(5)參考圖5所示,圖5是設(shè)備狀態(tài)機(jī)的狀態(tài)變化示意圖。共有init(初始化)、fault(中斷),active.recovery(恢復(fù))、active.hands(手動(dòng)示教)、active.halt(暫停)、active.nominal(運(yùn)轉(zhuǎn))六個(gè)狀態(tài)。active.recovery、active.hands、active.halt、active.nominal四個(gè)狀態(tài)組成一個(gè)active(使能)的狀態(tài)集合,active的狀態(tài)轉(zhuǎn)移規(guī)則,對(duì)四個(gè)子狀態(tài)均有效。
(6)使用lua語(yǔ)言,編寫(xiě)該模塊的啟動(dòng)文件,該啟動(dòng)文件設(shè)置執(zhí)行如下動(dòng)作:
a)通過(guò)orocos的import方法,加載通信管理模塊;
b)定義通信管理模塊的刷新頻率,線程的優(yōu)先級(jí)別;
c)對(duì)通信管理模塊的屬性進(jìn)行賦值;
d)通過(guò)orocos的connect方法,將通信管理模塊的輸入,輸出接口和總控模塊和算法模等的接口建立連接。
e)通過(guò)orocos的start方法,運(yùn)行通信管理模塊,通信管理模塊先調(diào)用starthook()函數(shù),然后按定義好的刷新頻率,實(shí)時(shí)地周期性調(diào)用updatehook()函數(shù)。
對(duì)于上述總控模塊、算法模塊和通信管理模塊,設(shè)置為當(dāng)運(yùn)行后,如果用戶需要中途停止該模塊,同時(shí)按鍵盤(pán)的ctrl鍵與d鍵。
綜上實(shí)施例,基于ros和orocos,利用orocos的實(shí)時(shí)性特點(diǎn),保證軟件程序的實(shí)時(shí)性能;充分利用ros的開(kāi)放性,基于ros、orocos開(kāi)發(fā)的總控模塊、算法模塊和通信管理模塊進(jìn)行實(shí)時(shí)通信,共同構(gòu)成一個(gè)完整的機(jī)器人控制器軟件;進(jìn)一步建立了控制器狀態(tài)機(jī)和設(shè)備狀態(tài)機(jī),從而實(shí)現(xiàn)對(duì)總控模塊、通信管理模塊的業(yè)務(wù)邏輯進(jìn)行有效管理。
通過(guò)orocos的rtt::input,rtt::output方法建立總控模塊、算法模塊和通信管理模塊的數(shù)據(jù)輸入、輸出通道,通過(guò)orocos的operationalcaller方法定義函數(shù)調(diào)用接口,通過(guò)orocos的properties方法定義總控模塊、算法模塊和通信管理模塊的屬性。因此,保證了總控模塊、算法模塊和通信管理模塊之間的獨(dú)立性與解耦性。
以上所述實(shí)施例的各技術(shù)特征可以進(jìn)行任意的組合,為使描述簡(jiǎn)潔,未對(duì)上述實(shí)施例中的各個(gè)技術(shù)特征所有可能的組合都進(jìn)行描述,然而,只要這些技術(shù)特征的組合不存在矛盾,都應(yīng)當(dāng)認(rèn)為是本說(shuō)明書(shū)記載的范圍。
以上所述實(shí)施例僅表達(dá)了本發(fā)明的幾種實(shí)施方式,其描述較為具體和詳細(xì),但并不能因此而理解為對(duì)發(fā)明專利范圍的限制。應(yīng)當(dāng)指出的是,對(duì)于本領(lǐng)域的普通技術(shù)人員來(lái)說(shuō),在不脫離本發(fā)明構(gòu)思的前提下,還可以做出若干變形和改進(jìn),這些都屬于本發(fā)明的保護(hù)范圍。因此,本發(fā)明專利的保護(hù)范圍應(yīng)以所附權(quán)利要求為準(zhǔn)。