[發(fā)明專利]三維封閉非結(jié)構(gòu)空間內(nèi)膠囊機器人及待測目標的相對定位方法

有效

• 申請?zhí)枺?/td>: 201810285619.X
• 申請日: 2018-03-28
• 公開（公告）號: CN108759812B
• 公開（公告）日: 2021-04-20
• 發(fā)明（設計）人: 張永順;楊慧遠;田豐;王殿龍
• 申請（專利權）人: 大連理工大學
• 主分類號: A61B1/00
• 分類號: A61B1/00
• 代理公司: 大連理工大學專利中心 21200
• 代理人: 溫福雪;侯明遠

1.三維封閉非結(jié)構(gòu)空間內(nèi)膠囊機器人及待測目標的相對定位方法，其特征在于，步驟如下：

第一步，首先根據(jù)小孔成像模型以及攝像機成像點與目標點的映射關系，建立以世界坐標表示的點P(X_W,Y_W,Z_W)與以像素為單位的圖像坐標P″(u,v)之間的關系；之后通過對目標圖像的處理和分析，提取目標圖像特征點，將目標物到攝像機的距離關系轉(zhuǎn)化為具體特征點與攝像機光心之間距離關系，最終建立基于單目視覺的空間測距模型；

第二步，膠囊機器人進入三維非結(jié)構(gòu)封閉空間后，依據(jù)被動模態(tài)定點懸停調(diào)姿，通過控制旋轉(zhuǎn)磁矢量軸線的側(cè)擺角和俯仰角改變其方位，在磁力矩隨動效應下，膠囊機器人軸線與旋轉(zhuǎn)磁場軸線始終保持一致，并帶動膠囊機器人攝像機在待測區(qū)域定點側(cè)擺俯仰掃描觀察；當觀測到某參考標志時，對獲取的圖像進行圖像處理，提取該參考標志的形心并將其作為位置參考點；之后運用單目視覺空間測距模型，計算攝像機光心到位置參考點的距離L₁，并從磁場控制器讀取攝像機獲得參考位置圖像時膠囊機器人的側(cè)擺角θ₁和俯仰角δ₁；

第三步，通過改變磁矢量軸線方位帶動攝像機原地進行待測目標的搜索，當檢測到待測目標后再次通過圖像處理識別待測目標形心，并根據(jù)上述測距方式計算待測目標形心到攝像機光心的距離L₂，同時從磁場控制器讀取此時膠囊機器人的側(cè)擺角θ₂和俯仰角δ₂信息；

第四步，以第二步確定的位置參考點為原點，建立與外部磁場坐標系OXYZ相平行的世界坐標系O_wX_wY_wZ_w，由于該坐標系由外部磁場坐標系OXYZ經(jīng)空間平移得到，且膠囊機器人處于外部磁場的均勻區(qū)內(nèi)，膠囊機器人軸線與磁場坐標系OXYZ各坐標軸的夾角、膠囊機器人軸線與世界坐標系O_wX_wY_wZ_w各坐標軸的夾角，兩個夾角相同；之后根據(jù)膠囊機器人和待測目標與位置參考點距離及空間位置關系，并結(jié)合兩次觀測到目標區(qū)域時的俯仰、側(cè)擺角信息，分別確定出待測目標和膠囊機器人相對于位置參考點的空間方位信息；

所述的第二、三步中，待測目標及膠囊機器人的方位確定過程為：膠囊機器人在待測區(qū)域依據(jù)被動模態(tài)定點懸停調(diào)姿功能，通過控制旋轉(zhuǎn)磁矢量的側(cè)擺角和俯仰角帶動膠囊機器人攝像機在P₁定點掃描觀察，運用圖像處理識別某參考標志的形心，并將其作為確定膠囊機器人和待測目標方位的參考點；從磁場控制器讀取攝像機獲得參考標志圖像時膠囊機器人的側(cè)擺角θ₁和俯仰角δ₁，設該參考標志的形心圖像坐標為P₁'(u₁，v₁)，根據(jù)單目視覺測距原理由式(14)得膠囊機器人球心到該參考標志的形心的距離d₁為

通過改變磁矢量軸線方位帶動攝像機原地進行待測目標的搜索，當檢測到待測目標后再次通過圖像處理識別其形心P₂，并通過磁場控制器讀取此時膠囊機器人的側(cè)擺角θ₂和俯仰角δ₂；設待測目標形心的圖像坐標為P₂'(u₂，v₂)，根據(jù)上述方法計算得膠囊機器人球心到待測目標形心的距離d₂為

其中，a_x，a_y，u₀和v₀為攝像機內(nèi)部參數(shù)，可由攝像機定標確定；

以位置參考點O_w為坐標原點，建立世界坐標系O_wX_wY_wZ_w；位置參考點O_w和待測目標形心P₂在點P₁所在的水平面內(nèi)的投影分別為O₂、P₂”,設P₁點坐標為(X₁，Y₁，Z₁)，則由圖中幾何關系并結(jié)合式(15)(16)得

設P₂點坐標為(X₂，Y₂，Z₂)，則

綜上，膠囊機器人在世界坐標系O_wX_wY_wZ_w中的實時方位表示為：

P₁(d₁cosδ₁sinθ₁,-d₁cosδ₁cosθ₁,-d₁sinδ₁)

待測目標的方位表示為：

P₂(d₁cosδ₁sinθ₁-d₂cosδ₂sinθ₂,-d₁cosδ₁cosθ₁+d₂cosδ₂cosθ₂,d₂sinδ₂-d₁sinδ₁)。

2.根據(jù)權利要求1所述的三維封閉非結(jié)構(gòu)空間內(nèi)膠囊機器人及待測目標的相對定位方法，其特征在于，所述的第一步中，根據(jù)小孔成像模型建立單目視覺空間測距模型的推導過程如下：

O₀為攝像機的光心，O(x₀,y₀)為攝像機光軸O₀O_w與像平面的交點，以O(x₀,y₀)為原點建立像平面坐標系Oxy，同時以成像面右上角為坐標原點建立與Oxy各坐標軸平行的以像素為單位的坐標系ouv；設攝像機在豎直面內(nèi)俯仰δ角觀測到平面上一目標點P，P′為被測點P在像平面上的映射點，P_x'、P_y'點分別為P'在x、y軸上的投影點；以光軸與水平面的交點O_w為原點建立二維平面坐標系OX_wY_w,點P在各坐標軸上的投影點分別為P_x、P_y；攝像機光心O₀在水平面的投影點為O₁，距離水平面的高度為h，攝像機的焦距為f，即OO₀＝f；

設光軸O₀O與O₀P_y'的夾角為β，與O₀ P_x'的夾角為γ，P'點在坐標系Oxy中的坐標為P'(x₁,y₁)，攝像機光心O₀與點P的空間距離為L，則

在三角形O₀PP_y中，L表示為

在三角形O₀O₁P_y中，O₀P_y表示為

在三角形O₀OP_y'中，滿足

在坐標系O_wX_wY_w中，由投影關系知PP_y＝P_xO_w，在三角形O₀O_wP_x中

P_xO_w＝O₀O_wtanγ (4)

在三角形O₀O₁O_w中

在三角形O₀OP_x'中

由式(2)(3)得

由式(4)(5)(6)得

由式(1)(7)(8)得

雙半球膠囊機器人的攝像機安裝在機器人的頂部，膠囊機器人的半徑為R，膠囊軸線在豎直面內(nèi)的俯仰角為δ，并定義膠囊機器人定點掃描觀察時，側(cè)擺和俯仰角逆時針為正，根據(jù)幾何關系，得攝像機光心到水平面的高度h為

h＝R(1+sinδ) (10)

由式(9)(10)得

如果(u,v)為以像素為單位的圖像坐標系中的坐標，則O″(u₀,v₀)為攝像機光軸與像平面交點O(x₀,y₀)的以像素為單位的坐標，P″(u,v)是P′(x,y)的坐標，設像平面上一個像素在x軸和y軸上的物理尺寸分別為dx、dy，則有：

由式(12)得x＝(u-u₀)dx，y＝(v-v₀)dy，令a_x＝f/dx,a_y＝f/dy，由式(11)得被測點P與攝像機光心之間的空間距離為

雙半球膠囊機器人球心到攝像機光心的距離近似等于球形機器人半徑R，因此得機器人球心到被測點的空間距離d表示為

其中，a_x，a_y，u₀和v₀為攝像機內(nèi)部參數(shù)，由攝像機定標確定。