本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，特別是涉及一種機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法和系統(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好。

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

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

基于此，有必要針對(duì)上述技術(shù)問(wèn)題，提供一種機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法，降低系統(tǒng)開(kāi)發(fā)成本，提高控制效果。

一種機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法，包括：

接收控制端傳送的返回原點(diǎn)運(yùn)動(dòng)指令；

獲取機(jī)械臂的當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度；按照最快加速度與最大速度返回原點(diǎn)原則，根據(jù)所述關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度分別計(jì)算機(jī)械臂由當(dāng)前的關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度降為0時(shí)所需要的運(yùn)行時(shí)間，將運(yùn)行時(shí)間中的最大值設(shè)為原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間；

分別計(jì)算所述機(jī)械臂在原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間內(nèi)，由當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度回到原點(diǎn)經(jīng)過(guò)的軌跡，得到返回原點(diǎn)運(yùn)動(dòng)軌跡；

計(jì)算機(jī)械臂各個(gè)關(guān)節(jié)在所述返回原點(diǎn)運(yùn)動(dòng)軌跡上運(yùn)行的各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度，發(fā)送給設(shè)備通信軟件轉(zhuǎn)發(fā)至控制主站。

一種機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制系統(tǒng)，包括：總控模塊、算法模塊和通信管理模塊；

所述總控模塊，用于接收控制端傳送的返回原點(diǎn)運(yùn)動(dòng)指令；

所述算法模塊，用于獲取機(jī)械臂的當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度；按照最快加速度與最大速度返回原點(diǎn)原則，根據(jù)所述關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度分別計(jì)算機(jī)械臂由當(dāng)前的關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度降為0時(shí)所需要的運(yùn)行時(shí)間，將運(yùn)行時(shí)間中的最大值設(shè)為原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間；分別計(jì)算所述機(jī)械臂在原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間內(nèi)，由當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度回到原點(diǎn)經(jīng)過(guò)的軌跡，得到返回原點(diǎn)運(yùn)動(dòng)軌跡；計(jì)算機(jī)械臂各個(gè)關(guān)節(jié)在所述返回原點(diǎn)運(yùn)動(dòng)軌跡上運(yùn)行的各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；

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

上述機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法和系統(tǒng)，接收控制端傳送的返回原點(diǎn)運(yùn)動(dòng)指令，計(jì)算返回原點(diǎn)運(yùn)動(dòng)軌跡，并實(shí)時(shí)計(jì)算機(jī)械臂各個(gè)關(guān)節(jié)在該運(yùn)動(dòng)軌跡上運(yùn)行的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度等狀態(tài)參數(shù)，通過(guò)設(shè)備通信軟件將上述狀態(tài)參數(shù)轉(zhuǎn)發(fā)至控制主站，實(shí)現(xiàn)對(duì)機(jī)器人的返回原點(diǎn)運(yùn)動(dòng)的過(guò)程控制；該技術(shù)方案可以構(gòu)成一個(gè)完整的機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制系統(tǒng)，降低機(jī)器人控制系統(tǒng)開(kāi)發(fā)成本，提高控制效果。

附圖說(shuō)明

圖1是本發(fā)明實(shí)施例的機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法流程圖；

圖2是機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制系統(tǒng)結(jié)構(gòu)示意圖；

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

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

圖5是算法模塊執(zhí)行算法狀態(tài)轉(zhuǎn)換圖；

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

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

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

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

具體實(shí)施方式

下面結(jié)合附圖闡述本發(fā)明的機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法的實(shí)施例。

本發(fā)明實(shí)施例中，所述原點(diǎn)指機(jī)械臂的各個(gè)關(guān)節(jié)角度都為0，返回原點(diǎn)運(yùn)動(dòng)，是指機(jī)械臂的關(guān)節(jié)角從當(dāng)前關(guān)節(jié)角度θ0運(yùn)動(dòng)到原點(diǎn)的運(yùn)動(dòng)過(guò)程。

參考圖1所示，圖1是本發(fā)明實(shí)施例的機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法流程圖，包括：

s10，接收控制端傳送的返回原點(diǎn)運(yùn)動(dòng)指令；

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

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

s20，獲取機(jī)械臂的當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度；按照最快加速度與最大速度返回原點(diǎn)原則，根據(jù)所述關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度分別計(jì)算機(jī)械臂由當(dāng)前的關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度降為0時(shí)所需要的運(yùn)行時(shí)間，將運(yùn)行時(shí)間中的最大值設(shè)為原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間；

在一個(gè)實(shí)施例中，在接收到所述返回原點(diǎn)運(yùn)動(dòng)指令后，異步觸發(fā)返回原點(diǎn)運(yùn)動(dòng)執(zhí)行函數(shù)，通過(guò)第一接口并根據(jù)所述返回原點(diǎn)運(yùn)動(dòng)執(zhí)行函數(shù)調(diào)用返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)；其中，所述第一接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口。

在一個(gè)實(shí)施例中，在調(diào)用返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)前，可以首先由返回原點(diǎn)運(yùn)動(dòng)執(zhí)行函數(shù)判斷控制器狀態(tài)機(jī)是否為準(zhǔn)備(ready)狀態(tài)；

若是，通過(guò)orocos的operationalcaller方法調(diào)用所述返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)，并將控制器狀態(tài)機(jī)切換為執(zhí)行返回原點(diǎn)運(yùn)動(dòng)(active.tozero)狀態(tài)；若否，則拒絕執(zhí)行此次指令；

其中，所述控制器狀態(tài)機(jī)的可被改變狀態(tài)，并讀取狀態(tài)，設(shè)有初始化、指令等待、指令執(zhí)行、中斷和使能對(duì)應(yīng)的狀態(tài)。

s30，分別計(jì)算所述機(jī)械臂在原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間內(nèi)，由當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度回到原點(diǎn)經(jīng)過(guò)的軌跡，得到返回原點(diǎn)運(yùn)動(dòng)軌跡；

在一個(gè)實(shí)施例中，在調(diào)用所述返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)后，讀取機(jī)械臂關(guān)節(jié)當(dāng)前的節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度，并根據(jù)所述節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度計(jì)算返回原點(diǎn)運(yùn)動(dòng)軌跡。

進(jìn)一步地，在調(diào)用返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)后，可以根據(jù)所述返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)執(zhí)行返回原點(diǎn)運(yùn)動(dòng)規(guī)劃流程，并檢查控制器狀態(tài)機(jī)是否為執(zhí)行返回原點(diǎn)運(yùn)動(dòng)狀態(tài)；若是，執(zhí)行所述計(jì)算返回原點(diǎn)的運(yùn)動(dòng)軌跡的步驟，否則，退出執(zhí)行流程。

作為實(shí)施例，計(jì)算返回原點(diǎn)的運(yùn)動(dòng)軌跡的方法，可以包括如下：

(1)獲取機(jī)械臂當(dāng)前的關(guān)節(jié)角度θ0、關(guān)節(jié)角速度和關(guān)節(jié)角加速度以及將目標(biāo)關(guān)節(jié)角度，關(guān)節(jié)角速度，關(guān)節(jié)角加速度設(shè)為0；

(2)按照最快加速度與最大速度返回原點(diǎn)原則，根據(jù)所述關(guān)節(jié)角度θ0、關(guān)節(jié)角速度和關(guān)節(jié)角加速度計(jì)算返回原點(diǎn)運(yùn)動(dòng)需要的時(shí)間t；具體的，對(duì)于運(yùn)行時(shí)長(zhǎng)t的計(jì)算公式，可以如下：

式中，當(dāng)前的關(guān)節(jié)角度θ0，關(guān)節(jié)角速度關(guān)節(jié)角加速度i表示任一關(guān)節(jié)；

(3)根據(jù)所述關(guān)節(jié)角度θ0、關(guān)節(jié)角速度和關(guān)節(jié)角加速度以及返回原點(diǎn)運(yùn)動(dòng)需要的時(shí)間t，生成返回原點(diǎn)運(yùn)動(dòng)軌跡；

具體的，所述返回原點(diǎn)運(yùn)動(dòng)軌跡的計(jì)算公式為：

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

a0＝θ0

式中，s(t)是五次多項(xiàng)式，t是運(yùn)行時(shí)間；ai，i＝1,…,5是系數(shù)。

s40，計(jì)算機(jī)械臂各個(gè)關(guān)節(jié)在所述返回原點(diǎn)運(yùn)動(dòng)軌跡上運(yùn)行的各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度，發(fā)送給設(shè)備通信軟件轉(zhuǎn)發(fā)至控制主站；

在一個(gè)實(shí)施例中，在計(jì)算各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度后，通過(guò)第二接口將所述目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度發(fā)送給設(shè)備通信軟件轉(zhuǎn)發(fā)至控制主站；其中，所述第二接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口；

在此過(guò)程中，實(shí)時(shí)計(jì)算各個(gè)位置的參數(shù)，然后作為控制指令輸出至機(jī)器人進(jìn)行控制其在運(yùn)行軌跡上的位置，快速回到原點(diǎn)；

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

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

式中，任意時(shí)刻t時(shí)的關(guān)節(jié)角位置θt、角速度和角加速度

作為實(shí)施例，計(jì)算目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法包括：

1)在機(jī)械臂開(kāi)始運(yùn)動(dòng)后，由0開(kāi)始每隔設(shè)定時(shí)間τ統(tǒng)計(jì)機(jī)械臂的運(yùn)動(dòng)時(shí)間tn；τ＝1/f，f表示刷新頻率；

即由零開(kāi)始統(tǒng)計(jì)時(shí)間，按照刷新率不斷計(jì)算機(jī)械臂各個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；上述實(shí)施例中，一般情況下，所述τ為1毫秒；

2)若所述運(yùn)動(dòng)時(shí)間滿足：tn≤at，每隔設(shè)定時(shí)間τ依據(jù)所述計(jì)算模型分別計(jì)算一次機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；一般情況下，所述a＝70％；

3)若運(yùn)動(dòng)時(shí)間tn滿足：at＜tn＜t，將標(biāo)志位更改為過(guò)渡狀態(tài)，將控制器狀態(tài)機(jī)狀態(tài)設(shè)定為準(zhǔn)備狀態(tài)；

作為實(shí)施例，在過(guò)渡狀態(tài)下，若有新指令輸入，則啟動(dòng)過(guò)渡運(yùn)動(dòng)規(guī)劃；若沒(méi)有新指令輸入，則每隔設(shè)定時(shí)間依據(jù)所述計(jì)算模型分別計(jì)算一次機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度，目標(biāo)角速度和目標(biāo)角加速度，并發(fā)送給設(shè)備通信軟件。

4)若運(yùn)動(dòng)時(shí)間tn滿足：tn≥t，點(diǎn)到點(diǎn)運(yùn)動(dòng)結(jié)束，將標(biāo)志位更改為等待狀態(tài)。上述實(shí)施例中，一般情況下，所述a＝70％，τ為1毫秒。

所述通信管理模塊，可以進(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é)角加速度、末端位姿、末端線速度、末端角速度、末端線加速度和末端線加速度等。

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

針對(duì)于機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制方法，本發(fā)明提供了該控制方法對(duì)應(yīng)的機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制系統(tǒng)，

參考圖2所示，圖2是機(jī)器人返回原點(diǎn)運(yùn)動(dòng)控制系統(tǒng)結(jié)構(gòu)示意圖，包括：總控模塊、算法模塊和通信管理模塊；

所述總控模塊，用于接收控制端傳送的返回原點(diǎn)運(yùn)動(dòng)指令；

所述算法模塊，用于獲取機(jī)械臂的當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度；按照最快加速度與最大速度返回原點(diǎn)原則，根據(jù)所述關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度分別計(jì)算機(jī)械臂由當(dāng)前的關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度降為0時(shí)所需要的運(yùn)行時(shí)間，將運(yùn)行時(shí)間中的最大值設(shè)為原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間；分別計(jì)算所述機(jī)械臂在原點(diǎn)運(yùn)動(dòng)指令的執(zhí)行時(shí)間內(nèi)，由當(dāng)前關(guān)節(jié)角度、當(dāng)前關(guān)節(jié)角速度和當(dāng)前關(guān)節(jié)角加速度回到原點(diǎn)經(jīng)過(guò)的軌跡，得到返回原點(diǎn)運(yùn)動(dòng)軌跡；計(jì)算機(jī)械臂各個(gè)關(guān)節(jié)在所述返回原點(diǎn)運(yùn)動(dòng)軌跡上運(yùn)行的各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；

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

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

所述總控模塊在接收到所述返回原點(diǎn)運(yùn)動(dòng)指令后，異步觸發(fā)返回原點(diǎn)運(yùn)動(dòng)執(zhí)行函數(shù)，通過(guò)第一接口并根據(jù)所述返回原點(diǎn)運(yùn)動(dòng)執(zhí)行函數(shù)調(diào)用算法模塊的返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)；

所述算法模塊將通過(guò)第二接口將所述目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度發(fā)送給通信管理模塊；

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

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

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

(1)操作者通過(guò)人機(jī)交互界面生成返回原點(diǎn)運(yùn)動(dòng)指令。該指令無(wú)需傳遞參數(shù)；

(2)返回原點(diǎn)運(yùn)動(dòng)指令通過(guò)通信協(xié)議，以異步遠(yuǎn)程過(guò)程調(diào)用的方式從人機(jī)交互界面發(fā)送給總控模塊；

(3)指令到達(dá)總控模塊后，會(huì)異步觸發(fā)點(diǎn)到點(diǎn)運(yùn)動(dòng)執(zhí)行函數(shù)。該函數(shù)首先判斷控制器狀態(tài)機(jī)是否為準(zhǔn)備(ready)狀態(tài)。若不是ready狀態(tài)，則拒絕執(zhí)行此次指令；

(4)若控制器狀態(tài)機(jī)是ready狀態(tài)，則執(zhí)行如下操作：

4.1、則通過(guò)orocos的operationalcaller方法調(diào)用算法模塊的返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)；

4.2、將控制器狀態(tài)機(jī)轉(zhuǎn)換為active.tozero狀態(tài)；

(5)判斷控制器狀態(tài)機(jī)是否重新變?yōu)閞eady狀態(tài)。若是ready狀態(tài)，則本次返回原點(diǎn)運(yùn)動(dòng)完成。

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

(1)算法模塊的返回原點(diǎn)運(yùn)動(dòng)規(guī)劃函數(shù)被總控模塊調(diào)用后，開(kāi)始執(zhí)行返回原點(diǎn)運(yùn)動(dòng)規(guī)劃流程；

(2)檢查控制器狀態(tài)機(jī)是否為active.tozero狀態(tài)，若不是則退出；

(3)讀取機(jī)械臂當(dāng)前的關(guān)節(jié)角度θ0、關(guān)節(jié)角速度和關(guān)節(jié)角加速度設(shè)定目標(biāo)關(guān)節(jié)角度、關(guān)節(jié)角速度和關(guān)節(jié)角加速度都為0，生成返回原點(diǎn)運(yùn)動(dòng)軌跡；計(jì)算過(guò)程如下：

按照最快加速度與最大速度返回原點(diǎn)原則，可求得各個(gè)關(guān)節(jié)返回原點(diǎn)的時(shí)間：

選取公式(1)計(jì)算得到返回時(shí)間最大值作為運(yùn)動(dòng)時(shí)長(zhǎng)t。

返回原點(diǎn)的運(yùn)動(dòng)軌跡用如下公式表示：

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

式中，t是運(yùn)行時(shí)間；ai,i＝1,…,5是系數(shù)，則可求得系數(shù)：

a0＝θ0

根據(jù)以下公式計(jì)算出任意時(shí)刻t時(shí)的關(guān)節(jié)角位置θt，角速度角加速度

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

(4)將運(yùn)動(dòng)時(shí)間tn記為0，參考圖5，圖5是算法模塊執(zhí)行算法狀態(tài)轉(zhuǎn)換圖，將算法模塊的狀態(tài)標(biāo)志位設(shè)為運(yùn)動(dòng)狀態(tài)。

(5)算法模塊的updatehook()函數(shù)檢測(cè)到狀態(tài)標(biāo)志位為運(yùn)動(dòng)狀態(tài)后，且運(yùn)動(dòng)時(shí)間tn不超過(guò)運(yùn)動(dòng)時(shí)長(zhǎng)t的70％；

5.1、每隔1毫秒依據(jù)公式(3)、(4)、(5)分別計(jì)算一次機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；

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

(6)若運(yùn)動(dòng)時(shí)間tn滿足：0.7t＜tn＜t：

6.1、將算法模塊的標(biāo)志位更改為過(guò)渡狀態(tài)；

6.2、將控制器狀態(tài)機(jī)狀態(tài)設(shè)定為ready狀態(tài)；

6.3、有新指令輸入，則啟動(dòng)過(guò)渡運(yùn)動(dòng)規(guī)劃；

6.4、每隔1毫秒依據(jù)公式(3)、(4)、(5)分別計(jì)算一次機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度；并將機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度，目標(biāo)角速度和目標(biāo)角加速度，發(fā)送給通信管理模塊。

(7)若運(yùn)動(dòng)時(shí)間tn滿足：tn≥t：

本次返回原點(diǎn)運(yùn)動(dòng)結(jié)束，將算法模塊的標(biāo)志位更改為等待狀態(tài)。

對(duì)于通信管理模塊，其用于讀取電機(jī)的狀態(tài)，包括位置、速度、力矩；依據(jù)機(jī)器人運(yùn)動(dòng)模型，計(jì)算機(jī)器人的狀態(tài)，包括關(guān)節(jié)角度、關(guān)節(jié)角速度、關(guān)節(jié)角加速度、末端位姿末端線速度、末端角速度、末端線加速度、末端線加速度等；將機(jī)器人的狀態(tài)反饋給總控模塊和算法模塊。

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

參考圖6所示，圖6是一應(yīng)用實(shí)例的機(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等軟件。

參考圖7所示，圖7是基于ros和orocos的搭建的軟件架構(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)返回原點(diǎn)響應(yīng)函數(shù)進(jìn)行調(diào)用。

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ī)劃的運(yùn)動(dòng)指令觸發(fā)函數(shù)。

(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ī)，參考圖8所示，圖8是控制器狀態(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è)，寫入“e_ready”事件，將控制器狀態(tài)機(jī)的狀態(tài)從當(dāng)前狀態(tài)轉(zhuǎn)移到ready狀態(tài)(即等待指令輸入狀態(tài))。

另外，還可以使用lua語(yǔ)言，編寫總控模塊的啟動(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ī)劃的返回原點(diǎn)指令響應(yīng)函數(shù)。

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)闀和Ｇ暗臓顟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ǔ)言，編寫算法模塊的啟動(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)參考圖9所示，圖9是設(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ǔ)言，編寫該模塊的啟動(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í)按鍵盤的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í)施例僅表達(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)。