本發(fā)明涉及具有空間運(yùn)動(dòng)特征的機(jī)器人技術(shù)領(lǐng)域，尤其涉及一種機(jī)器人笛卡爾空間平滑軌跡的規(guī)劃方法。

背景技術(shù)：

機(jī)器人笛卡兒空間運(yùn)動(dòng)路徑多采用直線、圓弧，為提升機(jī)器人的執(zhí)行效率和平穩(wěn)性，要求路徑段具有平滑過(guò)渡的特征，即在軌跡運(yùn)動(dòng)過(guò)程中，不減速或不停止。

目前，軌跡過(guò)渡規(guī)劃的研究中，多采用圓弧、高次曲線來(lái)構(gòu)造過(guò)渡段路徑。其中圓弧過(guò)渡路徑應(yīng)用最為廣泛，在相鄰兩段直線之間具有很好的效果，但對(duì)于空間直線與圓弧、空間圓弧與圓弧之間則存在著加速度不連續(xù)的情況；采用高次曲線，特別是五次多項(xiàng)式，其過(guò)渡路徑非常平滑，但是存在著路徑隨著速度、加速度參數(shù)的改變而改變的特征，存在著路徑不可控的缺點(diǎn)；中國(guó)發(fā)明專利“一種工業(yè)機(jī)器人應(yīng)用的過(guò)渡軌跡規(guī)劃方法”(申請(qǐng)?zhí)?01610075346.7)，采用兩段拋物線作為過(guò)渡路徑，以位置規(guī)劃為主，姿態(tài)規(guī)劃為輔，即可保證路徑的平滑性，又能保證速度、加速度的連續(xù)性，但是在路徑段位置距離過(guò)短、姿態(tài)變化大等情況下，機(jī)器人運(yùn)動(dòng)性能還有很大的提升空間。

目前機(jī)器人姿態(tài)多表示為歐拉角或姿態(tài)矩陣，但歐拉角不具有向量的性質(zhì)，而姿態(tài)矩陣插值比較困難。因此，如何提供一種簡(jiǎn)單高效的機(jī)器人笛卡爾空間平滑軌跡規(guī)劃方法，以進(jìn)一步提升機(jī)器人運(yùn)動(dòng)性能是很有必要的。

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

為了克服現(xiàn)有技術(shù)存在的缺點(diǎn)與不足，本發(fā)明提供一種機(jī)器人笛卡爾空間平滑軌跡的規(guī)劃方法，對(duì)以直線、圓弧為主的機(jī)器人空間軌跡進(jìn)行平滑規(guī)劃，以解決機(jī)器人軌跡停頓、不流暢、抖動(dòng)、偏離路徑、超速等問(wèn)題，進(jìn)一步提升機(jī)器人運(yùn)動(dòng)的效率和平穩(wěn)性。

為解決上述技術(shù)問(wèn)題，本發(fā)明提供如下技術(shù)方案：一種機(jī)器人笛卡爾空間平滑軌跡的規(guī)劃方法，包括擺線加減速規(guī)劃方法、空間任意兩點(diǎn)位置過(guò)渡路徑規(guī)劃方法以及空間任意兩點(diǎn)姿態(tài)過(guò)渡路經(jīng)規(guī)劃方法，其中

所述擺線加減速規(guī)劃方法，其過(guò)程分為加速、勻速和減速三個(gè)階段，加速階段和減速階段的軌跡形狀為正則擺線，具體包括如下步驟：

s11、已知路徑長(zhǎng)度l、初始速度vs、期望速度v、期望末速度ve、關(guān)節(jié)最大速度和關(guān)節(jié)最大加速度計(jì)算該路徑速度、加速度最大值vm、am；

s12、若期望末速度ve不能到達(dá)，需要重新規(guī)劃期望速度v＝ve，加速度、勻速段和減速段的運(yùn)動(dòng)時(shí)間分別為t1＝|v-vs|/am、t2＝0、t3＝|v-ve|/am；

s13、若期望末速度ve能夠到達(dá)，進(jìn)而判斷期望速度v能否到達(dá)；若期望速度速度v不能到達(dá)，需重重新規(guī)劃期望速度軌跡不含勻速段，即勻速段的運(yùn)動(dòng)時(shí)間t2＝0；若期望速度速度v能夠到達(dá)，勻速段的運(yùn)動(dòng)時(shí)間

s14、計(jì)算機(jī)器人運(yùn)動(dòng)軌跡，完成擺線加減速規(guī)劃；

所述空間任意兩點(diǎn)位置過(guò)渡路徑規(guī)劃方法，已知兩過(guò)渡點(diǎn)位置矢量pa、pb，兩過(guò)渡點(diǎn)速度方向的單位矢量na、nb，過(guò)渡段為兩段拋物線，兩過(guò)渡點(diǎn)所在過(guò)渡段的大小分別為λ1、λ2，兩過(guò)渡點(diǎn)的速度大小相等，具體的空間任意兩點(diǎn)位置過(guò)渡路徑規(guī)劃方法包括如下步驟：

s21、假設(shè)過(guò)渡段中間點(diǎn)矢量為pm，等效為三段相鄰直線之間的過(guò)渡，其中，中間段直線的兩個(gè)頂點(diǎn)為pm1＝pa+λ1·na和pm2＝pb-λ2·nb，則中間段直線的長(zhǎng)度為λ12＝λ1′+λ2′＝||pm1pm2||，λ1′和λ2′分別為pm1和pm2到中間點(diǎn)pm的距離；

s22、求解過(guò)渡段中間點(diǎn)速度方向矢量nm＝(pm2-pm1)/λ12；

s23、確定過(guò)渡段兩段拋物線的運(yùn)動(dòng)時(shí)間τ1＝2λ1/v，τ2＝2λ2/v；

s24、由步驟s21至s23求解的參數(shù)確定過(guò)渡段的運(yùn)動(dòng)軌跡；

所述空間任意兩點(diǎn)姿態(tài)過(guò)渡軌跡規(guī)劃方法，已知過(guò)渡段線速度大小為v，兩過(guò)渡位置長(zhǎng)度為l1、l2，兩過(guò)渡點(diǎn)姿態(tài)為ga、gb，兩過(guò)渡點(diǎn)姿態(tài)速度方向的單位矢量na、nb，過(guò)渡段等效為單位球面上的兩段拋物線，具體的空間任意兩點(diǎn)姿態(tài)過(guò)渡軌跡規(guī)劃方法包括如下步驟：

s31、計(jì)算兩過(guò)渡點(diǎn)的姿態(tài)角速度大小ω1、ω2；

s32、確定兩段拋物線的運(yùn)動(dòng)時(shí)間，由空間任意兩點(diǎn)位置過(guò)渡規(guī)劃方法確定，分別為t1和t2，t1＝2l1/v，t2＝2l2/v；

s33、將空間任意兩點(diǎn)姿態(tài)的過(guò)渡等效為單位球面上的三段相鄰直線段之間的過(guò)渡；計(jì)算姿態(tài)中間過(guò)渡段總長(zhǎng)度λ12，λ12表示為單位球面上兩姿態(tài)變換轉(zhuǎn)動(dòng)的最小距離，即四元數(shù)插值時(shí)沿球面旋轉(zhuǎn)的最小路徑，其等效轉(zhuǎn)動(dòng)軸即過(guò)渡段中間點(diǎn)方向矢量nm；

s34、計(jì)算過(guò)渡段中間點(diǎn)的姿態(tài)矢量，gm＝gm1·rot(nm,λ1′)，其中，λ1′為中間過(guò)渡段前半段的長(zhǎng)度，中間點(diǎn)角速度大小為

s35、由步驟s32得到的運(yùn)動(dòng)時(shí)間以及由步驟s34得到的過(guò)渡段長(zhǎng)度和中間點(diǎn)姿態(tài)，確定過(guò)渡段姿態(tài)運(yùn)動(dòng)軌跡。

進(jìn)一步地，所述步驟s11中計(jì)算路徑速度、加速度最大值的公式為：

式中，笛卡爾速度矢量雅可比矩陣關(guān)節(jié)速度向量h(θ)為機(jī)器人hessian矩陣，是一個(gè)6層n×n矩陣，前三層為分別對(duì)x、y、z軸的移動(dòng)，后三層分別為對(duì)x、y、z軸的轉(zhuǎn)動(dòng)，為關(guān)節(jié)加速度，a為笛卡兒加速度。

進(jìn)一步地，所述步驟s14中計(jì)算機(jī)器人運(yùn)動(dòng)軌跡的公式為：

式中，p為在t時(shí)刻的位置，ps為初始位置，vs為初始速度，和為中間變量，其中加速段加速度減速段加速度am為加速度最大值。

進(jìn)一步地，所述步驟s24中過(guò)渡段的運(yùn)動(dòng)軌跡的計(jì)算公式為：

式中，p(t)為t時(shí)刻機(jī)器人在笛卡兒空間中的位置矢量，pa和pm分別為過(guò)渡段起點(diǎn)和過(guò)渡段中間點(diǎn)的位置矢量，v1和vm分別為過(guò)渡段起點(diǎn)和過(guò)渡段中間點(diǎn)的速度矢量，v1＝nav，vm＝nmvm，a1和a2分別為過(guò)渡段起點(diǎn)和過(guò)渡段終點(diǎn)的加速度矢量，a1＝(vm-v1)/τ1，a2＝(v2-vm)/τ2；τ1和τ2分別為過(guò)渡段兩段拋物線的運(yùn)動(dòng)時(shí)間。

進(jìn)一步地，所述步驟s31中計(jì)算兩過(guò)渡點(diǎn)的姿態(tài)角速度大小的公式為：

式中，k為當(dāng)前過(guò)渡點(diǎn)角速度大小ω和線速度大小v的比例，為比例角，由以下公式確定：

式中，n為路徑節(jié)點(diǎn)速度單位矢量，nv和nω分別為線速度和角速度的單位矢量。

進(jìn)一步地，所述步驟s33中計(jì)算姿態(tài)中間過(guò)渡段總長(zhǎng)度的公式為：

λ12＝|gm1gm2|＝slerp(gm1,gm2)

式中，slerp()表示單位四元數(shù)插值，gm1和gm2分別為中間過(guò)渡段兩個(gè)頂點(diǎn)的姿態(tài)矢量，gm1＝ga·rot(na,λ1)，gm2＝gb·rot(nb,-λ2)，rot()表示沿單位球面旋轉(zhuǎn)。

進(jìn)一步地,所述步驟s35中確定過(guò)渡段姿態(tài)運(yùn)動(dòng)軌跡的公式為：

gt＝ga·rot(na,σ1)·rot(nm1,σ2)

式中，gt表示第一段拋物線在t時(shí)刻當(dāng)前點(diǎn)的姿態(tài)，即由初始姿態(tài)沿初始方向軸na轉(zhuǎn)動(dòng)σ1，然后再繞末方向軸nm1轉(zhuǎn)動(dòng)σ2，轉(zhuǎn)動(dòng)量的計(jì)算公式如下：

式中，t為當(dāng)前時(shí)間，t∈[0,τ]，τ為本段拋物線總運(yùn)動(dòng)時(shí)間，λ1和λ1′分別為首過(guò)渡段和中間過(guò)渡段前半段的長(zhǎng)度。

采用上述技術(shù)方案后，本發(fā)明至少具有如下有益效果：

1、本發(fā)明的擺線加減速規(guī)劃方法，使用簡(jiǎn)化的正弦曲線或余弦曲線加減速模型，其完整的加減速過(guò)程分為加加速、減加速、勻速、加減速、減減速五個(gè)階段，相對(duì)s曲線加減速，該方法具有加加速度連續(xù)、計(jì)算簡(jiǎn)單等優(yōu)點(diǎn)；

2、本發(fā)明的空間任意兩點(diǎn)位置過(guò)渡路徑規(guī)劃方法，適用于空間任意直線、圓弧之間的過(guò)渡路徑規(guī)劃，由過(guò)渡等級(jí)控制空間路徑過(guò)渡區(qū)域的大小，空間任意兩點(diǎn)間的過(guò)渡路徑段具有連續(xù)拋物線特征，路經(jīng)段之間的位置速度、加速度、加加速度保證連續(xù)；

3、本發(fā)明的空間任意兩點(diǎn)姿態(tài)過(guò)渡路徑規(guī)劃方法，使用姿態(tài)采用四元數(shù)表示，兩點(diǎn)之間的姿態(tài)運(yùn)動(dòng)表示為沿單位球表面最短距離的旋轉(zhuǎn)；姿態(tài)規(guī)劃以該旋轉(zhuǎn)角度為路徑的線性規(guī)劃，與位置規(guī)劃保證同步；任意路徑段之間的姿態(tài)過(guò)渡保證姿態(tài)角速度、角加速度、角加加速度的連續(xù)；

4、本發(fā)明適用于多種構(gòu)型機(jī)器人，解決了軌跡停頓、不流暢、偏離規(guī)定路徑等問(wèn)題，保證機(jī)器人位置、姿態(tài)規(guī)劃的連續(xù)性，特別是姿態(tài)過(guò)渡點(diǎn)角速度無(wú)需為零，即姿態(tài)運(yùn)動(dòng)不停止，進(jìn)一步提升了機(jī)器人運(yùn)動(dòng)效率及平穩(wěn)性。

附圖說(shuō)明

圖1為本發(fā)明一種機(jī)器人笛卡爾空間平滑軌跡的規(guī)劃方法的實(shí)施流程圖；

圖2(a)是本發(fā)明一種機(jī)器人笛卡兒空間平滑軌跡的規(guī)劃方法的過(guò)渡路徑示例圖；

圖2(b)是本發(fā)明一種機(jī)器人笛卡兒空間平滑軌跡的規(guī)劃方法的過(guò)渡路徑示例圖；

圖3是本發(fā)明一種機(jī)器人笛卡兒空間平滑軌跡的規(guī)劃方法的路徑過(guò)渡情況判斷流程圖；

圖4是本發(fā)明一種機(jī)器人笛卡兒空間平滑軌跡的規(guī)劃方法的擺線加減速曲線圖；

圖5是本發(fā)明一種機(jī)器人笛卡兒空間平滑軌跡的規(guī)劃方法的任意兩點(diǎn)之間的位置過(guò)渡示意圖；

圖6是本發(fā)明一種機(jī)器人笛卡兒空間平滑軌跡的規(guī)劃方法的任意兩點(diǎn)之間的姿態(tài)平滑過(guò)渡示意圖。

具體實(shí)施方式

需要說(shuō)明的是，在不沖突的情況下，本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互結(jié)合，下面結(jié)合附圖和具體實(shí)施例對(duì)本申請(qǐng)作進(jìn)一步詳細(xì)說(shuō)明。

如圖1所示，本發(fā)明提供一種機(jī)器人笛卡爾空間平滑軌跡的規(guī)劃方法，其主要實(shí)施內(nèi)容包括擺線加減速規(guī)劃、空間任意兩點(diǎn)位置過(guò)渡軌跡規(guī)劃以及空間任意兩點(diǎn)姿態(tài)過(guò)渡軌跡規(guī)劃，其中

利用所述的擺線加減速規(guī)劃方法，對(duì)笛卡兒空間直線、圓弧等單段路徑進(jìn)行劃，主要步驟為：

s11、計(jì)算該路徑速度、加速度的最大值；

s12、判斷期望末速度能否到達(dá)，若不能到達(dá)，則重新規(guī)劃末速度；

s13、若期望末速度能夠到達(dá)，進(jìn)而判斷期望速度能否到達(dá)，若期望速度速度v不能到達(dá)，需重新規(guī)劃；

s14、計(jì)算機(jī)器人運(yùn)動(dòng)軌跡，完成擺線加減速規(guī)劃。

利用所述空間任意兩點(diǎn)位置過(guò)渡軌跡規(guī)劃方法，對(duì)相鄰兩段空間路徑進(jìn)行位置過(guò)渡規(guī)劃，主要步驟為：

s21、將過(guò)渡段等效為三段相鄰直線之間的過(guò)渡，計(jì)算中間段直線的兩個(gè)頂點(diǎn)位置，以及中間段直線的長(zhǎng)度；

s22、求解過(guò)渡段中間點(diǎn)速度和方向矢量；

s23、確定過(guò)渡段兩段拋物線的運(yùn)動(dòng)時(shí)間；

s24、確定過(guò)渡段的運(yùn)動(dòng)軌跡。

利用所述的空間任意兩點(diǎn)姿態(tài)過(guò)渡軌跡規(guī)劃方法，對(duì)相鄰兩段空間路徑進(jìn)行姿態(tài)過(guò)渡規(guī)劃，主要步驟為：

s31、計(jì)算兩過(guò)渡點(diǎn)的姿態(tài)角速度大??；

s32、確定兩段拋物線的運(yùn)動(dòng)時(shí)間；

s33、將空間任意兩點(diǎn)姿態(tài)的過(guò)渡等效為單位球面上的三段相鄰直線段之間的過(guò)渡，計(jì)算姿態(tài)中間過(guò)渡段總長(zhǎng)度；

s34、計(jì)算過(guò)渡段中間點(diǎn)的姿態(tài)矢量；

s35、確定過(guò)渡段姿態(tài)運(yùn)動(dòng)軌跡。

實(shí)施例

本發(fā)明的實(shí)施例具體流程：判斷過(guò)渡區(qū)域——非過(guò)渡段規(guī)劃——過(guò)渡段位置規(guī)劃——過(guò)渡段姿態(tài)規(guī)劃。

如圖2(a)、圖2(b)所示，本發(fā)明所涉及的路徑過(guò)渡區(qū)域大小由過(guò)渡等級(jí)設(shè)定，當(dāng)機(jī)器人運(yùn)動(dòng)到過(guò)渡區(qū)域時(shí)，將飛越當(dāng)前路徑點(diǎn)朝下一個(gè)路徑點(diǎn)運(yùn)動(dòng)，如圖中的p1點(diǎn)，若下一個(gè)路徑點(diǎn)仍在過(guò)渡區(qū)域內(nèi)，則直接越過(guò)下一個(gè)路徑點(diǎn)，如圖2(a)中的p2點(diǎn)。

如圖3所示為路經(jīng)過(guò)渡情況判斷的具體流程，首先根據(jù)過(guò)渡等級(jí)確定各程序段的過(guò)渡距離，直線距離是其長(zhǎng)度，圓弧距離則由圓弧的弧度確定。過(guò)渡情況判斷結(jié)果分為完全過(guò)渡(如圖2(a)中的路徑段p0p1)、部分過(guò)渡(如圖2(b)中的路徑段p1p2)和全部過(guò)渡三種情況(如圖2(a)中的路徑段p1p2)。由此可將路徑分為非過(guò)渡段和過(guò)渡段兩種情況。

對(duì)于笛卡兒空間非過(guò)渡段，采用擺線加減速規(guī)劃方法對(duì)直線、圓弧等路徑進(jìn)行單段軌跡規(guī)劃，將單段路徑分解成加速段、勻速段和減速段，并根據(jù)如圖4所示的擺線加減速的加加速度、加速度、速度和位置曲線，對(duì)其軌跡進(jìn)行求解。

本發(fā)明提供的空間任意兩點(diǎn)位置過(guò)渡軌跡規(guī)劃方法，可對(duì)空間任意兩個(gè)具有不同速度方向的位置矢量進(jìn)行平滑過(guò)渡規(guī)劃，如圖5所示，已知兩過(guò)渡點(diǎn)位置矢量以及速度方向的單位矢量，將過(guò)渡段等效為兩段拋物線，由已知的兩過(guò)渡點(diǎn)所在過(guò)渡段的大小和速度大小，首先可計(jì)算中間段直線的兩個(gè)頂點(diǎn)pm1和pm2，然后求解過(guò)渡段中間點(diǎn)速度vm，再計(jì)算兩段拋物線的運(yùn)動(dòng)時(shí)間，最終確定過(guò)渡段的運(yùn)動(dòng)軌跡。

姿態(tài)軌跡為單位球面上的兩段拋物線。對(duì)于過(guò)渡段的首末節(jié)點(diǎn)，其速度方向已知，位置和姿態(tài)速度大小的比率已知。由于姿態(tài)保持與位置同步，若位置軌跡已定，則姿態(tài)過(guò)渡段的兩段拋物線的運(yùn)動(dòng)時(shí)間可事先確定，然后可確定整個(gè)姿態(tài)過(guò)渡段的軌跡。如圖6所示，已知兩過(guò)渡點(diǎn)姿態(tài)ga、gb，以及兩過(guò)渡點(diǎn)姿態(tài)速度方向的單位矢量na、nb，同理，將過(guò)渡段等效為單位球面上的兩段拋物線，兩過(guò)渡點(diǎn)的姿態(tài)角速度和兩段拋物線的運(yùn)動(dòng)時(shí)間可事先求得。利用本發(fā)明提供的空間任意兩點(diǎn)姿態(tài)過(guò)渡規(guī)劃方法，首先計(jì)算中間段直線的兩個(gè)頂點(diǎn)gm1和gm2，然后計(jì)算過(guò)渡段中間點(diǎn)矢量gm，最終確定過(guò)渡段姿態(tài)的運(yùn)動(dòng)軌跡。

本發(fā)明所提供的機(jī)器人笛卡爾空間平滑軌跡規(guī)劃方法，適用于笛卡爾空間任意兩點(diǎn)之間的平滑過(guò)渡，同時(shí)保證位置速度、加速度以及姿態(tài)角速度、角加速度等連續(xù)平滑過(guò)渡，可進(jìn)一步提升機(jī)器人運(yùn)動(dòng)的效率和平穩(wěn)性。

盡管已經(jīng)示出和描述了本發(fā)明的實(shí)施例，對(duì)于本領(lǐng)域的普通技術(shù)人員而言，可以理解的是，在不脫離本發(fā)明的原理和精神的情況下可以對(duì)這些實(shí)施例進(jìn)行多種等效的變化、修改、替換和變型，本發(fā)明的范圍由所附權(quán)利要求及其等同范圍限定。