本發(fā)明涉及自動化控制領(lǐng)域，特別涉及一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法與裝置。

背景技術(shù)：

在自動化控制領(lǐng)域中，機(jī)器人在執(zhí)行任務(wù)時，往往是多段多種路徑的組合，如果在段與段的相接處不做處理，則需要將機(jī)器人的速度降為零，如果不將機(jī)器人的速度降為零，則機(jī)器人會因?yàn)樗俣扰c加速度的不連續(xù)，產(chǎn)生劇烈震蕩，且極有可能破壞機(jī)器人，但是，若將機(jī)器人的速度降為零，又會影響機(jī)器人的節(jié)拍，尤其是在路徑段數(shù)比較多的情況下。為了使機(jī)器人能夠在不同路徑上連續(xù)運(yùn)動，需要在不同路徑間用轉(zhuǎn)接曲線進(jìn)行連接，轉(zhuǎn)接曲線有拋物線、圓弧、樣條曲線等，雖然拋物線、圓弧比較簡單，但是只能保證速度連續(xù)，卻不能保證加速度連續(xù)，并且只能用于共面曲線的過渡轉(zhuǎn)接，其它滿足加速度連續(xù)的更高階的多項(xiàng)式函數(shù)，也只能用于共面曲線的過渡轉(zhuǎn)接，無法應(yīng)用于三維空間內(nèi)的運(yùn)動路徑的過渡轉(zhuǎn)接。目前，通常是將根據(jù)轉(zhuǎn)接始末點(diǎn)位置、一二階導(dǎo)數(shù)求得的樣條曲線，作為空間路徑的過渡轉(zhuǎn)接曲線，并且三次樣條曲線即可滿足過渡轉(zhuǎn)接前后加速度的連續(xù)性。

然而，在實(shí)現(xiàn)發(fā)明的過程中，本申請的發(fā)明人發(fā)現(xiàn)，直接根據(jù)轉(zhuǎn)接始末點(diǎn)位置、一二階導(dǎo)數(shù)求得的樣條曲線，往往存在尖點(diǎn)、拐點(diǎn)、二重點(diǎn)等嚴(yán)重影響路徑光順性的問題。

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

本發(fā)明實(shí)施方式的目的在于提供一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法與裝置，消除了影響連續(xù)軌跡規(guī)劃過渡路徑光順性的尖點(diǎn)、拐點(diǎn)、二重點(diǎn)等，并使連續(xù)軌跡規(guī)劃過渡路徑的曲率更加平滑，從而實(shí)現(xiàn)了不同路徑之間的平滑轉(zhuǎn)接。

為解決上述技術(shù)問題，本發(fā)明的實(shí)施方式提供了一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法，包括：

根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的所述連續(xù)軌跡規(guī)劃過渡路徑的邊界值；

根據(jù)所述邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成所述連續(xù)軌跡規(guī)劃過渡路徑；

根據(jù)生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化所述邊界值；

根據(jù)優(yōu)化后的邊界值及所述函數(shù)表達(dá)式，平滑所述生成的所述連續(xù)軌跡規(guī)劃過渡路徑。

本發(fā)明的實(shí)施方式還提供了一種連續(xù)軌跡規(guī)劃過渡路徑的平滑裝置，包括：

邊界值計算模塊，用于根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的所述連續(xù)軌跡規(guī)劃過渡路徑的邊界值；

路徑生成模塊，用于根據(jù)所述邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成所述連續(xù)軌跡規(guī)劃過渡路徑；

優(yōu)化模塊，用于根據(jù)生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化所述邊界值；

平滑模塊，用于根據(jù)優(yōu)化后的邊界值及所述函數(shù)表達(dá)式，平滑所述生成的所述連續(xù)軌跡規(guī)劃過渡路徑。

本發(fā)明實(shí)施方式相對于現(xiàn)有技術(shù)而言，根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的連續(xù)軌跡規(guī)劃過渡路徑的邊界值，得到生成過渡路徑所需要的初始條件，再根據(jù)該邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成連續(xù)軌跡規(guī)劃過渡路徑，得到第一路徑與第二路徑之間的初始過渡路徑，根據(jù)生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化邊界值，得到平滑初始過渡路徑時的依據(jù)，根據(jù)優(yōu)化后的邊界值及上述函數(shù)表達(dá)式，平滑生成的連續(xù)軌跡規(guī)劃過渡路徑，消除影響連續(xù)軌跡規(guī)劃過渡路徑光順性的尖點(diǎn)、拐點(diǎn)、二重點(diǎn)等，實(shí)現(xiàn)不同路徑之間的平滑轉(zhuǎn)接。

另外，所述根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的所述連續(xù)軌跡規(guī)劃過渡路徑的邊界值，具體包括：確定在所述第一路徑上的第一過渡點(diǎn)P_s，并計算所述P_s的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)其中，所述P_s與所述相接點(diǎn)的距離為第一預(yù)設(shè)距離；確定在所述第二路徑上的第二過渡點(diǎn)P_e，并計算所述P_e的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)其中，所述P_e與所述相接點(diǎn)的距離為所述第一預(yù)設(shè)距離。根據(jù)第一預(yù)設(shè)距離，生成合理的邊界值P_s、P_e、便于進(jìn)一步根據(jù)該邊界值生成恰當(dāng)?shù)倪^渡路徑。

另外，所述根據(jù)生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化所述邊界值，具體包括：根據(jù)生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率，計算所述的第一優(yōu)化權(quán)重λ與所述的第二優(yōu)化權(quán)重μ；根據(jù)所述λ與所述μ，計算優(yōu)化后的所述與所述其中，表示優(yōu)化后的所述表示優(yōu)化后的所述準(zhǔn)確計算出了平滑連續(xù)軌跡規(guī)劃過渡路徑時的依據(jù)與

另外，所述根據(jù)生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率，計算所述的第一優(yōu)化權(quán)重λ與所述的第二優(yōu)化權(quán)重μ，具體包括：根據(jù)所述函數(shù)表達(dá)式，計算所述生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率k，其中，與分別表示所述生成的所述連續(xù)軌跡規(guī)劃過渡路徑的節(jié)點(diǎn)u處的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)，u∈[0,1]；根據(jù)計算所述的優(yōu)化權(quán)重λ與所述的優(yōu)化權(quán)重μ。通過得到了最優(yōu)的λ與μ，從而進(jìn)一步根據(jù)該λ與μ，有效消除影響連續(xù)軌跡規(guī)劃過渡路徑光順性的尖點(diǎn)、拐點(diǎn)、二重點(diǎn)等。

附圖說明

圖1是根據(jù)本發(fā)明第一實(shí)施方式的一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法流程圖；

圖2是根據(jù)本發(fā)明第二實(shí)施方式的一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法流程圖；

圖3是根據(jù)本發(fā)明第二實(shí)施方式的一種連續(xù)軌跡規(guī)劃過渡路徑的生成示意圖；

圖4是根據(jù)本發(fā)明第三實(shí)施方式的一種連續(xù)軌跡規(guī)劃過渡路徑的平滑裝置的結(jié)構(gòu)示意圖；

圖5是根據(jù)本發(fā)明第四實(shí)施方式的一種連續(xù)軌跡規(guī)劃過渡路徑的平滑裝置的結(jié)構(gòu)示意圖。

具體實(shí)施方式

為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對本發(fā)明的各實(shí)施方式進(jìn)行詳細(xì)的闡述。然而，本領(lǐng)域的普通技術(shù)人員可以理解，在本發(fā)明各實(shí)施方式中，為了使讀者更好地理解本申請而提出了許多技術(shù)細(xì)節(jié)。但是，即使沒有這些技術(shù)細(xì)節(jié)和基于以下各實(shí)施方式的種種變化和修改，也可以實(shí)現(xiàn)本申請所要求保護(hù)的技術(shù)方案。

本發(fā)明的第一實(shí)施方式涉及一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法。具體流程如圖1所示。

在步驟101中，計算邊界值。

具體地說，根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的連續(xù)軌跡規(guī)劃過渡路徑的邊界值。

在步驟102中，生成連續(xù)軌跡規(guī)劃過渡路徑。

具體地說，根據(jù)邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成連續(xù)軌跡規(guī)劃過渡路徑。

在步驟103中，優(yōu)化邊界值。

具體地說，根據(jù)生成的所述連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化邊界值。

在步驟104中，平滑連線軌跡規(guī)劃過渡路徑。

具體地說，根據(jù)優(yōu)化后的邊界值及函數(shù)表達(dá)式，平滑生成的連續(xù)軌跡規(guī)劃過渡路徑。

與現(xiàn)有技術(shù)相比，在本實(shí)施方式中，根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的連續(xù)軌跡規(guī)劃過渡路徑的邊界值，得到生成過渡路徑所需要的初始條件，再根據(jù)該邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成連續(xù)軌跡規(guī)劃過渡路徑，得到第一路徑與第二路徑之間的初始過渡路徑，根據(jù)生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化邊界值，得到平滑初始過渡路徑時的依據(jù)，根據(jù)優(yōu)化后的邊界值及上述函數(shù)表達(dá)式，平滑生成的連續(xù)軌跡規(guī)劃過渡路徑，消除影響連續(xù)軌跡規(guī)劃過渡路徑光順性的尖點(diǎn)、拐點(diǎn)、二重點(diǎn)等，實(shí)現(xiàn)不同路徑之間的平滑轉(zhuǎn)接。

本發(fā)明的第二實(shí)施方式涉及一種連續(xù)軌跡規(guī)劃過渡路徑的平滑方法。第二實(shí)施方式在第一實(shí)施方式的基礎(chǔ)上進(jìn)行了進(jìn)一步改進(jìn)，主要改進(jìn)之處在于：在本發(fā)明第二實(shí)施方式中，具體給出了，計算的邊界值及計算過程，并給出了優(yōu)化邊界值的具體過程，具體流程如圖2所示。

在步驟201中，計算邊界值。

具體地說，根據(jù)相接點(diǎn)與第一過渡點(diǎn)P_s的第一預(yù)設(shè)距離，確定在第一路徑上的第一過渡點(diǎn)P_s，并計算P_s的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)再根據(jù)該相接點(diǎn)與第二過渡點(diǎn)P_e的第一預(yù)設(shè)距離，確定在第二路徑上的第二過渡點(diǎn)P_e，并計算P_e的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)

進(jìn)一步地說，P_s與P_e的確定，通常是以相接點(diǎn)為中心，以用戶給定的第一預(yù)設(shè)距離為過渡圓滑半徑，將該過渡圓滑半徑與第一路徑的交點(diǎn)記作第一過渡點(diǎn)P_s，將該過渡圓滑半徑與第二路徑的交點(diǎn)記作第二過渡點(diǎn)P_e，如圖3所示。圖中301表示第一過渡點(diǎn)P_s，也即第一個控制點(diǎn)，302至305分別表示4個控制點(diǎn)，306表示第二過渡點(diǎn)P_e，也即第六個控制點(diǎn)，307表示第一路徑與第二路徑的相接點(diǎn)，308表示P_s處路徑的二階導(dǎo)向量，309表示P_s處路徑一階導(dǎo)向量，310表示P_e處路徑一階導(dǎo)向量，311表示P_e處路徑二階導(dǎo)向量。

更進(jìn)一步地說，由于P_s是第一路徑上的點(diǎn)，因此可以根據(jù)第一路徑的函數(shù)表達(dá)式，計算出P_s的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)又由于P_e是第二路徑上的點(diǎn)，因此可以根據(jù)第二路徑的函數(shù)表達(dá)式，計算出P_e的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)并將P_s、P_e、與作為待生成的連續(xù)軌跡規(guī)劃過渡路徑的邊界值。根據(jù)用戶給定的第一預(yù)設(shè)距離，生成合理的邊界值P_s、P_e、便于進(jìn)一步該邊界值生成恰當(dāng)?shù)倪^渡路徑。

在步驟202中，生成連續(xù)軌跡規(guī)劃過渡路徑。

具體地說，根據(jù)上述六個已知的邊界值P_s、P_e、及六個方程可以求得六個未知數(shù)的原則，選擇六個控制點(diǎn)的準(zhǔn)均勻三次B樣條曲線作為連續(xù)軌跡規(guī)劃過渡路徑，其中，且該連續(xù)軌跡規(guī)劃過渡路徑的函數(shù)表達(dá)式為：其中，p_i表示第i個控制點(diǎn)，即圖3中的301至306，B_i(u)表示第i個控制點(diǎn)的基函數(shù)，u∈[0,1]。

進(jìn)一步地說，控制點(diǎn)的個數(shù)p、連續(xù)軌跡規(guī)劃過渡路徑的階次d，及節(jié)點(diǎn)矢量U的元素個數(shù)n，滿足關(guān)系式：n＝p+d+1，于是，可以計算出節(jié)點(diǎn)矢量為：U＝[0 0 0 0 1/3 2/3 1 1 1 1]，從而可以得到節(jié)點(diǎn)u為0和1時的基函數(shù)值，及其一階導(dǎo)數(shù)、二階導(dǎo)數(shù)，如下表1所示。

表1

根據(jù)表1及P_s、P_e、與可以得到如下表達(dá)式(1)，其中，

通過公式(1)，可以計算出連續(xù)軌跡規(guī)劃過渡路徑的六個控制點(diǎn)p₁、p₂、p₃、p₄、p₅與p₆，從而進(jìn)一步根據(jù)生成連續(xù)軌跡規(guī)劃過渡路徑。

在步驟203中，計算生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率。

具體地說，當(dāng)給定某一點(diǎn)的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)后，可以求得該點(diǎn)處的曲率k，其中，與分別表示該點(diǎn)的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)，因此，可以根據(jù)連續(xù)軌跡規(guī)劃過渡路徑的函數(shù)表達(dá)式計算出該連續(xù)軌跡規(guī)劃過渡路徑的曲率k，其中，與分別表示所述生成的所述連續(xù)軌跡規(guī)劃過渡路徑的節(jié)點(diǎn)u處的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)，u∈[0,1]。

在步驟204中，計算優(yōu)化權(quán)重

具體地說，直接根據(jù)一、二階導(dǎo)數(shù)等邊界值條件，求得的準(zhǔn)均勻三次B樣條曲線，雖然可以滿足過渡轉(zhuǎn)接前后的速度及加速度連續(xù)，但可能存在尖點(diǎn)、拐點(diǎn)等不符合曲線光順性條件的問題。由此可見，若要實(shí)現(xiàn)運(yùn)動路徑在過渡轉(zhuǎn)接前后的速度及加速度連續(xù)，即不同路徑之間的平滑轉(zhuǎn)接，不僅需要保證轉(zhuǎn)接處(即第一過渡點(diǎn)與第二過渡點(diǎn)處)的一階左導(dǎo)數(shù)(過渡點(diǎn)處路徑函數(shù))與一階右導(dǎo)數(shù)(過渡路徑函數(shù))方向相同，二階左導(dǎo)數(shù)與二階右導(dǎo)數(shù)方向相同，還需要保證轉(zhuǎn)接處的左曲率與右曲率相等，只有當(dāng)保證轉(zhuǎn)接處的左曲率與右曲率相等時，才能使得生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率變化更加平滑，實(shí)現(xiàn)不同路徑之間的平滑轉(zhuǎn)接。于是，需要根據(jù)樣條曲線的曲率計算出合適的第一優(yōu)化權(quán)重λ與第二優(yōu)化權(quán)重μ，在實(shí)際應(yīng)用中，可以根據(jù)樣條曲線的曲率導(dǎo)數(shù)積分最小，即根據(jù)計算出最優(yōu)的第一優(yōu)化權(quán)重λ與第二優(yōu)化權(quán)重μ，進(jìn)一步地說，由于樣條曲線的曲率導(dǎo)數(shù)積分沒有具體表達(dá)式，因此可采用數(shù)值積分的方法得到，即f＝Σ(κ(u_i+1)-κ(u_i))*Δu。更進(jìn)一步地說，在實(shí)際應(yīng)用中，還可以根據(jù)樣條曲線曲率平方積分最小，或者根據(jù)樣條曲線曲率導(dǎo)數(shù)的絕對值積分最小，計算出最優(yōu)的第一優(yōu)化權(quán)重λ與第二優(yōu)化權(quán)重μ。

在步驟205中，計算優(yōu)化后的邊界值

具體地說，當(dāng)對一階導(dǎo)數(shù)與二階導(dǎo)數(shù)施加重時，不會影響一階導(dǎo)數(shù)與二階導(dǎo)數(shù)的方向，于是，為了保證轉(zhuǎn)接處的左曲率與右曲率相等，需要根據(jù)步驟204計算出的λ與μ，優(yōu)化邊界值與得到優(yōu)化后的邊界值與其中，

在步驟206中，平滑連續(xù)軌跡規(guī)劃過渡路徑

具體地說，把優(yōu)化后的邊界值與代入公式(1)，從而根據(jù)公式(1)計算出連續(xù)軌跡規(guī)劃過渡路徑的六個控制點(diǎn)p₁、p₂、p₃、p₄、p₅與p₆的優(yōu)化取值，從而進(jìn)一步根據(jù)生成較平滑的連續(xù)軌跡規(guī)劃過渡路徑，完成本次的連續(xù)軌跡規(guī)劃過渡路徑的平滑過程。當(dāng)完成本次連續(xù)后，接著繼續(xù)進(jìn)行下次的優(yōu)化過程，即重復(fù)執(zhí)行步驟203至步驟206，直到得到最優(yōu)的連續(xù)軌跡規(guī)劃過渡路徑。

本實(shí)施方式，具體給出了邊界值P_s、P_e、與的計算過程，并給出了優(yōu)化邊界值的具體過程，其中，根據(jù)生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率，可以快速準(zhǔn)確地計算出所需要的優(yōu)化權(quán)重λ與μ，從而進(jìn)一步根據(jù)優(yōu)化權(quán)重λ與μ，構(gòu)造出合理的邊界值與最終得到曲率平滑的連續(xù)軌跡規(guī)劃過渡路徑，消除影響連續(xù)軌跡規(guī)劃過渡路徑光順性的尖點(diǎn)、拐點(diǎn)、二重點(diǎn)等，實(shí)現(xiàn)不同路徑之間的平滑轉(zhuǎn)接。

上面各種方法的步驟劃分，只是為了描述清楚，實(shí)現(xiàn)時可以合并為一個步驟或者對某些步驟進(jìn)行拆分，分解為多個步驟，只要包含相同的邏輯關(guān)系，都在本專利的保護(hù)范圍內(nèi)；對算法中或者流程中添加無關(guān)緊要的修改或者引入無關(guān)緊要的設(shè)計，但不改變其算法和流程的核心設(shè)計都在該專利的保護(hù)范圍內(nèi)。

本發(fā)明第三實(shí)施方式涉及一種連續(xù)軌跡規(guī)劃過渡路徑的平滑裝置，如圖4所示，包括：邊界值計算模塊41、路徑生成模塊42、優(yōu)化模塊43與平滑模塊44。

邊界值計算模塊41，用于根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的連續(xù)軌跡規(guī)劃過渡路徑的邊界值。

路徑生成模塊42，用于根據(jù)邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成連續(xù)軌跡規(guī)劃過渡路徑。

優(yōu)化模塊43，用于根據(jù)生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化邊界值。

平滑模塊44，用于根據(jù)優(yōu)化后的邊界值及上述函數(shù)表達(dá)式，平滑生成的連續(xù)軌跡規(guī)劃過渡路徑。

不難發(fā)現(xiàn)，本實(shí)施方式為與第一實(shí)施方式相對應(yīng)的系統(tǒng)實(shí)施例，本實(shí)施方式可與第一實(shí)施方式互相配合實(shí)施。第一實(shí)施方式中提到的相關(guān)技術(shù)細(xì)節(jié)在本實(shí)施方式中依然有效，為了減少重復(fù)，這里不再贅述。相應(yīng)地，本實(shí)施方式中提到的相關(guān)技術(shù)細(xì)節(jié)也可應(yīng)用在第一實(shí)施方式中。

值得一提的是，本實(shí)施方式中所涉及到的各模塊均為邏輯模塊，在實(shí)際應(yīng)用中，一個邏輯單元可以是一個物理單元，也可以是一個物理單元的一部分，還可以以多個物理單元的組合實(shí)現(xiàn)。此外，為了突出本發(fā)明的創(chuàng)新部分，本實(shí)施方式中并沒有將與解決本發(fā)明所提出的技術(shù)問題關(guān)系不太密切的單元引入，但這并不表明本實(shí)施方式中不存在其它的單元。

本發(fā)明第四實(shí)施方式涉及一種連續(xù)軌跡規(guī)劃過渡路徑的平滑裝置。第四實(shí)施方式在第三實(shí)施方式的基礎(chǔ)上進(jìn)行了進(jìn)一步改進(jìn)，主要改進(jìn)之處在于：在本發(fā)明第四實(shí)施方式中，具體給出了邊界值計算模塊與優(yōu)化模塊包括的子模塊，其中，邊界值計算模塊41具體包括：第一確定子模塊411與第二確定子模塊412，優(yōu)化模塊43具體包括：優(yōu)化權(quán)重計算子模塊431、第一計算子模塊432與第二計算子模塊433，優(yōu)化權(quán)重計算子模塊431又進(jìn)一步包括：曲率計算子模塊4311與第三計算子模塊4312，如圖5所示。

邊界值計算模塊41，用于根據(jù)需要進(jìn)行連續(xù)軌跡規(guī)劃過渡的第一路徑與第二路徑的相接點(diǎn)，計算待生成的連續(xù)軌跡規(guī)劃過渡路徑的邊界值。

第一確定子模塊411，用于確定在第一路徑上的第一過渡點(diǎn)P_s，并計算P_s的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)其中，P_s與相接點(diǎn)的距離為第一預(yù)設(shè)距離。

第二確定子模塊412，用于確定在第二路徑上的第二過渡點(diǎn)P_e，并計算所述P_e的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)其中，P_e與相接點(diǎn)的距離為第一預(yù)設(shè)距離。

路徑生成模塊42，用于根據(jù)邊界值及預(yù)設(shè)的函數(shù)表達(dá)式，生成連續(xù)軌跡規(guī)劃過渡路徑。

優(yōu)化模塊43，用于根據(jù)生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率，優(yōu)化邊界值。

優(yōu)化權(quán)重計算子模塊431，用于根據(jù)生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率，計算的第一優(yōu)化權(quán)重λ與的第二優(yōu)化權(quán)重μ。

曲率計算子模塊4311，用于根據(jù)上述函數(shù)表達(dá)式，計算生成的連續(xù)軌跡規(guī)劃過渡路徑的曲率k，其中，與分別表示所述生成的所述連續(xù)軌跡規(guī)劃過渡路徑的節(jié)點(diǎn)u處的一階導(dǎo)數(shù)與二階導(dǎo)數(shù)，u∈[0,1]。

第三計算子模塊4312，用于根據(jù)計算的優(yōu)化權(quán)重λ與的優(yōu)化權(quán)重μ。

第一計算子模塊432，用于根據(jù)λ與μ，計算優(yōu)化后的與其中，表示優(yōu)化后的表示優(yōu)化后的

第二計算子模塊433，用于根據(jù)λ與μ，計算優(yōu)化后的與其中，表示優(yōu)化后的表示優(yōu)化后的

平滑模塊44，用于根據(jù)優(yōu)化后的邊界值及上述函數(shù)表達(dá)式，平滑生成的連續(xù)軌跡規(guī)劃過渡路徑。

由于第二實(shí)施方式與本實(shí)施方式相互對應(yīng)，因此本實(shí)施方式可與第二實(shí)施方式互相配合實(shí)施。第二實(shí)施方式中提到的相關(guān)技術(shù)細(xì)節(jié)在本實(shí)施方式中依然有效，在第二實(shí)施方式中所能達(dá)到的技術(shù)效果在本實(shí)施方式中也同樣可以實(shí)現(xiàn)，為了減少重復(fù)，這里不再贅述。相應(yīng)地，本實(shí)施方式中提到的相關(guān)技術(shù)細(xì)節(jié)也可應(yīng)用在第二實(shí)施方式中。

本領(lǐng)域技術(shù)人員可以理解實(shí)現(xiàn)上述實(shí)施例方法中的全部或部分步驟是可以通過程序來指令相關(guān)的硬件來完成，該程序存儲在一個存儲介質(zhì)中，包括若干指令用以使得一個設(shè)備(可以是單片機(jī)，芯片等)或處理器(processor)執(zhí)行本申請各個實(shí)施例所述方法的全部或部分步驟。而前述的存儲介質(zhì)包括：U盤、移動硬盤、只讀存儲器(ROM，Read-Only Memory)、隨機(jī)存取存儲器(RAM，Random Access Memory)、磁碟或者光盤等各種可以存儲程序代碼的介質(zhì)。

本領(lǐng)域的普通技術(shù)人員可以理解，上述各實(shí)施方式是實(shí)現(xiàn)本發(fā)明的具體實(shí)施例，而在實(shí)際應(yīng)用中，可以在形式上和細(xì)節(jié)上對其作各種改變，而不偏離本發(fā)明的精神和范圍。