機器人點到點運動控制方法和系統與流程

本發明涉及機器人控制技術領域，特別是涉及一種機器人點到點運動控制方法和系統。

背景技術：

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

因此，機器人點到點運動是機器人一種重要運動方式，目前在一些應用方案中，在ros上采用orocos，但現有技術方案在搭建的架構上，不能構成一個完整的機器人控制器軟件，在執行點到點運動時，無法充分利用ros和orocos的特性，系統開發成本高，控制效果差。

技術實現要素：

基于此，有必要針對上述系統開發成本高技術問題，提供一種機器人點到點運動控制方法，降低系統開發成本，提高控制效果。

一種機器人點到點運動控制方法，包括：

接收控制端傳送的點到點運動指令；對點到點運動指令進行解析，獲取目標關節角度和點到點運動需要的時間；

獲取機械臂的當前關節角度、當前關節角速度和當前關節角加速度；根據點到點運動需要的時間計算機械臂由當前關節角度運行到目標關節角度經過的第一軌跡，以及分別計算機械臂在所述點到點運動需要的時間內由當前關節角速度和當前關節角加速度降為0所經過的第二軌跡和第三軌跡；根據第一軌跡、第二軌跡和第三軌跡得到點到點的運動軌跡；

計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度轉發至控制主站。

一種機器人點到點運動控制系統，包括：總控模塊、算法模塊和通信管理模塊；

所述總控模塊，用于接收控制端傳送的點到點運動指令；

所述算法模塊，用于對點到點運動指令進行解析，獲取目標關節角度和點到點運動需要的時間；獲取機械臂的當前關節角度、當前關節角速度和當前關節角加速度；根據點到點運動需要的時間計算機械臂由當前關節角度運行到目標關節角度經過的第一軌跡，以及分別計算機械臂在所述點到點運動需要的時間內由當前關節角速度和當前關節角加速度降為0所經過的第二軌跡和第三軌跡；根據第一軌跡、第二軌跡和第三軌跡得到點到點的運動軌跡；以及計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度；

所述通信管理模塊，用于將機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度轉發至控制主站。

上述機器人點到點運動控制方法和系統，在接收到控制端的點到點運動指令后，計算點到點的運動軌跡，并實時計算機械臂各個關節在該運動軌跡上運行的目標角度、目標角速度和目標角加速度等狀態參數，通過設備通信軟件將上述狀態參數轉發至控制主站，實現對機器人的點到點運動控制過程；該技術方案可以構成一個完整的機器人點到點運動控制系統，降低機器人控制系統開發成本，提高控制效果。

附圖說明

圖1是本發明實施例的機器人點到點運動控制方法流程圖；

圖2是機器人點到點運動控制系統結構示意圖；

圖3是總控模塊執行算法流程圖；

圖4是算法模塊執行算法流程圖；

圖5是算法模塊執行算法狀態轉換圖；

圖6是一應用實例的機器人控制系統的硬件結構模型；

圖7是基于ros和orocos的搭建的軟件架構圖；

圖8是控制器狀態機的狀態變化示意圖；

圖9是設備狀態機的狀態變化示意圖。

具體實施方式

下面結合附圖闡述本發明的機器人點到點運動控制方法的實施例。

本發明實施例中，所述點到點運動，是指機械臂的關節角從當前關節角度θ0運動到目標關節角度θ1的運動過程。

s10，接收控制端傳送的點到點運動指令；對點到點運動指令進行解析，獲取目標關節角度和點到點運動需要的時間；

上述步驟中，可以利用預設的通信協議并以異步遠程過程調用的方式接收點到點運動指令；例如，所述點到點運動指令包括目標關節角度θ1和點到點運動需要的時間t；

在此過程中，可以是操作者通過人機交互界面生成點到點運動指令，該指令無需傳遞參數；通過預設的通信協議，如基于iec(theinternetcommunicationsengine，互聯網通信引擎)開發的通信協議，以異步遠程過程調用的方式從人機交互界面接收點到點運動指令。

在一個實施例中，根據所述點到點運動指令異步觸發點到點運動執行函數并通過第一接口調用點到點運動規劃函數；其中，所述第一接口是基于ros上創建orocos的實時輸入/輸出接口。

s20，獲取機械臂的當前關節角度、當前關節角速度和當前關節角加速度；根據點到點運動需要的時間計算機械臂由當前關節角度運行到目標關節角度經過的第一軌跡，以及分別計算機械臂在所述點到點運動需要的時間內由當前關節角速度和當前關節角加速度降為0所經過的第二軌跡和第三軌跡；根據第一軌跡、第二軌跡和第三軌跡得到點到點的運動軌跡；

在一個實施例中，在調用所述點到點運動規劃函數后，讀取機械臂當前的關節角度、關節角速度和關節角加速度，并根據所述關節角度、關節角速度和關節角加速度和點到點運動指令計算點到點的運動軌跡；其中，所述關節角度、關節角速度和關節角加速度包括關節角度、關節角速度和關節角加速度；

進一步地，調用點到點運動規劃函數前，所述點到點運動執行函數判斷控制器狀態機是否為準備(ready)狀態；若是，通過orocos的operationalcaller方法調用所述點到點運動規劃函數，并將控制器狀態機切換為執行點到點運動(active.ptp)狀態；若否，則拒絕執行此次指令；其中，所述控制器狀態機的可被改變狀態、并讀取狀態，設有初始化、指令等待、指令執行、中斷和使能對應的狀態。

進一步地，在調用點到點運動規劃函數后，根據所述點到點運動規劃函數執行點到點運動規劃流程，并檢查控制器狀態機是否為執行點到點運動狀態；若是，執行所述計算點到點的運動軌跡的步驟，否則，退出執行流程。

作為實施例，所述計算點到點的運動軌跡的方法，可以包括如下：

(1)獲取機械臂當前的關節角度θ0、關節角速度和關節角加速度

(2)從點到點運動指令包括獲取目標關節角度θ1和點到點運動需要的時間t；

(3)根據所述關節角度θ0、關節角速度和關節角加速度以及目標關節角度θ1，將機械臂關節目標角速度和目標角加速度設為0，確定點到點的運動軌跡；

作為實施例，所述點到點的運動軌跡可以表示如下：

s(t)＝a0+a1t+a2t²+a3t³+a4t⁴+a5t⁵

a0＝θ0

式中，t是運行時間；ai，i＝1,…,5是系數。

s30，計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度轉發至控制主站。

具體的，可以計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度，通過第二接口將目標角度、目標角速度和目標角加速度發送給設備通信軟件轉發至控制主站；所述第二接口是基于ros上創建orocos的實時輸入/輸出接口；

在一個實施例中，所述計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度的步驟，可以包括如下：

(1)根據所述點到點的運動軌跡計算任意時刻t時的關節角位置θt。、角速度。和角加速度

進一步地，所述計算公式可以表示如下：

θt＝a0+a1t+a2t²+a3t³+a4t⁴+a5t⁵

式中，任意時刻t時的關節角位置θt。、角速度。和角加速度

(2)根據點到點運動需要的時間t，調用所述計算公式計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度。

進一步地，計算目標角度、目標角速度和目標角加速度的步驟可以包括如下：

(a)在機械臂開始運動后，由0開始每隔設定時間τ統計機械臂的運動時間tn；τ＝1/f，f表示刷新頻率；

即由零開始統計時間，按照刷新率不斷計算機械臂各個關節的目標角度、目標角速度和目標角加速度；上述實施例中，一般情況下，所述τ為1毫秒；

(b)若所述運動時間滿足：tn≤at，每隔設定時間τ依據所述計算公式分別計算一次機械臂每個關節的目標角度、目標角速度和目標角加速度；一般情況下，所述a＝70％；

(c)若運動時間tn滿足：at＜tn＜t，將標志位更改為過渡狀態，將控制器狀態機狀態設定為準備狀態；

進一步地，在過渡狀態下，若有新指令輸入，則啟動過渡運動規劃；若沒有新指令輸入，則每隔設定時間依據所述計算公式分別計算一次機械臂每個關節的目標角度，目標角速度和目標角加速度，并發送給設備通信軟件；

(d)若運動時間tn滿足：tn≥t，點到點運動結束，將標志位更改為等待狀態。

上述實施例的技術方案，可以利用orocos的實時輸入/輸出接口，通過設定通信協議以異步遠程過程調用的方式接收點到點運動指令，啟動點到點運動指令觸發函數，調用機器人運動規劃的點到點運動執行函數，根據該函數計算點到點的運動軌跡，并實時計算機械臂各個關節在該運動軌跡上運行的目標角度、目標角速度和目標角加速度等狀態參數，通過設備通信軟件將上述狀態參數轉發至控制主站，實現對機器人的點到點運動控制過程；該技術方案可以進行控制指令的接收、解析，算法調用、執行，參數實時計算和傳輸等功能，可以構成一個完整的機器人點到點運動控制系統，降低機器人控制系統開發成本，提高控制效果。另外結合了控制器狀態機和標志位的應用，實現對算法處理過程的優化控制，進一步提高了控制效果。

針對于機器人點到點運動控制方法，本發明提供了該控制方法對應的機器人點到點運動控制系統，

參考圖2所示，圖2是機器人點到點運動控制系統結構示意圖，包括：總控模塊、算法模塊和通信管理模塊；

所述總控模塊，用于接收控制端傳送的點到點運動指令；

所述算法模塊，用于對點到點運動指令進行解析，獲取目標關節角度和點到點運動需要的時間；獲取機械臂的當前關節角度、當前關節角速度和當前關節角加速度；根據點到點運動需要的時間計算機械臂由當前關節角度運行到目標關節角度經過的第一軌跡，以及分別計算機械臂在所述點到點運動需要的時間內由當前關節角速度和當前關節角加速度降為0所經過的第二軌跡和第三軌跡；根據第一軌跡、第二軌跡和第三軌跡得到點到點的運動軌跡；以及計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度；

所述通信管理模塊，用于將機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度轉發至控制主站。

作為實施例，所述總控模塊可以通過第一接口與算法模塊進行通信連接，算法模塊通過第二接口與通信管理模塊進行通信連接，所述第一接口、第二接口是基于ros上創建orocos的實時輸入/輸出接口；

所述總控模塊利用預設的通信協議并以異步遠程過程調用的方式接收點到點運動指令；在接收到所述點到點運動指令后，異步觸發點到點運動執行函數并通過第一接口調用算法模塊的點到點運動規劃函數；

所述算法模塊在調用所述點到點運動規劃函數后，讀取機械臂當前的關節角度、關節角速度和關節角加速度，并計算點到點的運動軌跡；計算機械臂各個關節在所述點到點運動軌跡上運行的各個位置的目標角度、目標角速度和目標角加速度，通過第二接口將所述目標角度、目標角速度和目標角加速度發送給通信管理模塊；

所述通信管理模塊將所述目標角度、目標角速度和目標角加速度轉發至控制主站。

上述實施例的技術方案，利用orocos的實時輸入/輸出接口，總控模塊通過設定通信協議以異步遠程過程調用的方式接收點到點運動指令，啟動點到點運動指令觸發函數，調用算法模塊的機器人運動規劃的點到點運動執行函數，根據該函數計算點到點的運動軌跡，并實時計算機械臂各個關節在該運動軌跡上運行的目標角度、目標角速度和目標角加速度等狀態參數，通信管理模塊將上述狀態參數轉發至控制主站，實現對機器人的點到點運動控制過程；該技術方案可以進行控制指令的接收、解析，算法調用、執行，參數實時計算和傳輸等功能，可以構成一個完整的機器人點到點運動控制系統，降低機器人控制系統開發成本，提高控制效果。

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

1)操作者通過人機交互界面生成點到點運動指令，指令的內容，包括目標關節角度θ1，運動需要的時間t。

2)點到點運動指令通過通信協議，以異步遠程過程調用的方式從人機交互界面發送給總控模塊。

3)指令到達總控模塊后，會異步觸發點到點運動執行函數。該函數首先判斷控制器狀態機是否為ready狀態。若不是ready狀態，則拒絕執行此次指令。

4)若控制器狀態機是ready狀態，則執行如下操作：

a)則通過orocos的operationalcaller方法調用算法模塊的點到點運動規劃函數，并且傳遞指令參數。

b)將控制器狀態機轉換為active.ptp狀態。

5)判斷控制器狀態機是否重新變為ready狀態。若是ready狀態，則本次運動完成。

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

1)點到點運動規劃函數被總控模塊調用，開始執行點到點運動規劃流程。

2)檢查控制器狀態機是否為active.ptp狀態，若不是則退出。

3)讀取機械臂當前的關節角度θ0，關節角速度關節角加速度采用五次多項式方法，生成點到點運動軌跡。

該方法的原理如下：

點到點的運動軌跡表示如下：

s(t)＝a0+a1t+a2t²+a3t³+a4t⁴+a5t⁵(1)

式中，t是運行時間；ai,i＝1,…,5是系數。

由于已知機械臂當前的關節角度θ0，關節角速度關節角加速度以及目標關節角度θ1。并將目標的關節角速度和角加速度設為0。則可求得系數：

然后就可以根據以下公式計算出任意時刻t時的關節角位置θt。，角速度。，角加速度

θt＝a0+a1t+a2t²+a3t³+a4t⁴+a5t⁵(3)

4)將運動時間tn記為0。參考圖5所示，圖5是算法模塊執行算法狀態轉換圖；將算法模塊的狀態標志位設為運動狀態。

5)算法模塊的updatehook()函數檢測到狀態標志位為運動狀態后，且運動時間tn不超過運動時長t的70％：

a)每隔1毫秒依據公式(3)、(4)、(5)分別計算一次機械臂每個關節的目標角度，目標角速度和目標角加速度。

b)將機械臂每個關節的目標角度，目標角速度和目標角加速度，發送給通信管理模塊。

6)若運動時間tn滿足：0.7t＜tn＜t:

a)將算法模塊的標志位更改為過渡狀態。

b)將控制器狀態機狀態設定為ready狀態。

c)若有新指令輸入，則啟動過渡運動規劃。

d)若沒有新指令輸入，則每隔1毫秒依據公式(3)、(4)、(5)分別計算一次機械臂每個關節的目標角度，目標角速度和目標角加速度。并將機械臂每個關節的目標角度，目標角速度和目標角加速度，發送給通信管理模塊。

7)若運動時間tn滿足：tn≥t:

a)本次點到點運動結束。

b)將算法模塊的標志位更改為等待狀態。

所述通信管理模塊，可以進一步用于讀取機器人電機的狀態信息，根據機器人運動模型計算機器人機械臂的狀態信息，將機器人的狀態信息通過第三接口反饋給總控模塊，將機器人的狀態信息通過第二接口反饋給算法模塊；其中，所述電機的狀態信息包括位置、速度和力矩等；所述機械臂的狀態信息包括關節角度、關節角速度、關節角加速度、末端位姿、末端線速度、末端角速度、末端線加速度和末端線加速度等。

為了更加清晰本發明的實施例的技術方案，下面闡述采用本發明的硬件和軟件環境應用實例：

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

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

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

1、對于總控模塊：

(1)總控模塊使用ros的orocreate-catkin-pkg方法創建ros的package，記為ec_control_system，然后在package中，通過繼承orocos的rtt：：taskcontext類，記為ec_control_system_component。

在ec_control_system_component類的構造函數中，設置為執行如下操作：

a)利用orocos的rtt：：input與rtt：：output方法，對模塊的輸入，輸出接口進行定義。

其中輸入的接口包括：

①通信管理模塊傳入的診斷數據；

②通信管理模塊傳入的狀態反饋信息：包括電機運行狀態等等；

③控制器狀態機的狀態；

輸出的接口包括：

①控制器狀態機事件觸發，輸出給控制器狀態機。

b)利用orocos的operationalcaller方法設置函數調用接口。

第一，設置事件報告的回調函數：對事件報告處理請求進行響應，包括生成錯誤的時間戳，事件級別等信息，并將事件信息，發送給人機交互界面顯示。

第二，設置警報設置的回調函數：依據診斷信息，判斷是否生成警報。比如，位置，速度，加速度是否超限等等。

第三，設置各種運動規劃的控制指令觸發函數，這些函數將對算法模塊的相應響應函數進行調用。

c)調用orocos的properties方法定義總控模塊的屬性，將總控模塊定義一個機械臂關節個數的屬性。

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

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

b)通過ice開發的通信協議建立與人機交互界面的通信連接，調用通信協議提供的動態異步遠程過程調用(rpc)方法，對人機交互界面發起的控制指令進行響應的回調函數進行綁定。該回調函數，首先根據ice(theinternetcommunicationsengine，互聯網通信引擎)提供的遠程過程異步調用方法傳入的第一個參數，判斷調用類型，然后依據此類型選擇調用相應運動規劃的運動指令觸發函數。

(3)對于ec_control_system_component的cleanuphook()成員函數，為了使得該函數在總控模塊結束運行時，實現自動調用，還可以設置為執行如下操作：

a)調用ice開發的通信協議接口，關閉與人機交互界面的通信連接。

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

另外，還可以使用lua語言，編寫總控模塊的啟動文件，該啟動文件設置為執行如下動作：

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

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

c)對模塊的屬性進行賦值；

d)通過orocos的connect方法，將總控模塊的輸入、輸出接口和算法模塊和通信管理模塊的接口建立連接。

e)通過orocos的start方法，運行總控模塊，總控模塊將先調用starthook()函數，然后按預設刷新頻率，實時地周期性調用updatehook()函數。

2、對于算法模塊：

算法模塊使用ros的orocreate-catkin-pkg方法創建ros的package，記為ec_control_loop，然后在package中，通過繼承orocos的rtt：：taskcontext類，記為ec_control_loop_component。

(1)在ec_control_loop_component類的構造函數中，設置為執行如下操作：

a)算法模塊利用orocos的rtt：：input與rtt：：output方法，對輸入，輸出接口進行定義。

其中輸入的接口包括：

①通信管理模塊傳入的電機運行數據；

②通信管理模塊傳入的診斷數據；

③設備狀態機的狀態；

④控制器狀態機的狀態；

輸出的接口包括：

①電機控制指令數據，輸出給設備通信模塊；

②設備狀態機事件觸發，輸出給設備狀態機；

③控制器狀態機事件觸發，輸出給控制器狀態機。

b)利用orocos的operationalcaller方法設置函數調用接口，設置事件報告的接口：該接口將觸發總控模塊的事件報告處理函數設置各種運動規劃的返回原點指令響應函數。

c)調用orocos的properties方法定義算法模塊的屬性，算法模塊定義一個機械臂關節個數的屬性。

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

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

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

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

a)讀取控制器狀態機狀態；

b)根據控制器狀態機的不同狀態，執行不同操作：

ⅰ、如果是點到點運動，直線運動，圓弧運動，手動示教，急停，回到原點狀態。此時，執行如下操作：

如果指令緩沖區的控制指令個數小于20個，則將所有指令一起發送給通信管理模塊，并將控制器狀態機的狀態改變為等待指令輸入狀態；

如果指令緩沖區的控制指令個數大于20個，則取指令隊列末尾的20個，發送給通信管理模塊；

ⅱ、如果是暫停狀態，則什么也不做。

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

a)點到點運動，直線運動，圓弧運動，手動示教，回到原點的函數，內部實現如下：

檢查控制器狀態機是否處于等待指令輸入狀態。如果不是則退出，并將相關信息通過事件報告接口傳遞給總控模塊處理；

讀取電機的當前的狀態信息；

依據電機的當前狀態，分別調用點到點運動，直線運動，圓弧運動，手動示教，回到原點的運動規劃，并將生成的電機控制指令保存到指令緩沖區；

將控制器狀態機設為相應的狀態。比如點到點運動回調函數，則將控制器狀態機設為點到點運動狀態。

b)暫停函數，內部實現如下：

檢查控制器狀態機是否是點到點運動，直線運動，圓弧運動，手動示教，回到原點等狀態。如果不是則退出，并將相關信息通過事件報告接口傳遞給總控模塊進行處理；

記錄當前控制器狀態機的當前狀態，并將控制器狀態機的狀態轉變為暫停狀態。

c)恢復函數，內部實現如下：

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

將控制器狀態機的狀態轉變為暫停前的狀態。

d)急停函數，內部實現如下：

ⅰ、檢查控制器狀態機是否是點到點運動，直線運動，圓弧運動，手動示教，回到原點等狀態。如果不是則退出，并將相關信息通過事件報告接口傳遞給總控模塊處理；

ⅱ、讀取電機的當前的狀態信息；

ⅲ、將電機控制指令緩沖區清零；

ⅳ、調用速度規劃運動規劃，讓電機以最短時間，速度降為0，并將生成的電機控制指令保存到指令緩沖區。

(5)使用lua語言，編寫算法模塊的啟動文件，設置為執行如下動作：

a)通過orocos的import方法，加載算法模塊；

d)定義算法模塊的刷新頻率，線程的優先級別；

c)對算法模塊的屬性進行賦值；

d)通過orocos的connect方法，將算法模塊的輸入，輸出接口和總控模塊和通信管理模塊的接口建立連接。

e)通過orocos的start方法，運行算法模塊，算法模塊先調用starthook()函數，然后按設置的刷新頻率，實時地周期性調用updatehook()函數。

3、對于通信管理模塊：

通信管理模塊可以通過linux主機minicom中的ttyacm0與arm開發板通信，可以在該arm開發板上運行一個canopen主站協議，該主站協議可以設置一個指令緩存區，最多可以存儲25個指令。

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

利用rfsm軟件建立設備狀態機，對通信管理模塊的業務邏輯進行控制。

通信管理模塊利用orocos的rtt：：input與rtt：：output方法與設備狀態機連接，可改變設備狀態機的狀態，并讀取狀態。

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

在ec_component類的構造函數中，設置為執行如下操作：

a)通信管理模塊利用orocos的rtt：：input與rtt：：output方法，對輸入，輸出接口進行定義。

其中輸入的接口包括：

①算法模塊傳入的控制指令數據；

②設備狀態機的狀態；

輸出的接口包括：

①診斷數據，輸出至算法模塊和總控模塊；

②電機運行數據和機械臂狀態數據，輸出給算法模塊；

③狀態機事件觸發，輸出給設備狀態機。

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

c)調用orocos的properties方法定義通信管理模塊的屬性，通信管理模塊定義一個機械臂關節個數的屬性。

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

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

b)電機驅動初始化：

ⅰ、通過ttyacm0與電機驅動器建立通信；

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

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

ⅳ、讀取電機的位置，計算機器人的機械臂當前狀態，包括關節角度，機械臂末端位姿；

c)機械臂狀態初始化：

依據電機位置，判斷機械臂是否需要執行回零運動。如果機械臂任一關節角度與零度相差大于0.01度，則執行回零運動，調用點到點運動規劃，對回零運動進行規劃。

d)改變設備狀態機的狀態：

如果機器人的機械臂需要執行回零運動，則控制設備狀態機保持init狀態不變；否則，向設備狀態機發送“e_nominal”事件，將設備狀態機的狀態轉換為active.nominal。

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

a)讀取設備狀態機狀態；

b)根據設備狀態機的不同狀態，執行不同操作：

ⅰ、如果是init狀態，執行機械臂的回零運動。此時，執行如下操作：

讀取系統的時鐘，依據回零運動軌跡規劃結果，計算該時刻電機的運動指令，并將該單條運動指令發送給canopen主站。

如果運動已回零成功，向設備狀態機發送“e_nominal”事件，將設備狀態機轉換為active.nominal，并將該事件報告給總控模塊。

ⅱ、如果是active.nominal狀態。此時，執行如下操作：

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

讀取canopen主站指令緩沖區的現有指令個數，如果小于10個，則一次性從電機指令緩沖隊列中取出15個運動指令發送給canopen主站。如果電機指令緩沖隊列的指令個數小于15個，則一次性全部發送給canopen主站。

ⅲ、如果是active.recovery狀態。此時，通信管理模塊處于恢復狀態。

此時，依據診斷信息，對系統狀態進行恢復，若恢復成功，向設備狀態機發送“e_nominal”事件，將設備狀態機轉換為active.nominal。并將該事件報告給總控模塊。

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

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

ⅴ、如果是active.hands狀態。此時，模塊處于手控模式，執行如下操作：

讀取系統的時鐘，依據運動軌跡規劃結果，計算該時刻電機的運動指令，并將該單條運動指令發送給canopen主站。

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

c)讀取電機狀態，根據機械臂的模型計算機械臂關節以及末端運動狀態信息，并通過輸出數據通道，傳遞給算法模塊與總控模塊；

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

(4)對于ec_component的cleanuphook()成員函數，該函數在模塊結束運行時，自動調用，設置執行如下操作：

a)關閉電機驅動使能；

b)關閉電機驅動連接。

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

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

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

b)定義通信管理模塊的刷新頻率，線程的優先級別；

c)對通信管理模塊的屬性進行賦值；

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

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

對于上述總控模塊、算法模塊和通信管理模塊，設置為當運行后，如果用戶需要中途停止該模塊，同時按鍵盤的ctrl鍵與d鍵。

綜上實施例，基于ros和orocos，利用orocos的實時性特點，保證軟件程序的實時性能；充分利用ros的開放性，基于ros、orocos開發的總控模塊、算法模塊和通信管理模塊進行實時通信，共同構成一個完整的機器人控制器軟件；進一步建立了控制器狀態機和設備狀態機，從而實現對總控模塊、通信管理模塊的業務邏輯進行有效管理。

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

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

以上所述實施例僅表達了本發明的幾種實施方式，其描述較為具體和詳細，但并不能因此而理解為對發明專利范圍的限制。應當指出的是，對于本領域的普通技術人員來說，在不脫離本發明構思的前提下，還可以做出若干變形和改進，這些都屬于本發明的保護范圍。因此，本發明專利的保護范圍應以所附權利要求為準。