本發(fā)明涉及機器人控制技術(shù)領(lǐng)域，特別是涉及一種機器人暫停運動控制方法和系統(tǒng)。

背景技術(shù)：

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

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

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

基于此，有必要針對上述技術(shù)問題，提供一種機器人暫停運動控制方法，降低系統(tǒng)開發(fā)成本，提高控制效果。

一種機器人暫停運動控制方法，包括：

接收控制端傳送的暫停運動指令；

在當(dāng)前機器人處于點到點運動、直線運動、圓弧運動或回到原點運動的任意一種當(dāng)前運動狀態(tài)下，執(zhí)行所述暫停運動指令；

獲取當(dāng)前運動的目標(biāo)點和運動的剩余時間，將所述目標(biāo)點和運動的剩余時間保存為恢復(fù)當(dāng)前運動的軌跡計算參數(shù)；讀取機械臂末端的當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度，并計算機械臂末端由當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度變化到停止時所經(jīng)過的暫停運動軌跡；

計算機械臂末端在所述暫停運動軌跡上運行時，機械臂關(guān)節(jié)在各個位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

一種機器人暫停運動控制系統(tǒng)，包括：總控模塊、算法模塊和通信管理模塊；

所述總控模塊，用于接收控制端傳送的暫停運動指令；在當(dāng)前機器人處于點到點運動、直線運動、圓弧運動或回到原點運動的任意一種當(dāng)前運動狀態(tài)下，執(zhí)行所述暫停運動指令；

所述算法模塊，用于獲取當(dāng)前運動的目標(biāo)點和運動的剩余時間，將所述目標(biāo)點和運動的剩余時間保存為恢復(fù)當(dāng)前運動的軌跡計算參數(shù)；讀取機械臂末端的當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度，并計算機械臂末端由當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度變化到停止時所經(jīng)過的暫停運動軌跡；計算機械臂末端在所述暫停運動軌跡上運行時，機械臂關(guān)節(jié)在各個位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；

所述通信管理模塊，用于將所述目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

上述機器人暫停運動控制方法和系統(tǒng)，在接收到控制端傳送的暫停運動指令后，在當(dāng)前機器人處于點到點運動、直線運動、圓弧運動或回到原點運動的任意一種當(dāng)前運動狀態(tài)下，計算暫停運動的運動軌跡，并實時計算機械臂各個關(guān)節(jié)在該運動軌跡上運行的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度等狀態(tài)參數(shù)，并轉(zhuǎn)發(fā)至控制主站；實現(xiàn)對機器人的暫停運動控制過程；該技術(shù)方案可以構(gòu)成一個完整的機器人暫停運動控制系統(tǒng)，降低機器人控制系統(tǒng)開發(fā)成本，提高控制效果。

附圖說明

圖1是本發(fā)明實施例的機器人暫停運動控制方法流程圖；

圖2是機器人暫停運動控制系統(tǒng)結(jié)構(gòu)示意圖；

圖3是總控模塊執(zhí)行算法流程圖；

圖4是算法模塊執(zhí)行算法流程圖；

圖5是一應(yīng)用實例的機器人控制系統(tǒng)的硬件結(jié)構(gòu)模型；

圖6是基于ros和orocos的搭建的軟件架構(gòu)圖；

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

圖8是設(shè)備狀態(tài)機的狀態(tài)變化示意圖。

具體實施方式

下面結(jié)合附圖闡述本發(fā)明的機器人暫停運動控制方法的實施例。

本發(fā)明實施例中，所述暫停運動，是指機械臂暫停當(dāng)前運動指令，速度降為0，當(dāng)收到恢復(fù)指令后，將沿當(dāng)前規(guī)劃的軌跡繼續(xù)運動，直到達(dá)到當(dāng)前指令的目標(biāo)位置。

參考圖1所示，圖1是本發(fā)明實施例的機器人暫停運動控制方法流程圖，包括：

s10，接收控制端傳送的暫停運動指令；

上述步驟中，可以利用預(yù)設(shè)的通信協(xié)議并以異步遠(yuǎn)程過程調(diào)用的方式接收暫停運動指令；

在此過程中，可以是操作者通過控制端的人機交互界面生成暫停運動指令，該指令無需傳遞參數(shù)；通過預(yù)設(shè)的通信協(xié)議，如基于iec(theinternetcommunicationsengine，互聯(lián)網(wǎng)通信引擎)開發(fā)的通信協(xié)議，以異步遠(yuǎn)程過程調(diào)用的方式從人機交互界面接收暫停運動指令。

s20，在當(dāng)前機器人處于點到點運動、直線運動、圓弧運動或回到原點運動的任意一種當(dāng)前運動狀態(tài)下，執(zhí)行所述暫停運動指令；

本步驟中，通過判斷當(dāng)前機器人運動狀態(tài)是否為可執(zhí)行暫停運動的點到點運動、直線運動、圓弧運動或回到原點運動狀態(tài)，若是，執(zhí)行該暫停運動指令；

在一個實施例中，可以實時監(jiān)測控制器狀態(tài)機的狀態(tài)，當(dāng)轉(zhuǎn)換為執(zhí)行暫停運動狀態(tài)時，調(diào)用暫停運動規(guī)劃函數(shù)，開始執(zhí)行急停運動規(guī)劃流程；

對于判斷當(dāng)前機器人運動狀態(tài)是否為可執(zhí)行暫停運動的指定運動狀態(tài)，可以通過判斷控制器狀態(tài)機是否為點到點運動、直線運動、圓弧運動、回到原點運動的四種狀態(tài)之一，若是，執(zhí)行調(diào)用所述暫停運動規(guī)劃函數(shù)的步驟，否則，拒絕執(zhí)行該暫停運行指令。

s30，獲取當(dāng)前運動的目標(biāo)點和運動的剩余時間，將所述目標(biāo)點和運動的剩余時間保存為恢復(fù)當(dāng)前運動的軌跡計算參數(shù)；讀取機械臂末端的當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度，并計算機械臂末端由當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度變化到停止時所經(jīng)過的暫停運動軌跡；

在一個實施例中，執(zhí)行所述暫停運動指令后，可以異步觸發(fā)暫停運動執(zhí)行函數(shù)并通過第一接口調(diào)用暫停運動規(guī)劃函數(shù)；其中，所述第一接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口；

進(jìn)一步的，可以通過orocos的operationalcaller方法調(diào)用所述暫停運動規(guī)劃函數(shù)；

執(zhí)行所述暫停運動規(guī)劃函數(shù)，獲取并記錄當(dāng)前運動的目標(biāo)點和運動的剩余時間，以及讀取機械臂末端的當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度，并根據(jù)所述當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度計算暫停運動軌跡；

在一個實施例中，執(zhí)行所述暫停運動規(guī)劃函數(shù)，將所述控制器狀態(tài)機轉(zhuǎn)換為執(zhí)行暫停運動(active.halt)狀態(tài)。

作為實施例，若所述當(dāng)前運動為點到點運動或回到原點運動，則計算暫停運動軌跡的方法可以包括如下步驟：

(a1)計算當(dāng)前點到點或回到原點運動的目標(biāo)點，根據(jù)當(dāng)前運動的總時長與當(dāng)前運動已經(jīng)歷的時間計算當(dāng)前運動的剩余時間，記錄所述目標(biāo)點和剩余時間；

所述當(dāng)前點到點或回到原點運動的目標(biāo)點θ1，當(dāng)前運動的剩余時間：tr＝t0-t0，其中t0是當(dāng)前運動的總時長，t0是當(dāng)前運動已經(jīng)歷的時間；

(a2)讀取機械臂當(dāng)前的關(guān)節(jié)角度，關(guān)節(jié)角速度，以及關(guān)節(jié)角加速度；

具體的，讀取機械臂當(dāng)前的關(guān)節(jié)角度θ0，關(guān)節(jié)角速度關(guān)節(jié)角加速度

(a3)將當(dāng)前時間記為0，依據(jù)最大加速度使速度降為0的原則，計算各個關(guān)節(jié)速度降為0的時間，記為暫停運行指令執(zhí)行時間；

所述各個關(guān)節(jié)速度降為0的時間計算公式為：

其中，i代表關(guān)節(jié)的標(biāo)號，選取ti中的最大值，記為暫停運行指令執(zhí)行時間t；

(a4)根據(jù)暫停運行指令執(zhí)行時間t得到暫停運行軌跡。

在一個實施例中，若所述當(dāng)前運動為直線運動，則計算暫停運動軌跡的方法可以包括如下步驟：

(b1)計算當(dāng)前直線運動的目標(biāo)點，根據(jù)當(dāng)前運動的總時長與當(dāng)前運動已經(jīng)歷的時間計算當(dāng)前運動的剩余時間，記錄所述目標(biāo)點和剩余時間；

具體的，所述當(dāng)前直線運動的目標(biāo)點se，當(dāng)前運動的剩余時間：tr＝t0-t0，其中，t0是當(dāng)前運動的總時長，t0是當(dāng)前運動已經(jīng)歷的時間。

(b2)根據(jù)直線運動軌跡計算機械臂當(dāng)前時刻的軌跡，關(guān)節(jié)角速度和關(guān)節(jié)角加速度；

具體的，當(dāng)前直線運動軌跡已由已知算法規(guī)劃，因此可以計算機械臂當(dāng)前時刻的軌跡s0，關(guān)節(jié)角速度關(guān)節(jié)角加速度

(b3)根據(jù)所述軌跡，關(guān)節(jié)角速度和關(guān)節(jié)角加速度計算暫停直線運動需要走過的軌跡，得到暫停運動軌跡，并記錄恢復(fù)運動時機械臂沿直線已走過的軌跡；

具體的，暫停直線運動需要走過的軌跡計算公式為：

其中，amax為直線運動最大的加速度；記錄s2＝s0+s1，其中，s2為恢復(fù)運動時機械臂沿直線已走過的軌跡。

在一個實施例中，若所述當(dāng)前運動為圓弧運動，則計算暫停運動軌跡的方法可以包括如下步驟：

(c1)計算當(dāng)前圓弧運動的目標(biāo)點，根據(jù)當(dāng)前運動的總時長與當(dāng)前運動已經(jīng)歷的時間計算當(dāng)前運動的剩余時間，記錄所述目標(biāo)點和剩余時間；

所述當(dāng)前圓弧運動的目標(biāo)點se，當(dāng)前運動的剩余時間：tr＝t0-t0，其中，t0是當(dāng)前運動的總時長，t0是當(dāng)前運動已經(jīng)歷的時間；

(c2)根據(jù)圓弧運動軌跡計算機械臂當(dāng)前時刻的軌跡，關(guān)節(jié)角速度和關(guān)節(jié)角加速度；

具體的，當(dāng)前圓弧運動軌跡已由已知算法規(guī)劃，因此可以計算機械臂當(dāng)前時刻的軌跡s0，關(guān)節(jié)角速度關(guān)節(jié)角加速度

(c3)根據(jù)所述軌跡，關(guān)節(jié)角速度和關(guān)節(jié)角加速度計算暫停圓弧運動需要走過的軌跡，得到暫停運行軌跡，并記錄恢復(fù)運動時機械臂沿圓弧已走過的軌跡；

暫停圓弧運動需要走過的軌跡計算公式為：

其中，amax為圓弧運動最大的加速度；記錄s2＝s0+s1，其中，s2為恢復(fù)運動時機械臂沿圓弧已走過的軌跡。

s40，計算機械臂末端在所述暫停運動軌跡上運行時，機械臂關(guān)節(jié)在各個位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

在一個實施例中，通過第二接口將所述機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度發(fā)送給設(shè)備通信軟件轉(zhuǎn)發(fā)至控制主站；其中，所述第二接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口。

作為實施例，對于計算機械臂末端在所述暫停運動軌跡上運行時，機械臂關(guān)節(jié)在各個位置的機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法，可以包括如下步驟：

(a)在機械臂開始在所述暫停運動軌跡上運動后，統(tǒng)計機械臂的運動時間tn；

(b)若所述運動時間滿足：tn≤t，每隔設(shè)定時間τ計算一次機械臂每個關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；τ＝1/f，f表示刷新頻率；一般情況下，所述τ為1毫秒；

(c)若運動時間tn滿足：tn＞t，則停止運動；其中，t為暫停運行指令執(zhí)行時間。

如上述實施例，若所述當(dāng)前運動為點到點運動或回到原點運動，則計算機械臂每個關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法包括如下步驟：

(a5)根據(jù)機械臂當(dāng)前的關(guān)節(jié)角速度和所述暫停運行指令執(zhí)行時間計算各個機械臂關(guān)節(jié)的目標(biāo)角加速度；

具體的，機械臂關(guān)節(jié)的目標(biāo)角加速度計算公式為：

(a6)根據(jù)所述目標(biāo)角加速度計算各個機械臂關(guān)節(jié)的目標(biāo)角速度與關(guān)節(jié)目標(biāo)角度；

具體的，各個機械臂關(guān)節(jié)的目標(biāo)角速度與關(guān)節(jié)目標(biāo)角度θi的計算公式分別為：

式中，θ0為機械臂當(dāng)前的關(guān)節(jié)角度，為關(guān)節(jié)角速度，為關(guān)節(jié)角加速度；t表示暫停運動過程中的時刻。

如上述實施例，若所述當(dāng)前運動為直線運動，則計算機械臂每個關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法包括如下步驟：

(b4)計算暫停運動中機械臂任一時刻t的軌跡位置、軌跡速度和軌跡加速度：

具體的，任一時刻t的軌跡位置、軌跡速度和軌跡加速度：

其中，s0為機械臂當(dāng)前時刻的軌跡，為關(guān)節(jié)角速度，為關(guān)節(jié)角加速度，s為軌跡位置，為軌跡速度，為軌跡加速度；

(b5)根據(jù)所述軌跡位置、軌跡速度和軌跡加速度計算機械臂末端的目標(biāo)位置、速度與加速度；

具體的，機械臂末端的目標(biāo)位置、速度與加速度的計算公式為：

其中，pt為目標(biāo)位置，為速度，為加速度，是當(dāng)前直線運動的單位向量；

(b6)使用逆運動學(xué)方法根據(jù)所述目標(biāo)位置，速度與加速度計算機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；

具體的，所述機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的計算公式為：

θt＝invkinematics(pt)

其中，θt為目標(biāo)角度，為目標(biāo)角速度，為目標(biāo)角加速度，invkinematics()表示逆運動學(xué)的計算公式，j為雅可比矩陣，為雅可比矩陣的一階導(dǎo)，j⁺為的偽逆。

如上述實施例，若所述當(dāng)前運動為圓弧運動，則計算機械臂每個關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法包括如下步驟：

(c4)計算暫停運動中機械臂任一時刻t的軌跡位置、軌跡速度和軌跡加速度；

具體的，任一時刻t的軌跡位置、軌跡速度和軌跡加速度：

其中，s0為機械臂當(dāng)前時刻的軌跡，為關(guān)節(jié)角速度，為關(guān)節(jié)角加速度，s為軌跡位置，為軌跡速度，為軌跡加速度；

(c5)根據(jù)所述軌跡位置、軌跡速度和軌跡加速度計算機械臂末端的目標(biāo)位置、速度與加速度；

具體的，機械臂末端的目標(biāo)位置pt＝[xt,yt,zt]^t：

其中，θt為圓弧p0pt對應(yīng)的圓心角，滿足a1,a2,a3,b1,b2,b3，r等參數(shù)均是當(dāng)前圓弧運動軌跡已知參數(shù)；

機械臂末端的目標(biāo)速度和加速度計算公式為：

其中，是點pt的切向量；

(c6)使用逆運動學(xué)方法根據(jù)所述目標(biāo)位置，速度與加速度計算機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度。

所述機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的計算公式為：

θt＝invkinematics(pt)

其中，θt為目標(biāo)角度，為目標(biāo)角速度，為目標(biāo)角加速度，invkinematics()表示逆運動學(xué)的計算公式，j為雅可比矩陣，為雅可比矩陣的一階導(dǎo)，j⁺為的偽逆。

上述實施例的技術(shù)方案，利用orocos的實時輸入/輸出接口，通過設(shè)定通信協(xié)議以異步遠(yuǎn)程過程調(diào)用的方式接收暫停運動指令，啟動暫停運動指令觸發(fā)函數(shù)，調(diào)用機器人運動規(guī)劃的暫停運動執(zhí)行函數(shù)，根據(jù)該函數(shù)計算暫停運動的運動軌跡，并實時計算機械臂各個關(guān)節(jié)在該運動軌跡上運行的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度等狀態(tài)參數(shù)，通過設(shè)備通信軟件將上述狀態(tài)參數(shù)轉(zhuǎn)發(fā)至控制主站，實現(xiàn)對機器人的暫停運動控制過程；該技術(shù)方案可以進(jìn)行控制指令的接收、解析，算法調(diào)用、執(zhí)行，參數(shù)實時計算和傳輸?shù)裙δ?，可以?gòu)成一個完整的機器人暫停運動控制系統(tǒng)，降低機器人控制系統(tǒng)開發(fā)成本，提高控制效果；另外結(jié)合了控制器狀態(tài)機的應(yīng)用，實現(xiàn)對算法處理過程的優(yōu)化控制，進(jìn)一步提高了控制效果。

針對于機器人暫停運動控制方法，本發(fā)明提供了該控制方法對應(yīng)的機器人暫停運動控制系統(tǒng)，

參考圖2所示，圖2是機器人暫停運動控制系統(tǒng)結(jié)構(gòu)示意圖，包括：總控模塊、算法模塊和通信管理模塊；

所述總控模塊，用于接收控制端傳送的暫停運動指令；在當(dāng)前機器人處于點到點運動、直線運動、圓弧運動或回到原點運動的任意一種當(dāng)前運動狀態(tài)下，執(zhí)行所述暫停運動指令；

所述算法模塊，用于獲取當(dāng)前運動的目標(biāo)點和運動的剩余時間，將所述目標(biāo)點和運動的剩余時間保存為恢復(fù)當(dāng)前運動的軌跡計算參數(shù)；讀取機械臂末端的當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度，并計算機械臂末端由當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度變化到停止時所經(jīng)過的暫停運動軌跡；計算機械臂末端在所述暫停運動軌跡上運行時，機械臂關(guān)節(jié)在各個位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；

所述通信管理模塊，用于將所述目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

在一個實施例中，所述總控模塊通過第一接口與算法模塊進(jìn)行通信連接，算法模塊通過第二接口與通信管理模塊進(jìn)行通信連接，所述第一接口、第二接口是基于ros上創(chuàng)建orocos的實時輸入/輸出接口；

所述總控模塊執(zhí)行暫停運動指令后，異步觸發(fā)暫停運動執(zhí)行函數(shù)并通過第一接口調(diào)用暫停運動規(guī)劃函數(shù)；

所述算法模塊執(zhí)行所述暫停運動規(guī)劃函數(shù)，并計算暫停運動軌跡；

以及通過第二接口將所述機械臂關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度發(fā)送給通信管理模塊。

上述實施例的技術(shù)方案，利用orocos的實時輸入/輸出接口，總控模塊通過設(shè)定通信協(xié)議以異步遠(yuǎn)程過程調(diào)用的方式接收暫停運動指令，啟動暫停運動指令觸發(fā)函數(shù)，調(diào)用算法模塊的機器人運動規(guī)劃的暫停運動執(zhí)行函數(shù)，根據(jù)該函數(shù)計算暫停運動的運動軌跡，并實時計算機械臂各個關(guān)節(jié)在該運動軌跡上運行的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度等狀態(tài)參數(shù)，通信管理模塊將上述狀態(tài)參數(shù)轉(zhuǎn)發(fā)至控制主站，實現(xiàn)對機器人的暫停運動控制過程；該技術(shù)方案可以進(jìn)行控制指令的接收、解析，算法調(diào)用、執(zhí)行，參數(shù)實時計算和傳輸?shù)裙δ埽梢詷?gòu)成一個完整的機器人暫停運動控制系統(tǒng)，降低機器人控制系統(tǒng)開發(fā)成本，提高控制效果。

作為實施例，總控模塊執(zhí)行算法流程，可以參考圖3所示，圖3是總控模塊執(zhí)行算法流程圖；具體如下：

1)操作者通過人機交互界面生成暫停運動指令。

2)總控模塊通過通信協(xié)議，以異步遠(yuǎn)程過程調(diào)用的方式從人機交互界面接收暫停運動指令。

3)指令到達(dá)總控模塊后，異步觸發(fā)暫停運動執(zhí)行函數(shù)，該函數(shù)首先判斷控制器狀態(tài)機是否為點到點運動，直線運動，圓弧運動，回到原點的四種狀態(tài)之一。若不是，則拒絕執(zhí)行此次暫停運行指令。

4)若控制器狀態(tài)機是點到點運動，直線運動，圓弧運動，回到原點的四種狀態(tài)之一，則執(zhí)行如下操作：

a)通過orocos的operationalcaller方法調(diào)用算法模塊的暫停運動規(guī)劃函數(shù)。

b)將控制器狀態(tài)機轉(zhuǎn)換為執(zhí)行暫停運動(active.halt)狀態(tài)。

作為實施例，算法模塊執(zhí)行算法流程，可以參考圖4所示，圖4是算法模塊執(zhí)行算法流程圖；具體如下：

1)暫停運動規(guī)劃函數(shù)被總控模塊調(diào)用，開始執(zhí)行暫停運動規(guī)劃流程。

2)若當(dāng)前運動是回零運動或者點到點運動，則調(diào)用點到點暫停規(guī)劃算法：

a)記錄當(dāng)前運動的目標(biāo)點θ1；

b)記錄當(dāng)前運動的剩余時間：tr＝t–t。其中t是當(dāng)前運動的總時長，t是當(dāng)前運動已經(jīng)歷的時間；

c)讀取機械臂當(dāng)前的關(guān)節(jié)角度θ0，關(guān)節(jié)角速度關(guān)節(jié)角加速度

d)將當(dāng)前時間記為0，依據(jù)最大加速度使速度降為0的原則，計算各個關(guān)節(jié)速度降為0的時間：

其中，i代表關(guān)節(jié)的標(biāo)號。選取ti中的最大值，記為暫停指令執(zhí)行時間，記為t。

e)計算各個關(guān)節(jié)的加速度為：

f)t時刻對應(yīng)的速度與關(guān)節(jié)角位置分別為：

3)若當(dāng)前運動是直線運動，則調(diào)用直線暫停的算法：

a)記錄當(dāng)前運動的目標(biāo)點se。

b)記錄當(dāng)前運動的剩余時間：tr＝t–t。其中t是當(dāng)前運動的總時長，t是當(dāng)前運動已經(jīng)歷的時間；

c)由于當(dāng)前直線運動軌跡可以由五次多項式算法規(guī)劃，因此可以計算機械臂當(dāng)前時刻的軌跡s0，關(guān)節(jié)角速度關(guān)節(jié)角加速度

d)計算暫停直線運動需要走過的軌跡：

其中，amax就直線運動最大的加速度。

e)記錄恢復(fù)運動時，機械臂沿直線已走過的軌跡：s2＝s0+s1；

f)計算暫停運動中機械臂任一時刻t的軌跡，軌跡速度，軌跡加速度：

g)計算機械臂末端的目標(biāo)位置，速度與加速度：

其中，是直線運動的單位向量。

h)使用逆運動學(xué)方法，計算關(guān)節(jié)角的位置：

θt＝invkinematics(pt)(8)

i)關(guān)節(jié)的角速度與角加速度，可由如下表達(dá)式求出：

其中，j為雅可比(jacobian)矩陣，為雅可比矩陣的一階導(dǎo)，j⁺為的偽逆(pseudoinverse)。

4)若當(dāng)前運動是圓弧運動，則調(diào)用圓弧暫停的算法：

a)記錄當(dāng)前運動的目標(biāo)軌跡se；

b)記錄當(dāng)前運動的剩余時間：tr＝t–t。其中t是當(dāng)前運動的總時長，t是當(dāng)前運動已經(jīng)歷的時間；

c)由于當(dāng)前圓弧運動軌跡可以由五次多項式算法規(guī)劃，因此可以計算機械臂當(dāng)前時刻的軌跡s0，關(guān)節(jié)角速度關(guān)節(jié)角加速度

d)計算暫停圓弧運動需要走過的軌跡：

其中，amax就直線運動最大的加速度。

e)記錄恢復(fù)運動時，機械臂沿圓弧已走過的軌跡：s2＝s0+s1

f)計算暫停運動中機械臂任一時刻t的軌跡，軌跡速度，軌跡加速度：

g)計算末端的目標(biāo)位置pt＝[xt,yt,zt]^t：

其中，θt為圓弧p0pt對應(yīng)的圓心角，滿足a1,a2,a3,b1,b2,b3，r等參數(shù)可以由圓弧運動五次多項式規(guī)劃算法求得；

h)求得末端的目標(biāo)速度和加速度

其中，是點pt的切向量；

i)使用逆運動學(xué)方法，計算關(guān)節(jié)角的位置：

θt＝invkinematics(pt)(16)

其中，invkinematics()代表逆運動學(xué)的計算公式。

j)關(guān)節(jié)的角速度與角加速度，可由如下表達(dá)式求出：

其中，j為雅可比(jacobian)矩陣，為雅可比矩陣的一階導(dǎo)，j⁺為的偽逆(pseudoinverse)。

5)運動時間tn不超過運動時長t：

a)每隔1毫秒分別計算一次機械臂每個關(guān)節(jié)的目標(biāo)角度，目標(biāo)角速度和目標(biāo)角加速度；

b)將機械臂每個關(guān)節(jié)的目標(biāo)角度，目標(biāo)角速度和目標(biāo)角加速度，發(fā)送給通信管理模塊。

6)運動時間tn超過運動時長t，則停止運動。

作為實施例，通信管理模塊，可以進(jìn)一步用于執(zhí)行如下算法：

讀取機器人電機的狀態(tài)信息，根據(jù)機器人運動模型計算機器人機械臂的狀態(tài)信息，將機器人的狀態(tài)信息通過第三接口反饋給總控模塊，將機器人的狀態(tài)信息通過第二接口反饋給算法模塊；其中，所述電機的狀態(tài)信息包括位置、速度和力矩等；所述機械臂的狀態(tài)信息包括關(guān)節(jié)角度、關(guān)節(jié)角速度、關(guān)節(jié)角加速度、末端位姿、末端線速度、末端角速度、末端線加速度和末端線加速度等。

為了更加清晰本發(fā)明的實施例的技術(shù)方案，下面闡述采用本發(fā)明的硬件和軟件環(huán)境應(yīng)用實例：

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

linux主機可以安裝如下軟件：安裝xenomai或者rtai或者rtpreempt的實時內(nèi)核補??；安裝ros，orocos，rfsm等軟件。

參考圖6所示，圖6是基于ros和orocos的搭建的軟件架構(gòu)圖；在控制過程中，操作系統(tǒng)上運行總控模塊、算法模塊和通信管理模塊。

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)反饋信息：包括電機運行狀態(tài)等等；

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

輸出的接口包括：

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

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

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

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

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

c)調(diào)用orocos的properties方法定義總控模塊的屬性，將總控模塊定義一個機械臂關(guān)節(jié)個數(shù)的屬性。

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

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

b)通過ice開發(fā)的通信協(xié)議建立與人機交互界面的通信連接，調(diào)用通信協(xié)議提供的動態(tài)異步遠(yuǎn)程過程調(diào)用(rpc)方法，對人機交互界面發(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ù)。

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

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

(4)對于控制器狀態(tài)機，參考圖7所示，圖7是控制器狀態(tài)機的狀態(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)機的狀態(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方法，運行總控模塊，總控模塊將先調(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)行定義。

其中輸入的接口包括：

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

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

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

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

輸出的接口包括：

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

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

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

b)利用orocos的operationalcaller方法設(shè)置函數(shù)調(diào)用接口，設(shè)置事件報告的接口：該接口將觸發(fā)總控模塊的事件報告處理函數(shù)設(shè)置各種運動規(guī)劃的返回原點指令響應(yīng)函數(shù)。

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

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

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

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

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

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

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

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

如果指令緩沖區(qū)的控制指令個數(shù)小于20個，則將所有指令一起發(fā)送給通信管理模塊，并將控制器狀態(tài)機的狀態(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)機是否處于等待指令輸入狀態(tài)。如果不是則退出，并將相關(guān)信息通過事件報告接口傳遞給總控模塊處理；

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

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

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

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

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

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

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

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

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

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

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

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

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

ⅳ、調(diào)用速度規(guī)劃運動規(guī)劃，讓電機以最短時間，速度降為0，并將生成的電機控制指令保存到指令緩沖區(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主機minicom中的ttyacm0與arm開發(fā)板通信，可以在該arm開發(fā)板上運行一個canopen主站協(xié)議，該主站協(xié)議可以設(shè)置一個指令緩存區(qū)，最多可以存儲25個指令。

通信管理模塊可以利用orocos的rtt：：input與rtt：：output方法與機器人算法模塊和總控模塊進(jìn)行通信。

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

通信管理模塊利用orocos的rtt：：input與rtt：：output方法與設(shè)備狀態(tài)機連接，可改變設(shè)備狀態(tài)機的狀態(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)機的狀態(tài)；

輸出的接口包括：

①診斷數(shù)據(jù)，輸出至算法模塊和總控模塊；

②電機運行數(shù)據(jù)和機械臂狀態(tài)數(shù)據(jù)，輸出給算法模塊；

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

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

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

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

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

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

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

ⅱ、電機指令緩沖隊列清空；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

c)讀取電機狀態(tài)，根據(jù)機械臂的模型計算機械臂關(guān)節(jié)以及末端運動狀態(tài)信息，并通過輸出數(shù)據(jù)通道，傳遞給算法模塊與總控模塊；

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

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

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

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

(5)參考圖8所示，圖8是設(shè)備狀態(tài)機的狀態(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ìn)一步建立了控制器狀態(tài)機和設(shè)備狀態(tài)機，從而實現(xiàn)對總控模塊、通信管理模塊的業(yè)務(wù)邏輯進(jìn)行有效管理。

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

以上所述實施例的各技術(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)。