[發(fā)明專利]一種基于三維點云模型的智能消防機(jī)器人控制方法

在審

• 申請?zhí)枺?/td>: 202110285775.8
• 申請日: 2021-03-17
• 公開（公告）號: CN113101584A
• 公開（公告）日: 2021-07-13
• 發(fā)明（設(shè)計）人: 湯錦慧;伍發(fā)元;肖齊;辛建波;劉專;毛夢婷
• 申請（專利權(quán)）人: 國網(wǎng)江西省電力有限公司電力科學(xué)研究院;國網(wǎng)江西省電力有限公司;國家電網(wǎng)有限公司
• 主分類號: A62C37/00
• 分類號: A62C37/00;A62C27/00
• 代理公司: 南昌豐擇知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 36137
• 代理人: 吳稱生

1.基于三維點云模型的智能消防機(jī)器人控制方法，其特征在于步驟如下：

步驟1、通過三維激光雷達(dá)現(xiàn)場掃描建立現(xiàn)場三維點云模型；

步驟2、建立機(jī)器人坐標(biāo)系和三維點云模型的映射關(guān)系；

步驟3、將紅外相機(jī)捕獲到畫面的異常升溫點與三維點云模型進(jìn)行比對，提取異常升溫點坐標(biāo)；

步驟4、確定機(jī)器人末端期待位姿，建立齊次坐標(biāo)矩陣；

步驟5、使用Pieper方法求解機(jī)器人各軸關(guān)節(jié)角度，選擇最小移動角度為最優(yōu)解；

步驟6、控制機(jī)器人移動至相應(yīng)位置，進(jìn)行降溫，直至溫度降至告警閾值以下；

步驟7、結(jié)束降溫，機(jī)器人復(fù)位。

2.根據(jù)權(quán)利要求1所述的基于三維點云模型的智能消防機(jī)器人控制方法，其特征在于，步驟2中，機(jī)器人上電，將立柱電機(jī)降至立柱最底部；通過示教器上的按鈕調(diào)整各軸，使得機(jī)器人第二軸第三軸間的連桿垂直于立柱，第三軸第四軸連桿與立柱平面，第六軸的軸線z方向指向世界坐標(biāo)系z軸的反方向，將此位置設(shè)置為機(jī)械臂各軸電機(jī)原點；將機(jī)械臂調(diào)整至最佳啟動位姿待機(jī)，記錄并保存此時機(jī)械臂位姿的關(guān)節(jié)偏移與世界坐標(biāo)；確定立柱電機(jī)底座為三維點云模型的原點，六軸機(jī)械臂底座為機(jī)器人坐標(biāo)系原點，建立機(jī)器人坐標(biāo)系與三維點云模型的映射關(guān)系。

3.根據(jù)權(quán)利要求1所述的基于三維點云模型的智能消防機(jī)器人控制方法，其特征在于，步驟4具體如下：

計算六軸機(jī)器人的正運(yùn)動學(xué)，由于機(jī)器人各關(guān)節(jié)參數(shù)已知，可求解其末端坐標(biāo)系{E}相對于基坐標(biāo)系{B}的變換映射；

其中，關(guān)節(jié)坐標(biāo)系到的變換關(guān)系為：

在上式中，i為機(jī)器人各軸的編號，其取值范圍為1到6；英文字母 為 連桿長度，表示關(guān)節(jié)軸i-1和關(guān)節(jié)軸i之間的公共法線長度；希臘字母為連桿扭轉(zhuǎn)，表示關(guān)節(jié)軸i相對于關(guān)節(jié)軸i-1繞它們的公共法線旋轉(zhuǎn)的角度；為連桿偏移，表示關(guān)節(jié)i與關(guān)節(jié)i+1的公共法線和它與關(guān)節(jié)i-1的公共法線沿這個關(guān)節(jié)軸的距離；為關(guān)節(jié)轉(zhuǎn)角，表示關(guān)節(jié)i與關(guān)節(jié)i+1的公共法線和它與關(guān)節(jié)i-1的公共法線繞這個關(guān)節(jié)軸的轉(zhuǎn)角；表示坐標(biāo)軸旋轉(zhuǎn)角度的姿態(tài)描述；表示坐標(biāo)軸旋轉(zhuǎn)角度的姿態(tài)描述；表示沿著坐標(biāo)軸進(jìn)行長度的平移變換；表示沿著坐標(biāo)軸進(jìn)行長度的平移變換；

其一般表達(dá)式為：

上式中，，，，各參數(shù)含義均與（1）式相同；將所有六個關(guān)節(jié)相鄰關(guān)節(jié)之間的變換映射由上式表示，即可獲得由末端關(guān)節(jié)坐標(biāo){E}系到基座標(biāo)系{B}的映射關(guān)系:

其中、、、、、分別表示其下表所示坐標(biāo)系到上標(biāo)所示坐標(biāo)系的齊次坐標(biāo)變換矩陣；

期望末端齊次變換矩陣的確定：在所建世界坐標(biāo)系與機(jī)器人坐標(biāo)系的基礎(chǔ)上，通過紅外相機(jī)獲得機(jī)器人可達(dá)空間外異常升溫點與基坐標(biāo)系原點連線交機(jī)器人可達(dá)空間球面于一點，交點即為期望末端位置坐標(biāo)點，作為平移向量計算的依據(jù)，取該交點處球面的法線作為末端坐標(biāo)系z軸的正方向，對末端坐標(biāo)系x軸y軸進(jìn)行與末端z軸相同的旋轉(zhuǎn)變換可得期望旋轉(zhuǎn)矩陣，至此，完成對機(jī)器人末端期望齊次變換矩陣的確定；

1)直線方程的確定：

首先是確定空間直線的方程，取機(jī)器人可達(dá)空間外一點和機(jī)器人基坐標(biāo)系原點構(gòu)造空間直線方程如下：

其中，為參變量，為基坐標(biāo)系原點 ，空間直線的方向向量可由機(jī)器人可達(dá)空間外一點即異常升溫點坐標(biāo)和機(jī)器人基坐標(biāo)系原點共同確定：

故最終確定的直線方程為：

而機(jī)器人可達(dá)空間的球面方程為：

其中R為六軸機(jī)器人可達(dá)空間最外層球殼的半徑；

2)聯(lián)立求交點：將上述直線方程同球面方程聯(lián)立，可得關(guān)于參變量t的一元二次方程：

解出t=±R，帶入(6)式，即可求得由異常升溫點和機(jī)器人基座標(biāo)系原點確定的空間直線與機(jī)器人可達(dá)空間球面方程的交點坐標(biāo)在機(jī)器人基座標(biāo)系中的表示；

末端位姿的確定：由式(5)，可得機(jī)器人末端z軸方向向量，分別求其關(guān)于基座標(biāo)系平面的投影及平面的投影，以同樣的方法求得機(jī)器人初始位姿下末端z軸分量在基坐標(biāo)系下平面的投影及平面的投影，即可求得由初始位姿z軸到期望位姿z軸的旋轉(zhuǎn)矩陣，令初始位姿機(jī)器人末端x軸、y軸方向向量進(jìn)行相同的旋轉(zhuǎn)變換，即可獲得期望的末端x軸、y軸方向向量，完成對末端期望齊次變換矩陣的確定。