[發(fā)明專利]一種移動(dòng)機(jī)器人平滑回歸給定軌跡的路徑規(guī)劃方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 202210083604.1
• 申請(qǐng)日: 2022-01-25
• 公開（公告）號(hào): CN114115299B
• 公開（公告）日: 2022-04-22
• 發(fā)明（設(shè)計(jì)）人: 張瑤港;趙越
• 申請(qǐng)（專利權(quán)）人: 上海仙工智能科技有限公司
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 北京中濟(jì)緯天專利代理有限公司 11429
• 代理人: 季永康

本發(fā)明提供的該移動(dòng)機(jī)器人平滑回歸給定軌跡的路徑規(guī)劃方法及裝置，通過五次多項(xiàng)式生成回歸路徑，克服了給定軌跡與回歸路徑在連接點(diǎn)處曲率不連續(xù)的情況，有利于移動(dòng)機(jī)器人平滑的回歸到給定軌跡。其次通過找到移動(dòng)機(jī)器人距離給定軌跡最小距離的索引，并在該索引到給定軌跡終點(diǎn)索引的區(qū)間內(nèi)尋找最優(yōu)回歸路徑，從而確保回歸路徑的行駛方向與給定軌跡的行駛方向一致，再而通過設(shè)置松弛變量和可行解集的方式找到最優(yōu)的回歸路徑，保證了回歸路徑的曲率和最小以及路徑長度最短，同時(shí)滿足最大曲率限制。

技術(shù)領(lǐng)域

本發(fā)明涉及移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)領(lǐng)域，尤其涉及移動(dòng)機(jī)器人基于五次多項(xiàng)式從給定軌跡外平滑回歸到給定軌跡的路徑規(guī)劃方法及裝置。

背景技術(shù)

當(dāng)移動(dòng)機(jī)器人接收指令前往目的地的過程中，需要根據(jù)預(yù)先計(jì)算的路徑規(guī)劃進(jìn)行移動(dòng)，然而在實(shí)際應(yīng)用中，預(yù)先規(guī)劃的路徑上往往不可避免的會(huì)存在障礙物，使得移動(dòng)機(jī)器人不得不暫時(shí)偏離預(yù)定路徑軌跡以避障。如在智能倉儲(chǔ)領(lǐng)域，該移動(dòng)機(jī)器人可以是倉庫中的無人倉儲(chǔ)車，其按照給定的軌跡進(jìn)行行駛是實(shí)際工作中必不可少的一部分，尤其是按照給定軌跡運(yùn)送貨物。

但是由于很多原因，無人倉儲(chǔ)車并不能確保每時(shí)每刻都在給定的軌跡上，所以當(dāng)倉儲(chǔ)車不在給定軌跡上時(shí)，如何快速且最優(yōu)的回歸到給定軌跡，這是一個(gè)非常重要的問題，不僅關(guān)系到能否準(zhǔn)確的將貨物運(yùn)送到指定點(diǎn)，還關(guān)系到倉儲(chǔ)管理人員的安全問題，因?yàn)闊o人倉儲(chǔ)車如果不按照給定軌跡行駛時(shí)，倉庫的管理人員將不得不使用額外的精力來留意無人倉儲(chǔ)車。這一方面，會(huì)影響工作人員的工作，同時(shí)會(huì)給工作人員帶來額外的安全隱患。

目前針對(duì)于在給定軌跡外平滑回歸到給定軌跡的問題，業(yè)內(nèi)并沒有統(tǒng)一的標(biāo)準(zhǔn)和較好的標(biāo)桿解決方案。然而發(fā)明人認(rèn)為在當(dāng)前技術(shù)條件下，如果能在確保回歸路徑與給定軌跡在連接點(diǎn)曲率連續(xù)的前提下，通過對(duì)回歸路徑的最大曲率、曲率和以及回歸路徑的長度等約束來找到最優(yōu)的回歸點(diǎn)，可能是引導(dǎo)此類工作條件下的無人倉儲(chǔ)車回歸到給定軌跡的較優(yōu)方式。

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

本發(fā)明的主要目的在于提供一種移動(dòng)機(jī)器人平滑回歸給定軌跡的路徑規(guī)劃方法，以實(shí)現(xiàn)背景技術(shù)中發(fā)明人提出的方案。

為了實(shí)現(xiàn)上述目的，根據(jù)本發(fā)明的一個(gè)方面，提供了一種移動(dòng)機(jī)器人平滑回歸給定軌跡的路徑規(guī)劃方法，其步驟包括：

S1 路徑規(guī)劃裝置接收移動(dòng)機(jī)器人的位姿與給定軌跡信息，采用啟發(fā)式快速隨機(jī)迭代算法以生成移動(dòng)機(jī)器人距離給定軌跡最近點(diǎn)的索引，其步驟包括：

S11 在任意一個(gè)有限區(qū)間，在起始區(qū)間內(nèi)隨機(jī)生成n個(gè)點(diǎn)，且n大于2，將自變量索引最小的點(diǎn)的索引值賦給Left，自變量索引最大的點(diǎn)的索引值賦給Right，通過比較n個(gè)隨機(jī)點(diǎn)的代價(jià)值的大小，找出代價(jià)值最小的索引MinCostIndex，根據(jù)MinCostIndex所在的位置對(duì)區(qū)間進(jìn)行收縮；

S12 循環(huán)執(zhí)行步驟S11，直至整個(gè)迭代區(qū)間長度小于預(yù)設(shè)值，以生成移動(dòng)機(jī)器人距離給定軌跡最近點(diǎn)的索引；

S2 路徑規(guī)劃裝置從步驟S1的索引開始遍歷，根據(jù)位姿信息及遍歷到的給定軌跡的點(diǎn)以五次多項(xiàng)式參數(shù)方程形式生成路徑；

S3路徑規(guī)劃裝置對(duì)步驟S2生成的路徑的最大曲率進(jìn)行判斷，當(dāng)回歸路徑的最大曲率小于1/Rmin時(shí)，Rmin為最小曲率半徑，將路徑加入到可行解集中；

S4 路徑規(guī)劃裝置計(jì)算步驟S3中各可行解集的回歸路徑長度，以選擇最短的回歸路徑作為最優(yōu)回歸路徑。

在可能的優(yōu)選實(shí)施方式中，步驟S11中根據(jù)MinCostIndex所在的位置對(duì)區(qū)間進(jìn)行收縮的步驟包括：