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

在審

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

1.一種機(jī)器人關(guān)節(jié)角度測(cè)量方法，其特征在于，所述機(jī)器人關(guān)節(jié)包括相連的第一連桿和第二連桿，在所述第一連桿和/或所述第二連桿上安裝姿態(tài)傳感器，由所述姿態(tài)傳感器返回的歐拉角或四元數(shù)測(cè)算所述機(jī)器人關(guān)節(jié)角度；所述歐拉角包括俯仰角、橫滾角及航向角。

2.根據(jù)權(quán)利要求1所述的一種機(jī)器人關(guān)節(jié)角度測(cè)量方法，其特征在于，所述第一連桿和所述第二連桿活動(dòng)連接，在所述第一連桿上固定安裝第一姿態(tài)傳感器，在所述第二連桿上固定安裝第二姿態(tài)傳感器；所述第一姿態(tài)傳感器和所述第二姿態(tài)傳感器分別返回第一歐拉角和第二歐拉角，根據(jù)所述第一歐拉角和所述第二歐拉角測(cè)算所述機(jī)器人關(guān)節(jié)角度。

3.根據(jù)權(quán)利要求2所述的一種機(jī)器人關(guān)節(jié)角度測(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é)角度。

4.根據(jù)權(quán)利要求3所述的一種機(jī)器人關(guān)節(jié)角度測(cè)量方法，其特征在于，所述第一歐拉角包括第一橫滾角、第一俯仰角、第一航向角，所述第二歐拉角包括第二橫滾角、第二俯仰角、第二航向角，所述第三歐拉角包括第三橫滾角、第三俯仰角、第三航向角，所述第四歐拉角包括第四橫滾角、第四俯仰角、第四航向角。

5.根據(jù)權(quán)利要求3所述的一種機(jī)器人關(guān)節(jié)角度測(cè)量方法，其特征在于，所述第一歐拉角是所述第一姿態(tài)傳感器與所述慣性坐標(biāo)系之間的夾角，所述第二歐拉角是所述第二姿態(tài)傳感器與所述慣性坐標(biāo)系之間的夾角。

6.根據(jù)權(quán)利要求3所述的一種機(jī)器人關(guān)節(jié)角度測(cè)量方法，其特征在于，所述慣性坐標(biāo)系是固定于地球的大地坐標(biāo)系。