專利名稱：一種基于余弦二階的機器人軌跡規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種基于余弦二階的機器人軌跡規(guī)劃方法，屬于機器人軌跡規(guī) 劃領(lǐng)域。
背景技術(shù)：
通常情況下，機器人的期望軌跡是事先給定一系列笛卡爾或關(guān)節(jié)空間的點， 且給定通過該點的速度或兩點之間的時間，另外還會限制機器人運動允許的最 大速度。軌跡規(guī)劃的目的就是根據(jù)這些約束條件，建立通過這些點的平滑軌跡。 為了縮短運動路徑，通常規(guī)劃這些點之間通過直線相連，并在連接點處通過曲 線過渡。最為常用的是直線段加拋物線軌跡規(guī)劃。
具有拋物線擬合的線性段軌跡包括勻加速、勻速和勻減速階段，在考慮最 大運行速度的情況下達到經(jīng)歷時間最短。這種方法能夠很好地保證軌跡的位置
和速度連續(xù)，如圖2-1至圖2-3所示，但是，這種方法在過渡點tl、 t2處都將 存在加速度的跳躍，以及無窮大的加速度導(dǎo)數(shù)值，這就意味著驅(qū)動力矩及驅(qū)動 力矩一階導(dǎo)數(shù)突然增大，由此生成的軌跡并不容易跟蹤。

發(fā)明內(nèi)容
本發(fā)明的目的是解決現(xiàn)有機器人軌跡采用拋物線擬合的線性段軌跡的方法 存在加速度的階躍變化，使驅(qū)動力矩及驅(qū)動力矩一階導(dǎo)數(shù)突然增大，從而導(dǎo)致 軌跡不容易跟蹤的問題，提供了一種基于余弦二階的機器人軌跡規(guī)劃方法。
本發(fā)明實現(xiàn)基于余弦二階的機器人軌跡規(guī)劃方法的步驟包括
步驟一、建立基于余弦二階的機器人機械臂軌跡曲線的位置的數(shù)學(xué)模型、 速度的數(shù)學(xué)模型、加速度的數(shù)學(xué)模型和加速度的導(dǎo)數(shù)的數(shù)學(xué)模型；
步驟二、給定機器人機械臂運動的起始期望點位置^、起始期望點速度k、 末端期望點位置^和末端期望點速度v2 ，按最大加速度"_和最大加速度的導(dǎo)數(shù) 厶M進行基于余弦二階的機器人機械臂軌跡曲線的規(guī)劃；步驟三、依據(jù)最短時間原則將機器人機械臂從起始期望點到末端期望點的 軌跡分成四段，分別為加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段
和速度平穩(wěn)段；
步驟四、將加速度傾斜上升段的邊界條件的值代入步驟一所述數(shù)學(xué)模型中， 列出方程組并求解出所述數(shù)學(xué)模型的參數(shù)；
步驟五、判斷是否滿足條件S2-S^D1 + D2 + D3， 判斷結(jié)果為是，執(zhí)行步驟六，判斷結(jié)果為否，執(zhí)行步驟九， 步驟六、判斷是否滿足條件v2^V2min，
判斷結(jié)果為是，執(zhí)行步驟七，判斷結(jié)果為否，執(zhí)行步驟八，
其中，v2mm"1+Y， 、n為基于余弦二階的軌跡只包括加速度傾斜上升段
和加速度傾斜下降段時的最小的末端速度值，
步驟七、基于余弦二階的軌跡按最大加速度"^和最大加速度的導(dǎo)數(shù)J^進
行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃
方法完成，
本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度平 穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段四段，則 D4 = HDl —D2-D3，
既而獲得機器人機械臂期望的輸出軌跡，
步驟八、基于余弦二階的軌跡按加速度閾值",和最大加速度的導(dǎo)數(shù)J^進 行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃 方法完成，
本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度傾 斜下降段和速度平穩(wěn)段三段，且滿足以下方程組步驟九、基于余弦二階的軌跡按加速度閾值。,和加速度的導(dǎo)數(shù)閾值^進行 規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃方 法完成，
本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段和加速度傾 斜下降段兩段，且滿足以下方程組
—，
獲得余弦周期7\加速度閾值a,和加速度的導(dǎo)數(shù)閾值々 T = 2(S2-S)
/ r
既而獲得機器人機械臂期望的輸出軌跡，
其中，Dl為加速度傾斜上升段運行距離，D2為加速度平穩(wěn)段運行距離，D3 為加速度傾斜下降段運行距離，D4為速度平穩(wěn)段運行距離。
本發(fā)明的優(yōu)點是本發(fā)明可計算出一條位置、速度、加速度可導(dǎo)，并且加 速度的導(dǎo)數(shù)在某一個限定的范圍內(nèi)連續(xù)的軌跡。由此方法生成的軌跡，更容易 被跟蹤，可大大降低機器人的運動的位置跟蹤誤差。


圖1是本發(fā)明方法的流程圖，圖2-1是拋物線擬合的線性段軌跡-加速度曲 線，圖2-2是拋物線擬合的線性段軌跡-速度曲線，圖2-3是拋物線擬合的線性 段軌跡-位置曲線，圖3是軌跡規(guī)劃分段示意圖(A表示持續(xù)脈沖，B表示脈沖， C表示平穩(wěn)段，D表示傾斜段)，圖4-1是基于余弦二階的軌跡規(guī)劃-位置曲線， 圖4-2是基于余弦二階的軌跡規(guī)劃-速度曲線，圖4-3是基于余弦二階的軌跡規(guī) 劃-加速度曲線，圖4-4是基于余弦二階的軌跡規(guī)劃-加速度的導(dǎo)數(shù)曲線，圖5-l
9是速度不能滿足要求的基于余弦二階的軌跡規(guī)劃-位置曲線，圖5-2是速度不能 滿足要求的基于余弦二階的軌跡規(guī)劃-速度曲線，5-3是速度不能滿足要求的基 于余弦二階的軌跡規(guī)劃-加速度曲線，圖5-4是速度不能滿足要求的基于余弦二 階的軌跡規(guī)劃-加速度導(dǎo)數(shù)的曲線，圖6-1是恒速下的基于余弦二階的軌跡規(guī)劃 -位置曲線，圖6-2是恒速下的基于余弦二階的軌跡規(guī)劃-速度曲線，圖6-3是 恒速下的基于余弦二階的軌跡規(guī)劃-加速度曲線，圖6-4是恒速下的基于余弦二 階的軌跡規(guī)劃-加速度的導(dǎo)數(shù)的曲線，圖7-1是位移不能滿足要求的基于余弦二 階的軌跡規(guī)劃-位置曲線，圖7-2是位移不能滿足要求的基于余弦二階的軌跡規(guī) 劃-速度曲線，圖7-3是位移不能滿足要求的基于余弦二階的軌跡規(guī)劃-加速度 曲線，圖7-4是位移不能滿足要求的基于余弦二階的軌跡規(guī)劃-加速度的導(dǎo)數(shù)的 曲線，圖8-l是基于正弦一階軌跡規(guī)劃-位置曲線，圖8-2是基于正弦一階軌跡 規(guī)劃-速度曲線，圖8-3是基于正弦一階軌跡規(guī)劃-加速度曲線，圖8-4是基于 正弦一階軌跡規(guī)劃-加速度的導(dǎo)數(shù)的曲線，圖8-5是基于正弦一階軌跡跟蹤效果 -誤差曲線，圖8-6是基于正弦一階軌跡跟蹤效果-力矩曲線，圖9-1是基于余 弦二階軌跡規(guī)劃-位置曲線，圖9-2是基于余弦二階軌跡規(guī)劃-速度曲線，圖9-3 是基于余弦二階軌跡規(guī)劃-加速度曲線，圖9-4是基于余弦二階軌跡規(guī)劃-加速 度曲線，圖9-5是基于余弦二階軌跡跟蹤效果-誤差曲線，圖9-6是基于余弦二 階軌跡跟蹤效果-力矩曲線。
具體實施例方式
具體實施方式
一下面結(jié)合圖l、圖3、圖4-1 圖4-4、圖5-1 圖5-4、 圖6-1 圖6-4、圖7-l 圖7-4說明本實施方式，本實施方式實現(xiàn)基于余弦二 階的機器人軌跡規(guī)劃方法包括以下步驟
步驟一、建立基于余弦二階的機器人機械臂軌跡曲線的位置的數(shù)學(xué)模型、 速度的數(shù)學(xué)模型、加速度的數(shù)學(xué)模型和加速度的導(dǎo)數(shù)的數(shù)學(xué)模型；
通常將兩點間的軌跡根據(jù)控制參數(shù)(位置、速度和加速度)的變化分為兩 段傾斜段D和平穩(wěn)段C，參見圖3所示。
控制參數(shù)發(fā)生變化的階段為傾斜段D，例如速度上升段是指速度發(fā)生變化， 屬于傾斜段D?？刂茀?shù)不發(fā)生變化的階段為平穩(wěn)段c，例速度平穩(wěn)是指恒速運行，屬于平 穩(wěn)段c。
在圖3中，控制參數(shù)經(jīng)過上升傾斜段后緊接下降傾斜段稱為脈沖B;經(jīng)歷上
升傾斜段，平穩(wěn)段后緊接下降傾斜段稱為持續(xù)脈沖A。
基于余弦二階的機器人機械臂軌跡曲線的位置的數(shù)學(xué)模型^)為' jc(O = 63 cos(^~) + + V + 6。 (1)
其中6。， ~ 62， 63為系數(shù)，r為所述余弦的周期；
基于余弦二階的機器人機械臂軌跡曲線的速度的數(shù)學(xué)模型v(O為 V(,)=-爭^sin(^) + 2V + 、 (2)
對公式(1)求導(dǎo)獲得速度的數(shù)學(xué)模型；
基于余弦二階的機器人機械臂軌跡曲線的加速度的數(shù)學(xué)模型"(O為 "(,)= 一享—(學(xué))+ 262 (3)
對公式(2)求導(dǎo)獲得加速度的數(shù)學(xué)模型；
基于余弦二階的機器人機械臂軌跡曲線的加速度的導(dǎo)數(shù)的數(shù)學(xué)模型J^^)
為:
,)=爭—(學(xué)) (4)
對公式(3)求導(dǎo)獲得加速度的導(dǎo)數(shù)的數(shù)學(xué)模型；
步驟二、給定機器人機械臂運動的起始期望點位置S,、起始期望點速度v。 末端期望點位置&和末端期望點速度^，按最大加速度"_和最大加速度的導(dǎo)數(shù) Jmm進行基于余弦二階的機器人機械臂軌跡曲線的規(guī)劃；
最大加速度"^和最大加速度的導(dǎo)數(shù)人^之間存在如下關(guān)系 4=， (5)
最大加速度"^由機器人機械臂的最大輸出力矩性能決定的，為己知量，最 大加速度的導(dǎo)數(shù)J^也為已知量，由公式(3)和公式(4)可推導(dǎo)出公式(5)，由公式(5)可求得余弦周期7的數(shù)值。
步驟三、依據(jù)最短時間原則(在加速度傾斜上升段以r/2時間達到加速度最 大值"_)將機器人機械臂從起始期望點到末端期望點的軌跡分成四段，分別為 加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段，參見圖
4-l 圖4-4，具體為
加速度傾斜上升段的時間區(qū)間為[O，tl]，速度由v,增加到v。，運行距離為 Dl，加速度達到最大值"_，
加速度平穩(wěn)段的時間區(qū)間為[tl，t2]，速度由v。增加到、，運行距離為D2， 加速度保持最大值《_ ，加速度的導(dǎo)數(shù)為0，
加速度傾斜下降段的時間區(qū)間為[t2,t3]，速度由^增加到、，運行距離為 D3，加速度為由"_下降到0，
速度平穩(wěn)段的時間區(qū)間為[t3，t4]，速度保持v,，運行距離為D4，加速度為 0，加速度的導(dǎo)數(shù)為0。
給定起始期望點的位置和速度(S， Vl)、末端期望點的位置和速度(S2， v2)，且52>&， v2>Vl，限定最大加速度和最大加速度的導(dǎo)數(shù)分別為"^和厶M。 期望在最短的時間內(nèi)完成從S到&的過渡，因此理想的軌跡規(guī)劃將以。^和J^ 讓速度在最短的時間內(nèi)達到末端期望點速度^，并以末端期望點速度^ (最大的 速度)勻速達到末端期望點位置&。
步驟四、將加速度傾斜上升段的邊界條件的值代入步驟一所述數(shù)學(xué)模型中， 列出方程組并求解出所述數(shù)學(xué)模型的參數(shù)；
加速度傾斜上升段滿足在r/2時間達到加速度最大值 ^ ，所述基于余弦二
階的機器人機械臂軌跡曲線在 0時位置x(0)、速度HO)、加速度"(0)和加速度
的導(dǎo)數(shù)丄A(O)滿足的約束條件為 x(O) = &
/(0) = 、 (6) a(O) = 0
M(O) = 0
所述基于余弦二階的機器人機械臂軌跡曲線在^r/2時位置x(772)、速度 v(772)、加速度"(772)和加速度的導(dǎo)數(shù)丄A(772)滿足的約束條件為
12'jc(r/2) = S+Dl
v(r/2) = ^
《/2)="臓 =0
(7)
將方程組(6)和方程組(7)代入公式(1)至公式(4)中，獲得有關(guān)四 個系數(shù)V 6P 62， 63的值、加速度傾斜上升段末端速度v。和加速度傾斜上升段運 行距離D1:
8"2
&，=.
4
8冗2
16 4tt2 鵬 2
(8)
則加速度平穩(wěn)段末端速度Va為:
Va=V2—卞
(9)
加速度平穩(wěn)段的運行距離D2和加速度傾斜下降段的運行距離D3為:
D2 =
v6 -、 2a—
D3 = 4-A)"j2+^
(10)
、16 4;r"'脇 2 步驟五、判斷是否滿足條件S2-S^D1 + D2 + D3，
判斷結(jié)果為是，執(zhí)行步驟六，判斷結(jié)果為否，執(zhí)行步驟九，
本步驟所述的基于余弦二階的機器人機械臂運動軌跡按最大加速度"_和
最大加速度的導(dǎo)數(shù)進行規(guī)劃時需滿足兩個限定條件 丁&-pDl + D2 + D3
其中，Dl + D2 + D3為最小位移，^^為基于余弦二階的軌跡只包括加速度傾 斜上升段和加速度傾斜下降段時的最小的末端速度值，且v^n滿足以下關(guān)系式
v2
2
(11)步驟六、判斷是否滿足條件V^v2min，
判斷結(jié)果為是，執(zhí)行步驟七，判斷結(jié)果為否，執(zhí)行步驟八，
步驟七、基于余弦二階的軌跡按最大加速度^和最大加速度的導(dǎo)數(shù)/^進
行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃
方法完成，
本步驟所述的是位移和速度兩個限定條件都滿足要求的情況，如圖4-l 圖
4-4所示，
基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度平穩(wěn)段、加速度 傾斜下降段和速度平穩(wěn)段四段，則
D4 = HD1-D2-D3 (12)
依據(jù)公式(5)獲得到余弦周期7，根據(jù)公式(9)獲得加速度傾斜上升段末 端速度v。、加速度平穩(wěn)段末端速度、以及四段的運行距離D1 D4，既而獲得機 器人機械臂期望的輸出軌跡；
步驟八、基于余弦二階的軌跡按加速度閾值",和最大加速度的導(dǎo)數(shù)J^進 行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃 方法完成，
本步驟所述情況為 f&—& 2D1 + D2 + D3
即速度不能滿足要求的情況，表示末端速度如果在最大加速度"_和最大加 速度的導(dǎo)數(shù)々 的限制條件下，能達到末端期望點位置&但將超出末端期望點的 速度^。要想使軌跡規(guī)劃的末端期望點能夠滿足要求，我們只能令加速度的幅 值下降，設(shè)定加速度的閾值為",，但同時要保證時間最短，軌跡規(guī)劃時應(yīng)該令 加速度的導(dǎo)數(shù)值仍舊為々M，并且軌跡規(guī)劃僅包括加速度傾斜上升段、加速度傾 斜下降段和速度平穩(wěn)段三段。其物理意義是讓速度以最小的時間上升到^，并 以的速度勻速運動到末端期望點位置&
所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度傾斜下降 段和速度平穩(wěn)段三段滿足以下方程組<formula>formula see original document page 15</formula>(13)獲得加速度閾值",和余弦周期r，既而獲得機器人機械臂期望的輸出軌跡， 如圖5-l 圖5-4如示，
當K =^時，機器人機械臂期望的輸出軌跡如圖6-l 圖6-4如示，為恒速 狀態(tài)，保持起始期望點速度v,勻速行駛；
步驟九、基于余弦二階的軌跡按加速度閾值^和加速度的導(dǎo)數(shù)閾值^進行 規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃方 法完成，
本步驟所述情況為位移不能滿足要求 HD1 + D2 + D3
表示無論末端期望點速度v2無論如何給定，在最大加速度flmax和最大加速度 的導(dǎo)數(shù)厶雙的限制條件下，末端期望點的位置都將超過&。此時必須調(diào)整末端期 望點速度、和加速度幅值，設(shè)定加速度的閾值為",，保證期望的軌跡不會超出 末端期望點位置& 。為了令位移最小并且保證加速度的導(dǎo)數(shù)的幅值恒等于J^ ， 規(guī)劃的軌跡只包含加速度傾斜上升段和下降段。期望的軌跡滿足如下關(guān)系
<formula>formula see original document page 15</formula>(14)
上式可整理為
<formula>formula see original document page 15</formula> (15)
且r只有一個正解。
在實際應(yīng)用時，上述方程并不容易在數(shù)字控制器中計算；且上述方程將以 犧牲末端期望點的速度為代價保證J^，不適合于實際應(yīng)用；另外通常是在較為 精確的定時下(小于250ms)給定下一個期望點，當位移不能滿足要求時主要發(fā) 生在近距離逼近目標點狀態(tài)，此時優(yōu)先條件是機械臂的位移和速度均滿足要求。 為了令小位移步進下的加速度幅值最小，規(guī)劃的軌跡仍只包括加速度傾斜上升段和加速度傾斜下降段，且滿足以下方程組 "2)|
v2-v1+f (16) 、 T
獲得余弦周期7\加速度閾值^和加速度的導(dǎo)數(shù)閾值々 _2(v2—v,)
廣 r
/ - f
既而獲得機器人機械臂期望的輸出軌跡。
當々> /皿時，需警告上位機，給定的末端期望點速度、的值過大，加速度 上升過快，即輸出的扭矩瞬時變化過快，易引起機械響應(yīng)速度不能跟隨輸入達 到預(yù)期的扭矩值。
具體實施方式
二下面結(jié)合圖8-l 圖8-6、圖9-l 圖9-6說明本實施方 式，本實施方式給出一個實施例。
本實施例以四自由度機器人為例。該機器人由四個模塊化旋轉(zhuǎn)關(guān)節(jié)構(gòu)成。 關(guān)節(jié)采用無刷直流電機驅(qū)動，諧波減速器(減速比l: 160)作為力矩傳動。每 個關(guān)節(jié)都安裝有一個電位計和磁編碼器用于測量關(guān)節(jié)和電機的角度。軌跡跟蹤 實驗主要用于驗證加速度的導(dǎo)數(shù)連續(xù)對系統(tǒng)性能的影響。本實施例參照基于正 弦一階數(shù)學(xué)模型的機器人機械臂的軌跡規(guī)劃方法，突出本發(fā)明基于余弦二階的 機器人機械臂軌跡規(guī)劃方法的優(yōu)勢。
基于正弦一階機器人機械臂軌跡曲線的位置數(shù)學(xué)模型為
2對
= a2 sin(-) + "Z + "0 (17)
其中，r,為所述正弦的周期， "p ^為系數(shù)，
貝U，基于正弦一階機器人機械臂的速度數(shù)學(xué)模型v^)為<formula>formula see original document page 17</formula>
基于正弦一階機器人機械臂的加速度數(shù)學(xué)模型為
<formula>formula see original document page 17</formula>
基于正弦一階機器人機械臂的加速度的導(dǎo)數(shù)數(shù)學(xué)模型^")為
Js(0 = ^r"2cos(^) (20) 正弦周期r為加速段所需的時間。由公式(18) 公式(20)可以看出，
正弦一階函數(shù)具有連續(xù)的位置、速度和加速度，但是加速度的導(dǎo)數(shù)在加速度靜
止到零時G = r)不連續(xù)且不能限制其幅值。
加速度和加速度的導(dǎo)數(shù)將直接影響機械臂的力矩和力矩導(dǎo)數(shù)。而基于位置 的阻抗控制由位置外環(huán)和力矩內(nèi)環(huán)組成，當輸出力矩發(fā)生變化時將會對阻抗控 制的跟蹤效果造成明顯的影響。實驗時令四自由度機器人的第二關(guān)節(jié)在30度至 60度間做往復(fù)運動，其最大速度在兩種跟蹤模式下均設(shè)置為15度/秒。在本發(fā) 明所述基于余弦二階軌跡方法中限定最大加速度"^和最大加速度的導(dǎo)數(shù)J^ 分別為6.5度/秒2和15度/秒3。
圖8-5 圖8-6是采用正弦一階軌跡的跟蹤效果圖，圖8-l 圖8-4分別代 表期望的位置、速度、加速度和加速度的導(dǎo)數(shù)的軌跡曲線。由圖8-4可以看出 加速度的導(dǎo)數(shù)值不連續(xù)且不能夠限定其幅值。圖8-5是二關(guān)節(jié)的跟蹤誤差曲線， 其誤差為±0.7度。圖8-6是二關(guān)節(jié)所受外力矩(去除重力矩)的測量值，可以 看出關(guān)節(jié)力矩容易出現(xiàn)波動，其波動范圍為土8Nm。
圖9-5 圖9-6是基于余弦二階軌跡的跟蹤效果，圖9-1 圖9-4的曲線意 義與圖8-1 圖8-4相仿。由圖9-3 圖9-4可以分析出二關(guān)節(jié)的加速度和加速 度的導(dǎo)數(shù)值連續(xù)且都在限定范圍內(nèi)。由于輸出力矩平穩(wěn)，在阻抗控制下二關(guān)節(jié) 的位置跟蹤誤差減小至土0.3度；所測量的力矩波動減小為±3 Nm。
由本實施例可以看出基于余弦二階的軌跡保證了電機側(cè)輸入力矩的平穩(wěn)， 從而在具有位置和力控制的阻抗控制中得到關(guān)節(jié)輸出力矩的穩(wěn)定。
權(quán)利要求
1、一種基于余弦二階的機器人軌跡規(guī)劃方法，其特征在于實現(xiàn)該方法的步驟包括步驟一、建立基于余弦二階的機器人機械臂軌跡曲線的位置的數(shù)學(xué)模型、速度的數(shù)學(xué)模型、加速度的數(shù)學(xué)模型和加速度的導(dǎo)數(shù)的數(shù)學(xué)模型；步驟二、給定機器人機械臂運動的起始期望點位置S1、起始期望點速度v1、末端期望點位置S2和末端期望點速度v2，按最大加速度amax和最大加速度的導(dǎo)數(shù)Jmax進行基于余弦二階的機器人機械臂軌跡曲線的規(guī)劃；步驟三、依據(jù)最短時間原則將機器人機械臂從起始期望點到末端期望點的軌跡分成四段，分別為加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段；步驟四、將加速度傾斜上升段的邊界條件的值代入步驟一所述數(shù)學(xué)模型中，列出方程組并求解出所述數(shù)學(xué)模型的參數(shù)；步驟五、判斷是否滿足條件S2-S1≥D1+D2+D3，如果判斷結(jié)果為是，執(zhí)行步驟六，如果判斷結(jié)果為否，執(zhí)行步驟九，步驟六、判斷是否滿足條件v2≥v2min，如果判斷結(jié)果為是，執(zhí)行步驟七，如果判斷結(jié)果為否，執(zhí)行步驟八，其中，v2min為基于余弦二階的軌跡只包括加速度傾斜上升段和加速度傾斜下降段時的最小的末端速度值；步驟七、基于余弦二階的軌跡按最大加速度amax和最大加速度的導(dǎo)數(shù)Jmax進行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃方法完成，本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段四段，則D4＝S2-S1-D1-D2-D3既而獲得機器人機械臂期望的輸出軌跡，步驟八、基于余弦二階的軌跡按加速度閾值af和最大加速度的導(dǎo)數(shù)Jmax進行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃方法完成，本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度傾斜下降段和速度平穩(wěn)段三段，且滿足以下方程組獲得加速度閾值af和余弦周期T，既而獲得機器人機械臂期望的輸出軌跡，步驟九、基于余弦二階的軌跡按加速度閾值af和加速度的導(dǎo)數(shù)閾值Jf進行規(guī)劃，獲得機器人機械臂期望的輸出軌跡，基于余弦二階的機器人軌跡規(guī)劃方法完成，本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段和加速度傾斜下降段兩段，且滿足以下方程組獲得余弦周期T、加速度閾值af和加速度的導(dǎo)數(shù)閾值Jf既而獲得機器人機械臂期望的輸出軌跡，其中，D1為加速度傾斜上升段運行距離，D2為加速度平穩(wěn)段運行距離，D3為加速度傾斜下降段運行距離，D4為速度平穩(wěn)段運行距離。
2、 根據(jù)權(quán)利要求1所述的一種基于余弦二階的機器人軌跡規(guī)劃方法，其特 征在于步驟一所述基于余弦二階的機器人機械臂軌跡曲線的位置雄)的數(shù)學(xué)模 型為x(O = 63 cos(^") + 62r + V + 60 ，其中Z>。， ^ 62，、為系數(shù)，r為所述余弦的周期；所述基于余弦二階的機器人機械臂軌跡曲線的速度W)的數(shù)學(xué)模型為 z 、 2;r T ■ z2t Y、。'」 '所述基于余弦二階的機器人機械臂軌跡曲線的加速度"(0的數(shù)學(xué)模型為,、 4;r2 , ,2對、。7 a(O = - yr乜cos(7) + 262;所述基于余弦二階的機器人機械臂軌跡曲線的加速度的導(dǎo)數(shù)^^W的數(shù)學(xué) 模型為
3、 根據(jù)權(quán)利要求1所述的一種基于余弦二階的機器人軌跡規(guī)劃方法，其特 征在于步驟二中所述的最大加速度o^和最大加速度的導(dǎo)數(shù)J，之間存在如下加 r
4、 根據(jù)權(quán)利要求l所述的一種基于余弦二階的機器人軌跡規(guī)劃方法，其特 征在于，步驟三中所述的最短時間原則為在加速度傾斜上升段以r/2時間達到 加速度最大值a^ 。
5、 根據(jù)權(quán)利要求l所述的一種基于余弦二階的機器人軌跡規(guī)劃方法，其特征在于步驟三中所述加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段 和速度平穩(wěn)段分別為加速度傾斜上升段的時間區(qū)間為[O，tl]，速度由v,增加到v。，運行距離為Dl，加速度達到最大值《_ ，加速度平穩(wěn)段的時間區(qū)間為[tl，t2]，速度由v。增加到、，運行距離為D2， 加速度保持最大值"^ ，加速度的導(dǎo)數(shù)為0，加速度傾斜下降段的時間區(qū)間為[t2，t3]，速度由、增加到v,，運行距離為 D3，加速度為由"_下降到0，速度平穩(wěn)段的時間區(qū)間為[t3，t4]，速度保持力，運行距離為D4，加速度為 0，加速度的導(dǎo)數(shù)為0。
6、根據(jù)權(quán)利要求1所述的一種基于余弦二階的機器人軌跡規(guī)劃方法，其特 征在于，步驟四求解數(shù)學(xué)模型參數(shù)的方法為加速度傾斜上升段滿足在r/2時間達到加速度最大值。_ ，所述基于余弦二 階的機器人機械臂軌跡曲線在 0時位置x(0)、速度v(O)、加速度fl(O)和加速度 的導(dǎo)數(shù)^A(0)滿足的約束條件為x(O) = S, v(O)二v, ， 'a(O) = 0 ， ，A:(O) = 0所述基于余弦二階的機器人機械臂軌跡曲線在 r/2時位置x(772)、速度 v(772)、加速度"(r/2)和加速度的導(dǎo)數(shù)AA(J72)滿足的約束條件為x(r/2)、+Dl,v(r/2)"。 ，"(7/2) = "max ，j^(7y2) = 0將上述兩組邊界約束條件方程組代入步驟一建立的四個數(shù)學(xué)模型中，獲得有關(guān)四個系數(shù)6。， ^， 62， 63的值、加速度傾斜上升段末端速度v。和加速度傾斜上 升段運行距離D1:<formula>formula see original document page 6</formula>則加速度平穩(wěn)段末端速度^為:<formula>formula see original document page 6</formula>加速度平穩(wěn)段的運行距離D2和加速度傾斜下降段的運行距離D3為:<formula>formula see original document page 6</formula>
全文摘要
一種基于余弦二階的機器人軌跡規(guī)劃方法，屬于機器人軌跡規(guī)劃領(lǐng)域。本發(fā)明的目的是解決現(xiàn)有機器人軌跡采用拋物線擬合的線性段軌跡的方法存在加速度的階躍變化，使驅(qū)動力矩及驅(qū)動力矩一階導(dǎo)數(shù)突然增大，從而導(dǎo)致軌跡不容易跟蹤的問題。本發(fā)明首先給定基于余弦的二階軌跡曲線的位置、速度、加速度以及加速度的導(dǎo)數(shù)的數(shù)學(xué)模型，給定兩個期望點的位置和速度。然后，將邊界條件的值代入所述數(shù)學(xué)模型中，列出方程組并求解出模型的參數(shù)。最后，根據(jù)兩個期望點間的位置和速度的關(guān)系及加速度和加速度的導(dǎo)數(shù)的幅值來限制加速度的導(dǎo)數(shù)的閥值。確定最終的規(guī)劃軌跡。本發(fā)明適用于機器人的軌跡規(guī)劃，能夠生成平滑的曲線，容易跟蹤的軌跡。
文檔編號B25J9/16GK101508113SQ20091007152
公開日2009年8月19日 申請日期2009年3月11日 優(yōu)先權(quán)日2009年3月11日
發(fā)明者宏 劉, 熊根良, 王再明, 謝宗武, 黃劍斌 申請人:哈爾濱工業(yè)大學(xué)