[發(fā)明專利]一種基于固態(tài)面陣激光雷達(dá)的太陽(yáng)位置識(shí)別與規(guī)避方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010911783.4
• 申請(qǐng)日: 2020-09-02
• 公開(kāi)（公告）號(hào): CN112008730B
• 公開(kāi)（公告）日: 2021-08-17
• 發(fā)明（設(shè)計(jì)）人: 程敏;李栗;羅志競(jìng);孫建亞
• 申請(qǐng)（專利權(quán)）人: 億嘉和科技股份有限公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 南京理工信達(dá)知識(shí)產(chǎn)權(quán)代理有限公司 32542
• 代理人: 唐紹焜

本發(fā)明公開(kāi)了一種基于固態(tài)面陣激光雷達(dá)的太陽(yáng)位置識(shí)別與規(guī)避方法，包括步驟：(1)在操作手臂末端安裝固態(tài)面陣激光雷達(dá)，固態(tài)面陣激光雷達(dá)根據(jù)點(diǎn)云圖和灰度圖識(shí)別出太陽(yáng)所在位置，將該塊區(qū)域內(nèi)的像素點(diǎn)云距離設(shè)置為極大值代表過(guò)曝點(diǎn)云團(tuán)；(2)遍歷所有固態(tài)面陣激光雷達(dá)實(shí)時(shí)采集的目標(biāo)位置的點(diǎn)云數(shù)據(jù)的點(diǎn)提取得到所有曝光點(diǎn)；(3)進(jìn)行點(diǎn)云聚類，提取聚類點(diǎn)云質(zhì)心，得到在固態(tài)面陣激光雷達(dá)坐標(biāo)系下的太陽(yáng)中心位置；(4)轉(zhuǎn)換至機(jī)器人坐標(biāo)系下；(5)機(jī)器人根據(jù)太陽(yáng)指向向量和電線走向向量之間的差值計(jì)算固態(tài)面陣激光雷達(dá)掃描電線的角度并規(guī)劃操作手臂運(yùn)動(dòng)。本發(fā)明不需要等待太陽(yáng)移出視野范圍再進(jìn)行作業(yè)，適應(yīng)各種戶外陽(yáng)光環(huán)境。

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

本發(fā)明涉及激光雷達(dá)探測(cè)領(lǐng)域，尤其涉及一種基于固態(tài)面陣激光雷達(dá)的太陽(yáng)位置識(shí)別與規(guī)避方法。

背景技術(shù)

帶電作業(yè)機(jī)器人需要在戶外空中作業(yè)，強(qiáng)光的情況不可避免，在抓線、剝線、掛線等動(dòng)作中又嚴(yán)重依賴固態(tài)面陣激光雷達(dá)在局部建模中提供的精確數(shù)據(jù)，但是固態(tài)面陣激光雷達(dá)視野中存在太陽(yáng)的情況下，如圖1所示，會(huì)造成局部曝光，并影響目標(biāo)點(diǎn)云精度，造成固態(tài)面陣激光對(duì)目標(biāo)物識(shí)別失敗，只能等待太陽(yáng)改變角度后才能繼續(xù)流程，因此需要一套合理有效的太陽(yáng)位置識(shí)別與規(guī)避方案。

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

發(fā)明目的：本發(fā)明針對(duì)上述不足，提出了一種基于固態(tài)面陣激光雷達(dá)的太陽(yáng)位置識(shí)別與規(guī)避方法。

技術(shù)方案：

一種基于固態(tài)面陣激光雷達(dá)的太陽(yáng)位置識(shí)別與規(guī)避方法，包括步驟：

(1)在機(jī)器人的操作手臂末端安裝固態(tài)面陣激光雷達(dá)，固態(tài)面陣激光雷達(dá)根據(jù)點(diǎn)云圖和灰度圖識(shí)別出太陽(yáng)所在區(qū)域，將該區(qū)域內(nèi)的像素點(diǎn)云距離設(shè)置為極大值代表過(guò)曝點(diǎn)云團(tuán)；

(2)機(jī)器人遍歷所有固態(tài)面陣激光雷達(dá)實(shí)時(shí)采集的目標(biāo)位置的點(diǎn)云數(shù)據(jù)的點(diǎn)，提取得到所有像素點(diǎn)云距離為極大值的點(diǎn)即為曝光點(diǎn)；

(3)進(jìn)行點(diǎn)云聚類，提取達(dá)到曝光點(diǎn)個(gè)數(shù)閾值的聚類點(diǎn)云質(zhì)心，得到在固態(tài)面陣激光雷達(dá)的坐標(biāo)系下的太陽(yáng)中心位置；

(4)將固態(tài)面陣激光雷達(dá)坐標(biāo)系下的太陽(yáng)中心位置轉(zhuǎn)換至機(jī)器人坐標(biāo)系下，得到機(jī)器人坐標(biāo)系下太陽(yáng)中心位置及其指向向量，其中，太陽(yáng)指向向量為固態(tài)面陣激光雷達(dá)原點(diǎn)指向太陽(yáng)的向量；

(5)機(jī)器人控制其操作手臂根據(jù)步驟(4)得到的機(jī)器人坐標(biāo)系下太陽(yáng)中心位置和指向向量，根據(jù)太陽(yáng)指向向量和電線走向向量之間的差值計(jì)算固態(tài)面陣激光雷達(dá)掃描電線的角度，并重新規(guī)劃?rùn)C(jī)器人操作手臂的運(yùn)動(dòng)。

所述步驟(3)具體為：

(31)在所述步驟(2)提取得到的所有曝光點(diǎn)中某點(diǎn)，在所有曝光點(diǎn)中用kdTree找到離它最近的n個(gè)點(diǎn)，判斷這n個(gè)點(diǎn)到該點(diǎn)的距離，將距離小于閾值r的點(diǎn)放在類Q里；

(32)在類Q里找到一點(diǎn)，在所有曝光點(diǎn)中用kdTree找到離該最近的n個(gè)點(diǎn)，判斷這n個(gè)點(diǎn)到該點(diǎn)的距離，將距離小于閾值r的點(diǎn)放在類Q里，直到類Q無(wú)新點(diǎn)加入；

(33)判斷類Q中點(diǎn)云個(gè)數(shù)是否在設(shè)置的曝光點(diǎn)個(gè)數(shù)閾值之間，若在，則完成搜索，轉(zhuǎn)至步驟(34)；若不在，則返回步驟(31)；

(34)計(jì)算類Q中所有像素點(diǎn)的坐標(biāo)均值(q_{Average_x}、q_{Average_y}、q_{Average_z})即為聚類點(diǎn)云的質(zhì)心，即為太陽(yáng)在相機(jī)坐標(biāo)系下的位置(Sun_{Camera_x}、Sun_{Camera_y}、Sun_{Camera_z})。