[發(fā)明專利]一種基于姿態(tài)傳感器的機(jī)器人關(guān)節(jié)角度測(cè)量方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710170099.3
• 申請(qǐng)日: 2017-03-21
• 公開（公告）號(hào): CN106996765A
• 公開（公告）日: 2017-08-01
• 發(fā)明（設(shè)計(jì)）人: 曹俊亮
• 申請(qǐng)（專利權(quán)）人: 上海嶺先機(jī)器人科技股份有限公司
• 主分類號(hào): G01B21/22
• 分類號(hào): G01B21/22
• 代理公司: 上海容慧專利代理事務(wù)所(普通合伙)31287
• 代理人: 于曉菁

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

本發(fā)明涉及一種角度測(cè)量方法，特別涉及一種基于姿態(tài)傳感器的機(jī)器人關(guān)節(jié)角度測(cè)量方法，屬于機(jī)器人技術(shù)領(lǐng)域。

背景技術(shù)

近年來(lái)，機(jī)器人技術(shù)不論在工業(yè)還是在民用領(lǐng)域的應(yīng)用都有飛速的發(fā)展，而機(jī)器人各個(gè)關(guān)節(jié)角是機(jī)器人控制的基礎(chǔ)。因此，具有高度通用性的機(jī)器人關(guān)節(jié)角的測(cè)量方法顯得十分必要。

現(xiàn)有的機(jī)器人關(guān)節(jié)角通常使用安裝在關(guān)節(jié)上的角度傳感器直接測(cè)量，其中主要包括霍爾磁性角度傳感器，光學(xué)編碼盤等。這些角度傳感器雖然精度高，但是價(jià)格昂貴，對(duì)安裝方式有極高的要求，所占空間較大，且易受環(huán)境因素干擾。

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

本發(fā)明要解決現(xiàn)有技術(shù)中測(cè)量機(jī)器人關(guān)節(jié)角度存在的以下幾個(gè)技術(shù)問題：(1)傳感器制造成本高的問題；(2)安裝困難的問題；(3)體積大，重量大的問題；(4)易受環(huán)境因素干擾。

為了實(shí)現(xiàn)上述目的，本發(fā)明提供了一種基于姿態(tài)傳感器的機(jī)器人關(guān)節(jié)角度測(cè)量方法，機(jī)器人關(guān)節(jié)包括相連的第一連桿和第二連桿，在第一連桿和/或第二連桿上安裝姿態(tài)傳感器，由姿態(tài)傳感器返回的歐拉角或四元數(shù)測(cè)算機(jī)器人關(guān)節(jié)角度；歐拉角包括俯仰角、橫滾角及航向角。

優(yōu)選地，第一連桿和第二連桿活動(dòng)連接，在第一連桿上固定安裝第一姿態(tài)傳感器，在第二連桿上固定安裝第二姿態(tài)傳感器；第一姿態(tài)傳感器和第二姿態(tài)傳感器分別返回第一歐拉角和第二歐拉角，根據(jù)第一歐拉角和第二歐拉角測(cè)算機(jī)器人關(guān)節(jié)角度。

優(yōu)選地，上述測(cè)量方法包括如下步驟：

步驟一、定義慣性坐標(biāo)系，定義固連于第一姿態(tài)傳感器的第一坐標(biāo)系，定義固連于第二姿態(tài)傳感器的第二坐標(biāo)系；

步驟二、將機(jī)器人關(guān)節(jié)復(fù)位至第一位置，第一姿態(tài)傳感器返回第一歐拉角，第二姿態(tài)傳感器返回第二歐拉角；

步驟三、根據(jù)第一歐拉角計(jì)算第一旋轉(zhuǎn)矩陣，根據(jù)第二歐拉角計(jì)算第二旋轉(zhuǎn)矩陣；再根據(jù)第一旋轉(zhuǎn)矩陣和第二旋轉(zhuǎn)矩陣計(jì)算第一連桿與第二連桿的第一位置相對(duì)旋轉(zhuǎn)矩陣；

步驟四、將機(jī)器人關(guān)節(jié)調(diào)節(jié)至第二位置，第一姿態(tài)傳感器返回第三歐拉角，第二姿態(tài)傳感器返回第四歐拉角；

步驟五、根據(jù)第三歐拉角計(jì)算第三旋轉(zhuǎn)矩陣，根據(jù)第四歐拉角計(jì)算第四旋轉(zhuǎn)矩陣；再根據(jù)第三旋轉(zhuǎn)矩陣和第四旋轉(zhuǎn)矩陣計(jì)算第一連桿與第二連桿的第二位置相對(duì)旋轉(zhuǎn)矩陣；

步驟六、根據(jù)第一位置相對(duì)旋轉(zhuǎn)矩陣和第二位置相對(duì)旋轉(zhuǎn)矩陣計(jì)算旋轉(zhuǎn)矩陣變化量，再根據(jù)旋轉(zhuǎn)矩陣變化量計(jì)算機(jī)器人關(guān)節(jié)角度。

優(yōu)選地，第一歐拉角包括第一橫滾角、第一俯仰角、第一航向角，第二歐拉角包括第二橫滾角、第二俯仰角、第二航向角，第三歐拉角包括第三橫滾角、第三俯仰角、第三航向角，第四歐拉角包括第四橫滾角、第四俯仰角、第四航向角。

優(yōu)選地，第一歐拉角是第一姿態(tài)傳感器與慣性坐標(biāo)系之間的夾角，第二歐拉角是第二姿態(tài)傳感器與慣性坐標(biāo)系之間的夾角。

優(yōu)選地，慣性坐標(biāo)系是固定于地球的大地坐標(biāo)系。

優(yōu)選地，測(cè)量方法包括如下具體步驟：

步驟一、定義坐標(biāo)系：定義慣性坐標(biāo)系為xyz，定義第一坐標(biāo)系為x’y’z’，慣性坐標(biāo)系和第一坐標(biāo)系原點(diǎn)重合為O，慣性坐標(biāo)系和第一坐標(biāo)系之間的轉(zhuǎn)換用歐拉角來(lái)描述，其中N為地磁北極方向；按同樣的方式定義第二坐標(biāo)系為x”y”z”；

步驟二、初始化：將機(jī)器人關(guān)節(jié)復(fù)位至第一位置，記為α₀；此時(shí)第一姿態(tài)傳感器返回第一橫滾角記為φ_1|0，第一俯仰角記為θ_1|0，第一航向角記為ψ_1|0；第二姿態(tài)傳感器返回第二橫滾角記為φ_2|0，第二俯仰角記為θ_2|0，第二航向角記為ψ_2|0；

步驟三、標(biāo)定：計(jì)算第一旋轉(zhuǎn)矩陣并記為¹R₀，計(jì)算第二旋轉(zhuǎn)矩陣記為²R₀，第一位置相對(duì)旋轉(zhuǎn)矩陣表示為通過以下公式計(jì)算：

步驟四、數(shù)據(jù)采集：機(jī)器人運(yùn)動(dòng)至第二位置，第三橫滾角、第三俯仰角、第三航向角、第四橫滾角、第四俯仰角、第四航向角分別記為φ_1|1，θ_1|1，ψ_1|1，φ_2|1，θ_2|1，ψ_2|1；

步驟五、標(biāo)定：計(jì)算第三旋轉(zhuǎn)矩陣記為¹R₁，計(jì)算第四旋轉(zhuǎn)矩陣記為²R₁，通過以下公式計(jì)算第二位置相對(duì)旋轉(zhuǎn)矩陣：