一種導航模塊坐標系與機器人坐標系的轉換方法
【專利摘要】本發明涉及一種導航模塊坐標系與機器人坐標系的轉換方法,包括:S100,機器人自轉,利用所述相機拍攝的第一預設路標計算出導航模塊坐標系原點與機器人坐標系原點的偏移量;S200,機器人直線行走,利用所述相機拍攝的第二預設路標計算出導航模塊坐標系與機器人坐標系的X軸旋轉角;S300,計算導航模塊坐標系與機器人坐標系的轉換參數,確定轉換關系矩陣。本發明通過機器人自轉與直線行走,計算出導航模塊坐標系與機器人坐標系之間的位移加旋轉關系,就可以得到導航模塊坐標系與機器人坐標系之間的轉換關系,導航模塊坐標系可與機器人坐標系進行轉換,使得機器人能夠在定位與導航過程中得到準確的基于自身的坐標與位姿,導航精度更高。
【專利說明】
一種導航模塊坐標系與機器人坐標系的轉換方法
技術領域
[0001] 本發明屬于機器人視覺導航領域,特別涉及一種導航模塊坐標系與機器人坐標系 的轉換方法。
【背景技術】
[0002] 為了實現機器人視覺導航,需要設置路標和導航模塊,所述路標可以設置在室內 的天花板上;所述導航模塊為了拍攝與識別所述路標,一般包括相機、圖像采集單元、圖像 處理單元與計算單元,所述相機設置在機器人頭部的頂端,所述圖像采集單元、圖像處理單 元、計算單元設置在機器人內部。
[0003] -般而言,所述導航模塊坐標系是指所述相機的坐標系,更確切的說是指所述相 機的感光芯片靶面上設定的坐標系;所述機器人坐標系是指基于機器人本身的坐標系,根 據機器人的整體結構與導航精度要求進行設定,比如可以將所述機器人坐標系原點設定為 機器人上表面的中心。
[0004]因而,由于所述導航模塊中的相機所處位置、或者設計誤差與安裝誤差,可能導致 所述導航模塊坐標系與所述機器人坐標系不相同,導航過程中機器人不能準確識別自身的 坐標及姿態,得到的是所述導航模塊的坐標及姿態。
【發明內容】
[0005] 本發明的目的在于解決現有技術中的機器人不能準確識別自身坐標及姿態,得到 的是所述導航模塊的坐標及姿態的問題。
[0006] 為實現上述目的,一種導航模塊坐標系與機器人坐標系的轉換方法,所述機器人 包括導航模塊,并通過導航模塊和設置的路標進行視覺導航,所述導航模塊包括相機,其特 征在于,包括:
[0007] S100:機器人自轉,利用所述相機拍攝的第一預設路標計算出導航模塊坐標系原 點與機器人坐標系原點的偏移量;
[0008] S200:機器人直線行走,利用所述相機拍攝的第二預設路標計算出導航模塊坐標 系與機器人坐標系的X軸旋轉角;
[0009] S300:計算導航模塊坐標系與機器人坐標系的轉換參數,確定轉換關系矩陣。
[0010] 所述機器人坐標系原點為機器人的豎直軸線上任意一點,機器人坐標系所在的平 面與所述機器人的豎直軸線互相垂直。
[0011] 所述導航模塊坐標系原點是指所述相機感光靶面上分辨率劃分的第〇行第〇列的 頂點。
[0012] 所述S100包括:
[0013] S110:機器人自轉,所述相機拍攝所述第一預設路標3幅或3幅以上的圖像;
[0014] S120:所述第一預設路標在相機的感光芯片靶面上的成像形成對應的點,3個或3 個以上的不在同一條直線上的對應的點經過計算擬合出所述第一預設路標的圓形運動軌 跡;
[0015] S130:計算所述圓形運動軌跡的圓心與其在所述導航模塊坐標系下的坐標,所述 圓心就是所述機器人坐標系原點在所述導航模塊坐標系中的投影點,所述坐標就是所述導 航模塊坐標系原點與機器人坐標系原點的偏移量。
[0016] 所述S100中,機器人在能夠拍攝到所述第一預設路標的任意位置自轉。
[0017] 進一步地,所述S100中,所述機器人在所述第一預設路標正下方自轉。
[0018] 所述機器人坐標系的X軸正方向是機器人向前直線行走的方向,經過所述機器人 坐標系原點的為X軸;所述機器人的右側與所述X軸垂直的指向所述機器人坐標系原點的反 方向為Y軸的正方向,經過所述機器人坐標系原點的為Y軸。
[0019] 所述導航模塊坐標系的X軸是指所述相機的感光芯片靶面中第0行的邊,指向所述 導航模塊坐標系原點的反方向為X軸的正方向;所述相機的感光芯片靶面中的第0列的邊為 所述導航模塊坐標系的Y軸,指向所述導航模塊坐標系原點的反方向為Y軸的正方向。
[0020] 所述S200包括:
[0021] S210,機器人向前直線行走,所述相機拍攝2幅或2幅以上的所述第二預設路標的 圖像;
[0022] S220,所述第二預設路標在所述相機的感光芯片靶面上的成像具有對應的點,2個 或2個以上的對應的點經過計算擬合出了所述第二預設路標的直線運動軌跡;
[0023] S230,所述直線運動軌跡與所述機器人坐標系X軸平行,計算所述直線運動軌跡與 所述導航模塊坐標系X軸的夾角,該夾角即為所述導航模塊坐標系與所述機器人坐標系X軸 的旋轉角。
[0024] 所述S200中,所述機器人在能夠拍到所述第二預設路標的任意位置向前直線行 走。
[0025] 進一步地,所述S200中,所述機器人在所述第二預設路標正下方位置向前直線行 走。
[0026]所述第一預設路標與第二預設路標可以為同一個路標。
[0027]所述轉換參數包括導航模塊坐標系與機器人坐標系原點的偏移量(tx,ty),以及 機器人坐標系X軸與導航模塊坐標系X軸的旋轉角Θ,所述轉換關系矩陣為:
[0028]
[0029] 其中,(x,y)是機器人坐標系中的一個坐標,,y')是(x,y)在導航模塊坐標系中 的坐標。
[0030] 與現有技術相比,本技術方案具有以下優點:
[0031 ]本技術方案的一種導航模塊坐標系與機器人坐標系的轉換方法,通過機器人做自 轉與直線行走,計算出導航模塊坐標系與機器人坐標系之間的位移加旋轉關系,就可以得 到導航模塊坐標系與機器人坐標系之間的轉換關系,導航模塊坐標系可與機器人坐標系進 行轉換,使得機器人能夠在定位與導航過程中得到準確的基于自身的坐標與位姿,導航精 度更高。
【附圖說明】
[0032] 圖1為本發明一種導航模塊坐標系與機器人坐標系的轉換方法的流程圖。
[0033] 圖2為本發明機器人坐標系原點與導航模塊坐標系原點的偏移量的示意圖。
[0034] 圖3為本發明一種導航模塊坐標系與機器人坐標系的轉換方法的示意圖。
[0035] (注意:附圖中的所示結構只是為了說明本發明特征的示意,并非是要依據附圖所 示結構。)
【具體實施方式】
[0036]為使本發明的上述目的、特征和優點能夠更為明顯易懂,下面結合附圖對本發明 的具體實施例做詳細的說明。
[0037] 本發明提供了一種導航模塊坐標系與機器人坐標系的轉換方法。
[0038] 所述機器人坐標系的原點位于機器人的豎直軸線上,機器人坐標系所在的平面與 所述機器人的豎直軸線互相垂直。
[0039] 所述機器人坐標系所在平面與所述機器人豎直軸線垂直。
[0040]所述導航模塊坐標系是指所述導航模塊中設置的相機的坐標系,所述導航模塊坐 標系位于所述相機的感光芯片靶面上,所述相機的感光芯片靶面與所述豎直軸線垂直。
[0041] 所述機器人坐標系與所述導航模塊坐標系位于同一平面或位于不同的互相平行 的平面。
[0042] 所述的導航模塊坐標系與機器人坐標系轉換方法中,還需利用為了導航而設置的 若干路標。
[0043] 本發明中,機器人在空間內的導航不涉及到機器人往上走或往下走等高度上的變 化,因而只需考慮機器人在水平地面上的運動,也就是說,將所述導航模塊坐標系與機器人 坐標系轉換到同一個平面上即可計算導航模塊坐標系與機器人坐標系之間的轉換。
[0044] 參考圖1,所述的導航模塊坐標系與機器人坐標系的轉換方法,包括如下步驟:
[0045] S100,機器人自轉,利用所述相機拍攝的第一預設路標計算出導航模塊坐標系原 點與機器人坐標系原點的偏移量。
[0046] 所述導航模塊坐標系是指導航模塊包括的所述相機的坐標系,所述相機用于機器 人的視覺導航,現有技術中相機通常設置在機器人頭部的頂端;所述導航模塊坐標系的原 點是指具有特定分辨率的相機感光芯片靶面中的點,所述分辨率數量與所述感光芯片靶面 中的行列數數量相同。
[0047]在本實施例中,所述相機分辨率為640*480,即所述相機有640個像素點,所述感光 芯片靶面分為640列與480行,所述感光芯片靶面的第0行第0列的頂點為導航模塊坐標系的 原點。
[0048] 所述機器人坐標系原點,位于機器人的豎直軸線上任意一點,機器人坐標系所在 的平面與所述機器人的豎直軸線互相處置。
[0049] 所述機器人坐標系原點與所述機器人的車輪轉速、方向以及車輪間的距離有關; 所述機器人坐標系原點所在的豎直軸線穿過機器人自轉時車輪運動軌跡的圓心。
[0050] 在本實施例中,所述機器人有兩個車輪,所述車輪相同,所述車輪轉速設定相同, 當所述車輪做順時針或逆時針運動使得機器人自轉時,車輪運動軌跡為一個圓,所述機器 人的豎直軸線穿過所述車輪圓形運動軌跡的圓心。
[0051] 在其他實施例中,所述機器人有三個車輪,所述車輪相同,所述車輪間的距離、方 向、轉速設定相同,當所述車輪做順時針或逆時針運動驅動機器人自轉時,車輪運動軌跡為 一個圓,穿過圓心的豎直軸線上分布著所述機器人坐標系的原點,也就是說,所述機器人坐 標系的原點可以是所述圓心、可以是所述豎直軸線與路標所在平面的交點、可以是所述軸 線與所述導航模塊坐標系所在平面的交點。
[0052] 在本實施例中,參考圖2,所述S100包括:
[0053] S110,所述機器人在所述第一預設路標正下方以經過機器人坐標系原點的豎直軸 線為軸自轉,在所述機器人自轉時,如果以機器人作為參照物,則所述第一預設路標在相對 于所述機器人與所述相機轉動,所述第一預設路標在所述相機的感光芯片靶面1上成像,所 述相機拍攝所述第一預設路標3幅或3幅以上的圖像;
[0054] S120,所述第一預設路標在相機的感光芯片靶面上的成像具有對應的點,在所述 導航模塊坐標系V X'r中,利用3個或3個以上的不在同一直線上的對應的點,經過最小二 乘法擬合出所述第一預設路標的圓形運動軌跡;
[0055] S130,所述圓形運動軌跡的圓心正好是所述機器人坐標系原點在所述導航模塊坐 標系平面的投影點Q,將所述機器人坐標系轉換到了所述導航模塊坐標系平面上,根據數學 中的已知圓求圓心,確定圓心的位置,并且計算出該圓心在所述導航模塊坐標系下 的坐標,就可以得到所述機器人坐標系原點到所述導航模塊坐標系原點的偏移量。
[0056] 在其他實施例中,所述機器人在所述相機能夠拍攝到所述預設路標的任意位置自 轉,只要在所述導航模塊坐標系中的3幅或3幅以上的成像形成不在同一直線上的3個或3個 以上的點即可,此時可以運用最小二乘法擬合出圓形軌跡;同樣,計算出的所述第一預設路 標圓形運動軌跡的圓心在所述導航模塊坐標系下的坐標,即為所述機器人坐標系原點在所 述導航模塊坐標系下的坐標,即為所述機器人坐標系原點與所述導航模塊坐標系原點的偏 移量。
[0057] 在其他實施例中,也可以通過增加相機分辨率來使所述機器人自轉時所述相機采 集到的盡可能多的圖像形成的是所述第一預設路標相對運動的近圓形軌跡,這樣可以減少 最小二乘法或其他方法再計算圓心,也就是所述機器人坐標系原點在所述導航模塊坐標系 平面的投影點的坐標。
[0058] S200,機器人直線行走,利用所述相機拍攝的第二預設路標計算出導航模塊坐標 系與機器人坐標系的X軸的旋轉角。
[0059] 所述的第二預設路標與S100中所述的第一預設路標可以是同一個路標,也可以是 不同的路標。
[0060] 在本實施例中,S100中所述的第一預設路標與所述第二預設路標選擇了同一個路 標。
[0061] 所述導航模塊坐標系的X軸與Y軸可以是所述相機的感光芯片靶面上互相垂直的 任意直線。
[0062] 在本實施例中,所述導航模塊坐標系的X軸,是指所述相機的感光芯片靶面中的第 〇行的邊,指向所述導航模塊坐標系原點的反方向為X軸的正方向;所述相機的感光芯片靶 面中的第0列的的邊為所述導航模塊坐標系的Y軸,指向所述導航模塊坐標系原點的反方向 為Υ軸的正方向。
[0063]在其他實施例中,所述導航模塊坐標系的X軸與Υ軸可以做其他的設定。比如,可以 規定感光芯片靶面上中的第1行第1列的兩條垂直邊分別為所述導航模塊坐標系的坐標軸, 交點為所述導航模塊坐標系原點。
[0064]所述機器人坐標系的X軸正方向是指機器人向前直線行走的方向。
[0065]在本實施例中,所述機器人坐標系的X軸正方向是指在機器人車輪速度相同時向 前直線行走的方向,經過所述機器人坐標系原點的為X軸;所述機器人的右側與所述X軸垂 直的指向所述機器人坐標系原點的反方向為Υ軸的正方向,且經過所述機器人坐標系原點 的為Υ軸。
[0066]在本實施例中,機器人停止自轉后,所述機器人在第二預設路標正下方,兩個車輪 以相同的速度向前直線行走,即沿所述機器人坐標系ΟΧΥ的X軸正方向行走。
[0067]在其他實施例中,所述機器人可以在能夠拍攝到所述第二預設路標的任意位置行 走,只要所述機器人是沿所述機器人坐標系ΟΧΥ的X軸正方向行走即可。
[0068] 在本實施例中,參考圖3,所述S200包括:
[0069] S210,所述機器人向前直線行走,所述相機拍攝2幅或2幅以上的所述第二預設路 標的圖像;
[0070] S220,如果以所述機器人作為參照物,則所述第二預設路標相對于所述機器人與 所述相機運動,所述第二預設路標在所述相機的感光芯片靶面1上成像具有對應的點,在所 述導航模塊坐標系(/X'Υ'中,利用2個或2個以上的對應的點,采用最小二乘法擬合出所述 第二預設路標的直線運動軌跡;
[0071] S230,所述直線運動軌跡與所述機器人坐標系ΟΧΥ的X軸平行,可看做是所述機器 人坐標系在所述導航模塊坐標系中的投影線,所述直線運動軌跡與所述導航模塊坐標系 X' Υ'的X'軸具有一個旋轉角Θ,因此,得到所述導航模塊坐標系〇' X' Υ'的X'軸與所述機器人 坐標系ΟΧΥ的X軸的旋轉角θ。
[0072] S300,計算導航模塊坐標系與機器人坐標系的轉換參數,確定轉換關系矩陣。
[0073]通過S100與S200已經確定的所述導航模塊坐標系與機器人坐標系之間的原點偏 移量與旋轉角Θ,基于平面直角坐標系之間的轉換關系確定所述導航模塊坐標系與所述機 器人坐標系之間的轉換矩陣。
[0074]在本實施例中,已知偏移量與旋轉角,基于平面直角坐標系之間的轉換關系就可 以確定所述導航模塊坐標系與所述機器人坐標系的轉換矩陣。
[0075]參考圖3,0ΧΥ為機器人坐標系,(x,y)是機器人坐標系中的坐標;是導航模 塊坐標系,)是導航模塊坐標系中的坐標;機器人坐標系原點0在導航模塊坐標系亇 W中的坐標為(tx,ty),即坐標(tx,ty)是所述導航模塊坐標系原點與所述機器人坐標 系原點〇的偏移量;機器人坐標系的X軸與導航模塊坐標系f軸的旋轉角為Θ;坐標轉換公式 是:x 7 =xcos0-ysin0+tx,y/ =xsin0+ycos0+ty〇
[0076] SP,轉換矩陣如下所示:
[0077]
[0078] 為了更進一步簡化公式,便于坐標系之間的轉換計算,引入齊次坐標形式,則得到 以下公式:
[0079]
[0080] 將所述導航模塊坐標系轉換成所述機器人坐標系,則是在上述公式兩邊乘以一個 相同的逆矩陣。
[0081] 通過以上計算完成的轉換矩陣,任意所述機器人坐標系下的坐標均可以轉換為所 述導航模塊坐標系下的坐標;通過乘以逆矩陣所述導航模塊坐標系下的坐標也可以轉換為 所述機器人坐標系下的坐標。
[0082] 本技術方案的一種導航模塊坐標系與機器人坐標系的轉換方法,通過機器人做自 轉與直線行走,計算出導航模塊坐標系與機器人坐標系之間的位移加旋轉關系,就可以得 到導航模塊坐標系與機器人坐標系之間的轉換關系,導航模塊坐標系可與機器人坐標系進 行轉換,使得機器人能夠在定位與導航過程中得到準確的基于自身的坐標與位姿,導航精 度更高。
[0083] 以上所述,僅為本發明的較佳實施例而已,并非用于限定本發明的保護范圍,凡在 本發明的精神和原則之內所作的任何修改、等同替換和改進等,均應包含在本發明的保護 范圍之內。
【主權項】
1. 一種導航模塊坐標系與機器人坐標系的轉換方法,所述機器人包括導航模塊,并通 過導航模塊和設置的路標進行視覺導航,所述導航模塊包括相機,其特征在于,包括: S100:機器人自轉,利用所述相機拍攝的第一預設路標計算出導航模塊坐標系原點與 機器人坐標系原點的偏移量; S200:機器人直線行走,利用所述相機拍攝的第二預設路標計算出導航模塊坐標系與 機器人坐標系的X軸旋轉角; S300:計算導航模塊坐標系與機器人坐標系的轉換參數,確定轉換關系矩陣。2. 根據權利要求1所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于,所 述機器人坐標系原點為機器人的豎直軸線上任意一點,機器人坐標系所在的平面與所述機 器人的豎直軸線互相垂直。3. 根據權利要求1所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于,所 述導航模塊坐標系原點是指所述相機感光祀面上分辨率劃分的第0行第0列的點。4. 根據權利要求1-3任意一項所述的導航模塊坐標系與機器人坐標系的轉換方法,其 特征在于,所述S100包括: S110:機器人自轉,所述相機拍攝所述第一預設路標3幅或3幅W上的圖像; S120:所述第一預設路標在相機的感光忍片祀面上的成像具有對應的點,3個或3個W 上的不在同一條直線上的對應的點經過計算擬合出所述第一預設路標的圓形運動軌跡; S130:計算所述圓形運動軌跡的圓屯、與其在所述導航模塊坐標系下的坐標,所述圓屯、 就是所述機器人坐標系原點在所述導航模塊坐標系中的投影點,所述圓屯、坐標就是所述導 航模塊坐標系原點與機器人坐標系原點的偏移量。5. 根據權利要求1所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于,所 述S100中,機器人在能夠拍攝到所述第一預設路標的任意位置自轉。6. 根據權利要求5所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于,所 述S100中,所述機器人在所述第一預設路標正下方自轉。7. 根據權利要求1所述導航模塊坐標系與機器人坐標系的轉換方法,其特征在于,所述 機器人坐標系的X軸正方向是機器人向前直線行走的方向,經過所述機器人坐標系原點的 為X軸;所述機器人的右側與所述X軸垂直的指向所述機器人坐標系原點的反方向為Y軸的 正方向,經過所述機器人坐標系原點的為Y軸。8. 根據權利要求1所述導航模塊坐標系與機器人坐標系的轉換方法,其特征在于,所述 導航模塊坐標系的X軸是指所述相機的感光忍片祀面中第0行的邊,指向所述導航模塊坐標 系原點的反方向為X軸的正方向;所述相機的感光忍片祀面中的第0列的邊為所述導航模塊 坐標系的Y軸,指向所述導航模塊坐標系原點的反方向為Y軸的正方向。9. 根據權利要求1或7或8所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征 在于,所述S200包括: S210,機器人向前直線行走,所述相機拍攝2幅或2幅W上的所述第二預設路標的圖像; S220,所述第二預設路標在所述相機的感光忍片祀面上的成像具有對應的點,2個或2 個W上的對應的點經過計算擬合出了所述第二預設路標的直線運動軌跡; S230,所述直線運動軌跡與所述機器人坐標系X軸平行,計算所述直線運動軌跡與所述 導航模塊坐標系X軸的夾角,該夾角即為所述導航模塊坐標系與所述機器人坐標系X軸的旋 轉角。10. 根據權利要求1所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于, 所述S200中,所述機器人在能夠拍到所述第二預設路標的任意位置向前直線行走。11. 根據權利要求10所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于, 所述S200中,所述機器人在所述第二預設路標正下方位置向前直線行走。12. 根據權利要求1所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于, 所述第一預設路標與第二預設路標可W為同一個路標。13. 根據權利要求1所述的導航模塊坐標系與機器人坐標系的轉換方法,其特征在于, 所述轉換參數包括導航模塊坐標系與機器人坐標系原點的偏移量(tx,ty),W及機器人坐 標系X軸與導航模塊坐標系X軸的旋轉角Θ,所述轉換關系矩陣為:其中,(x,y)是機器人坐標系中的一個坐標,(x',y')是(x,y)在導航模塊坐標系中的坐 標。
【文檔編號】G01C21/20GK105973240SQ201610560903
【公開日】2016年9月28日
【申請日】2016年7月15日
【發明人】喬徽, 張騰飛
【申請人】哈爾濱工大服務機器人有限公司