專利名稱：一種鏡面清潔機器人進行三維重構與定位的系統(tǒng)及方法
技術領域：
本發(fā)明一般涉及太陽能熱發(fā)電領域，具體涉及一種鏡面清潔機器人三維重構與定位的系統(tǒng)及方法。
背景技術：
太陽能熱發(fā)電，也叫聚焦型太陽能熱發(fā)電(Concentrating Solar Power，簡稱 CSP)，通過大量反射鏡以聚焦的方式將太陽能直射光聚集起來，加熱工質，產生高溫高壓的蒸汽，蒸汽驅動汽輪機發(fā)電。太陽能鏡場主要由大量的衍架和安裝在上面的平面反光鏡組成，為提高單位土地面積的光熱利用率，相鄰衍架排列緊湊。鏡場所在的高原地區(qū)全年溫差較大，整個鏡場地面平整度可能會發(fā)生變化。鏡面清潔機器人(或稱鏡面車)用于高海拔地區(qū)太陽能光熱發(fā)電鏡場的鏡面清潔。鏡面車需要在相鄰衍架之間的間隙穿行，并利用安裝在車上的清洗機構清潔平面反光鏡。為保證鏡場設施的安全，需要一種比較精確的定位方案來確定車體和衍架的相對位置關系，進而使鏡面車穩(wěn)定可靠的執(zhí)行任務。傳統(tǒng)的室內外定位方式主要有依靠激光測距傳感器的相對定位，依靠里程計航位推算定位，通過接收GPS信號的絕對定位，也有采用以上幾種方式的組合導航方案。以上方式都有各自獨特優(yōu)勢，激光測距傳感器應用于室內定位可以獲得穩(wěn)定的環(huán)境輪廓，里程計是一種簡單有效的定位方式，GPS更是一種通用性極強的解決方案。然而在本項目的應用中，對以上定位方式提出了新的挑戰(zhàn)。GPS民用信號定位精度依然不能滿足本應用的要求，并且GPS定位信號存在比較大的跳變，對于高頻應用尚力不從心，此外還存在GPS信號丟失的風險。里程計只提供載體前進方向的位移變化量，對于平整度差的路面會存在較大誤差，在單獨應用時存在空轉和滑行的故障風險。2D激光測距傳感器在室外的應用一是受限于傳感器本身的測距量程，二是室外環(huán)境輪廓特征不顯著，尤其是本項目的環(huán)境是結構復雜的衍架，大大增加了不確定性?？傊?，由于本應用中復雜的環(huán)境結構，以及平整度差的路面條件，常用的定位方式均不能滿足要求，需要一種特殊的定位方案。

發(fā)明內容
本發(fā)明的第一目的在于提供一種鏡面清潔機器人進行三維重構與定位的系統(tǒng)，以解決現(xiàn)有技術中在平整度差的路面條件定位不能滿足精度要求的技術問題。本發(fā)明的第二目的在于提供一種鏡面清潔機器人進行三維重構與定位的方法，以解決現(xiàn)有技術中在平整度差的路面條件定位不能滿足精度要求的技術問題。一種鏡面清潔機器人進行三維重構與定位的系統(tǒng)，用于安裝在所述鏡面清潔機器人的車頭部分，其進一步包括激光測距傳感器、電機、偏心輪、搖擺支架、絕對值編碼器、 MEMS慣性傳感器、上位機及里程計，其中，
偏心輪固定在電機上，并傳動在所述搖擺支架上，用于將電機的旋轉運動轉化為支架的搖擺運動；所述激光測距傳感器，固定連接搖擺支架，用于隨搖擺支架的搖擺運動而運動，并獲得激光掃描數(shù)據(jù)；所述絕對值編碼器，設置在激光測距傳感器的搖擺軸上，用于直接測量相對于車體的搖擺角度；MEMS慣性傳感器，用于測量車體在不平整地面運動時相對于地理水平面的橫滾角和俯仰角；里程計安裝在鏡面清潔機器人上；上位機分別連接激光測距傳感器、絕對值編碼器、MEMS慣性傳感器和里程計，用于接收包括激光測距傳感器在內發(fā)送的傳送數(shù)據(jù)，構建一 3D輪廓圖，提取三維數(shù)據(jù)點中的共面數(shù)據(jù)點，確定出衍架位置。所述MEMS慣性傳感器包括陀螺儀和加速度計，所述上位機進一步包括車體姿態(tài)測量計算單元，用于根據(jù)陀螺儀和加速度計的輸出，計算出鏡面清潔機器人這個載體的姿態(tài)；車體姿態(tài)融合計算單元，用于將鏡面清潔機器人這個載體的姿態(tài)，再考慮絕對值編碼器獲取搖擺云臺的俯仰角α，最終計算出激光掃描器相對于當?shù)氐乩硭矫娴膶嶋H姿態(tài)的橫滾角Y和俯仰角θ ；激光數(shù)據(jù)姿態(tài)投投影修正計算單元，用于計算激光數(shù)據(jù)點在地理水平面的投影坐標信息；激光數(shù)據(jù)平移修正計算單元，用于通用測量時間間隔內里程儀的增量進行修正， 并將修正后的數(shù)據(jù)描繪到一三繪坐標系中，形成一掃描點云圖；定位計算單元，用于在該掃描點云圖上定位上預設定的點。一種鏡面清潔機器人進行三維重構與定位的方法，包括接收陀螺儀和加速度計的輸出，計算出鏡面清潔機器人這個載體的姿態(tài)；將鏡面清潔機器人這個載體的姿態(tài)，再考慮絕對值編碼器獲取搖擺云臺的俯仰角 α，最終計算出激光掃描器相對于當?shù)氐乩硭矫娴膶嶋H姿態(tài)的橫滾角Y和俯仰角θ ；計算激光數(shù)據(jù)點在地理水平面的投影坐標信息；通用測量時間間隔內里程儀的增量進行修正，并將修正后的數(shù)據(jù)描繪到一三繪坐標系中，形成一掃描點云圖；在該掃描點云圖上定位上預設定的點。與現(xiàn)有技術相比，本發(fā)明提供一種鏡面清潔機器人進行三維重構與定位的系統(tǒng)， 本發(fā)明提供的系統(tǒng)能夠在復雜的環(huán)境結構以及平整度差的路面條件下，完成重構定位工作。還有，本發(fā)明可以提供利用二維激光雷達進行三維重構與定位操作，成本非常低， 而且實現(xiàn)方便。


圖1為一種鏡面清潔機器人進行三維重構與定位的部分系統(tǒng)結構正視示意圖2為一種鏡面清潔機器人進行三維重構與定位的部分系統(tǒng)結構后視示意圖；圖3為一種鏡面清潔機器人進行三維重構與定位的部分系統(tǒng)結構側視圖；圖4為一種鏡面清潔機器人進行三維重構與定位的流程圖；圖5為一現(xiàn)有坐標定義圖。
具體實施例方式以下結合附圖，具體說明本發(fā)明。請參閱圖1、圖2、圖3，一種鏡面清潔機器人進行三維重構與定位的系統(tǒng)。用于安裝在所述鏡面清潔機器人的車頭部分，其進一步包括激光測距傳感器15、電機11、偏心輪 13、搖擺支架、絕對值編碼器、MEMS慣性傳感器、上位機及里程計，其中，偏心輪13固定在電機11上，并傳動在搖擺支架上，用于將電機的旋轉運動轉化為支架的搖擺運動。激光測距傳感器15，固定連接搖擺支架，用于隨搖擺支架的搖擺運動而運動，并獲得激光掃描數(shù)據(jù)；所述絕對值編碼器，設置在激光測距傳感器15的搖擺軸上，用于直接相對于車體搖擺角度；MEMS慣性傳感器，用于測量車體在不平整地面運動時相對于地理水平面的橫滾角和俯仰角；里程計安裝在鏡面清潔機器人上；上位機分別連接激光測距傳感器15、絕對值編碼器、MEMS慣性傳感器和里程計， 用于接收包括激光測距傳感器在內發(fā)送的傳送數(shù)據(jù)，構建一 3D輪廓圖，提取三維數(shù)據(jù)點中的共面數(shù)據(jù)點，確定出衍架位置。激光測距傳感器沿著搖擺支點17搖擺，搖擺支點17可以固定在鏡面清潔機器人上。搖擺支架至少包括上支架141和下支架142，上支架141和下支架142構成一中空的長方形狀，偏心輪13設置在所述中空的長方形內，偏心輪13落在所述下支架142上，下支架的左豎支條可以通過支點18與激光測距傳感器15固定連接，下支架142的下橫支條與所豎支條連接，偏心輪13在下支架的半包圍圈內運動，帶動下支架142的搖擺運動。本實例中還包括一支撐條，支撐條的一端連接上支架的右豎支條的下端，另一端活動連接激光測距傳感器上。這樣，偏心輪13在下支架的半包圍圈內運動時，至少能被支撐條托住。電機旋轉帶動偏心輪，使支架產生往復的搖擺運動，該搖擺運動的相對角度通過絕對值編碼器測得。激光傳感器固連于支架，在一個搖擺周期內定時采樣一定數(shù)目的二維掃描幀(支架搖擺的幅度大約為20°，這個幅度是通過偏心輪安裝偏心量可調的，在一個搖擺周期內每隔2°進行一次激光掃描數(shù)據(jù)的采集，即一個搖擺來回進行20幀激光掃描)。在本實例中所述偏心輪的直徑等于所述下支架的左豎支條的高。所述激光測距傳感器設置在搖擺支架上或電機的一端。激光測距傳感器可以采用二維激光測距傳感器，以減少成本，也可以直接采用三維激光測距傳感器。在本實例中，所述MEMS慣性傳感器包括陀螺儀和加速度計，所述上位機進一步包括
車體姿態(tài)測量計算單元，用于根據(jù)陀螺儀和加速度計的輸出，計算出鏡面清潔機器人這個載體的姿態(tài)；車體姿態(tài)融合計算單元，用于將鏡面清潔機器人這個載體的姿態(tài)，再考慮絕對值編碼器獲取搖擺云臺的俯仰角α，最終計算出激光掃描器相對于當?shù)氐乩硭矫娴膶嶋H姿態(tài)的橫滾角Y和俯仰角θ ；激光數(shù)據(jù)姿態(tài)投投影修正計算單元，用于計算激光數(shù)據(jù)點在地理水平面的投影坐標信息；激光數(shù)據(jù)平移修正計算單元，用于通用測量時間間隔內里程儀的增量進行修正， 并將修正后的數(shù)據(jù)描繪到一三繪坐標系中，形成一掃描點云圖；定位計算單元，用于在該掃描點云圖上定位上預設定的點。針對上述的系統(tǒng)，一種鏡面清潔機器人進行三維重構與定位的方法(請參閱圖 4)，包括首先進行SllO 接收陀螺儀和加速度計的輸出，計算出鏡面清潔機器人這個載體的姿態(tài).(車體姿態(tài)測量)說明通過慣性傳感器進行載體姿態(tài)測量這個算法是一個成熟的算法，在這里我們只是重復實現(xiàn)并應用到項目中。陀螺儀數(shù)據(jù)進行卡爾曼濾波時間更新；根據(jù)加速度計的測量值判斷車的運動狀態(tài)，判斷的結果決定是否利用加速度計的量測值進行卡爾曼濾波量測更新；以上完成后根據(jù)濾波器狀態(tài)估計結果來解算姿態(tài)。(目的輸入角速率(陀螺儀測量)和加速度(加速度計測量)，輸出載體相對于當?shù)氐乩硭矫娴淖藨B(tài)俯仰角、橫滾角。為了說明清楚，下面分三部分說1.坐標系和角度量的定義2.由加速度計測量值判斷運動狀態(tài)3.系統(tǒng)方程4.姿態(tài)解算)1.定義(請參閱圖5)載體坐標系b 原點ο位于載體運動中心，OXb軸朝向載體右方，oyb軸朝向載體正前方，OZb與ο、、oyb軸按照右手法則構成直角坐標系0-xbyb、 當?shù)氐乩碜鴺讼郸?原點0與b系原點重合，OX軸朝地理東向，OY軸朝地理北向， OZ與0Y、OY軸按照右手法則構成直角坐標系O-XYZ載體相對于地理坐標系的空間角位置可由俯仰角θ和橫滾角Υ來表示，定義如下俯仰角θ，載體縱軸相對于當?shù)氐仄矫娴膴A角.橫滾角Υ，載體的縱向對稱面相對于通過載體縱軸鉛垂面所旋轉的角度.2.運動狀態(tài)判斷將載體的運動狀態(tài)分為兩種1)無主動機動。這種狀態(tài)下通過加速度計測量值可以確定載體相對地理水平面的姿態(tài)，因此可以用來作為一種冗余的信息來校正載體姿態(tài)；幻有主動機動。與上面相反，加速度計測量不能用于確定載體姿態(tài)。判斷原理當載體沒有線加速度時，載體為無主動機動。此時加速度計測量值
fb = [fx /； f^J ,其模值應為1個當?shù)刂亓铀俣?9. 8m/s2) ·
設f；； =-,g為當?shù)刂亓铀俣戎?。當時，認為載體無主動機動；當時，認為載體有主動機動；其中α a為判斷閾值，一般取不超過5% .3.系統(tǒng)方程輸入量角速率量測<=CBbnby ω, J ,加速度量測f6 = [fx /； //J .考
慮與θ、Y有關的狀態(tài)向量C3 C31 -sin χ cos 汐

以及等效陀螺漂移滬=Sby

其動力學方程為
權利要求
1.一種鏡面清潔機器人進行三維重構與定位的系統(tǒng)，其特征在于，用于安裝在所述鏡面清潔機器人的車頭部分，其進一步包括激光測距傳感器、電機、偏心輪、搖擺支架、絕對值編碼器、MEMS慣性傳感器、上位機及里程計，其中，偏心輪固定在電機上，并傳動在所述搖擺支架上，用于將電機的旋轉運動轉化為支架的搖擺運動；所述激光測距傳感器，固定連接搖擺支架，用于隨搖擺支架的搖擺運動而運動，并獲得激光掃描數(shù)據(jù)；所述絕對值編碼器，設置在激光測距傳感器的搖擺軸上，用于直接測量相對于車體的搖擺角度；MEMS慣性傳感器，用于測量車體在不平整地面運動時相對于地理水平面的橫滾角和俯仰角；里程計安裝在鏡面清潔機器人上；上位機分別連接激光測距傳感器、絕對值編碼器、MEMS慣性傳感器和里程計，用于接收包括激光測距傳感器在內發(fā)送的傳送數(shù)據(jù)，構建一 3D輪廓圖，提取三維數(shù)據(jù)點中的共面數(shù)據(jù)點，確定出衍架位置。
2.如權利要求1所述的系統(tǒng)，其特征在于，所述激光測距傳感器沿著搖擺支點搖擺，所述搖擺支點固定在所述鏡面清潔機器人上。
3.如權利要求1或2所述的系統(tǒng)，其特征在于，所述搖擺支架至少包括上支架和下支架，所述上支架和下支架構成一中空的長方形狀，所述偏心輪設置在所述中空的長方形內， 所述偏心輪落在所述下支架上，下支架的左豎支條與所述激光測距傳感器固定連接，所述下支架的下橫支條與所述左豎支條連接，所述偏心輪在下支架的半包圍圈內運動，帶動下支架的搖擺運動。
4.如權利要求3所述的系統(tǒng)，其特征在于，所述偏心輪的直徑等于所述下支架的左豎支條的高。
5.如權利要求3所述的系統(tǒng)，其特征在于，還包括一支撐條，所述支撐條的一端連接所述上支架的右豎支條的下端，另一端活動連接激光測距傳感器上。
6.如權利要求1所述的系統(tǒng)，其特征在于，所述激光測距傳感器設置在搖擺支架上或電機的一端。
7.如權利要求2所述的系統(tǒng)，其特征在于，所述激光測距傳感器采用二維激光測距傳感器。
8.如權利要求1所述的系統(tǒng)，其特征在于，所述MEMS慣性傳感器包括陀螺儀和加速度計，所述上位機進一步包括車體姿態(tài)測量計算單元，用于根據(jù)陀螺儀和加速度計的輸出，計算出鏡面清潔機器人這個載體的姿態(tài)；車體姿態(tài)融合計算單元，用于將鏡面清潔機器人這個載體的姿態(tài)，再考慮絕對值編碼器獲取搖擺云臺的俯仰角α，最終計算出激光掃描器相對于當?shù)氐乩硭矫娴膶嶋H姿態(tài)的橫滾角Y和俯仰角θ ；激光數(shù)據(jù)姿態(tài)投投影修正計算單元，用于計算激光數(shù)據(jù)點在地理水平面的投影坐標信息；激光數(shù)據(jù)平移修正計算單元，用于通用測量時間間隔內里程儀的增量進行修正，并將修正后的數(shù)據(jù)描繪到一三繪坐標系中，形成一掃描點云圖； 定位計算單元，用于在該掃描點云圖上定位上預設定的點。
9.一種鏡面清潔機器人進行三維重構與定位的方法，其特征在于，包括 接收陀螺儀和加速度計的輸出，計算出鏡面清潔機器人這個載體的姿態(tài)；將鏡面清潔機器人這個載體的姿態(tài)，再考慮絕對值編碼器獲取搖擺云臺的俯仰角α， 最終計算出激光掃描器相對于當?shù)氐乩硭矫娴膶嶋H姿態(tài)的橫滾角Y和俯仰角θ ； 計算激光數(shù)據(jù)點在地理水平面的投影坐標信息；通用測量時間間隔內里程儀的增量進行修正，并將修正后的數(shù)據(jù)描繪到一三繪坐標系中，形成一掃描點云圖；在該掃描點云圖上定位上預設定的點。
10.如權利要求9所述的方法，其特征在于，在該掃描點云圖上定位上預設定的點進一步包括在3D輪廓云圖上提取特征平面；確定載體相對兩側衍架的位置和載體前向與衍架方向的夾角。
全文摘要
一種鏡面清潔機器人進行三維環(huán)境重構與定位的系統(tǒng)，激光測距傳感器、包括電機、偏心輪、支架的搖擺支架、絕對值編碼器、MEMS慣性傳感器、上位機及里程計，其中，包括電機、偏心輪和支架的搖擺支架，支架設置在鏡面清潔機器人上，支架的前端設置電機，偏心輪連接所述電機，并傳動在所述支架上，用以將電機的旋轉運動轉化為支架的搖擺運動；激光測距傳感器，連動與所述支架，用于實時獲得到障礙物之間的激光掃描數(shù)據(jù)；所述絕對值編碼器，設置在激光測距傳感器的搖擺軸，用于直接測量激光測距傳感器相對于車體的搖擺角度；MEMS慣性傳感器，安裝所述搖擺支架上，用于測量車體在不平整地面運動時相對于地理水平面的橫滾角和俯仰角；上位機分別連接激光測距傳感器、絕對值編碼器、MEMS慣性傳感器和里程計，用于接收包括激光測距傳感器在內發(fā)送的傳送數(shù)據(jù)，構建一3D輪廓圖，提取三維數(shù)據(jù)點中的共面數(shù)據(jù)點，確定出衍架位置。
文檔編號G01C21/00GK102564416SQ201110455468
公開日2012年7月11日 申請日期2011年12月30日 優(yōu)先權日2011年12月30日
發(fā)明者吳劍, 沈慧, 沈繼中, 陶熠昆 申請人:浙江國自機器人技術有限公司