基於貝塞爾曲線的機器人路徑規劃方法及裝置的製作方法
2023-07-13 02:54:41 3
專利名稱：基於貝塞爾曲線的機器人路徑規劃方法及裝置的製作方法
技術領域：
本發明屬於機器人路徑規划算法研究，屬於複雜系統與智能控制領域。該發明可以用於地面移動機器人、水下航行器控制以及無人機控制等領域。
背景技術：
機器人技術的研究越來越熱門，越來越多的科研工作者展開了機器人路徑規劃的研究。
機器人路徑規劃的方法有很多，主要有基於行為的方法、遺傳算法、神經網絡等方法。鮑爾奇(Balch)與阿金(Arkin)提出了將行為的控制方法用於機器人路徑規劃與控制， 引入了一些基本行為，包括避障、避碰和目標導航等。荷蘭德(Holland)提出了遺傳算法的方法，該算法基本思想是以自然遺傳機制和自然選擇等生物進化理論為基礎，構造的一類隨機優化搜索算法，很多學者利用該算法實現路徑規劃。史丹福大學的尼爾森(misson) 提出可視圖法，這種方法能完成最短路徑的搜索，但並沒有考慮到機器人本身的形狀、大小，且隨著障礙物的增多，或障礙物的不規則性而導致圖中的頂點集過大，計算複雜性隨之增加，搜索時間長。在可視圖法的基礎上進行了改進的沃羅諾伊(Voronoi)圖法，基本思想是首先產生與障礙物多邊形所有的邊等距離的沃羅諾伊(Voronoi)邊，並將所有沃羅諾伊 (Voronoi)邊的交點組成頂點集，再通過可視圖類似的搜索最短路徑邊的方法來規劃路徑。 模糊邏輯算法是對經典的二值邏輯的模糊化，模糊邏輯具有的魯棒性與「感知一動作」行為結合起來，為移動機器人路徑規劃問題提出了一種新思路，利用模糊邏輯進行路徑規劃，解決了傳統人工勢場法中局部極小的問題，利用該算法對所處環境信息的模糊化過程，避開了傳統算法中對移動機器人定位精度要求高的約束。目前對於機器人路徑規劃的方法很多，但是對於路徑上各點處如何處理並滿足速度約束、加速度約束、轉彎半徑約束等約束條件保證機器人規劃路徑可行則很少有詳細的介紹。
機器人路徑規划算法主要是規劃出路徑，並解決路徑的可行性問題，即滿足機器人的物理性能上的各種約束以及避碰、避障約束等。
發明內容
為了解決多約束情況下的機器人路徑規劃問題，本發明的目的是提出一種機器人路徑規划算法，該算法能夠為機器人規劃無障礙條件下連續可行的路徑，解決機器人的各種物理約束條件。為此，本發明提出了一種基於貝塞爾曲線的機器人路徑規劃方法。
為達成所述目的，本發明的第一方面是提供一種基於貝塞爾(Bezier)曲線的機器人路徑規劃方法，該機器人路徑規劃方法的步驟如下 步驟Sl 機器人路徑規劃參數輸入單元設定機器人狀態參數和約束條件、路徑離散化最小時間解析度； 步驟S2 機器人路徑生成單元依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點Pi、P2> P3和P4,規劃出機器人起點位置到目標點位置的連續路徑；其中4維向量由空間三維坐標和一維時間坐標組成；規劃的路徑也由包含空間三維坐標和一維時間坐標的4維坐標描述； 步驟S3 路徑點參數計算單元按路徑離散化最小時間解析度形成時間點序列，再依據連續路徑計算離散化路徑，即計算各時間點處路徑點坐標、速度、加速度和轉彎半徑； 步驟S4 路徑點參數檢測單元依據約束條件檢測與時間點相應的各路徑點處的速度、加速度、轉彎半徑是否滿足約束條件，如果不滿足約束條件，則跳轉至步驟S2，重新生成控制點；如果滿足約束條件，則跳轉至步驟S5 ； 步驟S5 規劃路徑輸出單元輸出規劃的滿足約束條件的機器人路徑。
為達成所述目的，本發明的第二方面是提供一種基於貝塞爾(Bezier)曲線的機器人路徑規劃裝置，該裝置由機器人路徑規劃參數輸入單元、機器人路徑生成單元、規劃點參數計算單元、路徑點參數檢測單元、規劃路徑輸出單元組成，其中機器人路徑規劃參數輸入單元接收設定機器人狀態參數和約束條件、路徑離散化最小時間解析度；機器人路徑生成單元與機器人路徑規劃參數輸入單元連接，機器人路徑生成單元依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點，規劃出機器人起點位置到目標點位置的連續路徑；路徑點參數計算單元與機器人路徑生成單元連接，機器人路徑生成單元按路徑離散化最小時間解析度形成時間點序列，再依據連續路徑計算離散化路徑，即計算各時間點處路徑點坐標、速度、加速度和轉彎半徑；路徑點參數檢測單元與路徑點參數計算單元連接，路徑點參數檢測單元依據約束條件檢測與時間點相應的各路徑點處的速度、加速度、 轉彎半徑是否滿足約束條件，如果不滿足約束條件，重新生成機器人路徑；如果滿足約束條件，保存規劃的滿足約束條件的機器人路徑；規劃路徑輸出單元與路徑點參數檢測單元連接，接收並輸出規劃的滿足約束條件的機器人路徑。
本發明有益效果本發明採用基於貝塞爾曲線的機器人路徑規划算法來解決無障礙環境下機器人路徑規劃問題，規劃出來的路徑滿足機器人本身物理約束以及避碰約束等。在給定機器人起點位置、速度和方向，目標點位置、速度和方向條件下，可以規劃出從機器人起點到目標點的路徑，規劃的路徑光滑連續可導，滿足機器人的速度約束、加速度約束、轉彎半徑約束等，並且能按照預定時間到達目標點。該算法能快速實現機器人路徑規劃，方法簡單、可靠，易於實現，計算量小，實時性較好，能較好的滿足機器人路徑規劃時間要求。
圖1是本發明實施例基於貝塞爾曲線的機器人路徑規劃結構示意圖。
圖2是本發明基於貝塞爾曲線的機器人路徑規划算法流程圖。
具體實施例方式為使本發明的目的、技術方案和優點更加清楚明白，以下結合具體實施例，並參照附圖，對本發明基於貝塞爾曲線的機器人路徑規劃方法和裝置進一步詳細說明。
如圖1示出本發明實施例基於貝塞爾曲線的機器人路徑規劃結構示意圖，該結構是在一臺計算機上實現，是由機器人路徑規劃參數輸入單元1、機器人路徑生成單元2、規劃點參數計算單元3、路徑點參數檢測單元4、規劃路徑輸出單元5組成，機器人路徑規劃參數輸入單元1接收設定機器人狀態參數和約束條件、路徑離散化最小時間解析度；機器人路徑生成單元2與機器人路徑規劃參數輸入單元1連接，機器人路徑生成單元2依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點Pp P2> P3和P4,規劃出機器人起點位置到目標點位置的連續路徑；其中4維向量由空間三維坐標和一維時間坐標組成；規劃的路徑也由包含空間三維坐標和一維時間坐標的4維坐標描述；路徑點參數計算單元3與機器人路徑生成單元2連接，機器人路徑生成單元2按路徑離散化最小時間解析度形成時間點序列，再依據連續路徑計算離散化路徑，即計算各時間點處路徑點坐標、速度、加速度和轉彎半徑；路徑點參數檢測單元4與路徑點參數計算單元3連接，路徑點參數檢測單元4依據約束條件檢測與時間點相應的各路徑點處的速度、加速度、轉彎半徑是否滿足約束條件，如果不滿足約束條件，重新生成機器人路徑；如果滿足約束條件，保存規劃的滿足約束條件的機器人路徑；規劃路徑輸出單元5與路徑點參數檢測單元4連接，接收並輸出規劃的滿足約束條件的機器人路徑。
下面利用該結構實現本發明的基於貝塞爾曲線的機器人路徑規劃方法，請參考圖 2示出的具體步驟如下 步驟Sl 機器人路徑規劃參數輸入單元1設定機器人狀態參數和約束條件、路徑離散化最小時間解析度； 機器人狀態參數主要包括機器人起點位置坐標(xs，ys, zs)、起點位置處的速度 (vsx, V/, vsz)，機器人期望到達的目標點位置坐標(xg，yg, Zg)、到達目標點位置時的速度 (ν/, ν/, vgz)，期望的到達時間tf ；約束條件主要包括機器人的最大速度Vmax和最小速度 Vmin、最大加速度^iax和最小加速度^lin、最小轉彎半徑Rmin ；其中期望的到達時間tf是指機器人從起點位置坐標運動到目標點位置坐標期望花費的時間；設定機器人路徑離散化最小時間解析度tmin。
步驟S2 機器人路徑生成單元2依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點=PpPyP3和P4，規劃出機器人起點位置到目標點位置的連續路徑； 其中4維向量由空間三維坐標和一維時間坐標組成；規劃的路徑也由包含空間三維坐標和一維時間坐標的4維坐標描述；採用所述貝塞爾曲線描述路徑的三維空間坐標和一維時間坐標。
本發明的路徑規划算法中，規劃的機器人路徑包括三維空間位置坐標和到達該空間位置的時間坐標的4維坐標組成。要規劃的機器人路徑上各路徑點採用笛卡兒空間坐標系的X軸、Y軸、Z軸，和時間軸T構成的四維空間描述。要規劃的機器人路徑在X軸、Y軸、 Z軸和時間軸T上的投影用三次貝塞爾曲線表達，其表達式為 χ(τ) = ax τ 3+bx τ 2+cx τ +dx y( τ ) = ay τ 3+by τ 2+Cy τ +dy (1) z( τ ) = az τ 3+bz τ 2+cz τ +dz t ( τ ) = at τ 3+bt τ 2+ct τ +dt 其中，χ(τ)，γ(τ)，ζ(τ), t(x)表示要規劃的機器人路徑上各路徑點的位置坐標x、y、z和時間為貝塞爾曲線的參數，其取值為τ e
。τ =0時，χ(τ)， y(T)，z(T)，t(T)為機器人的起點位置(\，1，0，起點時間0; τ = l·時，χ(τ)，γ(τ)，ζ ( τ )，t ( τ )為機器人的期望到達的目標點位置(xg，yg, zg)，和期望的達到時間tf ；貝塞爾曲線的各項係數包括ax，bx，cx，dx為χ( τ )的係數、ay，by，cy，dy為y( τ )的係數、az，bz, cz，dz為ζ ( τ )的係數以及at, bt，ct，dt為t ( τ )的係數。
上述貝塞爾曲線的各項係數可以根據四個控制點唯一確定，每個控制點為(X，y， z,t)描述的四維空間點。假設三次貝塞爾曲線的四個控制點坐標分別為控制點P1O^y1, Z1,、)，控制點 P2 (x2，Y2, z2, t2)，控制點 P3(x3，Y3, z3, t3)，控制點 P4(x4，Y4, z4, t4)，由這四個控制點，可以求得對應的貝塞爾曲線的各項係數為 aXay = -yi+3y2-3y3+y4 bx=3X^6X2 + 3X3 by = Sy1-By^y3, Cx=-3x^3x2cy = -Sy^Sy2 dx=X1dy = Yl azZ1+3Z2 3ζ— -11+3 一3 3+ 4 K=3ZJ-6Z2+3Z3,bt = 3tr6t2+3t3, (2) Cz=-Sz^Sz2 dz=Z1dt = ti 利用機器人起點位置坐標、期望到達的目標點位置坐標和期望的到達時間tf確定
貝塞爾曲線的控制點PjP P4。在τ =0時，令貝塞爾曲線的控制點P1 (Xpy1^pt1)，為機器人的起點位置(xs，ys，zs)且、=0 ；在τ =1時，令貝塞爾曲線的控制點P4 (x4，y4, Z4, t4)，為機器人期望到達的目標點位置(xg，yg, %)，且、=tf，其中tf為機器人期望的到達時間。只需再確定兩個中間控制點IVP3即可規劃出機器人的路徑。
在去除時間軸一維的機器人路徑上，各點處的切向方向與該點處速度方向一致， 因此，控制點P1與控制點P2的連線與控制點P1處的切向方向一致，即與機器人起點位置處的速度方向一致；控制點P3與控制點P4的連線與控制點P4處的切向方向一致，即與機器人的目標點位置處的速度方向一致。
故兩個中間控制點P2、P3的X軸、Y軸、Z軸坐標可以表示為如下公式
P2 = (X1, Y1, Z1) +Ii1 X V1/ I V1 ,0 < k1 k2 < IP1P4(3) P3 = (x4, y4, z4) ~k2 X V4/ | V4 其中，(Xl, Yl, Z1)為控制AP1的X軸、Y軸、Z軸坐標，即機器人起點位置(xs，ys， zs) ;Vl表示起點位置處的機器人速度V1表示起點位置處的機器人速度值， V1/1V1為起點位置處的機器人速度方向；(x4，y4，z4)為控制點P4WX軸、Y軸、Z軸坐標，即機器人期望到達的目標點位置(xg，yg, zg) ;v4為機器人到達目標點位置時的速度(vgx，ν/, V；), Iv4I表示到達目標點位置時的機器人速度值，V4/| V4I為機器人到達目標點位置時的速度方向^為控制點P1和控制點P2間的距離ι為控制點P3和控制點P4之間的距離
I^l
O 公式(3)可用於確定兩個中間控制點己、&在三維空間上的X軸、Y軸、Z軸坐標， 但是為方便後續速度、加速度、轉彎半徑、期望的到達時間等約束條件判斷，故需要確定各個路徑點處的時刻。由於貝塞爾曲線控制AP1和貝塞爾曲線控制點P4對應的時刻分別為O和tf。設兩個中間控制點P2、P3對應的時刻分別為ち，t3,顯然ちく t3。
包含時間軸的中間控制點P2、P3的選取公式為
權利要求
1.一種基於貝塞爾曲線的機器人路徑規劃方法，其特徵在於，該機器人路徑規劃方法的步驟如下步驟Sl 機器人路徑規劃參數輸入單元設定機器人狀態參數和約束條件、路徑離散化最小時間解析度；步驟S2 機器人路徑生成單元依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點ΡρΡ2、Ρ3和P4,規劃出機器人起點位置到目標點位置的連續路徑；其中4維向量由空間三維坐標和一維時間坐標組成；規劃的路徑也由包含空間三維坐標和一維時間坐標的4維坐標描述；步驟S3 路徑點參數計算單元按路徑離散化最小時間解析度形成時間點序列，再依據連續路徑計算離散化路徑，即計算各時間點處路徑點坐標、速度、加速度和轉彎半徑；步驟S4 路徑點參數檢測單元依據約束條件檢測與時間點相應的各路徑點處的速度、 加速度、轉彎半徑是否滿足約束條件，如果不滿足約束條件，則跳轉至步驟S2，重新生成控制點；如果滿足約束條件，則跳轉至步驟S5 ；步驟S5 規劃路徑輸出單元輸出規劃的滿足約束條件的機器人路徑。
2.根據權利要求1的路徑規劃方法，其特徵在於，所述機器人狀態參數包括機器人起點位置坐標(xs，ys, %)、起點位置處的速度(vsx，ν/, vsz)，機器人期望到達的目標點位置坐標(xg，yg，zg)、到達目標點位置時的速度(Vgx，Vgy，Vgz)，期望的到達時間tf ；約束條件包括機器人的最大速度Vmax和最小速度Vmin、最大加速度amax和最小加速度^lin、最小轉彎半徑Rmin ； 其中期望的到達時間tf是指機器人從起點位置坐標運動到目標點位置坐標期望花費的時間。
3.根據權利要求1的路徑規劃方法，其特徵在於，採用所述貝塞爾曲線描述路徑的三維空間坐標和一維時間坐標。
4.根據權利要求1的路徑規劃方法，其特徵在於，利用機器人起點位置坐標、期望到達的目標點位置坐標和期望的到達時間tf確定貝塞爾曲線的控制點P1和P4。
5.根據權利要求1的路徑規劃方法，其特徵在於，所述貝塞爾曲線的控制點P2和P3利用如下公式選取
其中，(Xl，yi，Zl)為控制點P1的X軸、Y軸、Z軸坐標，(x4, y4, z4)為控制點P4W X軸、 Y軸、Z軸坐標，ν:, ν/, vsz為起點位置處機器人速度V1在X軸、Y軸、Z軸的投影；ν/，ν/, ν/為機器人到達目標點位置時的速度V4在X軸、Y軸、Z軸的投影，ki; 1 為隨機選取的實數，且滿足
V1表示起點位置處的機器人速度值，V4表示到達目標點位置時的機器人速度值，tf為機器人期望的到達時間。
6.一種基於貝塞爾曲線的機器人路徑規劃裝置，其特徵在於，由機器人路徑規劃參數輸入單元、機器人路徑生成單元、規劃點參數計算單元、路徑點參數檢測單元、規劃路徑輸出單元組成，其中機器人路徑規劃參數輸入單元接收設定機器人狀態參數和約束條件、路徑離散化最小時間解析度；機器人路徑生成單元與機器人路徑規劃參數輸入單元連接，機器人路徑生成單元依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點，規劃出機器人起點位置到目標點位置的連續路徑；路徑點參數計算單元與機器人路徑生成單元連接，機器人路徑生成單元按路徑離散化最小時間解析度形成時間點序列，再依據連續路徑計算離散化路徑，即計算各時間點處路徑點坐標、速度、加速度和轉彎半徑；路徑點參數檢測單元與路徑點參數計算單元連接，路徑點參數檢測單元依據約束條件檢測與時間點相應的各路徑點處的速度、加速度、轉彎半徑是否滿足約束條件，如果不滿足約束條件，重新生成機器人路徑；如果滿足約束條件，保存規劃的滿足約束條件的機器人路徑；規劃路徑輸出單元與路徑點參數檢測單元連接，接收並輸出規劃的滿足約束條件的機器人路徑。
7.根據權利要求6所述的基於貝塞爾曲線的機器人路徑規劃裝置，其特徵在於，所述4 維向量由空間三維坐標和一維時間坐標組成。
8.根據權利要求6所述的基於貝塞爾曲線的機器人路徑規劃裝置，其特徵在於，所述規劃的路徑也由包含空間三維坐標和一維時間坐標的4維坐標描述。
全文摘要
本發明是一種基於貝塞爾曲線的機器人路徑規劃方法及裝置，機器人路徑規劃參數輸入單元接收設定機器人狀態參數和約束條件、路徑離散化最小時間解析度；機器人路徑生成單元依據機器人狀態參數生成一組由四個4維向量組成的貝塞爾曲線控制點，規劃出機器人起點位置到目標點位置的連續路徑；機器人路徑生成單元按路徑離散化最小時間解析度形成時間點序列，再依據連續路徑計算離散化路徑；路徑點參數檢測單元依據約束條件檢測與時間點相應的各路徑點處的速度、加速度、轉彎半徑是否滿足約束條件，如果不滿足約束條件，重新生成控制點；如果滿足約束條件，規劃路徑輸出單元接收並輸出路徑點參數檢測單元規劃的滿足約束條件的機器人路徑。
文檔編號G05D1/00GK102207736SQ20101013909
公開日2011年10月5日 申請日期2010年3月31日 優先權日2010年3月31日
發明者王碩, 譚民, 胡峰 申請人:中國科學院自動化研究所