一種基于速度最優(yōu)控制的空間軌跡平滑方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，特別涉及一種基于速度最優(yōu)控制的空間軌跡平 滑方法。
【背景技術(shù)】
[0002] 空間軌跡平滑是空間運(yùn)動(dòng)學(xué)的一個(gè)分支?？臻g軌跡可由一系列簡(jiǎn)單軌跡拼接而 成。簡(jiǎn)單軌跡分為直線和圓弧兩種，其起點(diǎn)和終點(diǎn)稱(chēng)為節(jié)點(diǎn)。需進(jìn)行操作任務(wù)的節(jié)點(diǎn)稱(chēng)為停 止點(diǎn)，無(wú)操作任務(wù)的節(jié)點(diǎn)稱(chēng)為過(guò)渡點(diǎn)。在運(yùn)動(dòng)過(guò)程中，由速度為零至達(dá)到制定速度、由制定 速度減速至零，都是十分耗費(fèi)時(shí)間的。為了提高整體運(yùn)動(dòng)速度，提升工作效率，就要想方設(shè) 法免去不必要的加減速過(guò)程，盡可能保持高速運(yùn)行。過(guò)渡點(diǎn)的存在往往是為了繞開(kāi)障礙物。 沿軌跡行進(jìn)過(guò)程中，無(wú)需精確抵達(dá)過(guò)渡點(diǎn)，因此提出轉(zhuǎn)彎區(qū)平滑的概念，即在過(guò)渡點(diǎn)所處的 軌跡相接處，采取平滑曲線過(guò)渡，保持較高速度通過(guò)新的平滑軌跡，從而提升轉(zhuǎn)彎速度。
[0003] 現(xiàn)有軌跡平滑方案主要有以下兩類(lèi)。
[0004] (1)基于速度規(guī)劃曲線，按照等時(shí)的原則（即按照原速度規(guī)劃，轉(zhuǎn)彎起點(diǎn)到轉(zhuǎn)彎中 間點(diǎn)的時(shí)間與中間點(diǎn)到終點(diǎn)的時(shí)間相等）進(jìn)行軌跡的平滑規(guī)劃。由速度規(guī)劃曲線計(jì)算轉(zhuǎn)彎 路徑上各點(diǎn)速度，進(jìn)行位移的空間向量疊加。
[0005] (2)基于速度規(guī)劃曲線，按照等距原則（即按照原速度規(guī)劃，轉(zhuǎn)彎起點(diǎn)到轉(zhuǎn)彎中間 點(diǎn)的距離與中間點(diǎn)到終點(diǎn)的距離相等）進(jìn)行軌跡的平滑規(guī)劃。由速度規(guī)劃曲線計(jì)算轉(zhuǎn)彎路 徑上各點(diǎn)速度，進(jìn)行位移的空間向量疊加。
[0006] 現(xiàn)有的上述兩種方案的缺陷在于：沒(méi)有顯式的軌跡表達(dá)式，或軌跡表達(dá)式過(guò)于復(fù) 雜，不是簡(jiǎn)單曲線，難以完成對(duì)機(jī)械臂軌跡規(guī)劃的精確控制，也不利于計(jì)算障礙規(guī)避空間。 同時(shí)，按照上述方案執(zhí)行，得到的在歐幾里得空間中的軌跡并不是簡(jiǎn)單平滑曲線，其軌跡不 直觀，不利于操作者的對(duì)運(yùn)行結(jié)果的直接觀察與評(píng)價(jià)。

【發(fā)明內(nèi)容】

[0007] 本發(fā)明的目的旨在至少解決所述技術(shù)缺陷之一。
[0008] 為此，本發(fā)明的目的在于提出一種基于速度最優(yōu)控制的空間軌跡平滑方法，可以 保證速度方向的連續(xù)性，可方便定位機(jī)械臂的末端執(zhí)行器在任意時(shí)刻的空間位置，便于控 制。
[0009] 為了實(shí)現(xiàn)上述目的，本發(fā)明的實(shí)施例提供一種基于速度最優(yōu)控制的空間軌跡平滑 方法，包括如下步驟：
[0010] 步驟S1，獲取機(jī)器人機(jī)械臂運(yùn)行的空間軌跡的起點(diǎn)Ps、中點(diǎn)Pm、終點(diǎn)Pe、預(yù)設(shè)轉(zhuǎn)彎 區(qū)半徑r和預(yù)設(shè)參數(shù)flag_dist，其中，所述起點(diǎn)Ps和中點(diǎn)Pm之間構(gòu)成第一簡(jiǎn)單軌跡，所述 中點(diǎn)Pm和終點(diǎn)Pe之間構(gòu)成第二簡(jiǎn)單軌跡；
[0011] 步驟S2,將所述預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r與所述第一簡(jiǎn)單軌跡和所述第二簡(jiǎn)單軌跡的長(zhǎng) 度進(jìn)行比較，如果所述預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r沒(méi)有超過(guò)所述第一簡(jiǎn)單軌跡的長(zhǎng)度的一半，且沒(méi) 有超過(guò)所述的第二簡(jiǎn)單軌跡的長(zhǎng)度的一半，則執(zhí)行步驟S3 ;
[0012] 步驟S3,選取所述機(jī)器人的轉(zhuǎn)彎方式為等長(zhǎng)度轉(zhuǎn)彎或等時(shí)間轉(zhuǎn)彎，計(jì)算得到第一 和第二簡(jiǎn)單軌跡相交形成的過(guò)渡點(diǎn)M的坐標(biāo)、所述第一和第二簡(jiǎn)單軌跡分別與預(yù)設(shè)圓形相 交的S和E點(diǎn)的坐標(biāo)；
[0013] 步驟S4,根據(jù)計(jì)算得到S、M和E點(diǎn)的坐標(biāo)建立局部坐標(biāo)系，計(jì)算所述空間軌跡在 所述局部坐標(biāo)系下的轉(zhuǎn)彎區(qū)的軌跡方程，其中，所述軌跡方程為平滑的轉(zhuǎn)彎區(qū)的二次曲線。
[0014] 進(jìn)一步，在所述步驟S2中，采用下式判斷所述預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r是否超過(guò)所述第 一簡(jiǎn)單軌跡的長(zhǎng)度的一半，
[0015]
[0016] 其中，ξξ是由起點(diǎn)Ps指向中點(diǎn)Pm的向量，:ξξ是由中點(diǎn)Pm指向終點(diǎn)Pe的向 量。
[0017] 進(jìn)一步，在所述步驟S3中，當(dāng)選取的轉(zhuǎn)彎方式為等長(zhǎng)度轉(zhuǎn)彎時(shí)，計(jì)算S、M和E點(diǎn)坐 標(biāo)包括如下步驟：
[0018] 根據(jù)等長(zhǎng)度轉(zhuǎn)彎需要滿足IPI #瞬1|，采用向量疊加方法，由中點(diǎn)1的坐標(biāo)分別疊 加兩個(gè)方向上長(zhǎng)度為r的向量，即可得轉(zhuǎn)彎區(qū)的起點(diǎn)坐標(biāo)S、終點(diǎn)坐標(biāo)E，其中，中 點(diǎn)M即為點(diǎn)Pm，計(jì)算出點(diǎn)S和E點(diǎn)坐標(biāo)如下：
[0019]
[0020]
[0021] 進(jìn)一步，在所述步驟S3中，當(dāng)選取的轉(zhuǎn)彎方式為等時(shí)間轉(zhuǎn)彎時(shí)，包括如下步驟：
[0022] 根據(jù)等時(shí)間轉(zhuǎn)彎需要滿足tl = t2,其中，tl為SM段轉(zhuǎn)彎時(shí)間，t2為ME段轉(zhuǎn)彎時(shí) 間，
[0023] t = mimUp t2)，
[0024] 代入速度規(guī)劃，計(jì)算得到S、E點(diǎn)距M點(diǎn)距離dists，distE，
[0025]
[0026]
[0027] 進(jìn)一步，在所述步驟S4中，所述計(jì)算所述空間軌跡在所述局部坐標(biāo)系下的轉(zhuǎn)彎區(qū) 的軌跡方程，包括如下步驟：
[0028] 步驟S41，計(jì)算坐標(biāo)變換參數(shù)Θ和A，包括如下步驟：
[0031] 其中，Θ為坐標(biāo)軸進(jìn)行旋轉(zhuǎn)變換的角度，A為旋轉(zhuǎn)矩陣，
[0032] 步驟S42,導(dǎo)出向量坐標(biāo)和,包括如下步驟：
[0033]
[0034]
[0035] 其中，艤為靈在所述局部坐標(biāo)系下的表示，為議在所述局部坐標(biāo)系下的表 示，
[0036] 步驟43,計(jì)算所述空間軌跡在所述局部坐標(biāo)系下的轉(zhuǎn)彎區(qū)的軌跡方程y為：
[0037] y = ax2,
[0038] 其中，
a為所述軌跡方程對(duì)應(yīng)的二次曲線的參數(shù)，
為, 0 = S-As，0為所述局部坐標(biāo)系的坐標(biāo)原來(lái)在全局坐標(biāo)系下的表示，為坐標(biāo)變換過(guò) 程中的平移部分，s(xs，ys)，m(xm，ym)，eUy yj分別為轉(zhuǎn)彎區(qū)的起點(diǎn)、中間點(diǎn)、終點(diǎn)坐標(biāo)。
[0039] 根據(jù)本發(fā)明實(shí)施例的基于速度最優(yōu)控制的空間軌跡平滑方法，可以將機(jī)器人機(jī)械 臂運(yùn)行空間軌跡的二次曲線經(jīng)坐標(biāo)變換生成平滑轉(zhuǎn)彎區(qū)表達(dá)式，從而保證速度方向的連續(xù) 性，通過(guò)平滑過(guò)渡兩直線軌跡，結(jié)合S型速度規(guī)劃，得到加速度、速度、位移平滑的轉(zhuǎn)彎方 案，軌跡簡(jiǎn)單直觀，可方便定位機(jī)械臂的末端執(zhí)行器在任意時(shí)刻的空間位置，便于控制。同 時(shí)由于軌跡為簡(jiǎn)單的二次曲線，因此可簡(jiǎn)便計(jì)算出障礙規(guī)避空間。此外，本發(fā)明可以適應(yīng)等 距與等時(shí)兩種轉(zhuǎn)彎需求，用戶(hù)可根據(jù)實(shí)際情況，方便地比較與選取更優(yōu)的方案。
[0040] 本發(fā)明附加的方面和優(yōu)點(diǎn)將在下面的描述中部分給出，部分將從下面的描述中變 得明顯，或通過(guò)本發(fā)明的實(shí)踐了解到。
【附圖說(shuō)明】
[0041] 本發(fā)明的上述和/或附加的方面和優(yōu)點(diǎn)從結(jié)合下面附圖對(duì)實(shí)施例的描述中將變 得明顯和容易理解，其中：
[0042] 圖1為根據(jù)本發(fā)明一個(gè)實(shí)施例的基于速度最優(yōu)控制的空間軌跡平滑方法的流程 圖；
[0043] 圖2為根據(jù)本發(fā)明實(shí)施例的第一和第二簡(jiǎn)單軌跡的示意圖；
[0044] 圖3根據(jù)本發(fā)明另一個(gè)實(shí)施例的基于速度最優(yōu)控制的空間軌跡平滑方法的流程 圖。
【具體實(shí)施方式】
[0045] 下面詳細(xì)描述本發(fā)明的實(shí)施例，實(shí)施例的示例在附圖中示出，其中自始至終相同 或類(lèi)似的標(biāo)號(hào)表示相同或類(lèi)似的元件或具有相同或類(lèi)似功能的元件。下面通過(guò)參考附圖描 述的實(shí)施例是示例性的，旨在用于解釋本發(fā)明，而不能理解為對(duì)本發(fā)明的限制。
[0046] 本發(fā)明提一種基于速度最優(yōu)控制的空間軌跡平滑方法，利用二次曲線的平滑特 性，根據(jù)兩直線路徑起點(diǎn)、終點(diǎn)及所夾轉(zhuǎn)彎區(qū)的轉(zhuǎn)彎半徑，生成二次曲線，平滑連接兩直線 路徑，將二次曲線經(jīng)坐標(biāo)變換生成平滑轉(zhuǎn)彎區(qū)表達(dá)式，從而保證速度方向的連續(xù)性。
[0047] 如圖1所示，本發(fā)明實(shí)施例的基于速度最優(yōu)控制的空間軌跡平滑方法，包括如下 步驟：
[0048] 步驟S1，獲取機(jī)器人機(jī)械臂運(yùn)行的空間軌跡的起點(diǎn)Ps、中點(diǎn)Pm、終點(diǎn)Pe、預(yù)設(shè)轉(zhuǎn)彎 區(qū)半徑r和預(yù)設(shè)參數(shù)flag_dist。其中，起點(diǎn)Ps和中點(diǎn)Pm之間構(gòu)成第一簡(jiǎn)單軌跡，中點(diǎn)Pm 和終點(diǎn)Pe之間構(gòu)成第二簡(jiǎn)單軌跡。預(yù)設(shè)參數(shù)flag_dist為采取等距轉(zhuǎn)彎或等時(shí)轉(zhuǎn)彎的參 數(shù)，其值為1時(shí)為等距轉(zhuǎn)彎，否則為等時(shí)轉(zhuǎn)彎。
[0049] 圖2為根據(jù)本發(fā)明實(shí)施例的第一和第二簡(jiǎn)單軌跡的示意圖。
[0050] 首先對(duì)轉(zhuǎn)彎區(qū)的定義進(jìn)行說(shuō)明：在兩個(gè)簡(jiǎn)單軌跡相交形成的過(guò)渡點(diǎn)M處，以過(guò)渡 點(diǎn)為原點(diǎn)，以某一特定值R為半徑做圓，相交兩簡(jiǎn)單軌跡于S，E兩點(diǎn)，由線段SM，ME，圓弧ES 圍成的區(qū)域即為轉(zhuǎn)彎區(qū)。參考圖2,第一簡(jiǎn)單軌跡的起點(diǎn)為Ps，終點(diǎn)為過(guò)渡點(diǎn)M;第二簡(jiǎn)單 軌跡的起點(diǎn)為M，終點(diǎn)為Pe。第一和第二簡(jiǎn)單軌跡均為直線軌跡。
[0051] 在開(kāi)始規(guī)劃轉(zhuǎn)彎區(qū)軌跡之前，需由兩相交簡(jiǎn)單軌跡確定當(dāng)前路徑所在的平面，并 建立二維平面與三維空間的坐標(biāo)轉(zhuǎn)換方程，然后確定各關(guān)鍵點(diǎn)在二維平面的坐標(biāo)。后續(xù)步 驟均在該平面上進(jìn)行。
[0052] 步驟S2,將預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r與第一簡(jiǎn)單軌跡和第二簡(jiǎn)單軌跡的長(zhǎng)度進(jìn)行比較， 如果預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r沒(méi)有超過(guò)第一簡(jiǎn)單軌跡的長(zhǎng)度的一半，且沒(méi)有超過(guò)的第二簡(jiǎn)單軌跡 的長(zhǎng)度的一半，則執(zhí)行步驟S3。
[0053] 在本步驟中，采用下式判斷預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r是否超過(guò)第一簡(jiǎn)單軌跡的長(zhǎng)度的一 半，
[0054]
[0055] 其中，1?是由起點(diǎn)Ps指向中點(diǎn)Pm的向量，即原軌跡中的第一簡(jiǎn)單軌跡，1?是 由中點(diǎn)Pm指向終點(diǎn)Pe的向量，即原軌跡中的第二簡(jiǎn)單軌跡。
[0056] 步驟S3,選取機(jī)器人的轉(zhuǎn)彎方式為等長(zhǎng)度轉(zhuǎn)彎或等時(shí)間轉(zhuǎn)彎，計(jì)算得到第一和第 二簡(jiǎn)單軌跡相交形成的過(guò)渡點(diǎn)M的坐標(biāo)、第一和第二簡(jiǎn)單軌跡分別與預(yù)設(shè)圓形相交的S和 E點(diǎn)的坐標(biāo)。
[0057] 具體地，根據(jù)起點(diǎn)Ps、中點(diǎn)Pm、終點(diǎn)Pe、預(yù)設(shè)轉(zhuǎn)彎區(qū)半徑r進(jìn)行等距規(guī)劃或等時(shí)規(guī) 劃。其中，等距規(guī)劃可以由轉(zhuǎn)彎半徑r直接求得。對(duì)于等時(shí)規(guī)劃，由S型速度規(guī)劃得到轉(zhuǎn)彎 起點(diǎn)到過(guò)渡點(diǎn)的時(shí)間，以此計(jì)算轉(zhuǎn)彎終點(diǎn)位置，由此得到轉(zhuǎn)彎區(qū)起點(diǎn)終點(diǎn)坐標(biāo)。即，本發(fā)明 可以基于S型速度規(guī)劃且同時(shí)支持等時(shí)與等距兩種規(guī)劃的轉(zhuǎn)彎區(qū)算法。
[0058] 下面分別對(duì)等距規(guī)劃和等時(shí)規(guī)劃進(jìn)行說(shuō)明。