專利名稱：基于色標(biāo)塊的視覺實時測量方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機器人領(lǐng)域中的視覺測量，具體地說是用于機器人位姿測量的方法。
背景技術(shù)：
目前比較常用的視覺測量方法主要包括雙目立體視覺和基于η點透視 (Perspective-n-Point,PnP)算法的單目視覺。由于雙目立體視覺在每次測量時需要對兩 幅圖像進行處理，并且需要進行特征點匹配，所以其實時性較差。單目視覺每次測量僅對一 幅圖像進行處理，其實時性比雙目立體視覺高(徐德，譚民，李原，機器人視覺測量與控制， 國防工業(yè)出版社，第130頁，第140頁，2008)。在采用尺寸已知的矩形色標(biāo)塊作為機器人的標(biāo)記，特征點的提取即為矩形色標(biāo)塊 頂點的提取。目前比較常用的特征角點提取算法為哈里斯(Harris)角點檢測算法、最小 核值相似區(qū)(Smallest Univalve Segment Assimilating Nucleus，SUSAN)角點檢測法，高 斯差分(Difference-of-Gaussian，DoG)算子檢測法等。上述算法在實時性較差，在色標(biāo) 塊一個頂點被遮擋時，不能檢測出該頂點。采用先求矩形色標(biāo)塊四條邊緣直線，然后通過直 線求交點獲得特征角點的方法，可以有效地解決一個頂點被遮擋時的頂點特征提取問題。 雖然目前有很多種邊緣提取算法，如羅伯特(Robert)邊緣算子、坎尼(Carmy)邊緣算子、 索貝爾(Sobel)邊緣算子、克瑞斯(Kirsch)邊緣算子和普瑞維特(Prewitt)邊緣算子等， 然而這些邊緣提取算法一般是對整幅圖像進行處理，實時性不高(Zhang，Yang ；Rockett, P. I."The Bayesian Operating Point of the Canny Edge Detector,"IEEE Transactions on Image Processing, Vol. 15，No. 11，pp. 3409-3416，2006)。在獲得邊緣點后，利用霍夫 (Hough)變換提取直線，其實時性也較差。此外，目前的方法是在像素級精度邊緣點的基礎(chǔ) 上得到的直線，求交點得到的特征點精度不高，影響了基于PnP方法視覺測量的精度，且測 量結(jié)果中的姿態(tài)矩陣也不能保證是正交的。

發(fā)明內(nèi)容
為了解決現(xiàn)有技術(shù)中利用交點得到的特征點精度不高，影響了基于PnP方法視覺 測量的精度，且測量結(jié)果中的姿態(tài)矩陣也不能保證是正交的技術(shù)問題，本發(fā)明的目的在于 提供用于機器人位姿高精度實時視覺測量、保證測量結(jié)果中的姿態(tài)矩陣的正交性的一種基 于色標(biāo)塊的實時視覺測量方法。為實現(xiàn)上述目的，本發(fā)明提供基于色標(biāo)塊的實時視覺測量方法解決問題的技術(shù)方 案包括步驟如下步驟Sl 在機器人的背部上方設(shè)有一個已知尺寸的色標(biāo)塊；步驟S2 以單攝像機采集色標(biāo)塊的色標(biāo)圖像；步驟S3 跟蹤色標(biāo)塊的三點，所述三點是中心點、一條長邊中點和一條短邊中點， 確定色標(biāo)塊圖像區(qū)域；步驟S4 對色標(biāo)圖像按照設(shè)定的間隔和方向掃描處理，獲得具有亞像素精度的邊緣占.步驟S5 將亞像素精度的色標(biāo)塊邊緣點經(jīng)最小二乘擬合，獲得四條邊緣直線；步驟S6 求四條邊緣直線的交點，獲得四個頂點的圖像坐標(biāo)；步驟S7 計算色標(biāo)塊新的中心點、新的一條長邊中點和新的一條短邊中點；步驟S8 利用η點透視算法，求取機器人位姿；步驟S9 利用正交迭代算法對機器人位姿進行優(yōu)化，得到機器人的姿態(tài)矩陣正交 的位姿，循環(huán)測量則返回S3，單次測量則結(jié)束。本發(fā)明的有益效果本發(fā)明跟蹤色標(biāo)塊三點確定色標(biāo)塊的圖像區(qū)域，在此區(qū)域內(nèi) 按照一定間隔掃描出色標(biāo)塊的四組粗略邊緣點，擬合出四條邊緣直線，解決了邊緣直線提 取的實時性問題；本發(fā)明經(jīng)過坐標(biāo)變換，分別以各條邊緣為縱坐標(biāo)軸，按照邊緣的橫坐標(biāo)計 算業(yè)像素邊緣點，從而將二維圖像平面內(nèi)的亞像素邊緣點求取轉(zhuǎn)換為在一維空間內(nèi)的亞像 素邊緣點求取，既保證了亞像素邊緣點的精度，又保證了算法的效率；本發(fā)明利用亞像素邊 緣點擬合出精確的邊緣直線，求這些直線的交點得到色標(biāo)塊四個頂點的高精度圖像坐標(biāo)， 解決了單攝像機視覺測量精度低的問題，解決了色標(biāo)塊一個角被遮擋時不能測量的問題； 利用PnP和OI算法計算機器人的位姿，解決了測量結(jié)果中姿態(tài)矩陣的正交性問題。本發(fā)明在色標(biāo)塊的一個頂點遮擋時仍然能夠精確獲得機器人的位姿；本發(fā)明能夠 保證機器人位姿中的姿態(tài)矩陣的正交性；本發(fā)明實時性好，對于640X480像素的圖像，能 夠?qū)崿F(xiàn)70次/秒測量；本發(fā)明測量精度高，在攝像機距離目標(biāo)3000mm左右時的測量誤差小 于10mm。因此，本發(fā)明可以大幅度提高視覺測量的可用性。


圖1為單目視覺測量機器人位姿示意圖。圖2A為本發(fā)明基于色標(biāo)塊的視覺實時測量方法的流程圖。圖2B為本發(fā)明獲得具有亞像素精度的邊緣點的流程圖。圖3為按照一定間隔和方向快速掃描邊緣點示意圖。
具體實施例方式為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚明白，以下結(jié)合具體實施例，并參照 附圖，對本發(fā)明進一步詳細說明。請參閱圖1示出單目視覺測量機器人位姿示意圖，利用機器人1、色標(biāo)塊2、攝像機 3、網(wǎng)絡(luò)線4和計算機5，本發(fā)明利用粘貼到機器人背部上方的色標(biāo)塊作為機器人的標(biāo)記，針 對色標(biāo)塊提出了一種新的圖像處理和特征提取方法，以快速獲得矩形色標(biāo)塊四個頂點的高 精度圖像坐標(biāo)。如圖2A示出本發(fā)明基于色標(biāo)塊的視覺實時測量方法的流程圖，其步驟包括如下步驟Sl 在機器人1的背部上方設(shè)有一個已知尺寸的色標(biāo)塊2 ；步驟S2 以單攝像機3采集色標(biāo)塊2的色標(biāo)圖像；步驟S3 跟蹤色標(biāo)塊2的三點，所述三點是中心點、一條長邊中點和一條短邊中 點，確定色標(biāo)塊2圖像區(qū)域；步驟S4 對色標(biāo)圖像按照設(shè)定的間隔和方向掃描處理，獲得具有亞像素精度的邊緣占.步驟S5 將亞像素精度的色標(biāo)塊2的邊緣點經(jīng)最小二乘擬合，獲得四條邊緣直 線.
一入 ，步驟S6 求四條邊緣直線的交點，獲得四個頂點的圖像坐標(biāo)；步驟S7 計算色標(biāo)塊2新的中心點、新的一條長邊中點和新的一條短邊中點；步驟S8 利用η點透視(Perspective-n-Point^nP)算法，求取機器人1的位姿；步驟S9 利用正交迭代(Orthogonal Iteration, 01)算法對機器人1的位姿進行 優(yōu)化，得到機器人1的姿態(tài)矩陣正交的位姿，循環(huán)測量則返回S3，單次測量則結(jié)束。如圖2B示出獲得具有亞像素精度邊緣點的步驟如下步驟S41 在步驟S3確定的色標(biāo)塊圖像區(qū)域內(nèi)，按照設(shè)定間隔和方向快速掃描邊 緣點，獲得對應(yīng)于色標(biāo)塊四條邊緣的四組邊緣點；步驟S42 針對四組邊緣點，利用隨機采樣一致性(RANdom SAmple Consensus, RANSAC)方法去除偏差較大的噪聲點，獲得四組有效邊緣點；步驟S43 針對四組有效邊緣點，利用最小二乘擬合出四條邊緣直線；步驟S44 針對四條邊緣直線分別進行坐標(biāo)變換，依次使得各條邊緣為縱坐標(biāo)軸；步驟S45 分別針對為縱坐標(biāo)軸的各條邊緣上的有效邊緣點，求取具有亞像素精 度的邊緣點。其中，所述亞像素精度的邊緣點采用如下公式求取
χ. + χ, + 2χη σ2 ln(/n //,) σ2 ln(/n /17)μ =———!----—----—~—，
4zKX1-X0) 4(χ2 -Χ0)式中，μ為具有亞像素精度的邊緣點，Xtl為原邊緣上一點的橫坐標(biāo)，X1為原邊緣上 外側(cè)距離一個像素點的橫坐標(biāo)，X2為原邊緣上外側(cè)距離兩個像素點的橫坐標(biāo)，σ為預(yù)先統(tǒng) 計獲得的攝像機成像的近似高斯分布方差，Itl為對應(yīng)于Xtl的原邊緣上一點的像素灰度值， I1為對應(yīng)于X1的原邊緣外一點的像素灰度值，I2為對應(yīng)于X2的原邊緣外一點的像素灰度值。其中，所述設(shè)定間隔的范圍設(shè)為10至20像素。本實施例中間隔設(shè)置為10個像素。其中，所述色標(biāo)塊是矩形色標(biāo)塊或是四邊形色標(biāo)塊。實施例中色標(biāo)塊2選用了紅 色，但也可以選擇其他顏色。其中，所述攝像機是彩色攝像機或是黑白攝像機。請參閱圖2Α和圖3。在圖像處理中，首先跟蹤色標(biāo)塊2中心點Omm、一條長邊中點 Pum和一條短邊中點pvm。
Pvm= (Plm+P2m)/2Pum= (P2m+P3m )/ 2⑴
Omm =(P]m+p2m+P3ra+P4m )/4其中，Plffl, P2ffl, P3ffl, P4ffl是上一次獲得的色標(biāo)塊2的四個特征頂點。根據(jù)上一幅色標(biāo)圖像中中心點0_、長邊中點Pim和短邊中點Pvm的圖像坐標(biāo)，向四 周擴展一定區(qū)域作為當(dāng)前色標(biāo)圖像中色標(biāo)塊2所在的圖像區(qū)域。在圖像區(qū)域內(nèi)按照10個 像素的間隔，分別按照中心點0_到長邊中點Pim的方向和中心點Omm到短邊中點Pvm的方向， 掃描獲得對應(yīng)于色標(biāo)塊2的四條邊緣PlmP2m、P2mP3m、P3mP4m、P4mPlm上的四組邊緣點{EGn，i =1,2, ...，NJ、{EG2i，i = 1，2，...，N2}、{EG3i，i = 1，2，…，N3}、{EG4i，i = 1，2，...，N4}，其 中HN3和N4分別是四組邊緣點的個數(shù)，經(jīng)過RANSAC去噪后得到四組有效邊緣點{EG1J; j = 1,2, -,Nsl}, {EG2J, j = 1,2, -,NsJ, {EG3J, j = 1,2, ···，Ns3}、{EG4j，j = 1，2，…， Ns4I，其中Ns1、Ns2、Ns3和Ns4分別是四組有效邊緣點的個數(shù)，利用有效邊緣點擬合出四條邊緣
自:^^ L]_、1^2 Λ Lg Λ L^ ο根據(jù)每條邊緣直線1^、1^、1^、1^4方向，分別對四組有效邊緣點{EGlj;j = l，2，…， NsJ、(EG2j, j = 1,2,…，NsJ、{EG3J, j = 1,2,…，Ns3}、{EG4J, j = 1,2,…，NsJ 進行旋轉(zhuǎn) 變換，依次使得各條邊緣直線作為變換后的縱軸。根據(jù)預(yù)先統(tǒng)計獲得的攝像機3成像的近 似高斯分布，沿橫軸即垂直于各條邊緣的方向，分別求取具有亞像素精度的邊緣點。以邊緣直線L1和有效邊緣點(EGlj = (Ulj, Vlj)，j = 1，2，-,NsJ為例，說明亞像 素精度的邊緣點的求取。按照直線L1的方向進行旋轉(zhuǎn)變換，使得直線L1的方向作為變換后 的縱軸。經(jīng)過變換后，有效邊緣點EGlj的圖像坐標(biāo)(Ulj，Vlj)被變換為(xljCI，ylj(l)。根據(jù)變 換后有效邊緣點EGlj的圖像坐標(biāo)(xljCI，y1J0)以及外側(cè)圖像點的坐標(biāo)，按照下式計算有效邊 緣點EGlj變換后的亞像素精度的圖像橫坐標(biāo)
權(quán)利要求
1.一種利用色標(biāo)塊的實時視覺測量方法，其特征在于，該方法包括步驟如下 步驟Sl 在機器人的背部上方設(shè)有一個已知尺寸的色標(biāo)塊；步驟S2 以單攝像機采集色標(biāo)塊的色標(biāo)圖像；步驟S3 跟蹤色標(biāo)塊的三點，所述三點是中心點、一條長邊中點和一條短邊中點，確定 色標(biāo)塊圖像區(qū)域；步驟S4:對色標(biāo)圖像按照設(shè)定的間隔和方向掃描處理，獲得具有亞像素精度的邊緣點。步驟S5 將亞像素精度的色標(biāo)塊邊緣點經(jīng)最小二乘擬合，獲得四條邊緣直線； 步驟S6 求四條邊緣直線的交點，獲得四個頂點的圖像坐標(biāo)； 步驟S7 計算色標(biāo)塊新的中心點、新的一條長邊中點和新的一條短邊中點； 步驟S8 利用η點透視算法，求取機器人位姿；步驟S9 利用正交迭代算法對機器人位姿進行優(yōu)化，得到機器人的姿態(tài)矩陣正交的位 姿，循環(huán)測量則返回S3，單次測量則結(jié)束。
2.如權(quán)利要求1所述利用色標(biāo)塊的實時視覺測量方法，其特征在于，獲得具有亞像素 精度的邊緣點的步驟如下步驟S41 在步驟S3確定的色標(biāo)塊圖像區(qū)域內(nèi)，按照設(shè)定間隔和方向快速掃描邊緣點， 獲得對應(yīng)于色標(biāo)塊四條邊緣的四組邊緣點；步驟S42 針對四組邊緣點，利用隨機采樣一致性方法去除偏差較大的噪聲點，獲得四 組有效邊緣點；步驟S43 針對四組有效邊緣點，利用最小二乘擬合出四條邊緣直線； 步驟S44 針對四條邊緣直線分別進行坐標(biāo)變換，依次使得各條邊緣為縱坐標(biāo)軸； 步驟S45 分別針對為縱坐標(biāo)軸的各條邊緣上的有效邊緣點，求取具有亞像素精度的 邊緣點。
3.如權(quán)利要求2所述利用色標(biāo)塊的實時視覺測量方法，其特征在于，所述亞像素精度 的邊緣點采用如下公式求取
4.如權(quán)利要求1所述利用色標(biāo)塊的實時視覺測量方法，其特征在于，所述設(shè)定間隔范 圍為10至20像素。
5.如權(quán)利要求1所述利用色標(biāo)塊的實時視覺測量方法，其特征在于，所述色標(biāo)塊是矩 形色標(biāo)塊或是四邊形色標(biāo)塊。
6.如權(quán)利要求1所述利用色標(biāo)塊的實時視覺測量方法，其特征在于，所述攝像機是彩 色攝像機或是黑白攝像機。
全文摘要
本發(fā)明是基于色標(biāo)塊的視覺實時測量方法，步驟S1在機器人的背部上方設(shè)有已知尺寸的色標(biāo)塊；步驟S2以單攝像機采集色標(biāo)塊的色標(biāo)圖像；步驟S3跟蹤色標(biāo)塊的中心點、長邊中點和短邊中點確定色標(biāo)塊圖像區(qū)域；步驟S4對色標(biāo)圖像按照設(shè)定的間隔和方向掃描處理獲得具有亞像素精度的邊緣點；步驟S5將亞像素精度的色標(biāo)塊邊緣點經(jīng)最小二乘擬合獲得四條邊緣直線；步驟S6求四條邊緣直線的交點獲得四個頂點的圖像坐標(biāo)；步驟S7計算色標(biāo)塊新的中心點、新的一條長邊中點和新的一條短邊中點；步驟S8利用n點透視算法求取機器人位姿；步驟S9利用正交迭代算法對機器人位姿進行優(yōu)化，得到機器人的姿態(tài)矩陣正交的位姿。
文檔編號G01C11/00GK102128617SQ20101059289
公開日2011年7月20日 申請日期2010年12月8日 優(yōu)先權(quán)日2010年12月8日
發(fā)明者任艷青, 張正濤, 徐德, 楊平, 譚民, 陳國棟 申請人:中國科學(xué)院自動化研究所