本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，特別涉及一種基于通道選擇的移動(dòng)機(jī)器人實(shí)時(shí)運(yùn)動(dòng)規(guī)劃方法。

背景技術(shù)：

路徑規(guī)劃的任務(wù)是尋找一條從給定起點(diǎn)到目標(biāo)點(diǎn)來避開所有障礙物的運(yùn)動(dòng)路徑，是機(jī)器人避障技術(shù)中的重要研究?jī)?nèi)容?；趥鞲衅鞯木植柯窂揭?guī)劃又稱為動(dòng)態(tài)路徑規(guī)劃或者在線路徑規(guī)劃，它相對(duì)于全局路徑規(guī)劃來說，不需要知道準(zhǔn)確的全局信息，計(jì)算量小，實(shí)時(shí)性強(qiáng)。

機(jī)器人在局部規(guī)劃過程中，根據(jù)傳感器的信息來不斷地更新其內(nèi)部的環(huán)境模型，從而確定出周圍局部范圍內(nèi)的障礙物分布情況，并在此基礎(chǔ)上規(guī)劃出一條能避開所有障礙物的局部最優(yōu)路徑。目前比較有代表性的方法有勢(shì)場(chǎng)法、模糊邏輯算法、遺傳算法、滾動(dòng)窗口法等，但還沒有一個(gè)可以在任何條件下都適用的方法、也沒有考慮如何有效進(jìn)行速度控制的問題。所以現(xiàn)在非常有必要設(shè)計(jì)一種簡(jiǎn)單高效，同時(shí)能平滑地、連續(xù)地進(jìn)行避障的規(guī)劃與速度控制方法，把路徑規(guī)劃和速度控制結(jié)合在一起，完成實(shí)時(shí)的運(yùn)動(dòng)規(guī)劃。

鑒于此，本發(fā)明人為此研制出一種基于通道選擇的移動(dòng)機(jī)器人實(shí)時(shí)運(yùn)動(dòng)規(guī)劃方法，有效的解決了上述問題，本案由此產(chǎn)生。

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

本發(fā)明提供的一種基于通道選擇的移動(dòng)機(jī)器人實(shí)時(shí)運(yùn)動(dòng)規(guī)劃方法，根據(jù)確定的最優(yōu)可行通道選擇合適的速度和角速度控制機(jī)器人的運(yùn)動(dòng)，很好的把路徑和速度控制結(jié)合在一起，完成實(shí)時(shí)的運(yùn)動(dòng)規(guī)劃。

為了實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案如下：

一種基于通道選擇的移動(dòng)機(jī)器人實(shí)時(shí)運(yùn)動(dòng)規(guī)劃方法，包括

包括如下步驟：

S1障礙空間建模單元，基于機(jī)器人用激光傳感器逆時(shí)針掃描實(shí)時(shí)檢測(cè)周圍障礙物信息，建立空間模型，得到機(jī)器人前進(jìn)的可行通道；

S2避障決策單元，基于機(jī)器人實(shí)時(shí)的位置和姿態(tài)，以及目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度和姿態(tài)，選擇合適的可行通道作為局部最優(yōu)可行通道，定義局部最優(yōu)可行通道為對(duì)應(yīng)的激光傳感器掃描角度標(biāo)記為可行通道距離標(biāo)記為

S3避障控制單元，基于確定的局部最優(yōu)可行通道選擇合適的速度與角速度來控制機(jī)器人的運(yùn)動(dòng)；定義移動(dòng)機(jī)器人正前方即激光傳感器掃描角度為0°時(shí)其可行通道距離為dis_v，移動(dòng)機(jī)器人沿最優(yōu)可行通道運(yùn)動(dòng)的控制量為(v,ω)，其中v為移動(dòng)機(jī)器人在其坐標(biāo)系內(nèi)正前方的速度標(biāo)量，ω為移動(dòng)機(jī)器人在其坐標(biāo)系內(nèi)的偏航角速度，速度控制策略如下：

其中，k_v1、k_v2、k_ω為正系數(shù)。

所述的障礙空間建模單元包括所有可行通道的搜索空間S_p的建立和對(duì)搜索空間S_p優(yōu)化得出新的搜索空間

所述搜索空間S_p的建立過程為：

①將機(jī)器人形狀簡(jiǎn)化為半徑為r_robot的圓；

②機(jī)器人用激光傳感器來感知可行通道信息；在機(jī)器人坐標(biāo)系內(nèi)，逆時(shí)針對(duì)激光掃描點(diǎn)標(biāo)記為l₁,l₂,…,l_i,…,l_N，得到遍歷的激光掃描數(shù)據(jù)，其中l(wèi)_i以掃描角和對(duì)應(yīng)的測(cè)量距離d_i來表示，假設(shè)角度掃描間隔為則

③對(duì)劃分為M-1等份，既有M個(gè)方向，角度間隔則為在每個(gè)等分線線上，定義可行通道

p_j＝(γ_j,dis_j)

其中，p_j對(duì)應(yīng)的角度dis_j為其對(duì)應(yīng)方向上平移機(jī)器人不會(huì)碰撞到障礙物的最大安全距離，它是由上述遍歷的激光掃描數(shù)據(jù)映射到γ_j方向上的測(cè)量距離再減去r_robot得到；

逆時(shí)針對(duì)可行通道標(biāo)記為p₁,p₂,...,p_j,...,p_M，即定義搜索空間S_p＝{p₁,p₂,...p_j...,p_M}。

上述優(yōu)化得出新的所述搜索空間的建立過程為：

①定義窗口面積Λ：為在機(jī)器人坐標(biāo)系范圍內(nèi)，通過離散的激光雷達(dá)數(shù)據(jù)計(jì)算機(jī)器人可行區(qū)域的面積，其計(jì)算算法為：

a.初始化面積Λ＝0；

b.遍歷所有的激光掃描數(shù)據(jù)，并進(jìn)行累加：

②定義動(dòng)態(tài)窗口長(zhǎng)度w_len：

d_win＝k_w·Λ·cos(ψ_target)

其中，k_w為一正系數(shù)常量，ψ_target為在機(jī)器人坐標(biāo)系內(nèi)目標(biāo)點(diǎn)相對(duì)于機(jī)器人的方向角度，cos(·)為三角余弦函數(shù)，w_min為定義的最短窗口長(zhǎng)度；

③對(duì)于任意p_j∈S_p，若滿足dis_j≥w_len，則將它加入新的搜索空間集合在機(jī)器人坐標(biāo)系內(nèi)，逆時(shí)針對(duì)搜索空間內(nèi)的可行通道標(biāo)記為其對(duì)應(yīng)的角度標(biāo)記為對(duì)應(yīng)的可行通道距離標(biāo)記為

所述局部最優(yōu)可行通道為的建立過程為：

目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度為ψ_target，直線距離為d_target；如果目標(biāo)點(diǎn)在機(jī)器人搜索空間的可行通道內(nèi)，此時(shí)為ψ_target，設(shè)為ψ_target角度下的如果在ψ_target方向的可行通道距離大于機(jī)器人與目標(biāo)點(diǎn)的直線距離d_target，則直接將目標(biāo)點(diǎn)方向的可行通道作為此時(shí)機(jī)器人直接進(jìn)入趨向目標(biāo)模式；如果目標(biāo)點(diǎn)在機(jī)器人搜索空間的可行通道外，則的建立過程如下：

①引入距離增益g_dis：

式中，k_dis為一正的距離增益系數(shù)；

②引入目標(biāo)增益g_tar：

式中，k_tar為一正的目標(biāo)增益系數(shù)，ψ_target是目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度，abs(·)為絕對(duì)值函數(shù)；

③引入平滑增益g_smth：

式中，k_smth為一正的平滑增益系數(shù)；

④綜合考慮上述各因素，得到最優(yōu)可行通道

定義最優(yōu)可行通道對(duì)應(yīng)的角度為可行通道距離為

當(dāng)機(jī)器人進(jìn)入趨向目標(biāo)模式時(shí)，增加限速約束，限速約束速度為：

v_protect＝k_protect·d_target

其中，k_protect為一正系數(shù)，d_target為目標(biāo)點(diǎn)相對(duì)于機(jī)器人的直線距離。

采用上述方案后，本發(fā)明根據(jù)確定的最優(yōu)可行通道選擇合適的速度和角速度控制機(jī)器人的運(yùn)動(dòng)，很好的把路徑和速度控制結(jié)合在一起，完成實(shí)時(shí)的運(yùn)動(dòng)規(guī)劃。

附圖說明

圖1是本實(shí)施例激光掃描數(shù)據(jù)下的機(jī)器人通道概念圖；

圖2是本實(shí)施例最優(yōu)可行通道的示意圖。

具體實(shí)施方式

為了進(jìn)一步解釋本發(fā)明的技術(shù)方案，下面通過具體實(shí)施例來對(duì)本發(fā)明進(jìn)行詳細(xì)闡述。

結(jié)合圖1-2所示，是本發(fā)明揭示的一種基于通道選擇的移動(dòng)機(jī)器人實(shí)時(shí)運(yùn)動(dòng)規(guī)劃方法，包括如下步驟：

S1障礙空間建模單元，基于機(jī)器人用激光傳感器逆時(shí)針掃描實(shí)時(shí)檢測(cè)周圍障礙物信息，建立空間模型，得到機(jī)器人前進(jìn)的可行通道。障礙空間建模單元包括所有可行通道的搜索空間S_p的建立和對(duì)搜索空間S_p優(yōu)化得出新的搜索空間

優(yōu)選的，上述搜索空間S_p的建立過程為：

①將機(jī)器人形狀簡(jiǎn)化為半徑為r_robot的圓；

②機(jī)器人用激光傳感器來感知可行通道信息；在機(jī)器人坐標(biāo)系內(nèi)，逆時(shí)針對(duì)激光掃描點(diǎn)標(biāo)記為l₁,l₂,…,l_i,…,l_N，得到遍歷的激光掃描數(shù)據(jù)，其中l(wèi)_i以掃描角和對(duì)應(yīng)的測(cè)量距離d_i來表示，假設(shè)角度掃描間隔為則

③對(duì)劃分為M-1等份，既有M個(gè)方向，角度間隔則為在每個(gè)等分線線上，定義可行通道

p_j＝(γ_j,dis_j)

其中，p_j對(duì)應(yīng)的角度dis_j為其對(duì)應(yīng)方向上平移機(jī)器人不會(huì)碰撞到障礙物的最大安全距離，它是由上述遍歷的激光掃描數(shù)據(jù)映射到γj方向上的測(cè)量距離再減去r_robot得到；

逆時(shí)針對(duì)可行通道標(biāo)記為p₁,p₂,...,p_j,...,p_M，即定義搜索空間S_p＝{p₁,p₂,...p_j...,p_M}。

優(yōu)選的，上述優(yōu)化得出新的所述搜索空間的建立過程為：

①定義窗口面積Λ：為在機(jī)器人坐標(biāo)系范圍內(nèi)，通過離散的激光雷達(dá)數(shù)據(jù)計(jì)算機(jī)器人可行區(qū)域的面積，其計(jì)算算法為：

c.初始化面積Λ＝0；

d.遍歷所有的激光掃描數(shù)據(jù)，并進(jìn)行累加：

②定義動(dòng)態(tài)窗口長(zhǎng)度w_len：

d_win＝k_w·Λ·cos(ψ_target)

其中，k_w為一正系數(shù)常量，ψ_target為在機(jī)器人坐標(biāo)系內(nèi)目標(biāo)點(diǎn)相對(duì)于機(jī)器人的方向角度，cos(·)為三角余弦函數(shù)，w_min為定義的最短窗口長(zhǎng)度；

③對(duì)于任意p_j∈S_p，若滿足dis_j≥w_len，則將它加入新的搜索空間集合在機(jī)器人坐標(biāo)系內(nèi)，逆時(shí)針對(duì)搜索空間內(nèi)的可行通道標(biāo)記為其對(duì)應(yīng)的角度標(biāo)記為對(duì)應(yīng)的可行通道距離標(biāo)記為

S2避障決策單元，基于機(jī)器人實(shí)時(shí)的位置和姿態(tài)，以及目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度和姿態(tài)，選擇合適的可行通道作為局部最優(yōu)可行通道，定義局部最優(yōu)可行通道為對(duì)應(yīng)的激光傳感器掃描角度標(biāo)記為可行通道距離標(biāo)記為

優(yōu)選的，局部最優(yōu)可行通道為的建立過程為：

目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度為ψ_target，直線距離為d_target；如果目標(biāo)點(diǎn)在機(jī)器人搜索空間的可行通道內(nèi)，此時(shí)為ψ_target，設(shè)為ψ_target角度下的如果在ψ_target方向的可行通道距離大于機(jī)器人與目標(biāo)點(diǎn)的直線距離d_target，則直接將目標(biāo)點(diǎn)方向的可行通道作為此時(shí)機(jī)器人直接進(jìn)入趨向目標(biāo)模式；如果目標(biāo)點(diǎn)在機(jī)器人搜索空間的可行通道外，則的建立過程如下：

①基于搜索空間內(nèi)各方向的最大通行距離來說，某方向的越大，機(jī)器人沿著這個(gè)方向行進(jìn)的安全性就越高。因此引入距離增益g_dis：

式中，k_dis為一正的距離增益系數(shù)；

②與目標(biāo)方向的偏差值越小，到達(dá)目標(biāo)的可能性越大。因此引入目標(biāo)增益g_tar：

式中，k_tar為一正的目標(biāo)增益系數(shù)，ψ_target是目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度，abs(·)為絕對(duì)值函數(shù)；

③為了確保機(jī)器人的形式軌跡盡量平滑，轉(zhuǎn)向盡量平穩(wěn)。因此引入平滑增益g_smth：

式中，k_smth為一正的平滑增益系數(shù)；

④綜合考慮上述各因素，通過啟發(fā)式選擇安全距離最大、目標(biāo)可達(dá)最可靠和平滑性最佳的可行通道，得到最優(yōu)可行通道

定義目標(biāo)通道對(duì)應(yīng)的角度為可行通道距離為

S3避障控制單元，基于確定的局部最優(yōu)可行通道選擇合適的速度與角速度來控制機(jī)器人的運(yùn)動(dòng)；定義移動(dòng)機(jī)器人正前方即激光傳感器掃描角度為0°時(shí)其可行通道距離為dis_v，移動(dòng)機(jī)器人沿最優(yōu)可行通道運(yùn)動(dòng)的控制量為(v,ω)，其中v為移動(dòng)機(jī)器人在其坐標(biāo)系內(nèi)正前方的速度標(biāo)量，ω為移動(dòng)機(jī)器人在其坐標(biāo)系內(nèi)的偏航角速度，速度控制策略如下：

其中，k_v1、k_v2、k_ω為正系數(shù)。

當(dāng)機(jī)器人進(jìn)入趨向目標(biāo)模式時(shí)，增加限速約束，限速約束速度為：

v_protect＝k_protect·d_target

其中，k_protect為一正系數(shù)，d_target為目標(biāo)點(diǎn)相對(duì)于機(jī)器人的直線距離。

本實(shí)施例具體的路徑規(guī)劃中避障決策和速度控制過程如下：

Step 1：對(duì)系統(tǒng)參數(shù)r_robot，k_w，w_min，k_dis，k_tar，k_smth，k_v1，k_v2，k_ω進(jìn)行初始化，可將k_dis、k_tar和k_smth分別設(shè)為0.1、0.75和0.15，設(shè)置機(jī)器人初始位置和目標(biāo)點(diǎn)。

Step 2：遍歷所有方向上的激光掃描數(shù)據(jù)信息并計(jì)算各方向上的可行通道距離dis_j，計(jì)算窗口長(zhǎng)度w_len，并按w_len對(duì)搜索空間S_p進(jìn)行優(yōu)化得到新的

Step 3：目標(biāo)點(diǎn)相對(duì)于機(jī)器人的角度為ψ_target，距離為d_target。當(dāng)目標(biāo)點(diǎn)在機(jī)器人的可行通道內(nèi)，即在ψ_target方向的通道安全距離大于機(jī)器人與目標(biāo)點(diǎn)的直線距離，則直接將目標(biāo)點(diǎn)方向的可行通道作為此時(shí)機(jī)器人進(jìn)入趨向目標(biāo)模式。當(dāng)目標(biāo)點(diǎn)不在機(jī)器人的可行通道內(nèi)，則綜合考慮距離增益g_dis、目標(biāo)增益g_tar和平滑增益g_smth，啟發(fā)式選擇安全距離最大、平滑性最佳、目標(biāo)可達(dá)最可靠的可行通道作為最優(yōu)可行通道

Step 4：計(jì)算得到移動(dòng)機(jī)器人的速度和角速度控制量為(v,ω)，控制機(jī)器人轉(zhuǎn)動(dòng)以避開障礙物。如果機(jī)器人進(jìn)入趨向目標(biāo)模式，則加入限速約束。

重復(fù)Step 2-Step 4直到機(jī)器人達(dá)到目標(biāo)點(diǎn)。

以上僅為本發(fā)明的較佳實(shí)施例，并非對(duì)本發(fā)明的保護(hù)范圍的限定。凡依本案的設(shè)計(jì)思路所做的等同變化，均落入本案的保護(hù)范圍。