[發(fā)明專利]一種機(jī)器人軌跡跟蹤方法、系統(tǒng)及存儲(chǔ)介質(zhì)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010410198.6
• 申請(qǐng)日: 2020-05-15
• 公開（公告）號(hào): CN111506081B
• 公開（公告）日: 2021-06-25
• 發(fā)明（設(shè)計(jì)）人: 余伶俐;王正久;霍淑欣;白宇;周開軍
• 申請(qǐng)（專利權(quán)）人: 中南大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 長沙正奇專利事務(wù)所有限責(zé)任公司 43113
• 代理人: 馬強(qiáng);王娟

1.一種機(jī)器人軌跡跟蹤方法，其特征在于，包括以下步驟：

1)獲取全局路徑序列其中，表示全局規(guī)劃路徑在全局坐標(biāo)系下的第i個(gè)路徑點(diǎn)對(duì)應(yīng)的橫坐標(biāo)，表示全局規(guī)劃路徑在全局坐標(biāo)系下的第i個(gè)路徑點(diǎn)對(duì)應(yīng)的縱坐標(biāo)，表示全局規(guī)劃序列在全局坐標(biāo)系下的第i個(gè)路徑點(diǎn)對(duì)應(yīng)的航向角，N對(duì)應(yīng)全局規(guī)劃的路徑中路徑點(diǎn)的數(shù)量；

2)將機(jī)器人軌跡跟蹤問題構(gòu)建成最優(yōu)化規(guī)劃問題B:＝{X,U}，采用動(dòng)態(tài)窗口法求解所述最優(yōu)化規(guī)劃問題，得到局部最優(yōu)軌跡序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1；其中，γ_k代表各個(gè)優(yōu)化目標(biāo)的權(quán)重，f_k(B)代表相應(yīng)的各部分子代價(jià)函數(shù)；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表軌跡序列；U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分別為機(jī)器人從路徑點(diǎn)i-1到路徑點(diǎn)i的速度和角速度，n對(duì)應(yīng)求解出的軌跡點(diǎn)的數(shù)量；構(gòu)建最優(yōu)化規(guī)劃問題的具體實(shí)現(xiàn)過程包括：

A)構(gòu)建機(jī)器人全局坐標(biāo)系和機(jī)器人運(yùn)動(dòng)坐標(biāo)系，并獲取當(dāng)前機(jī)器人在全局坐標(biāo)系下的位姿x_i＝(x_i,y_i,θ_i)，即運(yùn)動(dòng)坐標(biāo)系相對(duì)于全局坐標(biāo)系的位姿；其中，機(jī)器人運(yùn)動(dòng)坐標(biāo)系以機(jī)器人質(zhì)心為原點(diǎn)，機(jī)器人速度方向?yàn)閤軸正向，垂直x軸逆時(shí)針方向?yàn)閥軸正向；全局坐標(biāo)系與大地固連，定義為初始時(shí)刻機(jī)器人的運(yùn)動(dòng)坐標(biāo)系，即機(jī)器人將要開始運(yùn)動(dòng)時(shí)，以機(jī)器人質(zhì)心為原點(diǎn)，機(jī)器人將要行進(jìn)的速度方向?yàn)閤軸正向，垂直x軸逆時(shí)針方向?yàn)閥軸正向構(gòu)建的坐標(biāo)系；

B)在全局坐標(biāo)系下，將機(jī)器人軌跡跟蹤構(gòu)建成最優(yōu)化問題，即代價(jià)函數(shù)B:＝{X,U}；γ_k包括γ_path1、γ_path2、γ_obstacle1、γ_obstacle2、γ_goal、γ_velocity，分別對(duì)應(yīng)為各個(gè)子代價(jià)函數(shù)為f_path1、f_path2、f_obstacle1、f_obstacle2、f_goal、f_velocity的權(quán)重，其中，γ_obstacle1、γ_goal、γ_velocity為設(shè)定的固定權(quán)重，γ_path1、γ_path2、γ_obstacle2為自適應(yīng)變動(dòng)的權(quán)重：

d_omin表示當(dāng)前位置距離各個(gè)障礙物的最小距離，γ_path1,min和γ_path1,max分別表示權(quán)重γ_path1的最小值和最大值，γ_path2,max和γ_path1,min分別表示權(quán)重γ_path2的最大值和最小值，γ_obstacle2min表示權(quán)重γ_obstacle2的最小值，D_s為設(shè)定的閾值，K_c為設(shè)定的參數(shù)，a為設(shè)定的參數(shù)；

f_k(B)表示各個(gè)子代價(jià)函數(shù)，各子代價(jià)函數(shù)具體形式如下：

f_path1＝e_Γ(d_path,min,r_pmax,ε,S,n)；

f_path2＝d_path,min；

f_obstacle1＝e_Γ(-d^p_omin,-r_omin,ε,S,n)；

f_obstacle2＝d^p_omin；

其中，d_{path, min}代表求解出的軌跡序列中點(diǎn)與全局路徑序列中相應(yīng)點(diǎn)的距離中的最小值，r_pmax代表最大容許跟蹤誤差；d^p_omin代表求解出的軌跡序列中點(diǎn)與障礙物距離中的最小值，r_omin代表求解出的容許機(jī)器人靠近障礙物的最短距離；e_Γ(b,b_r,ε,S,n)為約束函數(shù)，b代表目標(biāo)變量，b_r代表期望目標(biāo)變量的上限，S、n、ε為人為設(shè)定參數(shù)；x_goal、y_goal是機(jī)器人終點(diǎn)位置坐標(biāo)，x_i、y_i是相應(yīng)的機(jī)器人位置坐標(biāo)，θ_i是相應(yīng)的機(jī)器人航向角；v代表優(yōu)化問題求解出的機(jī)器人局部軌跡行駛的平均速度；

所述優(yōu)化問題的約束如下：

0≤v_i≤v_max；

-ω_max≤ω_i≤ω_max；

其中，ΔT表示機(jī)器人控制周期，v_i,ω_i分別為機(jī)器人從路徑點(diǎn)i-1到路徑點(diǎn)i的速度和角速度，a_i,α_i分別為機(jī)器人從路徑點(diǎn)i-1到路徑點(diǎn)i點(diǎn)加速度與角加速度，a_max,α_max分別為機(jī)器人加速度和角加速度的上限；

3)將求解得到的速度、角速度序列的前m個(gè)值U_m^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下發(fā)給機(jī)器人執(zhí)行器；

4)若機(jī)器人到達(dá)終點(diǎn)，則停止；否則跳轉(zhuǎn)至步驟2)。