[發明專利]一種多坑洞路面環境下視覺導航算法

在審

• 申請號: 202210982473.0
• 申請日: 2022-08-16
• 公開（公告）號: CN115454050A
• 公開（公告）日: 2022-12-09
• 發明（設計）人: 劉海健;張敏;李濟甫;田建艷;張國清;馬韶艷
• 申請（專利權）人: 山西萬立科技有限公司;太原理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 山西科匯聯創知識產權代理事務所(特殊普通合伙) 14126
• 代理人: 胡新瑞

1.一種多坑洞路面環境下視覺導航算法，其特征在于，按下述步驟實施：

第一步，環境中有多排的坑洞，在每排坑洞前方的小車行進道路上鋪設地標二維碼，1號相機為二維彩色相機即可；假設坑洞規則排列，坑洞直徑為D，坑洞間隙為L，AGV底盤寬度為W，1號相機安裝于AGV底盤的前方中心位置，相鄰兩個坑洞的位置設為(x0,y0)和(x1,y1),規則排列時y0＝y1,x1＝x0+D+L，則二維碼的位置(x,y)為：(x方向為沿著小車行進道路的方向，y方向為垂直于道路的方向，即坑洞行進道路方向)；

x＝(x0+x1)/2；

y＝y0-D/2-W/2；

第二步，給定AGV運行的速度和加速度參數，控制AGV沿著小車行進道路，即X軸方向直線行走，在行走過程中1號相機不斷采集二維碼圖像，識別圖像中的二維碼信息，通過比較目標坑洞的序列數與識別到的二維碼中的序列數，相同的話代表AGV到達指定目標坑洞前方；不一致的話，控制AGV繼續前行，直到到達目標坑洞前方停下；

第三步，到達目標坑洞前方后，1號相機繼續采集識別二維碼圖像，獲取二維碼相對于1號相機的位姿矩陣，根據位置和姿態信息確定二維碼相對于相機中心位置的距離偏差和角度偏差，不斷微調AGV的位置和姿態，使距離和角度偏差都處于可接受的偏差范圍之內；

第三步，控制AGV在原地旋轉90度，使其正對坑洞行進道路方向，即Y軸方向；

第四步，2號相機為深度相機，可以同時獲取彩色和深度信息，2號相機位于機械臂的末端位置，控制AGV底盤上面的機械臂使相機移動到坑洞上方的初始位置，對坑洞進行拍照；

第五步，采集坑洞的深度圖片，由于彩色圖片受光線、環境影響較大，因此采用深度圖像進行處理；

根據深度圖像中的距離信息進行閾值分割，假設地面距離相機的距離是Dg(設為閾值),由于坑洞底部距離地面較低，因此坑洞底部距離相機的距離Dk遠大于Dg,遍歷圖像中的距離像素值Dis，滿足以下條件的提取出來：

DisDg+e；

其中，e代表深度相機的測距精度誤差值；然后再將提取出來的坑洞信息利用sobel算子進行邊緣檢測，提取出坑洞的邊緣信息；

第六步，由于坑洞多是橢圓或圓形，因此采用橢圓擬合對坑洞外輪廓進行擬合，擬合得到精定位的坑洞中心位置以及坑洞直徑信息，結合相機的內、外參矩陣參數以及手眼標定結果，獲得坑洞距離小車中心的實際位置(x’，y’)及直徑d，單位為mm；

判斷坑洞直徑d和AGV小車寬度W，如果dW-ε,其中ε為小車的運動誤差，則判斷該坑洞小車可跨越，否則不可跨越；

第七步，根據以下公式計算小車的旋轉角度θ和直行距離l：

θ＝arctan(x’/y’)；

第七步，根據旋轉角度θ和直行距離l控制規劃AGV的運動路徑，引導AGV在多坑洞的環境中自主導航行走。

2.根據權利要求1所述的一種多坑洞路面環境下視覺導航算法，其特征在于，所述二維碼的位置根據坑洞位置、坑洞尺寸、坑洞間隙、AGV底盤尺寸、1號相機安裝位置實際參數可調整，且所述二維碼信息中包含坑洞的序列數、該排坑洞個數、坑洞直徑尺寸信息。