本發(fā)明涉及一種面向家庭服務(wù)機器人的聯(lián)合定位系統(tǒng)及方法。

背景技術(shù)：

伴隨著人們對精確無線定位的迫切需求，其技術(shù)發(fā)展越來越迅猛，應(yīng)用領(lǐng)域也變的更加廣闊，例如：火場中被救人員的精確定位，物流倉庫中貨物的定位，家庭環(huán)境中服務(wù)機器人的定位等。目前實際工程中應(yīng)用廣泛的無線定位方法主要有：藍牙無線定位方法、wi-fi無線定位方法、zigbee無線定位方法等等。隨著無線通信技術(shù)的發(fā)展，可用的頻譜資源日漸匱乏，因此，迫切需要提出一種與其他通信頻譜互不干擾的通信技術(shù)，超寬帶(ultrawideband,uwb)定位技術(shù)應(yīng)運而生。

機器人自定位的精準性作為服務(wù)機器人重要的性能指標之一，是機器人完成運動控制和其它任務(wù)功能模塊的基礎(chǔ)。而傳統(tǒng)的無線定位方法存在著控制精度不高、控制算法過程復(fù)雜和計算量大等各種不足，如fang算法利用已知節(jié)點與未知節(jié)點之間的tdoa值建立雙曲線方程，然后對方程進行線性化近似求得未知節(jié)點的位置估計，其不能通過增加參考點數(shù)目提高定位精度，只能使用固定的tdoa測量值進行定位等。

因此研究基于超寬帶的定位技術(shù)具有重大的實際意義。

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

本發(fā)明為了解決上述問題，提出了一種面向家庭服務(wù)機器人的聯(lián)合定位系統(tǒng)及方法，本發(fā)明能夠?qū)崿F(xiàn)機器人在室內(nèi)環(huán)境下的絕對定位。

為了實現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：

一種面向家庭服務(wù)機器人的聯(lián)合定位系統(tǒng)，包括定位節(jié)點系統(tǒng)和機器人系統(tǒng)，其中：

所述定位節(jié)點系統(tǒng)包括一個標簽節(jié)點和多個錨節(jié)點，所述標簽節(jié)點設(shè)置于機器人本體上，跟隨機器人本體運動，所述錨節(jié)點記錄錨節(jié)點到標簽節(jié)點的toa值，并將該數(shù)據(jù)回傳給標簽節(jié)點；

所述機器人系統(tǒng)，包括設(shè)于機器人本體的上層處理系統(tǒng)和下層控制系統(tǒng)，所述上層處理系統(tǒng)包括處理器，被配置為采用fang算法和泰勒級數(shù)展開法進行聯(lián)合定位，確定標簽節(jié)點與各個錨節(jié)點的位置，以實現(xiàn)對于機器人的定位，并控制機器人本體進行相應(yīng)的運動；

所述下層控制系統(tǒng)，包括無線通信模塊和控制器，控制器通過無線通信模塊與上層處理系統(tǒng)通信，接收上層處理系統(tǒng)下達的控制指令，控制機器人本體執(zhí)行相應(yīng)的運動操作。

進一步的，所述定位節(jié)點系統(tǒng)包括一個標簽節(jié)點和至少三個錨節(jié)點，錨節(jié)點在分別固定在坐標確定的某個位置。

進一步的，所述標簽節(jié)點與錨節(jié)點之間用的協(xié)議是ieee超寬帶協(xié)議。

進一步的，所述下層控制系統(tǒng)包括主控制器，所述主控制器連接有電源模塊、藍牙模塊、存儲模塊和超寬帶芯片。

進一步的，所述主控制器與超寬帶芯片之間采用spi協(xié)議。

進一步的，所述主控制器與顯示屏連接，顯示調(diào)試信息，且與顯示屏采用的是i²e協(xié)議。

進一步的，所述機器人本體外表面設(shè)置避障探測器。

進一步的，所述機器人本體下端設(shè)置有若干個跌落探測器。

基于上述系統(tǒng)的工作方法，在機器人本體上設(shè)置標簽節(jié)點，跟隨機器人本體運動，在機器人本體的活動區(qū)域內(nèi)設(shè)置多個位于不同位置的錨節(jié)點，記錄錨節(jié)點到標簽節(jié)點的toa值，并將該數(shù)據(jù)回傳給標簽節(jié)點；接收標簽節(jié)點采集的各個toa值，采用fang算法和泰勒級數(shù)展開法進行聯(lián)合定位，確定標簽節(jié)點與各個錨節(jié)點的位置，以實現(xiàn)對于機器人的定位，并控制機器人本體進行相應(yīng)的運動。

進一步的，聯(lián)合定位的具體步驟包括：對得到的toa值進行求差，得到一組tdoa，對獲得的每一組tdoa數(shù)據(jù)信號直波鑒定，如果在數(shù)據(jù)中不存在nlos情況，對該組tdoa數(shù)據(jù)進行fang算法估計，否則對該組tdoa數(shù)據(jù)進行fang算法估計，確定初始值，運用泰勒級數(shù)展開法對初始值進行迭代，確定目標節(jié)點位置估計。

進一步的，上層處理系統(tǒng)被配置有藍牙串口數(shù)據(jù)包的接收線程、tdoa定位算法線程和節(jié)點配置線程，當定位系統(tǒng)開始運行時，上層處理系統(tǒng)接收上傳的數(shù)據(jù)，當數(shù)據(jù)包接收完成并且校驗正確后對數(shù)據(jù)包進行解析，之后調(diào)用tdoa定位算法函數(shù)進行定位計算。

進一步對，上電后系統(tǒng)首先進行初始化，檢查系統(tǒng)存儲歷史數(shù)據(jù)標志位是否置位，如果置位，則直接將存儲的數(shù)據(jù)通過無線的方式發(fā)送給上位機，發(fā)送完畢以后，開啟測距算法；如果存儲歷史數(shù)據(jù)標志位沒有被置位，直接開啟測距算法，在測距成功以后，底層會對數(shù)據(jù)進行整理打包，然后約定的協(xié)議通過無線的方式發(fā)給上位機，如果發(fā)送成功，則直接進行倒計時準備下一次測距算法的開啟；如果不成功，則將數(shù)據(jù)保存在外部存儲器，等待網(wǎng)絡(luò)恢復(fù)后再上傳數(shù)據(jù)。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果為：

家庭服務(wù)機器人所處的家庭環(huán)境通常比較復(fù)雜，機器人的定位是機器人完成運動和其他任務(wù)模塊的基礎(chǔ)，本文采用最新的超寬帶定位技術(shù)，對室內(nèi)靜止節(jié)點與運動節(jié)點進行了測試，定位精度超過傳統(tǒng)無線定位方式。

該定位系統(tǒng)底層采用雙向測距算法，上位機采用了聯(lián)合定位算法：fang算法-泰勒級數(shù)展開法，是該系統(tǒng)具有較高的定位性能。

為了確保機器人的安全可靠運行，服務(wù)機器人具備故障檢測及自處理功能，可以實時監(jiān)控機器人的運行狀況，及時處理瘋跑、過流、通信亂碼等機器故障。同時，該機器人配有避障系統(tǒng)和防跌落系統(tǒng)，在用戶使用的過程中自動規(guī)避障礙物，起到自我保護的作用。

附圖說明

構(gòu)成本申請的一部分的說明書附圖用來提供對本申請的進一步理解，本申請的示意性實施例及其說明用于解釋本申請，并不構(gòu)成對本申請的不當限定。

圖1為本發(fā)明的系統(tǒng)總體架構(gòu)；

圖2為本發(fā)明的系統(tǒng)硬件總體結(jié)構(gòu)；

圖3為本發(fā)明的定位系統(tǒng)底層的主要功能框圖；

圖4為本發(fā)明的定位系統(tǒng)底層軟件執(zhí)行過程；

圖5為本發(fā)明的標簽節(jié)點與錨節(jié)點顯示界面；

圖6為本發(fā)明的rtls定位流程圖；

圖7為本發(fā)明的機器人仰視圖；

圖8為本發(fā)明的側(cè)視圖。

具體實施方式：

下面結(jié)合附圖與實施例對本發(fā)明作進一步說明。

應(yīng)該指出，以下詳細說明都是例示性的，旨在對本申請?zhí)峁┻M一步的說明。除非另有指明，本文使用的所有技術(shù)和科學術(shù)語具有與本申請所屬技術(shù)領(lǐng)域的普通技術(shù)人員通常理解的相同含義。

需要注意的是，這里所使用的術(shù)語僅是為了描述具體實施方式，而非意圖限制根據(jù)本申請的示例性實施方式。如在這里所使用的，除非上下文另外明確指出，否則單數(shù)形式也意圖包括復(fù)數(shù)形式，此外，還應(yīng)當理解的是，當在本說明書中使用術(shù)語“包含”和/或“包括”時，其指明存在特征、步驟、操作、器件、組件和/或它們的組合。

正如背景技術(shù)所介紹的，現(xiàn)有技術(shù)中存在傳統(tǒng)的無線定位方法存在著控制精度不高、控制算法過程復(fù)雜和計算量大的不足，為了解決如上的技術(shù)問題，本申請?zhí)岢隽艘环N面向家庭服務(wù)機器人的定位系統(tǒng)。

整個設(shè)計包括定位節(jié)點、上位機顯示界面、機器人本體三部分。

所述定位節(jié)點包括anchor(錨節(jié)點)與tag(標簽節(jié)點)。在實際應(yīng)用中，系統(tǒng)需要一個標簽節(jié)點和n(n>＝3)個錨節(jié)點。錨節(jié)點在系統(tǒng)中是固定與坐標確定的某個位置，標簽節(jié)點可以隨意移動。其中錨節(jié)點記錄了錨節(jié)點到標簽節(jié)點的toa值，該數(shù)據(jù)會回傳給標簽節(jié)點，便于標簽節(jié)點的本地信息顯示。

所述上位機顯示界面，標簽節(jié)點通過無線設(shè)備將數(shù)據(jù)發(fā)送給上位機，由運行于上位機上的rtls軟件進行實時定位，rtls同時可以對錨節(jié)點和標簽節(jié)點進行配置。

所述機器人本體由上層處理系統(tǒng)和下層控制系統(tǒng)組成，它們之間通過rs232串口進行通信。標簽節(jié)點佩戴在機器人本體頂部，便于信號的接收。

所述節(jié)點需要要滿足一下要求：支持多種供電方式；具備無線通信功能，支持設(shè)備遠程交互；具備良好的顯示功能；數(shù)據(jù)存儲；超低功耗。

其中的超低功耗方面，在采用電池供電時，設(shè)備需要連續(xù)穩(wěn)定運行半年以上，因此在器件選型以及軟件設(shè)計方面都需要考慮好功耗的問題。其中底層需要具備無線通信功能，底層系統(tǒng)采集數(shù)據(jù)，然后向上位機發(fā)起通信請求，將采集的數(shù)據(jù)發(fā)送給上位機，然后上位機對底層系統(tǒng)下達各種命令等。由于在室內(nèi)應(yīng)用的情況下，傳輸距離不需要太遠，選擇的器件在滿足傳輸距離的情況下，選擇功耗較低的器件。

本申請的一種典型的實施方式中，如圖1所示，本系統(tǒng)由兩部分組成：系統(tǒng)硬件和系統(tǒng)軟件。其中硬件部分又包括底層硬件與底層嵌入式軟件兩部分。節(jié)點分為兩類：(anchor)錨節(jié)點與(tag)標簽節(jié)點。在實際應(yīng)用中，系統(tǒng)需要一個標簽節(jié)點和n(n>＝3)個錨節(jié)點。錨節(jié)點在系統(tǒng)中是固定在坐標確定的某個位置，標簽節(jié)點可以隨意移動。

本發(fā)現(xiàn)底層硬件總體結(jié)構(gòu)如圖2所示，其主體為微處理器最小系統(tǒng)、基于ieee802.15.4a協(xié)議的無線傳感器以及其他硬件部分等。電源管理單元為整個系統(tǒng)提供穩(wěn)定可靠的輸出電壓及電流，電池電量由主控核心測量，在oled上實時顯示，可實現(xiàn)低壓報警及關(guān)斷功能；

本發(fā)明底層的主要功能框圖如圖3所示，其主要包含數(shù)據(jù)采集、處理功能、各種信息顯示功能、上下位機通信功能和數(shù)據(jù)存儲功能等。定位系統(tǒng)底層軟件執(zhí)行過程如圖4所示。上電后系統(tǒng)首先進行初始化，開啟oled，接著檢查系統(tǒng)存儲歷史數(shù)據(jù)標志位是否置位，如果置位，則直接將存儲的數(shù)據(jù)通過無線的方式發(fā)送給上位機，發(fā)送完畢以后，開啟測距算法。如果存儲歷史數(shù)據(jù)標志位沒有被置位。則直接開啟測距算法，在測距成功以后，程序會通過oled對實時數(shù)據(jù)進行顯示，同時底層會對數(shù)據(jù)進行整理打包，然后約定的協(xié)議通過無線的方式發(fā)給上位機，如果發(fā)送成功，則直接進行倒計時準備下一次測距算法的開啟；如果不成功，則將數(shù)據(jù)保存在外部存儲器，等待網(wǎng)絡(luò)恢復(fù)后再上傳數(shù)據(jù)。而在測距不成功的時候，程序直接回到一開始的系統(tǒng)存儲歷史數(shù)據(jù)標志位的判斷出重新開始整個過程。

本發(fā)明底層軟件選用cm3+μc/os-ii框架，其任務(wù)劃分如表1所示。

表1

其中app_taskmeasuredistance()任務(wù)是測量錨節(jié)點與標簽節(jié)點距離的任務(wù)。app_taskstandbymode()任務(wù)是低功耗任務(wù)，主要用于啟動監(jiān)測終端的低功耗過程。stm32l15xxx系列mcu有七種低功耗模式：休眠模式，低功耗運行模式，低功耗休眠，停止模式(rtc開啟)，停止模式(rtc關(guān)閉)，待機模式(rtc開啟)，待機模式(rtc關(guān)閉)，以上七種模式功耗依次降低。在本系統(tǒng)中，根據(jù)實際情況選擇的低功耗模式為待機模式(rtc開啟)。進入待機模式(rtc開啟)的方式：將pddsbit+sleepdeepbit+wfiorwfe標志位置位。本系統(tǒng)退出待機模式(rtc開啟)的方式為rtc定時中斷。在進入待機模式的時候一定要將systick關(guān)掉，避免systick中斷喚醒待機模式。在進入待機模式之前要將在此模式下用不到的引腳配置為模擬輸入，在退出待機模式后重新配置引腳。在該任務(wù)的一開始是等待以上各種任務(wù)完成的事件標志組，在其他任務(wù)都完成的情況下，系統(tǒng)會進入待機模式。

底層系統(tǒng)中標簽節(jié)點與錨節(jié)點之間用的協(xié)議是ieee802.15.4-2011超寬帶協(xié)議，具體的協(xié)議參考相關(guān)官方文檔。底層與服務(wù)器之間通過藍牙實現(xiàn)無線通信，其通信協(xié)議在參考了一些文檔的基礎(chǔ)上，根據(jù)實際應(yīng)用背景及功能需求制定。底層系統(tǒng)與上位機通過rs232串口進行本地通訊，其通信格式與藍牙的通信協(xié)議一致。mcu與超寬帶芯片之間采用spi協(xié)議；與oled采用的是i²e協(xié)議。其中藍牙通信協(xié)議數(shù)據(jù)包如表2所以，表3為表2的詳細解釋。

表2

表3

本發(fā)明上位機rtls系統(tǒng)是用微軟公司開發(fā)的mfc框架，使用c++編程語言。主要完成tdoa定位分析，anchor節(jié)點(錨節(jié)點)、tag節(jié)點(標簽節(jié)點)的狀態(tài)、定位顯示等，其界面如圖5所示。rtls的運行比較復(fù)雜，主要有主線程，藍牙串口數(shù)據(jù)包的接收線程、tdoa定位算法線程、節(jié)點配置線程等構(gòu)成。當定位系統(tǒng)開始運行時，rtls接收串口上傳的數(shù)據(jù)，當數(shù)據(jù)包接收完成并且校驗正確后對數(shù)據(jù)包進行解析，之后調(diào)用tdoa定位算法函數(shù)進行定位計算，其運行流程圖如圖6所示，其中的調(diào)用算法，本發(fā)明在綜合各定位算法優(yōu)缺點的基礎(chǔ)上采用了聯(lián)合定位算法：fang算法-泰勒級數(shù)展開法，算法的具體描述步驟為：

(1)首先對獲得的每一組tdoa數(shù)據(jù)信號直波鑒定；

(2)如果在數(shù)據(jù)中不存在nlos情況，跳轉(zhuǎn)到(3)；否則跳轉(zhuǎn)到(4)；

(3)對該組tdoa數(shù)據(jù)進行fang算法估計；

(4)首先，對該組tdoa數(shù)據(jù)進行fang算法估計，確定初始值；然后，運用泰勒級數(shù)展開法對初始值進行迭代；最后，確定目標節(jié)點位置估計。

以上所述僅為本申請的優(yōu)選實施例而已，并不用于限制本申請，對于本領(lǐng)域的技術(shù)人員來說，本申請可以有各種更改和變化。凡在本申請的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應(yīng)包含在本申請的保護范圍之內(nèi)。

上述雖然結(jié)合附圖對本發(fā)明的具體實施方式進行了描述，但并非對本發(fā)明保護范圍的限制，所屬領(lǐng)域技術(shù)人員應(yīng)該明白，在本發(fā)明的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域技術(shù)人員不需要付出創(chuàng)造性勞動即可做出的各種修改或變形仍在本發(fā)明的保護范圍以內(nèi)。