国产精品1024永久观看,大尺度欧美暖暖视频在线观看,亚洲宅男精品一区在线观看,欧美日韩一区二区三区视频,2021中文字幕在线观看

  • <option id="fbvk0"></option>
    1. <rt id="fbvk0"><tr id="fbvk0"></tr></rt>
      <center id="fbvk0"><optgroup id="fbvk0"></optgroup></center>
      <center id="fbvk0"></center>

      <li id="fbvk0"><abbr id="fbvk0"><dl id="fbvk0"></dl></abbr></li>

      機(jī)器人圓弧運(yùn)動控制方法和系統(tǒng)與流程

      文檔序號:11728788閱讀:3119來源:國知局
      機(jī)器人圓弧運(yùn)動控制方法和系統(tǒng)與流程

      本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域,特別是涉及一種機(jī)器人圓弧運(yùn)動控制方法和系統(tǒng)。



      背景技術(shù):

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

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



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

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

      一種機(jī)器人圓弧運(yùn)動控制方法,包括:

      接收控制端傳送的圓弧運(yùn)動指令;所述圓弧運(yùn)動指令包括圓弧運(yùn)動的目標(biāo)中間點(diǎn)位置、終點(diǎn)位置以及運(yùn)動需要的時(shí)間;

      讀取機(jī)械臂末端的當(dāng)前狀態(tài)參數(shù);根據(jù)所述當(dāng)前狀態(tài)參數(shù)以及所述目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧運(yùn)動的圓心、圓弧半徑、圓心角;根據(jù)目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧所在平面的法矢量;根據(jù)圓心、圓弧半徑、圓心角及所述當(dāng)前狀態(tài)參數(shù)得到圓弧的軌跡方程,根據(jù)所述法矢量和法矢量確定圓弧運(yùn)動軌跡;其中,所述當(dāng)前狀態(tài)參數(shù)包括當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度;

      根據(jù)所述運(yùn)動需要的時(shí)間計(jì)算機(jī)械臂末端在所述圓弧運(yùn)動軌跡上運(yùn)行時(shí),機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

      一種機(jī)器人圓弧運(yùn)動控制系統(tǒng),包括:總控模塊、算法模塊和通信管理模塊;

      所述總控模塊,用于接收控制端傳送的圓弧運(yùn)動指令;所述圓弧運(yùn)動指令包括圓弧運(yùn)動的目標(biāo)中間點(diǎn)位置、終點(diǎn)位置以及運(yùn)動需要的時(shí)間;

      所述算法模塊,用于讀取機(jī)械臂末端的當(dāng)前狀態(tài)參數(shù);根據(jù)所述當(dāng)前狀態(tài)參數(shù)以及所述目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧運(yùn)動的圓心、圓弧半徑、圓心角;根據(jù)目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧所在平面的法矢量;根據(jù)圓心、圓弧半徑、圓心角及所述當(dāng)前狀態(tài)參數(shù)得到圓弧的軌跡方程,根據(jù)所述法矢量和法矢量確定圓弧運(yùn)動軌跡;根據(jù)所述運(yùn)動需要的時(shí)間計(jì)算機(jī)械臂末端在所述圓弧運(yùn)動軌跡上運(yùn)行時(shí),機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度;其中,所述當(dāng)前狀態(tài)參數(shù)包括當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度;

      所述通信管理模塊,用于將機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

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

      附圖說明

      圖1是本發(fā)明實(shí)施例的機(jī)器人圓弧運(yùn)動控制方法流程圖;

      圖2是機(jī)器人圓弧運(yùn)動控制系統(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ī)器人圓弧運(yùn)動控制方法的實(shí)施例。

      本發(fā)明實(shí)施例中,所述圓弧運(yùn)動,是指是指機(jī)械臂沿一條圓弧軌跡,由初始位置p0,經(jīng)過指定中間點(diǎn)p1,運(yùn)動到終點(diǎn)標(biāo)位置p2的運(yùn)動過程。

      參考圖1所示,圖1是本發(fā)明實(shí)施例的機(jī)器人圓弧運(yùn)動控制方法流程圖,包括:

      s10,接收控制端傳送的圓弧運(yùn)動指令;所述圓弧運(yùn)動指令包括圓弧運(yùn)動的目標(biāo)中間點(diǎn)位置、終點(diǎn)位置以及運(yùn)動需要的時(shí)間;

      在一個(gè)實(shí)施例中,在執(zhí)行圓弧運(yùn)動控制中,接收控制端傳送的圓弧運(yùn)動指令;所述圓弧運(yùn)動指令包括圓弧運(yùn)動的目標(biāo)中間點(diǎn)位置,終點(diǎn)位置以及運(yùn)動需要的時(shí)間;

      上述步驟中,可以利用預(yù)設(shè)的通信協(xié)議并以異步遠(yuǎn)程過程調(diào)用的方式接收圓弧運(yùn)動指令;其中,所述圓弧運(yùn)動指令包括機(jī)械臂末端位置p1和圓弧運(yùn)動需要的時(shí)間t。

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

      s20,讀取機(jī)械臂末端的當(dāng)前狀態(tài)參數(shù);根據(jù)所述當(dāng)前狀態(tài)參數(shù)以及所述目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧運(yùn)動的圓心、圓弧半徑、圓心角;根據(jù)目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧所在平面的法矢量;根據(jù)圓心、圓弧半徑、圓心角及所述當(dāng)前狀態(tài)參數(shù)得到圓弧的軌跡方程,根據(jù)所述法矢量和法矢量確定圓弧運(yùn)動軌跡;其中,所述當(dāng)前狀態(tài)參數(shù)包括當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度;

      在一個(gè)實(shí)施例中,在接收控制端傳送的圓弧運(yùn)動指令后,根據(jù)所述圓弧運(yùn)動指令異步觸發(fā)圓弧運(yùn)動執(zhí)行函數(shù),根據(jù)所述圓弧運(yùn)動執(zhí)行函數(shù)并通過第一接口調(diào)用圓弧運(yùn)動規(guī)劃函數(shù);其中,所述第一接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口。

      在一個(gè)實(shí)施例中,在調(diào)用圓弧運(yùn)動規(guī)劃函數(shù)前,所述圓弧運(yùn)動執(zhí)行函數(shù)判斷控制器狀態(tài)機(jī)是否為準(zhǔn)備狀態(tài);若是,通過orocos的operationalcaller方法調(diào)用所述圓弧運(yùn)動規(guī)劃函數(shù),并將控制器狀態(tài)機(jī)切換為執(zhí)行圓弧運(yùn)動狀態(tài);若否,則拒絕執(zhí)行此次指令。所述控制器狀態(tài)機(jī)的可被改變狀態(tài)、并讀取狀態(tài),設(shè)有初始化、指令等待、指令執(zhí)行、中斷和使能對應(yīng)的狀態(tài)。

      進(jìn)一步的,在調(diào)用所述圓弧運(yùn)動規(guī)劃函數(shù)后,讀取機(jī)械臂末端的當(dāng)前狀態(tài)參數(shù),并根據(jù)所述當(dāng)前狀態(tài)參數(shù)和圓弧運(yùn)動指令計(jì)算圓弧運(yùn)動軌跡;其中,所述當(dāng)前狀態(tài)參數(shù)包括末端的當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度。并且在調(diào)用圓弧運(yùn)動規(guī)劃函數(shù)后,根據(jù)所述圓弧運(yùn)動規(guī)劃函數(shù)執(zhí)行圓弧運(yùn)動規(guī)劃流程,并檢查控制器狀態(tài)機(jī)是否為執(zhí)行圓弧運(yùn)動狀態(tài);若是,執(zhí)行所述計(jì)算圓弧運(yùn)動軌跡的步驟,否則,退出執(zhí)行流程。

      作為實(shí)施例,計(jì)算圓弧運(yùn)動軌跡的方法,可以包括如下步驟:

      (1)讀取機(jī)械臂末端當(dāng)前位置,當(dāng)前速度和加速度;

      具體地,獲取機(jī)械臂末端當(dāng)前位置p0=[x0,y0,z0]t,當(dāng)前速度和加速度以及目標(biāo)中間點(diǎn)位置p1=[x1,y1,z1]t,終點(diǎn)位置p2=[x2,y2,z2]t

      (2)根據(jù)機(jī)械臂末端當(dāng)前位置,當(dāng)前速度和加速度以及所述圓弧運(yùn)動指令,并將目標(biāo)速度和加速度設(shè)為0,生成圓弧運(yùn)動軌跡;

      具體地,計(jì)算圓弧運(yùn)動的圓心、圓弧半徑和圓弧p0p1p2的圓心角θc,以及計(jì)算圓弧所在平面的法矢量,根據(jù)所述圓心、圓弧半徑和圓弧p0p1p2的圓心角θc計(jì)算圓弧軌跡;

      所述圓心計(jì)算公式為:

      其中,

      a1=y(tǒng)0z1-y0z2-z0y1+z0y2+y1z2-y2z1

      b1=-x0z1+x0z2+z0x1-z0x2-x1z2+x2z1

      c1=x0y1-x0y2-y0x1+y0x2+x1y2-x2y1

      d1=-x0y1z2+x0y2z1+x1y0z2-x2y0z1-x1y2z0+x2y1z0

      a2=2(x1-x0)

      b2=2(y1-y0)

      c2=2(z1-z0)

      a3=2(x2-x0)

      b3=2(y2-y0)

      c3=2(z2-z0)

      圓弧半徑計(jì)算公式為:

      圓弧所在平面的法矢量n計(jì)算公式為:

      其中,

      圓弧p0p1p2的圓心角θc計(jì)算公式為:

      當(dāng)0<θc<π時(shí),

      當(dāng)2π>θc>π時(shí),

      圓弧軌跡計(jì)算公式為:

      式中,θc為圓心角,r為圓弧半徑;

      s30,根據(jù)所述運(yùn)動需要的時(shí)間計(jì)算機(jī)械臂末端在所述圓弧運(yùn)動軌跡上運(yùn)行時(shí),機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站;

      在一個(gè)實(shí)施例中,計(jì)算機(jī)械臂末端在所述圓弧運(yùn)動軌跡上運(yùn)行時(shí),機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度,通過第二接口將所述目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度發(fā)送給設(shè)備通信軟件轉(zhuǎn)發(fā)至控制主站;其中,所述第二接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口。

      在一個(gè)實(shí)施例中,所述計(jì)算目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法,可以包括如下過程:

      (1)構(gòu)建所述直線運(yùn)動軌跡的方程;具體的,所述直線運(yùn)動軌跡可以用五次多項(xiàng)式表示:

      s(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

      式中,t是運(yùn)行時(shí)間;ai,i=1,…,5是系數(shù)。

      系數(shù):

      a0=s0

      (2)根據(jù)方程計(jì)算在所述圓弧運(yùn)動軌跡運(yùn)動后設(shè)定時(shí)刻t時(shí)的軌跡位置st,軌跡速度軌跡加速度

      任意時(shí)刻t時(shí)的軌跡位置st,軌跡速度軌跡加速度計(jì)算公式為:

      st=a0+a1t+a2t2+a3t3+a4t4+a5t5

      (3)計(jì)算機(jī)械臂末端在所述設(shè)定時(shí)刻t時(shí)的目標(biāo)位置pt,目標(biāo)速度和加速度

      機(jī)械臂末端的目標(biāo)位置pt=[xt,yt,zt]t;其中,

      θt為圓弧p0pt對應(yīng)的圓心角,滿足

      a1,a2,a3,b1,b2,b3滿足

      pt的切向量其中:

      機(jī)械臂末端的目標(biāo)速度和加速度

      (4)通過逆運(yùn)動學(xué)將所述機(jī)械臂末端的目標(biāo)位置pt,目標(biāo)速度和加速度轉(zhuǎn)換為機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度;

      機(jī)械臂關(guān)節(jié)角的位置:

      θt=invkinematics(pt)

      其中,invkinematics()代表逆運(yùn)動學(xué)的計(jì)算公式;

      關(guān)節(jié)的角速度與角加速度計(jì)算公式為:

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

      作為實(shí)施例,所述計(jì)算目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度的方法,可以包括如下:

      (1)在機(jī)械臂開始在所述圓弧運(yùn)動軌跡運(yùn)動后,統(tǒng)計(jì)機(jī)械臂的運(yùn)動時(shí)間tn;τ=1/f,f表示刷新頻率;

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

      (2)若所述運(yùn)動時(shí)間滿足:tn≤at,每隔設(shè)定時(shí)間τ計(jì)算一次機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度;一般情況下,所述a=70%。

      (3)若運(yùn)動時(shí)間tn滿足:at<tn<t,將標(biāo)志位更改為過渡狀態(tài),將控制器狀態(tài)機(jī)狀態(tài)設(shè)定為準(zhǔn)備狀態(tài);

      進(jìn)一步地,在過渡狀態(tài)下,若有新指令輸入,則啟動過渡運(yùn)動規(guī)劃;若沒有新指令輸入,則每隔設(shè)定時(shí)間τ計(jì)算一次機(jī)械臂每個(gè)關(guān)節(jié)的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度,并發(fā)送給設(shè)備通信軟件。

      (4)若運(yùn)動時(shí)間tn滿足:tn≥t,點(diǎn)到點(diǎn)運(yùn)動結(jié)束,將標(biāo)志位更改為等待狀態(tài)。

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

      針對于機(jī)器人圓弧運(yùn)動控制方法,本發(fā)明提供了該控制方法對應(yīng)的機(jī)器人圓弧運(yùn)動控制系統(tǒng),

      參考圖2所示,圖2是機(jī)器人圓弧運(yùn)動控制系統(tǒng)結(jié)構(gòu)示意圖,包括:總控模塊、算法模塊和通信管理模塊;

      所述總控模塊,用于接收控制端傳送的圓弧運(yùn)動指令;所述圓弧運(yùn)動指令包括圓弧運(yùn)動的目標(biāo)中間點(diǎn)位置、終點(diǎn)位置以及運(yùn)動需要的時(shí)間;

      所述算法模塊,用于讀取機(jī)械臂末端的當(dāng)前狀態(tài)參數(shù);根據(jù)所述當(dāng)前狀態(tài)參數(shù)以及所述目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧運(yùn)動的圓心、圓弧半徑、圓心角;根據(jù)目標(biāo)中間點(diǎn)位置、終點(diǎn)位置計(jì)算圓弧所在平面的法矢量;根據(jù)圓心、圓弧半徑、圓心角及所述當(dāng)前狀態(tài)參數(shù)得到圓弧的軌跡方程,根據(jù)所述法矢量和法矢量確定圓弧運(yùn)動軌跡;根據(jù)所述運(yùn)動需要的時(shí)間計(jì)算機(jī)械臂末端在所述圓弧運(yùn)動軌跡上運(yùn)行時(shí),機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度;其中,所述當(dāng)前狀態(tài)參數(shù)包括當(dāng)前位置、當(dāng)前速度和當(dāng)前加速度;

      所述通信管理模塊,用于將機(jī)械臂關(guān)節(jié)在各個(gè)位置的目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度轉(zhuǎn)發(fā)至控制主站。

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

      所述總控模塊在執(zhí)行圓弧運(yùn)動控制中,在接收控制端傳送的圓弧運(yùn)動指令后,根據(jù)所述圓弧運(yùn)動指令異步觸發(fā)圓弧運(yùn)動執(zhí)行函數(shù),根據(jù)所述圓弧運(yùn)動執(zhí)行函數(shù)并通過第一接口調(diào)用圓弧運(yùn)動規(guī)劃函數(shù);

      所述算法模塊在調(diào)用所述圓弧運(yùn)動規(guī)劃函數(shù)后,讀取機(jī)械臂末端的當(dāng)前狀態(tài)參數(shù)計(jì)算圓弧運(yùn)動軌跡;并通過第二接口將所述目標(biāo)角度、目標(biāo)角速度和目標(biāo)角加速度發(fā)送給通信管理模塊;其中,所述第二接口是基于ros上創(chuàng)建orocos的實(shí)時(shí)輸入/輸出接口;

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

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

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

      1)操作者通過人機(jī)交互界面生成圓弧運(yùn)動指令,指令的內(nèi)容,包括中間點(diǎn)位置p1,終點(diǎn)位置p2,以及運(yùn)動需要的時(shí)間t。

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

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

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

      a)通過orocos的operationalcaller方法調(diào)用算法模塊的圓弧運(yùn)動規(guī)劃函數(shù),并且傳遞指令參數(shù)。

      b)將控制器狀態(tài)機(jī)轉(zhuǎn)換為執(zhí)行圓弧運(yùn)動(active.circle)狀態(tài)。

      5)判斷控制器狀態(tài)機(jī)是否重新變?yōu)閞eady狀態(tài)。若是ready狀態(tài),則本次運(yùn)動完成。

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

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

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

      3)讀取機(jī)械臂末端當(dāng)前位置p0=[x0,y0,z0]t,當(dāng)前速度和加速度由于已知目標(biāo)中間點(diǎn)位置p1=[x1,y1,z1]t,終點(diǎn)位置p2=[x2,y2,z2]t,并將目標(biāo)的速度和加速度設(shè)為0。采用五次多項(xiàng)式方法,生成圓弧運(yùn)動軌跡。

      該方法的原理如下:

      該方法的原理如下:

      求圓心:

      其中,

      a1=y(tǒng)0z1-y0z2-z0y1+z0y2+y1z2-y2z1

      b1=-x0z1+x0z2+z0x1-z0x2-x1z2+x2z1

      c1=x0y1-x0y2-y0x1+y0x2+x1y2-x2y1

      d1=-x0y1z2+x0y2z1+x1y0z2-x2y0z1-x1y2z0+x2y1z0

      a2=2(x1-x0)

      b2=2(y1-y0)

      c2=2(z1-z0)

      a3=2(x2-x0)

      b3=2(y2-y0)

      c3=2(z2-z0)

      求圓弧半徑:

      求圓弧所在平面的法矢量n:

      其中,

      求圓弧p0p1p2的圓心角θc:

      當(dāng)0<θc<π時(shí),

      當(dāng)2π>θc>π時(shí),

      計(jì)算軌跡:

      直線的運(yùn)動軌跡用五次多項(xiàng)式(1)表示

      s(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5(6)

      式中,t是運(yùn)行時(shí)間;ai,i=1,…,5是系數(shù)。

      則可求得系數(shù):

      a0=s0

      然后就可以根據(jù)以下公式計(jì)算出任意時(shí)刻t時(shí)的軌跡位置st,軌跡速度軌跡加速度

      st=a0+a1t+a2t2+a3t3+a4t4+a5t5(8)

      計(jì)算末端的目標(biāo)位置pt=[xt,yt,zt]t,

      其中,θt為圓弧p0pt對應(yīng)的圓心角,滿足

      并且a1,a2,a3,b1,b2,b3滿足

      計(jì)算末端的目標(biāo)速度和加速度需要先求點(diǎn)pt的切向量

      然后可按式(12)求得末端的目標(biāo)速度和加速度

      使用逆運(yùn)動學(xué)方法,計(jì)算關(guān)節(jié)角的位置:

      θt=invkinematics(pt)(13)

      其中,invkinematics()代表逆運(yùn)動學(xué)的計(jì)算公式。

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

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

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

      5)算法模塊的updatehook()函數(shù)檢測到狀態(tài)標(biāo)志位為運(yùn)動狀態(tài)后,且運(yùn)動時(shí)間tn不超過運(yùn)動時(shí)長t的70%:

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

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

      6)若運(yùn)動時(shí)間tn滿足:0.7t<tn<t:

      a)將算法模塊的標(biāo)志位更改為過渡狀態(tài)。

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

      c)若有新指令輸入,則啟動過渡運(yùn)動規(guī)劃。

      d)若沒有新指令輸入,則每隔1毫秒依據(jù)公式(13)、(14)、(15)分別計(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)動時(shí)間tn滿足:tn≥t:

      a)本次圓弧運(yùn)動結(jié)束。

      b)將算法模塊的標(biāo)志位更改為等待狀態(tài)。

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

      為了更加清晰本發(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)的開發(fā)板,總控模塊接入控制端的人機(jī)交互界面的控制指令。

      linux主機(jī)可以安裝如下軟件:安裝xenomai或者rtai或者rtpreempt的實(shí)時(shí)內(nèi)核補(bǔ)??;安裝ros,orocos,rfsm等軟件。

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

      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ī)運(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ù):對事件報(bào)告處理請求進(jìn)行響應(yīng),包括生成錯(cuò)誤的時(shí)間戳,事件級別等信息,并將事件信息,發(fā)送給人機(jī)交互界面顯示。

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

      第三,設(shè)置各種運(yùn)動規(guī)劃的控制指令觸發(fā)函數(shù),這些函數(shù)將對算法模塊的相應(yīng)響應(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)信息通過事件報(bào)告接口傳遞給總控模塊處理;

      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)用方法傳入的第一個(gè)參數(shù),判斷調(diào)用類型,然后依據(jù)此類型選擇調(diào)用相應(yīng)運(yùn)動規(guī)劃的運(yùn)動指令觸發(fā)函數(shù)。

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

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

      (4)對于控制器狀態(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ù)、暫停、手動示教、回到原點(diǎn)、圓弧運(yùn)動、圓弧運(yùn)動、圓弧運(yùn)動、急停狀態(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ī)則,對八個(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語言,編寫總控模塊的啟動文件,該啟動文件設(shè)置為執(zhí)行如下動作:

      a)通過orocos的import方法,加載模塊進(jìn)行運(yùn)行;

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

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

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

      e)通過orocos的start方法,運(yùn)行總控模塊,總控模塊將先調(diào)用starthook()函數(shù),然后按預(yù)設(shè)刷新頻率,實(shí)時(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ī)運(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)動規(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)信息通過事件報(bào)告接口傳遞給總控模塊處理;

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

      (3)對于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í)行不同操作:

      ⅰ、如果是圓弧運(yùn)動,圓弧運(yùn)動,圓弧運(yùn)動,手動示教,急停,回到原點(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)對于ec_control_loop_component類,定義圓弧運(yùn)動,圓弧運(yùn)動,圓弧運(yùn)動,手動示教,急停,回到原點(diǎn),暫停,恢復(fù)等函數(shù)調(diào)用接口,實(shí)現(xiàn)如下:

      a)圓弧運(yùn)動,圓弧運(yùn)動,圓弧運(yùn)動,手動示教,回到原點(diǎn)的函數(shù),內(nèi)部實(shí)現(xiàn)如下:

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

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

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

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

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

      檢查控制器狀態(tài)機(jī)是否是圓弧運(yùn)動,圓弧運(yùn)動,圓弧運(yùn)動,手動示教,回到原點(diǎn)等狀態(tài)。如果不是則退出,并將相關(guān)信息通過事件報(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)信息通過事件報(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ī)是否是圓弧運(yùn)動,圓弧運(yùn)動,圓弧運(yùn)動,手動示教,回到原點(diǎn)等狀態(tài)。如果不是則退出,并將相關(guān)信息通過事件報(bào)告接口傳遞給總控模塊處理;

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

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

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

      (5)使用lua語言,編寫算法模塊的啟動文件,設(shè)置為執(zhí)行如下動作:

      a)通過orocos的import方法,加載算法模塊;

      d)定義算法模塊的刷新頻率,線程的優(yōu)先級別;

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

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

      e)通過orocos的start方法,運(yùn)行算法模塊,算法模塊先調(diào)用starthook()函數(shù),然后按設(shè)置的刷新頻率,實(shí)時(shí)地周期性調(diào)用updatehook()函數(shù)。

      3、對于通信管理模塊:

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

      通信管理模塊可以利用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)建一個(gè)orocos的實(shí)時(shí)模塊,記為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ī)運(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)告的接口,通過該接口觸發(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)信息通過事件報(bào)告接口傳遞給總控模塊進(jìn)行處理;

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

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

      ⅱ、電機(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)動。如果機(jī)械臂任一關(guān)節(jié)角度與零度相差大于0.01度,則執(zhí)行回零運(yùn)動,調(diào)用圓弧運(yùn)動規(guī)劃,對回零運(yùn)動進(jìn)行規(guī)劃。

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

      如果機(jī)器人的機(jī)械臂需要執(zhí)行回零運(yùn)動,則控制設(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)對于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)動。此時(shí),執(zhí)行如下操作:

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

      如果運(yùn)動已回零成功,向設(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ù)的輸入通道中,讀取控制指令,并存儲到電機(jī)指令緩沖隊(duì)列中。

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

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

      此時(shí),依據(jù)診斷信息,對系統(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ù)的輸入通道中是否有新的指令,若有新指令,則讀取控制指令,并存儲到電機(jī)指令緩沖隊(duì)列中。

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

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

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

      c)讀取電機(jī)狀態(tài),根據(jù)機(jī)械臂的模型計(jì)算機(jī)械臂關(guān)節(jié)以及末端運(yùn)動狀態(tài)信息,并通過輸出數(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)對于ec_component的cleanuphook()成員函數(shù),該函數(shù)在模塊結(jié)束運(yùn)行時(shí),自動調(diào)用,設(shè)置執(zhí)行如下操作:

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

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

      (5)參考圖9所示,圖9是設(shè)備狀態(tài)機(jī)的狀態(tài)變化示意圖。共有init(初始化)、fault(中斷),active.recovery(恢復(fù))、active.hands(手動示教)、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ī)則,對四個(gè)子狀態(tài)均有效。

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

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

      b)定義通信管理模塊的刷新頻率,線程的優(yōu)先級別;

      c)對通信管理模塊的屬性進(jìn)行賦值;

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

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

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

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

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

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

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

      當(dāng)前第1頁1 2 
      網(wǎng)友詢問留言 已有0條留言
      • 還沒有人留言評論。精彩留言會獲得點(diǎn)贊!
      1