[發(fā)明專利]機(jī)器人路徑跟蹤方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202210187181.8
• 申請(qǐng)日: 2022-02-28
• 公開（公告）號(hào): CN114779754A
• 公開（公告）日: 2022-07-22
• 發(fā)明（設(shè)計(jì)）人: 華逢彬;馬辰;高明
• 申請(qǐng)（專利權(quán)）人: 山東新一代信息產(chǎn)業(yè)技術(shù)研究院有限公司
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 濟(jì)南泉城專利商標(biāo)事務(wù)所 37218
• 代理人: 劉德

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

S1.獲取機(jī)器人的全局路徑，根據(jù)這段行走的距離判斷為直線模式或圓弧模式；

S2.直線模式采用pid控制算法來進(jìn)行線速度與角速度的控制；

S3.在圓弧模式下行走，利用貝塞爾曲線對(duì)轉(zhuǎn)彎點(diǎn)處進(jìn)行插值；

S4.計(jì)算機(jī)器人當(dāng)前位置到局部目標(biāo)點(diǎn)之間擬合弧線的相對(duì)曲率；

S5.在直線行走的過程中，不停地判斷機(jī)器人有沒有接近當(dāng)前的轉(zhuǎn)彎點(diǎn)位置，如果到達(dá)當(dāng)前的轉(zhuǎn)彎點(diǎn)的前2米，那么減速；等到達(dá)轉(zhuǎn)彎點(diǎn)位置后，停止運(yùn)動(dòng)，開始旋轉(zhuǎn)，使機(jī)器人朝向新的局部目標(biāo)點(diǎn)集，期間不停地判斷機(jī)器人有沒有到達(dá)轉(zhuǎn)彎點(diǎn)的位姿，如果滿足條件，那么開始加速前進(jìn)，不停地判斷機(jī)器人有沒有到達(dá)最終的目標(biāo)點(diǎn)，同樣如果到達(dá)目標(biāo)點(diǎn)前m米，減速，最后安全到達(dá)終點(diǎn)位置；

S6.在圓弧行走過程中，不停地判斷機(jī)器人有沒有到達(dá)最終的目標(biāo)點(diǎn)位置，如果到達(dá)目標(biāo)點(diǎn)前m米，那么減速，直到安全到達(dá)點(diǎn)位置后停止，在拐弧線模式下，機(jī)器人在拐彎處會(huì)根據(jù)當(dāng)前路徑的彎曲程度d，計(jì)算出一組線速度和角速度驅(qū)動(dòng)機(jī)器人過彎。

2.根據(jù)權(quán)利要求1所述機(jī)器人路徑跟蹤方法，其特征在于：步驟S1具體過程如下：

S11.根據(jù)機(jī)器人當(dāng)前位置(x,y)向前搜索5米的距離，取出這段距離中所有的點(diǎn)來進(jìn)行判斷；

S12.將這段距離中的點(diǎn)位(x_i,y_i)與(x_i+1,y_i+1)進(jìn)行計(jì)算，保存記錄兩個(gè)點(diǎn)之間的斜率

S13.遍歷這個(gè)兩點(diǎn)之間的斜率，當(dāng)前后n個(gè)k值保持相同的斜率范圍內(nèi)是，則這段行走的距離模式可判斷為直線模式，當(dāng)連續(xù)k＞α后，則這段行走的距離為圓弧模式，α為斜率閾值。

3.根據(jù)權(quán)利要求1所述機(jī)器人路徑跟蹤方法，其特征在于：步驟S2具體過程如下：

S21.在機(jī)器人當(dāng)前位置以初始速度V向下一個(gè)點(diǎn)位前進(jìn)，實(shí)時(shí)獲取機(jī)器人當(dāng)前位置，判斷機(jī)器人在計(jì)算兩點(diǎn)(x₁,y₁)與(x₂,y₂)之間的距離D及偏角β，并將直線的點(diǎn)擬合成直線，具體公式如下：

(x₁,y₁)與(x₂,y₂)兩個(gè)位姿之間的距離；

β為(x₁,y₁)與(x₂,y₂)兩個(gè)位姿的斜率；

擬合直線：Ax+By+C＝0，其中(x₁,y₁)為當(dāng)前位置坐標(biāo)，(x₂,y₂)為目標(biāo)點(diǎn)的位置坐標(biāo)，；

S22.機(jī)器人行走中，求取機(jī)器人位置(x₀,y₀)距離直線Ax+By+C＝0的距離：

S23.當(dāng)距離小于閾值a時(shí)，機(jī)器人按照固定的速度行駛，當(dāng)閾值大于a時(shí)，角速度ω根據(jù)距離D來進(jìn)行調(diào)整，

k為比系數(shù)，糾正轉(zhuǎn)角ω來靠近直線，按照速度V前進(jìn)，知道終點(diǎn)位置(x_e,y_e)。

4.根據(jù)權(quán)利要求1所述機(jī)器人路徑跟蹤方法，其特征在于：步驟S3具體過程如下：

S31.塞爾曲線公式P＝(1-t)²P0+2t(1-t)P1+t²P2，設(shè)置轉(zhuǎn)彎距離λ，根據(jù)轉(zhuǎn)彎點(diǎn)的下標(biāo)和λ，計(jì)算控制點(diǎn)c_i1，c_i2的下標(biāo)；

S32.輸入全局路徑、轉(zhuǎn)彎點(diǎn)和兩個(gè)控制點(diǎn)c_i1，c_i2；

S33.采用分段讀取的方法獲取所有的插值點(diǎn)和原路徑點(diǎn)，不包含被插值的點(diǎn)，具體如下：總共段數(shù)為L＝2×i+1，迭代次數(shù)為L次，當(dāng)L為偶數(shù)，則裝入直線段；當(dāng)L為奇數(shù)，則裝入弧線段，即插值點(diǎn)。

5.根據(jù)權(quán)利要求1所述機(jī)器人路徑跟蹤方法，其特征在于：步驟S4具體過程如下：

S41.利用純追蹤模型跟蹤預(yù)設(shè)的全局路徑，根據(jù)當(dāng)前速度計(jì)算前向距離，設(shè)置最大前向距離lookaheadmax＝v×t+ζ和最小前向距離lookaheadmin＝v×t-ζ，式中t為模擬時(shí)間、ζ為增益系數(shù)；

S42.根據(jù)前向距離和當(dāng)前機(jī)器人的位置，在局部目標(biāo)點(diǎn)集中找到當(dāng)前時(shí)刻需要跟蹤的局部目標(biāo)點(diǎn)，計(jì)算機(jī)器人當(dāng)前位置到局部目標(biāo)點(diǎn)之間擬合弧線的相對(duì)曲率即當(dāng)前點(diǎn)與選擇的前向點(diǎn)之間距離生成的直線的最遠(yuǎn)距離：

參考點(diǎn)(x₀,y₀),前向點(diǎn)(x₁,y₁)，在點(diǎn)的集合set{(x₀,y₀),......(x₁,y₁)}中，取最大值為圓弧的相對(duì)曲率：

兩點(diǎn)之間的直線表達(dá)式：y-y₁＝[(y₀-y₁)/(x₀-x₁)]*(x-x₁)

其中(x_i,y_i)為集合set中的點(diǎn)，max的取值的下標(biāo)i即為轉(zhuǎn)彎點(diǎn)；

S43.計(jì)算當(dāng)前時(shí)刻機(jī)器人的角速度ω＝t*v*γ，t為仿真時(shí)間，v為速度閾值，γ為增益系數(shù)。