一種基于橢圓約束的機(jī)器人路徑規(guī)劃方法和裝置制造方法
【專利摘要】本發(fā)明為使機(jī)器人能夠在環(huán)境障礙物形狀、位置未知的情況下進(jìn)行避障，提供一種基于橢圓約束的機(jī)器人路徑規(guī)劃方法，包括以下步驟，獲取機(jī)器人運(yùn)動信息、障礙物信息、目標(biāo)環(huán)境信息；確定機(jī)器人與障礙物發(fā)生碰撞的條件；針對機(jī)器人與障礙物發(fā)生碰撞的條件，建立基于橢圓約束的運(yùn)動避障模型；該橢圓滿足的約束條件為：機(jī)器人在橢圓內(nèi)、所檢測到的障礙物點(diǎn)在橢圓外、最終目標(biāo)點(diǎn)在橢圓邊界上或者在橢圓外；采用Bug路徑規(guī)劃算法進(jìn)行全局路徑規(guī)劃。達(dá)到規(guī)劃機(jī)器人行進(jìn)最短路徑，實(shí)現(xiàn)實(shí)時性避障的目的。
【專利說明】一種基于橢圓約束的機(jī)器人路徑規(guī)劃方法和裝置【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于機(jī)器人路徑規(guī)劃領(lǐng)域，具體涉及一種基于橢圓約束的機(jī)器人路徑規(guī)劃方法及裝置。
技術(shù)背景
[0002]隨著我國現(xiàn)代化技術(shù)的不斷發(fā)展，機(jī)器人技術(shù)得到了迅速發(fā)展，機(jī)器人的應(yīng)用也越來越廣泛。路徑規(guī)劃作為移動機(jī)器人技術(shù)的一個重要分支，在機(jī)器人的相關(guān)技術(shù)研究中顯得尤為重要。移動機(jī)器人路徑規(guī)劃是指在障礙物存在的環(huán)境下，機(jī)器人按照一定的優(yōu)化準(zhǔn)則，尋找一條從起始位置到目標(biāo)位置的安全無碰撞路徑。
[0003]現(xiàn)有技術(shù)中的路徑規(guī)劃方法，根據(jù)對環(huán)境信息的掌握程度不同可分為:全局路徑規(guī)劃方法和局部路徑規(guī)劃方法。全局路徑規(guī)劃是要求環(huán)境信息已知的一種移動機(jī)器人路徑規(guī)劃，典型方法有:柵格法、可視圖法和結(jié)構(gòu)空間法等。該類方法的缺點(diǎn)在于不能實(shí)時解決環(huán)境中障礙物發(fā)生變化的問題，可用性差。局部路徑規(guī)劃是依據(jù)作業(yè)環(huán)境信息部分或完全未知的一種路徑規(guī)劃，常用方法有:人工勢場法、模糊邏輯算法、遺傳算法等。該方法缺點(diǎn)為:只能對局部路徑進(jìn)行規(guī)劃，不能對全局規(guī)劃路徑進(jìn)行優(yōu)化。
[0004]現(xiàn)有全局路徑規(guī)劃法是針對避免機(jī)器人與形狀、位置已知障礙物發(fā)生碰撞時所采取的規(guī)劃策略，不能實(shí)時解決環(huán)境中障礙物發(fā)生變化的問題，沒有對障礙物進(jìn)行主動的避障預(yù)防。

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

[0005]本發(fā)明為實(shí)現(xiàn)機(jī)器人針對形狀、位置未知的障礙物進(jìn)行主動預(yù)防避障，提供一種基于橢圓約束的機(jī)器人路徑規(guī)劃方法。
[0006]本發(fā)明為實(shí)現(xiàn)發(fā)明目的所采取的技術(shù)方案為，該基于橢圓約束的機(jī)器人路徑規(guī)劃方法包括如下步驟:
[0007]獲取機(jī)器人運(yùn)動信息、障礙物信息、目標(biāo)環(huán)境信息；確定機(jī)器人與障礙物發(fā)生碰撞的條件；針對機(jī)器人與障礙物發(fā)生碰撞的條件，建立基于橢圓約束的運(yùn)動避障模型，該橢圓應(yīng)滿足的約束條件為，機(jī)器人在橢圓內(nèi)、所檢測到的障礙物點(diǎn)在橢圓外、最終目標(biāo)點(diǎn)在橢圓邊界上或者在橢圓外；采用Bug路徑規(guī)劃算法進(jìn)行全局路徑規(guī)劃，機(jī)器人沿連接起點(diǎn)和最終目標(biāo)點(diǎn)的直線路徑前進(jìn)，在遇到障礙物時建立基于橢圓約束的運(yùn)動避障模型，當(dāng)最終目標(biāo)點(diǎn)在橢圓邊界上時，無需再設(shè)置中間目標(biāo)點(diǎn)，機(jī)器人沿當(dāng)前位置向最終目標(biāo)點(diǎn)運(yùn)動；當(dāng)最終目標(biāo)點(diǎn)在橢圓邊界外時，設(shè)置中間目標(biāo)點(diǎn)；機(jī)器人沿當(dāng)前位置與所述中間目標(biāo)點(diǎn)之間的直線路徑運(yùn)動到所述中間目標(biāo)點(diǎn)；機(jī)器人避障后再次沿連接所述中間目標(biāo)點(diǎn)和最終目標(biāo)點(diǎn)的直線路徑前進(jìn)，到達(dá)最終目標(biāo)點(diǎn)。
[0008]機(jī)器人從起始點(diǎn)出發(fā)，沒有遇到障礙物時，沿起點(diǎn)與最終目標(biāo)點(diǎn)之間的直線路徑前進(jìn)，當(dāng)遇到第一個障礙物時，建立基于橢圓約束的運(yùn)動避障模型。根據(jù)橢圓應(yīng)滿足的約束條件求得橢圓方程。當(dāng)最終目標(biāo)點(diǎn)在所建立的橢圓的邊界上時，機(jī)器人直接沿當(dāng)前位置運(yùn)動到最終目標(biāo)點(diǎn)。當(dāng)最終目標(biāo)點(diǎn)在所建立的橢圓的邊界外時，選擇該次模型中的中間目標(biāo)點(diǎn)(第一中間目標(biāo)點(diǎn))，機(jī)器人沿直線從起始點(diǎn)運(yùn)動到第一中間目標(biāo)點(diǎn)，然后開始沿第一中間目標(biāo)點(diǎn)與最終目標(biāo)點(diǎn)之間的直線路徑運(yùn)動；當(dāng)遇到第二個障礙物時，通過建立運(yùn)動避障模型，選擇該次模型中的中間目標(biāo)點(diǎn)(第二中間目標(biāo)點(diǎn))，從機(jī)器人的當(dāng)前位置，沿直線運(yùn)動到第二中間目標(biāo)點(diǎn)，然后開始沿第二中間目標(biāo)點(diǎn)與最終目標(biāo)點(diǎn)之間的直線路徑運(yùn)動。以此類推，最終通過機(jī)器人多次自主避障，達(dá)到最終目標(biāo)點(diǎn)。建立運(yùn)動避障模型的過程為，在機(jī)器人、障礙物和機(jī)器人要到達(dá)的最終目標(biāo)點(diǎn)的位置已經(jīng)確定的條件下，建立一個橢圓，使橢圓滿足:機(jī)器人在橢圓內(nèi)、所檢測到的障礙物點(diǎn)在橢圓外、最終目標(biāo)點(diǎn)在橢圓邊界上或者在橢圓外。如果此時最終目標(biāo)點(diǎn)在橢圓邊界上，則機(jī)器人直接從當(dāng)前位置向最終目標(biāo)點(diǎn)沿直線運(yùn)動，無需選擇中間目標(biāo)點(diǎn)。如果此時最終目標(biāo)點(diǎn)在橢圓邊界外，則此時需要選擇各中間目標(biāo)點(diǎn)，通過避障運(yùn)動到達(dá)最終目標(biāo)點(diǎn)。
[0009]本發(fā)明方法中，所述機(jī)器人運(yùn)動信息包括機(jī)器人運(yùn)動方向、運(yùn)動速度、運(yùn)動坐標(biāo)位置信息及機(jī)器人可獲取環(huán)境信息范圍的最大距離r和最大角度Θ ;所述障礙物信息包括每個障礙物的坐標(biāo)位置信息；所述目標(biāo)環(huán)境信息包括指定目標(biāo)位置全局坐標(biāo)信息。
[0010]所述機(jī)器人與障礙物發(fā)生碰撞的條件為:
[0011]d I Sina |〈Rsafe
[0012]d ^ r (I)
[0013]0< α < Θ
[0014]其中:d表示機(jī)器人與檢測到的障礙物之間的距離，α表示機(jī)器人與障礙物之間距離向量和機(jī)器人運(yùn)動方向的夾角，Rsafe表示機(jī)器人與障礙物不發(fā)生碰撞的最小安全距離，r表示機(jī)器人可獲取環(huán)境信息范圍的最大距離，Θ表示機(jī)器人可獲取環(huán)境信息范圍的最大角度。
[0015]當(dāng)機(jī)器人到障礙物距離沿機(jī)器人運(yùn)動方向的距離分量大于安全距離Rsafe時，機(jī)器人運(yùn)動路線不變，繼續(xù)沿原直線路徑運(yùn)動，當(dāng)機(jī)器人到障礙物距離沿機(jī)器人運(yùn)動方向的距離分量小于安全距離Rsafe時，通過建立起基于橢圓約束的運(yùn)動避障模型，進(jìn)行避障運(yùn)動。
[0016]本發(fā)明方法中，所述建立基于橢圓約束的運(yùn)動避障模型所述的橢圓方程表示為Φ1 (P, q, r) = {x I xVx+xV+rt ≤ 0},即橢圓內(nèi)部滿足 inti?1 = {x I Ot (P, q, r)〈0},橢圓邊界值滿足Ot(X) = χΤρ^+χ、1+:^ = O,其中t為運(yùn)動的任一時刻，x為所規(guī)劃橢圓上任意一點(diǎn)坐標(biāo)，P表示二維正定矩陣，q表示2行I列實(shí)數(shù)矩陣，r為實(shí)數(shù)。T代表對矩陣進(jìn)行轉(zhuǎn)置運(yùn)算。
[0017]所述建立基于橢圓約束的運(yùn)動避障模型過程具體為，橢圓約束模型:
【權(quán)利要求】
1.一種基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于，包括如下步驟:獲取機(jī)器人運(yùn)動信息、障礙物信息、目標(biāo)環(huán)境信息； 確定機(jī)器人與障礙物發(fā)生碰撞的條件； 針對機(jī)器人與障礙物發(fā)生碰撞的條件，建立基于橢圓約束的運(yùn)動避障模型；該橢圓應(yīng)滿足的約束條件為:機(jī)器人在橢圓內(nèi)、所檢測到的障礙物點(diǎn)在橢圓外、最終目標(biāo)點(diǎn)在橢圓邊界上或者在橢圓外； 采用Bug路徑規(guī)劃算法進(jìn)行全局路徑規(guī)劃:機(jī)器人沿連接起點(diǎn)和最終目標(biāo)點(diǎn)的直線路徑前進(jìn)，在遇到障礙物時建立基于橢圓約束的運(yùn)動避障模型，當(dāng)最終目標(biāo)點(diǎn)在橢圓邊界上時，無需再設(shè)置中間目標(biāo)點(diǎn)，機(jī)器人 沿當(dāng)前位置向最終目標(biāo)點(diǎn)運(yùn)動；當(dāng)最終目標(biāo)點(diǎn)在橢圓邊界外時，設(shè)置中間目標(biāo)點(diǎn)；機(jī)器人沿當(dāng)前位置與所述中間目標(biāo)點(diǎn)之間的直線路徑運(yùn)動到所述中間目標(biāo)點(diǎn)；機(jī)器人避障后再次沿連接所述中間目標(biāo)點(diǎn)和最終目標(biāo)點(diǎn)的直線路徑前進(jìn)，到達(dá)最終目標(biāo)點(diǎn)。
2.根據(jù)權(quán)利要求1所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于: 所述機(jī)器人運(yùn)動信息包括機(jī)器人運(yùn)動方向、運(yùn)動速度、運(yùn)動坐標(biāo)位置信息及機(jī)器人可獲取環(huán)境信息范圍的最大距離r和最大角度Θ ;所述障礙物信息包括每個障礙物的坐標(biāo)位置信息；所述目標(biāo)環(huán)境信息包括指定目標(biāo)位置全局坐標(biāo)信息。
3.根據(jù)權(quán)利要求1所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于，所述機(jī)器人與障礙物發(fā)生碰撞的條件為
d I sina <Rsafe
d < r (I)
0〈α〈 Θ 其中:d表示機(jī)器人與檢測到的障礙物之間的距離， α表示機(jī)器人與障礙物之間距離向量和機(jī)器人運(yùn)動方向的夾角， Rsafe表示機(jī)器人與障礙物不發(fā)生碰撞的最小安全距離， r表示機(jī)器人可獲取環(huán)境信息范圍的最大距離， Θ表示機(jī)器人可獲取環(huán)境信息范圍的最大角度。
4.根據(jù)權(quán)利要求1或2或3所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法， 其特征在于， 所述運(yùn)動避障模型基于橢圓約束的橢圓方程表示為: Φ1 (P, q, r) = {x I xVx+xV+rt ≤ 0},即橢圓內(nèi)部滿足 ?η?Φ' = &|0’，110〈0}，橢圓邊界值滿足(^00 = xVx+xV+rt = 0，其中 t 為運(yùn)動的任一時刻，X為所規(guī)劃橢圓上任意一點(diǎn)坐標(biāo)，P表示二維正定矩陣，q表示2行I列實(shí)數(shù)矩陣，r為實(shí)數(shù)，T表示轉(zhuǎn)置計算。
5.根據(jù)權(quán)利要求4所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于，建立基于所述運(yùn)動避障模型過程具體為: 建立橢圓約束模型:
6.根據(jù)權(quán)利要求5所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于，所述常數(shù)α、β的范圍為:α在O~I之間取值，β在O~10_3之間取值。
7.根據(jù)權(quán)利要求6所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于，當(dāng)最終目標(biāo)點(diǎn)在所述橢圓邊界時，橢圓內(nèi)機(jī)器人運(yùn)動方向Xn為從機(jī)器人當(dāng)前位置指向最終目標(biāo)位置的單位向量。
8.根據(jù)權(quán)利要求6所述的基于橢圓約束的機(jī)器人路徑規(guī)劃方法，其特征在于，當(dāng)最終目標(biāo)點(diǎn)在所述橢圓外時，根據(jù)已經(jīng)求出的橢圓模型表達(dá)式，定義橢圓的長、短軸分別為所求橢圓參數(shù)矩陣P最大、最小特征值所對應(yīng)的特征向量χρ、χ0 ;所述X。在與Xp成±90°的方向；所述χρ方向左側(cè)障礙物的數(shù)量多于右側(cè)障礙物的數(shù)量時，X0選擇為矢量χρ的-90°方向；所述χρ方向左側(cè)障礙物的數(shù)量少于右側(cè)障礙物的數(shù)量時，X0選擇為矢量χρ的+90°方向； 判斷最終目標(biāo)點(diǎn)與橢圓X軸的位置關(guān)系，確定橢圓內(nèi)機(jī)器人的運(yùn)動方向xn ；t時刻橢圓圓心為CU, Λ)，所述最終目標(biāo)點(diǎn)坐標(biāo)G (xg, yg)，機(jī)器人坐標(biāo)RU，Ji).根據(jù)公式(7)判斷最終目標(biāo)點(diǎn)與橢圓X軸的位置關(guān)系；
9.一種基于橢圓約束的機(jī)器人路徑規(guī)劃裝置，其特征在于，包括:用于獲取機(jī)器人運(yùn)動信息、障礙物信息、目標(biāo)環(huán)境信息的信息采集模塊；與所述信息采集模塊相連接的信息處理模塊，用于對獲取的環(huán)境信息進(jìn)行處理，并產(chǎn)生相應(yīng)的環(huán)境數(shù)據(jù)；與所述信息處理模塊相連的運(yùn)動規(guī)劃控制模塊，用于根據(jù)所述的環(huán)境數(shù)據(jù)規(guī)劃運(yùn)動避障，建立基于橢圓約束的運(yùn)動避障模型，滿足機(jī)器人在橢圓內(nèi)，所檢測到的障礙物點(diǎn)在橢圓外，最終目標(biāo)點(diǎn)在橢圓邊界上或者在橢圓外的約束條件，采用Bug路徑規(guī)劃算法進(jìn)行全局路徑規(guī)劃，控制機(jī)器人運(yùn)動到達(dá)最終目標(biāo)點(diǎn)。
10.根據(jù)權(quán)利要求9所述的一種基于橢圓約束的機(jī)器人路徑規(guī)劃裝置，其特征在于，所述信號采集模塊為激光測距傳感器；所述運(yùn)動規(guī)劃控制模塊包括:定位模塊，用于對運(yùn)動過程中機(jī)器人位置進(jìn)行實(shí)時定位；避障模塊，用于對所述機(jī)器人運(yùn)動進(jìn)行規(guī)劃避障；伺服控制模塊，用于對所述機(jī)器人運(yùn)動及避障行為進(jìn)行控制，控制機(jī)器人本體運(yùn)動的速度及方向。
【文檔編號】G05D1/02GK103995984SQ201410255481
【公開日】2014年8月20日 申請日期:2014年6月9日 優(yōu)先權(quán)日:2014年6月9日
【發(fā)明者】吳懷宇, 張琴麗, 羅歡, 陳洋, 周致富, 滕雄, 李威凌 申請人:武漢科技大學(xué)