[發(fā)明專利]一種機(jī)械臂運(yùn)動(dòng)規(guī)劃方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202011132260.6
• 申請(qǐng)日: 2020-10-21
• 公開（公告）號(hào): CN112549016A
• 公開（公告）日: 2021-03-26
• 發(fā)明（設(shè)計(jì)）人: 張蕾;李揚(yáng);宋勇;王曉華;王文杰
• 申請(qǐng)（專利權(quán)）人: 西安工程大學(xué)
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16;B25J18/00
• 代理公司: 西安弘理專利事務(wù)所 61214
• 代理人: 燕肇琪

本發(fā)明公開了一種機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，具體按照以下步驟實(shí)施：步驟1、利用常見的空間規(guī)則物體近似包絡(luò)障礙物進(jìn)行障礙物建模；步驟2、給定機(jī)械臂起始位置與目標(biāo)位置，通過逆運(yùn)動(dòng)學(xué)方法獲得目標(biāo)位置的逆解；步驟3、通過p?rrt算法計(jì)算出一條軌跡；步驟4、導(dǎo)入七自由度機(jī)械臂模型并給定機(jī)械臂目標(biāo)位置，通過逆運(yùn)動(dòng)學(xué)得到目標(biāo)位置逆解；將得到的軌跡的路徑節(jié)點(diǎn)作為輸入并用B樣條曲線對(duì)該路徑進(jìn)行平滑優(yōu)化處理；步驟5，在Matlab中進(jìn)行仿真實(shí)驗(yàn)，加入B樣條曲線優(yōu)化后的仿真對(duì)比，對(duì)比了平滑前后的路徑曲線，驗(yàn)證有效性，完成械臂運(yùn)動(dòng)規(guī)劃，解決了現(xiàn)有技術(shù)中存在的規(guī)劃路徑平滑度差的問題。

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

本發(fā)明屬于機(jī)械臂運(yùn)動(dòng)規(guī)劃技術(shù)領(lǐng)域，涉及一種機(jī)械臂運(yùn)動(dòng)規(guī)劃方法。

背景技術(shù)

機(jī)器人的運(yùn)動(dòng)規(guī)劃問題是機(jī)器人研究領(lǐng)域中的重要問題，其主要由路徑規(guī)劃和軌跡規(guī)劃(時(shí)間)組成，連接起起點(diǎn)位置和目標(biāo)點(diǎn)位置的序列點(diǎn)或曲線稱之為路徑，構(gòu)成路徑的策略即路徑規(guī)劃。

路徑規(guī)劃是要找到一系列要經(jīng)過的路徑點(diǎn)，路徑點(diǎn)是空間中的位置或關(guān)節(jié)角度，而軌跡規(guī)劃是賦予路徑以時(shí)間信息。運(yùn)動(dòng)規(guī)劃是在給定的路徑端點(diǎn)之間插入用于控制的中間點(diǎn)序列從而實(shí)現(xiàn)沿給定的路徑平穩(wěn)運(yùn)動(dòng)。軌跡規(guī)劃在路徑規(guī)劃的基礎(chǔ)上加入時(shí)間序列信息，對(duì)機(jī)器人執(zhí)行任務(wù)時(shí)的速度與加速度進(jìn)行規(guī)劃，以滿足光滑性和速度可控性等要求。

采用快速擴(kuò)展隨機(jī)樹(Rapidly exploring Random Tree)路徑規(guī)劃算法，通過對(duì)狀態(tài)空間中的采樣點(diǎn)進(jìn)行碰撞檢測(cè)，避免了對(duì)空間的建模，能夠有效地解決機(jī)械臂在高維空間和復(fù)雜環(huán)境下的運(yùn)動(dòng)規(guī)劃問題。但傳統(tǒng)的RRT算法導(dǎo)向性較差，規(guī)劃空間增大時(shí)，存在所規(guī)劃的路徑平滑度差，且算法時(shí)間復(fù)雜度高的問題。

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

本發(fā)明的目的是提供一種機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，決了現(xiàn)有技術(shù)中存在的規(guī)劃路徑平滑度差的問題。

本發(fā)明所采用的技術(shù)方案是，一種機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，具體按照以下步驟實(shí)施：

步驟1、利用常見的空間規(guī)則物體近似包絡(luò)障礙物進(jìn)行障礙物建模；

步驟2、利用步驟1建立的模型，給定機(jī)械臂起始位置與目標(biāo)位置，通過逆運(yùn)動(dòng)學(xué)方法獲得目標(biāo)位置的逆解；

步驟3、利用步驟2得到的逆解的值通過p-rrt算法計(jì)算出一條軌跡；

步驟4、導(dǎo)入七自由度機(jī)械臂模型并給定機(jī)械臂目標(biāo)位置，通過逆運(yùn)動(dòng)學(xué)得到目標(biāo)位置逆解；將步驟3得到的軌跡的路徑節(jié)點(diǎn)作為輸入并用B樣條曲線對(duì)該路徑進(jìn)行平滑優(yōu)化處理；

步驟5，根據(jù)步驟4可以得到機(jī)械臂平滑后的路徑曲線，在保證對(duì)機(jī)械臂運(yùn)動(dòng)不與障礙物發(fā)生碰撞的前提下，在Matlab中進(jìn)行仿真實(shí)驗(yàn)，P加入B樣條曲線優(yōu)化后的仿真對(duì)比，對(duì)比了平滑前后的路徑曲線，驗(yàn)證有效性，完成械臂運(yùn)動(dòng)規(guī)劃。

本發(fā)明的特點(diǎn)還在于：

步驟1具體按照以下實(shí)施：獲取障礙物的大小、位置信息，設(shè)置碰撞檢測(cè)方案；為了避免機(jī)械臂各連桿與障礙物發(fā)生碰撞，在將障礙物包絡(luò)成球體后，需計(jì)算球形障礙物和機(jī)械臂連桿之間的空間位置關(guān)系，若機(jī)械臂連桿與球形障礙物圓心的距離小于半徑之和即機(jī)械臂連桿和障礙物有交集部分，則會(huì)發(fā)生碰撞，其余情況則表示不會(huì)發(fā)生碰撞。

步驟3具體按照以下步驟實(shí)施：

步驟3.1，在地圖環(huán)境中進(jìn)行隨機(jī)采樣并確定采樣點(diǎn)x_rand，找到距離x_rand最近的節(jié)點(diǎn)x_near；

步驟3.2，確定隨機(jī)點(diǎn)邊界并根據(jù)概率函數(shù)P的值來選擇樹的生長(zhǎng)方向是朝向目標(biāo)點(diǎn)方向還是隨機(jī)方向；此處設(shè)置的概率為有0.5概率直行；

步驟3.3，沿著x_near到x_rand方向前進(jìn)stepsize的距離得到x_new；