本發(fā)明屬于視覺(jué)和運(yùn)動(dòng)控制領(lǐng)域，尤其涉及一種基于視覺(jué)和超聲波組合的智能輪椅床對(duì)接控制方法。

背景技術(shù)：

人口老齡化的快速增長(zhǎng)以及對(duì)病殘人的護(hù)理成為了社會(huì)的一個(gè)重要問(wèn)題。在對(duì)老齡人和病殘人的護(hù)理中，會(huì)將老人和病殘人從床和輪椅進(jìn)行多次的轉(zhuǎn)移，因此需要一種床椅一體化系統(tǒng)。智能輪椅床系統(tǒng)不僅可以降低護(hù)理人員的工作量，而且有利于增強(qiáng)老人及病殘人的獨(dú)立生活能力。這對(duì)解決老人及病殘人的護(hù)理問(wèn)題具有重要的意義。

輪椅和床體組合時(shí)存在對(duì)接問(wèn)題。不同的輪椅床結(jié)構(gòu)及傳感器也有著不同的自動(dòng)對(duì)接策略。申請(qǐng)?zhí)?01410682694.1的專利公開(kāi)了一種利用紅外人工路標(biāo)實(shí)現(xiàn)自動(dòng)對(duì)接的方法。該方法是將紅外光源按一定的幾何形狀安裝在床體，然后使用裝有濾光片的相機(jī)采集圖像并進(jìn)行濾波和邊緣提取，最后通過(guò)對(duì)人工路標(biāo)的計(jì)算實(shí)現(xiàn)定位對(duì)接。此方法中使用的相機(jī)需要安裝濾光片，這樣就會(huì)增加傳感器設(shè)計(jì)的復(fù)雜度。此外，紅外人工路標(biāo)的提取不僅算法復(fù)雜而且也會(huì)存在提取誤差，這樣就會(huì)降低對(duì)接的準(zhǔn)確度。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)當(dāng)前的輪椅床對(duì)接問(wèn)題，本發(fā)明提出一種基于視覺(jué)系統(tǒng)和超聲波組合的自動(dòng)對(duì)接方法，引入人工路標(biāo)-棋盤格作為視覺(jué)引導(dǎo)物和超聲波測(cè)距反饋組合實(shí)現(xiàn)床椅系統(tǒng)的對(duì)接。此方法不需要對(duì)床體結(jié)構(gòu)和傳感器進(jìn)行特殊設(shè)計(jì)，同時(shí)降低算法的復(fù)雜度，具有一定的魯棒性和有效性。

應(yīng)用該方法的輪椅床系統(tǒng)：人工路標(biāo)安裝在床體，輪椅可以利用相機(jī)識(shí)別到人工路標(biāo)；相機(jī)安裝在輪椅的左扶手側(cè)，具有紅外功能；三個(gè)超聲波安裝在輪椅的左側(cè)；其中，人工路標(biāo)使用的是棋盤格。

一種基于視覺(jué)系統(tǒng)和超聲波組合的輪椅床對(duì)接方法，包括以下步驟：

步驟1：相機(jī)標(biāo)定獲得相機(jī)的內(nèi)參及去畸變系數(shù)，采用張正友平面標(biāo)定法進(jìn)行攝像機(jī)的標(biāo)定；

步驟2：通過(guò)視覺(jué)系統(tǒng)采集輪椅左側(cè)的圖像，并提取圖像中的人工路標(biāo)，目標(biāo)引導(dǎo)物為對(duì)接的重要人工標(biāo)志，需要將輪椅放在可以觀測(cè)到目標(biāo)引導(dǎo)物的區(qū)域。當(dāng)相機(jī)視野觀測(cè)不到路標(biāo)時(shí)，可以通過(guò)轉(zhuǎn)動(dòng)找到目標(biāo)引導(dǎo)物；

步驟3：分別在人工路標(biāo)、相機(jī)和輪椅上建立世界、相機(jī)和輪椅坐標(biāo)系系統(tǒng)；

步驟4:根據(jù)建立的坐標(biāo)系系統(tǒng)計(jì)算輪椅相對(duì)人工路標(biāo)的位置和偏轉(zhuǎn)角；

步驟5：當(dāng)輪椅相對(duì)人工路標(biāo)的距離小于設(shè)定閾值采用超聲波完成對(duì)接。

同現(xiàn)有的輪椅床對(duì)接方法相比，本發(fā)明的優(yōu)勢(shì)如下：

(1)本發(fā)明使用的人工路標(biāo)制作簡(jiǎn)單，特征明顯。

(2)本發(fā)明采用的傳感器及床椅對(duì)接環(huán)境設(shè)計(jì)簡(jiǎn)單，具有較好的實(shí)用性。

(3)人工路標(biāo)的提取及算法復(fù)雜度低且具有較好的魯棒性。

附圖說(shuō)明

圖1為輪椅床對(duì)接示意圖；

圖2為本發(fā)明的總體流程圖；

圖3為基于視覺(jué)和超聲波組合的對(duì)接流程圖；

圖4為建立世界、相機(jī)和輪椅坐標(biāo)系系統(tǒng)；

圖5為世界、相機(jī)和輪椅坐標(biāo)系位姿變換關(guān)系；

圖6(a)-圖6(d)為實(shí)際環(huán)境圖像的采集及位姿計(jì)算結(jié)果：圖6(a)輪椅和床相距50cm,輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為0度的結(jié)果；圖6(b)輪椅和床相距100cm，輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為0度的結(jié)果；圖6(c)輪椅和床相距100cm，輪椅相對(duì)床向里偏轉(zhuǎn)，輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為20度的結(jié)果；圖6(d)輪椅和床相距100cm，輪椅相對(duì)床向外偏轉(zhuǎn)，輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為20度的結(jié)果。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明作詳細(xì)說(shuō)明。

本發(fā)明提出基于視覺(jué)引導(dǎo)和超聲波傳感器組合的方法完成泊位對(duì)接，對(duì)接示意圖如圖1所示。本發(fā)明是將制作好的人工路標(biāo)固定在輔助床，輪椅通過(guò)自主運(yùn)動(dòng)可以找到相機(jī)視野中的人工路標(biāo)。工控機(jī)根據(jù)視覺(jué)系統(tǒng)計(jì)算的輪椅相對(duì)床的位置和姿態(tài)角控制輪椅向輔助床移動(dòng)，當(dāng)視覺(jué)系統(tǒng)計(jì)算輪椅相對(duì)人工路標(biāo)的距離小于設(shè)定的閾值時(shí)，由于相機(jī)視角原因，工控機(jī)不能從相機(jī)采集到的圖像中識(shí)別人工路標(biāo)從而采用超聲波閉環(huán)方式完成對(duì)接。

上述人工路標(biāo)使用的是一張尺寸為長(zhǎng)14.8cm、寬10.6cm的棋盤格,棋盤格上特征點(diǎn)的間隔為13mm。制作方法是以a4紙的1/4打印標(biāo)準(zhǔn)的棋盤格。

上述超聲波傳感器安裝在輪椅的左側(cè)且分布在同一水平直線。輪椅從前向后安裝超聲波1、超聲波2和超聲波3。超聲波1和輪椅底座前邊界距離為0.07m,超聲波1和超聲波2距離為0.26m，超聲波2和超聲波3距離為0.34m，超聲波3和輪椅底座后邊界距離為0.07m。

上述相機(jī)固定在輪椅的左扶手側(cè)，安裝在輪椅左側(cè)中點(diǎn)且距離地面高度為0.375m。相機(jī)采集到的圖像通過(guò)usb端口上傳到工控機(jī)。工控機(jī)完成圖像校正、人工路標(biāo)識(shí)別位姿計(jì)算以及控制策略。

如圖2和如圖3所示，智能輪椅的自動(dòng)對(duì)接具體過(guò)程如下：

步驟1，相機(jī)標(biāo)定獲得內(nèi)參和去畸變系數(shù)。

(1)打印一張尺寸為a4的靶標(biāo)模板，靶標(biāo)上特征點(diǎn)的間隔為30mm。

(2)從多個(gè)角度拍攝靶標(biāo)。拍攝時(shí)，應(yīng)盡量讓靶標(biāo)占滿屏幕，并保證靶標(biāo)的每一個(gè)角都在屏幕中，一共拍攝10張靶標(biāo)圖像,每張圖像分辨率為1920*1080。

(3)將采集到的10張靶標(biāo)圖像使用張正友平面標(biāo)定法進(jìn)行攝像機(jī)的標(biāo)定。

(4)通過(guò)相機(jī)標(biāo)定獲得相機(jī)的內(nèi)參矩陣及去畸變系數(shù)：

其中，(fx,fy)是相機(jī)的歸一化焦距，(u0,v0)是主點(diǎn)坐標(biāo)，γ是u軸和v軸的不垂直因子。k1,k2為徑向畸變系數(shù)，k3,k4為切向畸變系數(shù)。

步驟2，視覺(jué)系統(tǒng)采集圖像并提取人工路標(biāo)。

(1)假定輪椅放在與輔助床相對(duì)距離合理范圍內(nèi)，在范圍內(nèi)輪椅通過(guò)調(diào)整姿態(tài)可以觀測(cè)到人工路標(biāo)”。

(2)相機(jī)采集輪椅左側(cè)的圖像，并傳到工控機(jī)進(jìn)行圖像處理。

(3)利用步驟1獲得的去畸變系數(shù)對(duì)原始圖像進(jìn)行畸變校正。

(4)使用圖像處理中的棋盤格角點(diǎn)檢測(cè)方法提取圖像中的人工路標(biāo)。

(5)當(dāng)人工路標(biāo)不在相機(jī)視野時(shí)，工控機(jī)控制輪椅以0.02m/s的速度以一定的方向原地轉(zhuǎn)動(dòng)一定的角度停止。

(6)重復(fù)步驟(2)～(5),直到提取到人工路標(biāo)。

步驟3，分別在人工路標(biāo)、相機(jī)和輪椅上建立世界、相機(jī)和輪椅坐標(biāo)系系統(tǒng)，如圖4所示。

(1)根據(jù)步驟2獲得人工路標(biāo)。

(2)根據(jù)人工路標(biāo)建立世界坐標(biāo)系，棋盤格的特征點(diǎn)之間的距離可以通過(guò)測(cè)量得到。ow-xwywzw表示為世界坐標(biāo)系，原點(diǎn)ow為棋盤格的第一行(從下往上為一、二、三)與第五列的交點(diǎn)，xw沿輔助床體長(zhǎng)邊水平指向(如圖4所示方向)，yw垂直向上，zw指向前方。

(3)oc-xcyczc表示為相機(jī)坐標(biāo)系，原點(diǎn)oc為相機(jī)的光學(xué)中心，zc軸沿相機(jī)的光軸指向正前方，xc軸和yc軸與像素坐標(biāo)系的u軸和v軸平行。

(4)o-xyz表示為輪椅坐標(biāo)系，原點(diǎn)o為輪椅的左側(cè)中點(diǎn)且與相機(jī)等高，x軸、y軸和z軸分別與相機(jī)坐標(biāo)系的xc軸、yc軸、zc軸平行。

步驟4，根據(jù)坐標(biāo)系系統(tǒng)計(jì)算輪椅相對(duì)人工路標(biāo)的位置和偏轉(zhuǎn)角，如圖5所示。具體計(jì)算方法如下：

(1)攝像機(jī)坐標(biāo)系和世界坐標(biāo)系可以用旋轉(zhuǎn)矩陣r和平移矩陣t來(lái)描述。用齊次坐標(biāo)和矩陣表示上述關(guān)系為：

其中，r為3*3正交單位矩陣，t為3*1三維平移向量，0^t為(0,0,0)。

(2)第i個(gè)特征點(diǎn)pi在兩個(gè)坐標(biāo)系下的坐標(biāo)分別為:ci＝(xci,yci,zci)^t和wi＝(xwi,ywi,zwi)^t,則兩個(gè)坐標(biāo)系間的變換表達(dá)式為：

ci＝rwi+t

世界坐標(biāo)系的原點(diǎn)到攝像機(jī)坐標(biāo)的轉(zhuǎn)換表達(dá)式為：

c＝r0w+t0

相機(jī)坐標(biāo)系轉(zhuǎn)換到輪椅坐標(biāo)系的表達(dá)式為：

s＝r1c+t1

世界坐標(biāo)系轉(zhuǎn)換到輪椅坐標(biāo)系的表達(dá)式為：

s＝r2w+t2

其中，s＝(x,y,z)^t為輪椅坐標(biāo)，r2＝r1r0,t2＝r1t0+t1,r1＝i。

(3)從平移矩陣t2提取x值和z值。

(4)根據(jù)(2)計(jì)算得到的旋轉(zhuǎn)矩陣計(jì)算輪椅和人工路標(biāo)的姿態(tài)角。

輪椅的對(duì)接問(wèn)題只存在偏航角的變化，因此，只需要根據(jù)旋轉(zhuǎn)矩陣計(jì)算偏航角。計(jì)算公式如下：

當(dāng)r31不等于±1時(shí)，表達(dá)式為：

θ＝-arcsin(r31)

當(dāng)r31＝+1時(shí)，表達(dá)式為：

θ＝π/2

當(dāng)r31＝-1時(shí)，表達(dá)式為：

θ＝-π/2

步驟5，深度值小于設(shè)置的閾值，采用超聲波對(duì)接。

(1)根據(jù)步驟4得到的深度值小于設(shè)置的閾值時(shí)，視覺(jué)對(duì)接切換到超聲波對(duì)接。

(2)利用超聲波1和超聲波3的距離差調(diào)整姿態(tài)角。

(3)輪椅控制系統(tǒng)根據(jù)超聲波的數(shù)據(jù)控制輪椅向輔助床移動(dòng)

(4)重復(fù)(2)(3),超聲波測(cè)量距離小于設(shè)置的閾值，完成對(duì)接。

為驗(yàn)證本發(fā)明所述方法的有效性，下面給出應(yīng)用本發(fā)明所述方法在室內(nèi)真實(shí)環(huán)境下進(jìn)行位姿計(jì)算的一個(gè)實(shí)驗(yàn)實(shí)例。

輪椅床系統(tǒng)使用的是主頻為2.5ghz的mio-5290研華工控機(jī)，圖像采集使用的是顏色分辨率為1920*1080的高清攝像機(jī)。在不同距離和不同角度進(jìn)行采集圖像并計(jì)算輪椅相對(duì)人工路標(biāo)的位置和姿態(tài)角，如圖6所示。(a)輪椅和床相距50cm,輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為0度的結(jié)果；(b)輪椅和床相距100cm，輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為0度的結(jié)果；(c)輪椅和床相距100cm，輪椅相對(duì)床向里偏轉(zhuǎn)，輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為20度的結(jié)果；(d)輪椅和床相距100cm，輪椅相對(duì)床向外偏轉(zhuǎn)，輪椅的底座長(zhǎng)邊和輔助床的長(zhǎng)邊夾角為20度的結(jié)果。

實(shí)驗(yàn)結(jié)果表明，本發(fā)明所述方法能夠確定輪椅和人工路標(biāo)的相對(duì)位姿，且具有較好的精確度和魯棒性。