[發(fā)明專利]一種基于慣性模塊的移動(dòng)機(jī)器人力控算法

有效

• 申請(qǐng)?zhí)枺?/td>: 201710245895.9
• 申請(qǐng)日: 2017-04-14
• 公開(kāi)（公告）號(hào): CN106914899B
• 公開(kāi)（公告）日: 2019-06-25
• 發(fā)明（設(shè)計(jì)）人: 劉召;宋立濱;于濤;陳懇;劉莉;陳洪安
• 申請(qǐng)（專利權(quán)）人: 清研華宇智能機(jī)器人（天津）有限責(zé)任公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 天津?yàn)I海科緯知識(shí)產(chǎn)權(quán)代理有限公司 12211
• 代理人: 李成運(yùn)

本發(fā)明提供了一種基于慣性模塊的移動(dòng)機(jī)器人力控算法，利用安裝在機(jī)器人腰部的慣性模塊獲取機(jī)器人的姿態(tài)角；根據(jù)獲得的姿態(tài)角判斷機(jī)器人的速度，將速度給機(jī)器人，控制機(jī)器人運(yùn)動(dòng)。本發(fā)明使用成本低的彈簧加慣性裝置模塊實(shí)現(xiàn)對(duì)外力的大小和方向的判斷，進(jìn)而識(shí)別人的意圖，實(shí)現(xiàn)外力控制；在慣性裝置測(cè)量的機(jī)器人傾角變化到速度關(guān)系映射過(guò)程中，采用結(jié)合機(jī)器人歷史及當(dāng)前狀態(tài)的判斷方法，有效去除了彈簧的固有阻尼振蕩；在慣性裝置測(cè)量的機(jī)器人傾角變化到速度關(guān)系映射過(guò)程中，加入了運(yùn)動(dòng)學(xué)限制，將機(jī)器人的速度限制在一個(gè)范圍內(nèi)，一方面避免了運(yùn)動(dòng)中彈簧抖動(dòng)的誤判，另一方面避免剛度相對(duì)較小的彈簧，因形變較大而造成的速度過(guò)快。

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

本發(fā)明創(chuàng)造屬于移動(dòng)機(jī)器人協(xié)作控制領(lǐng)域，尤其是涉及一種基于慣性模塊的移動(dòng)機(jī)器人力控算法。

背景技術(shù)

自傳統(tǒng)工業(yè)機(jī)器人誕生以來(lái)，機(jī)器人技術(shù)在短短的60年內(nèi)便取得了顯著的進(jìn)步。如今，機(jī)器人已經(jīng)不再受限于工業(yè)應(yīng)用，它的研究重點(diǎn)和發(fā)展方向更趨向于日常生活的應(yīng)用，如餐飲、迎賓、娛樂(lè)、陪伴等。同時(shí)，機(jī)器人技術(shù)的發(fā)展也對(duì)人機(jī)協(xié)作的要求也越來(lái)越高，也就是說(shuō)，機(jī)器人不僅能夠?qū)崿F(xiàn)自主移動(dòng)，更要可以響應(yīng)人的作用力，與人互動(dòng)，從而增強(qiáng)機(jī)器人的智能性和趣味性。

目前，市場(chǎng)上絕大多數(shù)的移動(dòng)機(jī)器人并不能很好的與人互動(dòng)，無(wú)法響應(yīng)外力的作用；而目前已有的人機(jī)協(xié)作機(jī)器人控制系統(tǒng)中大多采用多維力傳感器來(lái)感受外力的大小和方向，以此作為控制算法的輸入。這種控制系統(tǒng)對(duì)外力的感知簡(jiǎn)便、準(zhǔn)確，但價(jià)格十分昂貴。

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

有鑒于此，本發(fā)明創(chuàng)造旨在提出一種基于慣性模塊的移動(dòng)機(jī)器人力控算法，通過(guò)感知機(jī)器人上身相對(duì)于穩(wěn)定姿態(tài)的角度變化，同時(shí)結(jié)合機(jī)器人歷史狀態(tài)來(lái)判斷機(jī)器人的受力大小和方向，識(shí)別人的意圖。

為達(dá)到上述目的，本發(fā)明創(chuàng)造的技術(shù)方案是這樣實(shí)現(xiàn)的：

一種基于慣性模塊的移動(dòng)機(jī)器人力控算法，具體包括如下步驟：

(1)利用安裝在機(jī)器人腰部的慣性模塊獲取機(jī)器人的姿態(tài)角；

(2)根據(jù)獲得的姿態(tài)角結(jié)合機(jī)器人歷史及當(dāng)前狀態(tài)將角速度映射到速度，得到速度值，將速度給機(jī)器人，控制機(jī)器人運(yùn)動(dòng)。

進(jìn)一步的，所述步驟(1)所述姿態(tài)角包括俯仰角和橫轉(zhuǎn)角，還包括對(duì)獲得的姿態(tài)角進(jìn)行中位值平均濾波，方法如下：

采集N₁個(gè)數(shù)據(jù)，去掉其中最大值和最小值，并對(duì)剩余的N₁-2個(gè)數(shù)據(jù)取平均值。

進(jìn)一步的，所述步驟(1)還包括對(duì)獲得的姿態(tài)角進(jìn)行去除零點(diǎn)周圍波動(dòng)，方法如下：

若連續(xù)N₂次相鄰兩個(gè)值的變化均不大于某一值，取這N₂次數(shù)據(jù)的平均值作為當(dāng)前靜止穩(wěn)態(tài)值。

進(jìn)一步的，所述步驟(2)具體包括如下步驟：

(21)機(jī)器人初始處于靜止?fàn)顟B(tài)，當(dāng)某一姿態(tài)角θ小于響應(yīng)閾值θ₁時(shí)，與該姿態(tài)角對(duì)應(yīng)的速度v＝0；當(dāng)θ₁≤θ≤θ_max時(shí)，速度與姿態(tài)角成函數(shù)關(guān)系，即v＝f₁(θ)，并且v_max＝f₁(θ_max)，其中，f₁(θ)＝ke^b(θ+a)+c，式中的固定參數(shù)通過(guò)實(shí)驗(yàn)選取；當(dāng)|θ|＞|θ_max|時(shí)，機(jī)器人該方向上速度不再變化；

(22)當(dāng)與姿態(tài)角對(duì)應(yīng)的速度不再變化時(shí)，姿態(tài)角接下來(lái)的變化只有兩種可能，一種是失去外力，彈簧經(jīng)歷阻尼震蕩后恢復(fù)靜止?fàn)顟B(tài)，另一種則是對(duì)彈簧施加反相作用力：