運(yùn)動(dòng)控制方法、裝置及系統(tǒng)的制作方法
【專利摘要】本發(fā)明提供了一種運(yùn)動(dòng)控制方法、裝置及系統(tǒng)，將計(jì)算機(jī)視覺(jué)技術(shù)和自動(dòng)化控制技術(shù)結(jié)合起來(lái)，實(shí)現(xiàn)了在三維環(huán)境下的追蹤，提高了追蹤控制的準(zhǔn)確度；其使用方式簡(jiǎn)單，用戶只需要選取指定跟隨目標(biāo)之后，之后所有的追蹤操作由系統(tǒng)自動(dòng)化實(shí)現(xiàn)，可移動(dòng)載體可以精確地、自動(dòng)化地跟蹤指定目標(biāo)；拓展了現(xiàn)有計(jì)算機(jī)視覺(jué)技術(shù)的應(yīng)用場(chǎng)景，不僅適用于計(jì)算機(jī)視覺(jué)追蹤技術(shù)的主流應(yīng)用場(chǎng)景，還適用于一些創(chuàng)新型的應(yīng)用場(chǎng)景，如超市自動(dòng)跟隨購(gòu)物車，家庭自動(dòng)看護(hù)機(jī)器人等。
【專利說(shuō)明】
運(yùn)動(dòng)控制方法、裝置及系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及計(jì)算機(jī)視覺(jué)技術(shù)領(lǐng)域和自動(dòng)化控制技術(shù)領(lǐng)域，尤其涉及一種運(yùn)動(dòng)控制 方法、裝置及系統(tǒng)。
【背景技術(shù)】
[0002] 計(jì)算機(jī)視覺(jué)基于特征點(diǎn)的快速跟蹤方法是指通過(guò)圖像內(nèi)的特征點(diǎn)進(jìn)行物體的跟 蹤。目前主要通過(guò)圖像特征點(diǎn)或者圖像表面的方法獲得目標(biāo)在圖像中的位置，從而實(shí)現(xiàn)用 戶通過(guò)傳感器傳回的圖像對(duì)指定跟隨目標(biāo)的跟蹤工作。可靠、精確的指定物體跟蹤在軍事、 公共安全、家庭服務(wù)等方面都擁有廣闊的市場(chǎng)前景，也是當(dāng)下研究的熱門技術(shù)之一。在軍事 上，跟蹤技術(shù)可以協(xié)助導(dǎo)彈跟蹤目標(biāo)，在軍用無(wú)人機(jī)上可以從空中通過(guò)傳感器實(shí)時(shí)跟蹤指 定目標(biāo);在公共安全方面，可以通過(guò)對(duì)罪犯在攝像頭內(nèi)進(jìn)行跟蹤，在交通控制上多目標(biāo)跟蹤 車輛行人以掌握交通流量和動(dòng)向；在家庭服務(wù)方面，可以使各種家用機(jī)器人主動(dòng)跟隨主人 行進(jìn)。
[0003] 自動(dòng)化控制技術(shù)廣泛用于工業(yè)、農(nóng)業(yè)、軍事、科學(xué)研究、交通運(yùn)輸、商業(yè)、醫(yī)療、服務(wù) 和家庭等方面。采用自動(dòng)化控制不僅可以把人從繁重的體力勞動(dòng)、部分腦力勞動(dòng)以及惡劣、 危險(xiǎn)的工作環(huán)境中解放出來(lái)，而且能擴(kuò)展人的器官功能，極大地提高勞動(dòng)生產(chǎn)率，增強(qiáng)人類 認(rèn)識(shí)世界和改造世界的能力。因此，自動(dòng)化控制是工業(yè)、農(nóng)業(yè)、國(guó)防和科學(xué)技術(shù)現(xiàn)代化的重 要條件和顯著標(biāo)志。
[0004] 計(jì)算機(jī)視覺(jué)追蹤技術(shù)現(xiàn)在的應(yīng)用范圍主要是在圖像中對(duì)指定目標(biāo)的追蹤，也可以 理解為是二維環(huán)境下的追蹤技術(shù)，這大大限制了該技術(shù)的發(fā)展。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的是提供一種運(yùn)動(dòng)控制方法、裝置及系統(tǒng)，解決普通運(yùn)動(dòng)控制方法其 運(yùn)動(dòng)控制方法本體無(wú)法處于正面視覺(jué)位置的問(wèn)題。
[0006] 本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案是：
[0007] 一種運(yùn)動(dòng)控制方法，包括：
[0008] 確定指定目標(biāo)；
[0009] 通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，識(shí)別出二 維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息； 獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目 標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相 對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目 標(biāo)在世界坐標(biāo)系中的位置信息；
[0010] 獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體的姿態(tài)信息 和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體周圍的障 礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信 息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài)；
[0011] 根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述 可移動(dòng)載體的世界位置信息為可移動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世 界位置信息和可移動(dòng)載體的世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度；
[0012] 根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn) 動(dòng)速度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的 里程信息并保存。
[0013] 在此基礎(chǔ)上，進(jìn)一步地，所述通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色 信息和深度信息，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取 指定目標(biāo)的相對(duì)位置信息；獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài) 信息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位 置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目 標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息的步驟，具體為：
[0014] 通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，當(dāng)二維圖 像中未發(fā)現(xiàn)指定目標(biāo)時(shí)，調(diào)取指定目標(biāo)之前的世界位置信息，作為指定目標(biāo)的世界位置信 息；
[0015] 當(dāng)二維圖像中發(fā)現(xiàn)指定目標(biāo)時(shí)，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì) 指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取可移動(dòng)載體姿態(tài)信息和里程信息， 根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣 獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的 位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息。
[0016] 在上述任意實(shí)施例的基礎(chǔ)上，進(jìn)一步地，所述根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度， 獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息， 控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存的步驟，具體為：
[0017] 根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn) 動(dòng)速度所需要的動(dòng)力信息;根據(jù)可移動(dòng)載體的速度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整;根據(jù)調(diào)整后 的動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存。
[0018] 一種運(yùn)動(dòng)控制裝置，包括：
[0019] 指定目標(biāo)確定模塊，用于確定指定目標(biāo)；
[0020] 指定目標(biāo)定位模塊，用于通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信 息和深度信息，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指 定目標(biāo)的相對(duì)位置信息；獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信 息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置 信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo) 的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息；
[0021] 可移動(dòng)載體解算模塊，用于獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息； 根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信 息，獲取可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障 礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的 姿態(tài)信息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息 為可移動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體的 世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度;根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn) 動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息，控制可移 動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存。
[0022] 在此基礎(chǔ)上，進(jìn)一步地：
[0023] 指定目標(biāo)定位模塊，用于通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信 息和深度信息，當(dāng)二維圖像中未發(fā)現(xiàn)指定目標(biāo)時(shí)，調(diào)取指定目標(biāo)之前的世界位置信息，作為 指定目標(biāo)的世界位置信息；當(dāng)二維圖像中發(fā)現(xiàn)指定目標(biāo)時(shí)，識(shí)別出二維圖像中的指定目標(biāo)， 在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息；獲取可移動(dòng)載體姿態(tài) 信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位置信息， 通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目 標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的 位置信息。
[0024] 在上述任意實(shí)施例的基礎(chǔ)上，進(jìn)一步地：
[0025] 可移動(dòng)載體解算模塊，用于獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息； 根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信 息，獲取可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障 礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的 姿態(tài)信息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息 為可移動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體的 世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度;根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn) 動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)可移動(dòng)載體的速度信息， 對(duì)動(dòng)力信息進(jìn)行調(diào)整;根據(jù)調(diào)整后的動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的 里程信息并保存。
[0026] 一種運(yùn)動(dòng)控制系統(tǒng)，包括：
[0027]數(shù)據(jù)采集裝置，包括RGBD攝像頭、激光雷達(dá)、超聲波發(fā)射器、慣性測(cè)量傳感器;RGBD 攝像頭獲取指定目標(biāo)的顏色信息和深度信息并上傳至數(shù)據(jù)分析平臺(tái)；激光雷達(dá)和超聲波發(fā) 射器進(jìn)行障礙信息檢測(cè)，獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息并上傳至數(shù)據(jù) 分析平臺(tái)；慣性測(cè)量傳感器獲取可移動(dòng)載體的姿態(tài)信息并上傳至數(shù)據(jù)分析平臺(tái)；
[0028]數(shù)據(jù)分析平臺(tái)，包括運(yùn)動(dòng)解算模塊和上層PID控制器;運(yùn)動(dòng)解算模塊確定指定目 標(biāo);通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，識(shí)別出二維圖像 中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取可 移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相 對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置 信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世 界坐標(biāo)系中的位置信息；獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng) 載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲取可移 動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在世界坐 標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的姿態(tài)信息和里 程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息為可移動(dòng)載體 在世界坐標(biāo)系中的位置信息；上層PID控制器根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體 的世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度；
[0029] 底層控制平臺(tái)，包括底層控制芯片、下層PID控制器、動(dòng)力裝置、運(yùn)動(dòng)模塊、測(cè)速模 塊、里程模塊;底層控制芯片接收數(shù)據(jù)分析平臺(tái)發(fā)送的運(yùn)動(dòng)狀態(tài)和運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀 態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的預(yù)定動(dòng)力信息；下層PID控制器接收測(cè)速模塊 發(fā)送的速度信息，通過(guò)自反饋機(jī)制，根據(jù)可移動(dòng)載體的速度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整并將 調(diào)整后的動(dòng)力信息傳輸?shù)絼?dòng)力裝置;動(dòng)力裝置根據(jù)調(diào)整后的動(dòng)力信息，控制運(yùn)動(dòng)模塊實(shí)現(xiàn) 可移動(dòng)載體的運(yùn)動(dòng)，并將調(diào)整后的動(dòng)力信息傳輸?shù)竭\(yùn)動(dòng)模塊、測(cè)速模塊和里程模塊;運(yùn)動(dòng)模 塊控制可移動(dòng)載體的移動(dòng);測(cè)速模塊測(cè)算可移動(dòng)載體的速度信息并發(fā)送到下層PID控制器； 里程模塊測(cè)算可移動(dòng)載體的里程信息并上傳至數(shù)據(jù)分析平臺(tái)。
[0030] 本發(fā)明的有益效果是：
[0031] 本發(fā)明提供了一種運(yùn)動(dòng)控制方法、裝置及系統(tǒng)，將計(jì)算機(jī)視覺(jué)技術(shù)和自動(dòng)化控制 技術(shù)結(jié)合起來(lái)，實(shí)現(xiàn)了在三維環(huán)境下的追蹤，提高了追蹤控制的準(zhǔn)確度;其使用方式簡(jiǎn)單， 用戶只需要選取指定跟隨目標(biāo)之后，之后所有的追蹤操作由系統(tǒng)自動(dòng)化實(shí)現(xiàn)，可移動(dòng)載體 可以精確地、自動(dòng)化地跟蹤指定目標(biāo);拓展了現(xiàn)有計(jì)算機(jī)視覺(jué)技術(shù)的應(yīng)用場(chǎng)景，不僅適用于 計(jì)算機(jī)視覺(jué)追蹤技術(shù)的主流應(yīng)用場(chǎng)景，還適用于一些創(chuàng)新型的應(yīng)用場(chǎng)景，如超市自動(dòng)跟隨 購(gòu)物車，家庭自動(dòng)看護(hù)機(jī)器人等。
【附圖說(shuō)明】
[0032]下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明進(jìn)一步說(shuō)明。
[0033] 圖1示出了本發(fā)明實(shí)施例提供的一種運(yùn)動(dòng)控制方法的流程圖；
[0034] 圖2示出了本發(fā)明實(shí)施例提供的一種運(yùn)動(dòng)控制裝置的結(jié)構(gòu)示意圖；
[0035] 圖3示出了本發(fā)明實(shí)施例提供的一種運(yùn)動(dòng)控制系統(tǒng)的結(jié)構(gòu)示意圖；
[0036] 圖4示出了本發(fā)明實(shí)施例提供的一種運(yùn)動(dòng)控制系統(tǒng)的結(jié)構(gòu)示意圖；
[0037] 圖5示出了本發(fā)明實(shí)施例提供的一種運(yùn)動(dòng)控制系統(tǒng)的狀態(tài)示意圖；
[0038] 圖6示出了本發(fā)明實(shí)施例提供的一種運(yùn)動(dòng)控制方法的流程示意圖。
【具體實(shí)施方式】
[0039]為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì) 本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅用以解釋本發(fā)明，并不 限定本發(fā)明。
[0040] 具體實(shí)施例一
[0041 ]如圖1所示，本發(fā)明實(shí)施例提供了一種運(yùn)動(dòng)控制方法，包括：
[0042] 步驟SlOl，確定指定目標(biāo)；
[0043]步驟S102,通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息， 識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì) 位置信息;獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息 以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指 定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信 息為指定目標(biāo)在世界坐標(biāo)系中的位置信息；
[0044] 步驟S103,獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體 的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載 體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系 中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài)；
[0045] 步驟S104,根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息，獲取可移動(dòng)載體的世界位置 信息，所述可移動(dòng)載體的世界位置信息為可移動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指 定目標(biāo)的世界位置信息和可移動(dòng)載體的世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度；
[0046] 步驟S105，根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要 達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移 動(dòng)載體的里程信息并保存。
[0047] 本發(fā)明實(shí)施例將計(jì)算機(jī)視覺(jué)技術(shù)和自動(dòng)化控制技術(shù)結(jié)合起來(lái)，實(shí)現(xiàn)了在三維環(huán)境 下的追蹤，提高了追蹤控制的準(zhǔn)確度;其使用方式簡(jiǎn)單，用戶只需要選取指定跟隨目標(biāo)之 后，之后所有的追蹤操作由系統(tǒng)自動(dòng)化實(shí)現(xiàn)，可移動(dòng)載體可以精確地、自動(dòng)化地跟蹤指定目 標(biāo);拓展了現(xiàn)有計(jì)算機(jī)視覺(jué)技術(shù)的應(yīng)用場(chǎng)景，不僅適用于計(jì)算機(jī)視覺(jué)追蹤技術(shù)的主流應(yīng)用 場(chǎng)景，還適用于一些創(chuàng)新型的應(yīng)用場(chǎng)景，如超市自動(dòng)跟隨購(gòu)物車，家庭自動(dòng)看護(hù)機(jī)器人等。
[0048] 考慮到現(xiàn)實(shí)環(huán)境的復(fù)雜性，本發(fā)明實(shí)施例還可以引入丟失找回算法。在上述實(shí)施 例的基礎(chǔ)上，優(yōu)選的，步驟S102可以具體為:通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo) 的顏色信息和深度信息，當(dāng)二維圖像中未發(fā)現(xiàn)指定目標(biāo)時(shí)，調(diào)取指定目標(biāo)之前的世界位置 信息，作為指定目標(biāo)的世界位置信息；當(dāng)二維圖像中發(fā)現(xiàn)指定目標(biāo)時(shí)，識(shí)別出二維圖像中的 指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取可移動(dòng) 載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位 置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息 為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐 標(biāo)系中的位置信息。丟失找回算法的引入提高了三維環(huán)境下追蹤技術(shù)的安全性和穩(wěn)定性。
[0049] 本發(fā)明實(shí)施例在測(cè)算到可移動(dòng)載體的速度信息后，可以根據(jù)該實(shí)際速度信息調(diào)整 動(dòng)力信息，提高跟蹤的精確度。優(yōu)選的，步驟S105可以為:根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速 度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)可移動(dòng)載體 的速度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整;根據(jù)調(diào)整后的動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取 可移動(dòng)載體的里程信息并保存。
[0050] 具體實(shí)施例二
[0051] 如圖2所示，本發(fā)明實(shí)施例提供了一種運(yùn)動(dòng)控制裝置，包括：
[0052]指定目標(biāo)確定模塊201，用于確定指定目標(biāo)；
[0053]指定目標(biāo)定位模塊202,用于通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色 信息和深度信息，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取 指定目標(biāo)的相對(duì)位置信息；獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài) 信息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位 置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目 標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息；
[0054] 可移動(dòng)載體解算模塊203,用于獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信 息;根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置 信息，獲取可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的 障礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體 的姿態(tài)信息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信 息為可移動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體 的世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度;根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該 運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息，控制可 移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存。
[0055] 本發(fā)明實(shí)施例將計(jì)算機(jī)視覺(jué)技術(shù)和自動(dòng)化控制技術(shù)結(jié)合起來(lái)，實(shí)現(xiàn)了在三維環(huán)境 下的追蹤，提高了追蹤控制的準(zhǔn)確度;其使用方式簡(jiǎn)單，用戶只需要選取指定跟隨目標(biāo)之 后，之后所有的追蹤操作由系統(tǒng)自動(dòng)化實(shí)現(xiàn)，可移動(dòng)載體可以精確地、自動(dòng)化地跟蹤指定目 標(biāo);拓展了現(xiàn)有計(jì)算機(jī)視覺(jué)技術(shù)的應(yīng)用場(chǎng)景，不僅適用于計(jì)算機(jī)視覺(jué)追蹤技術(shù)的主流應(yīng)用 場(chǎng)景，還適用于一些創(chuàng)新型的應(yīng)用場(chǎng)景，如超市自動(dòng)跟隨購(gòu)物車，家庭自動(dòng)看護(hù)機(jī)器人等。 [0056]在此基礎(chǔ)上，優(yōu)選的，指定目標(biāo)定位模塊203可以通過(guò)可移動(dòng)載體拍攝二維圖像； 根據(jù)指定目標(biāo)的顏色信息和深度信息，當(dāng)二維圖像中未發(fā)現(xiàn)指定目標(biāo)時(shí)，調(diào)取指定目標(biāo)之 前的世界位置信息，作為指定目標(biāo)的世界位置信息；當(dāng)二維圖像中發(fā)現(xiàn)指定目標(biāo)時(shí)，識(shí)別出 二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信 息;獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指 定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo) 的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指 定目標(biāo)在世界坐標(biāo)系中的位置信息?？紤]到現(xiàn)實(shí)環(huán)境的復(fù)雜性，丟失找回算法的引入，提高 了三維環(huán)境下追蹤技術(shù)的安全性和穩(wěn)定性。
[0057]本發(fā)明實(shí)施例在測(cè)算到可移動(dòng)載體的速度信息后，可以根據(jù)該實(shí)際速度信息調(diào)整 動(dòng)力信息，提高跟蹤的精確度，優(yōu)選的，可移動(dòng)載體解算模塊204可以根據(jù)可移動(dòng)載體的速 度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整，再根據(jù)調(diào)整后的動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可 移動(dòng)載體的里程信息并保存。
[0058] 具體實(shí)施例三
[0059] 如圖3所示，本發(fā)明實(shí)施例提供了一種運(yùn)動(dòng)控制系統(tǒng)，包括：
[0060] 數(shù)據(jù)采集裝置301，包括RGBD攝像頭、激光雷達(dá)、超聲波發(fā)射器、慣性測(cè)量傳感器； RGBD攝像頭獲取指定目標(biāo)的顏色信息和深度信息并上傳至數(shù)據(jù)分析平臺(tái)；激光雷達(dá)和超聲 波發(fā)射器進(jìn)行障礙信息檢測(cè)，獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息并上傳至 數(shù)據(jù)分析平臺(tái)；慣性測(cè)量傳感器獲取可移動(dòng)載體的姿態(tài)信息并上傳至數(shù)據(jù)分析平臺(tái)；
[0061 ]數(shù)據(jù)分析平臺(tái)302，包括運(yùn)動(dòng)解算模塊和上層PID控制器;運(yùn)動(dòng)解算模塊確定指定 目標(biāo);通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，識(shí)別出二維圖 像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取 可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的 相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位 置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在 世界坐標(biāo)系中的位置信息；獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移 動(dòng)載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲取可 移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在世界 坐標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的姿態(tài)信息和 里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息為可移動(dòng)載 體在世界坐標(biāo)系中的位置信息；上層PID控制器根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載 體的世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度；
[0062]底層控制平臺(tái)303,包括底層控制芯片、下層PID控制器、動(dòng)力裝置、運(yùn)動(dòng)模塊、測(cè)速 模塊、里程模塊;底層控制芯片接收數(shù)據(jù)分析平臺(tái)發(fā)送的運(yùn)動(dòng)狀態(tài)和運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng) 狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的預(yù)定動(dòng)力信息；下層PID控制器接收測(cè)速模 塊發(fā)送的速度信息，通過(guò)自反饋機(jī)制，根據(jù)可移動(dòng)載體的速度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整并 將調(diào)整后的動(dòng)力信息傳輸?shù)絼?dòng)力裝置;動(dòng)力裝置根據(jù)調(diào)整后的動(dòng)力信息，控制運(yùn)動(dòng)模塊實(shí) 現(xiàn)可移動(dòng)載體的運(yùn)動(dòng)，并將調(diào)整后的動(dòng)力信息傳輸?shù)竭\(yùn)動(dòng)模塊、測(cè)速模塊和里程模塊;運(yùn)動(dòng) 模塊控制可移動(dòng)載體的移動(dòng);測(cè)速模塊測(cè)算可移動(dòng)載體的速度信息并發(fā)送到下層PID控制 器;里程模塊測(cè)算可移動(dòng)載體的里程信息并上傳至數(shù)據(jù)分析平臺(tái)。
[0063]本發(fā)明實(shí)施例將計(jì)算機(jī)視覺(jué)技術(shù)和自動(dòng)化控制技術(shù)結(jié)合起來(lái)，實(shí)現(xiàn)了在三維環(huán)境 下的追蹤，提高了追蹤控制的準(zhǔn)確度;其使用方式簡(jiǎn)單，用戶只需要選取指定跟隨目標(biāo)之 后，之后所有的追蹤操作由系統(tǒng)自動(dòng)化實(shí)現(xiàn)，可移動(dòng)載體可以精確地、自動(dòng)化地跟蹤指定目 標(biāo);拓展了現(xiàn)有計(jì)算機(jī)視覺(jué)技術(shù)的應(yīng)用場(chǎng)景，不僅適用于計(jì)算機(jī)視覺(jué)追蹤技術(shù)的主流應(yīng)用 場(chǎng)景，還適用于一些創(chuàng)新型的應(yīng)用場(chǎng)景，如超市自動(dòng)跟隨購(gòu)物車，家庭自動(dòng)看護(hù)機(jī)器人等。 本發(fā)明實(shí)施例還對(duì)PID控制技術(shù)做了進(jìn)一步的優(yōu)化，采用了雙層PID控制技術(shù)，進(jìn)一步增加 了自動(dòng)化追蹤的精確度以及敏感度?？紤]到現(xiàn)實(shí)條件下環(huán)境的復(fù)雜，本發(fā)明實(shí)施例還添加 了相應(yīng)的避障措施以及丟失找回策略，進(jìn)一步保障了可移動(dòng)載體追蹤的安全性與穩(wěn)定性。 [0064]具體實(shí)施例四
[0065]如圖4所示，本發(fā)明實(shí)施例提供了一種運(yùn)動(dòng)控制系統(tǒng)。整個(gè)系統(tǒng)主要分為三個(gè)部 分，包括數(shù)據(jù)采集裝置，數(shù)據(jù)分析平臺(tái)和底層控制平臺(tái)。數(shù)據(jù)采集裝置是一個(gè)用于收集環(huán)境 中顏色及深度信息以及周圍環(huán)境信息，并可實(shí)時(shí)處理上傳的三維掃描設(shè)備。數(shù)據(jù)采集裝置 包含嵌入式中央處理器芯片、深度測(cè)距芯片、深度攝像機(jī)元件，顏色攝像機(jī)元件、超聲波元 件、雷達(dá)元件等模塊。數(shù)據(jù)分析平臺(tái)是在運(yùn)動(dòng)解算模塊中利用中央處理器芯片實(shí)現(xiàn)相應(yīng)的 運(yùn)動(dòng)解算算法，并且實(shí)現(xiàn)上層的PID控制。底層控制平臺(tái)主要包括底盤控制芯片、動(dòng)力裝置、 測(cè)算元件、里程元件等設(shè)備，主要是接受上層傳輸過(guò)來(lái)的運(yùn)動(dòng)信息，對(duì)可移動(dòng)載體進(jìn)行控 制。
[0066] 圖5示出了本發(fā)明所實(shí)現(xiàn)系統(tǒng)中的狀態(tài)。本發(fā)明中整個(gè)系統(tǒng)共存在6個(gè)狀態(tài)，包括 等待，失聯(lián)等待，跟隨，即將丟失，完全丟失，丟失找回。系統(tǒng)整個(gè)運(yùn)行過(guò)程中在六個(gè)狀態(tài)中 切換，實(shí)現(xiàn)了在三維環(huán)境下對(duì)指定目標(biāo)的追蹤。
[0067] 圖6所示的是系統(tǒng)在實(shí)際運(yùn)行中的流程圖，該流程圖用來(lái)具體說(shuō)明整套方案的工 作流程和細(xì)節(jié)實(shí)施方法。
[0068] 具體實(shí)施步驟如下：
[0069] 步驟1):構(gòu)造數(shù)據(jù)采集裝置，包括RGB-D傳感器、慣性測(cè)量傳感器、超聲波發(fā)射器、 激光雷達(dá)，收集指定目標(biāo)的顏色信息和深度信息、可移動(dòng)載體此時(shí)的姿態(tài)信息，可移動(dòng)載體 周圍的環(huán)境信息，并上傳至數(shù)據(jù)分析平臺(tái)進(jìn)行指定目標(biāo)的運(yùn)動(dòng)解算。
[0070] 步驟2):利用基于計(jì)算機(jī)視覺(jué)特征點(diǎn)的追蹤算法，通過(guò)RGBD傳感器收集的深度顏 色信息，識(shí)別出圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，實(shí)時(shí)獲得指定目標(biāo) 在圖像中的位置坐標(biāo)。
[0071] 利用基于計(jì)算機(jī)視覺(jué)測(cè)程原理的算法對(duì)數(shù)據(jù)采集裝置在不同時(shí)刻收集到的信息 進(jìn)行相對(duì)位置估計(jì)，目的是將攝像頭坐標(biāo)系下的點(diǎn)轉(zhuǎn)化為世界坐標(biāo)系下，從而獲得三維環(huán) 境中每一個(gè)位置的三維坐標(biāo)。首先，對(duì)于深度攝像頭下的點(diǎn)，我們能夠獲取它在二維圖像中 的位置(u，v)以及他的深度信息d，所以每個(gè)點(diǎn)的像素坐標(biāo)為[u，v，d]。在深度攝像頭中，可 以通過(guò)公式(1)，可以將點(diǎn)的像素坐標(biāo)轉(zhuǎn)換為攝像頭坐標(biāo)系的三維坐標(biāo)[x，y，z]。
[0072] z = dls，
[0073] X= (u-Cx) Xzlfx，
[0074] y=(V-Cy)χfyo
[0075] 接下來(lái)用姿態(tài)轉(zhuǎn)換矩陣將攝像頭坐標(biāo)系下的三維坐標(biāo)[x，y，z]轉(zhuǎn)換為世界坐標(biāo)系 下的三維坐標(biāo)[xl，yl，zl]。由公式(2)可知，通過(guò)攝像頭到可移動(dòng)載體的姿態(tài)旋轉(zhuǎn)矩陣和平 移矩陣可以得出在可移動(dòng)載體設(shè)備坐標(biāo)系下的三維坐標(biāo)[ X_r，y_r，z_r]表示，如下式所示：
[0076]
[0077]最后通過(guò)慣性測(cè)量裝置，計(jì)算出此時(shí)可移動(dòng)載體設(shè)備的姿態(tài)信息，計(jì)算出可移動(dòng) 載體設(shè)備到世界坐標(biāo)系下的姿態(tài)旋轉(zhuǎn)矩陣，如下式：
[0078]
[0079]可以最終計(jì)算出世界坐標(biāo)系下的三維坐標(biāo)。通過(guò)上述流程，可以指定目標(biāo)的像素 坐標(biāo)與世界坐標(biāo)建立一一映射關(guān)系，得到指定目標(biāo)世界坐標(biāo)系下的位置關(guān)系，將其上傳至 數(shù)據(jù)分析平臺(tái)并存儲(chǔ)于系統(tǒng)中位置信息存儲(chǔ)空間。
[0080]更優(yōu)選的，如果圖像中未發(fā)現(xiàn)指定目標(biāo)，則系統(tǒng)計(jì)入即將丟失狀態(tài)并且累計(jì)目標(biāo) 丟失的時(shí)間，如果累積丟失時(shí)間超過(guò)一定閾值，則系統(tǒng)進(jìn)入完全丟失狀態(tài)，自動(dòng)開啟丟失找 回模式，從位置信息存儲(chǔ)空間獲取之前目標(biāo)位置。
[0081 ]步驟3):獲取指定目標(biāo)的位置信息后，進(jìn)入運(yùn)動(dòng)解算模塊，通過(guò)指定目標(biāo)位置信息 與當(dāng)前可移動(dòng)載體的位置信息，可以計(jì)算出可移動(dòng)載體與指定目標(biāo)之間的偏移角度以及相 隔距離，將此運(yùn)動(dòng)信息傳輸給PID控制器，通過(guò)PID自反饋控制，在上層實(shí)現(xiàn)對(duì)運(yùn)動(dòng)狀態(tài)的粗 調(diào)節(jié)，并將其傳給底層控制芯片。
[0082]步驟4):將數(shù)據(jù)分析平臺(tái)計(jì)算出的運(yùn)動(dòng)狀態(tài)和運(yùn)動(dòng)速度信息傳輸給底層控制芯 片，底層控制芯片解算出此運(yùn)動(dòng)狀態(tài)下，要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力，并將其輸入給第 二層(底層)PID控制器，通過(guò)PID控制器的自反饋機(jī)制，該P(yáng)ID控制器從硬件的角度，精確估 計(jì)出此狀態(tài)下所需要的動(dòng)力，并且傳輸給動(dòng)力裝置。動(dòng)力裝置傳輸給運(yùn)動(dòng)模塊控制可移動(dòng) 載體的移動(dòng)，實(shí)現(xiàn)可移動(dòng)載體的精準(zhǔn)的跟隨。與此同時(shí)，動(dòng)力裝置也會(huì)將運(yùn)動(dòng)信息傳輸給測(cè) 速模塊以及里程模塊，實(shí)現(xiàn)系統(tǒng)的自反饋控制和里程信息的測(cè)算。
[0083]步驟5):更新測(cè)速信息和里程信息，判斷系統(tǒng)是否停止跟隨，如果停止跟隨則控制 可移動(dòng)載體原地等待，否則回到步驟2。
[0084]盡管本發(fā)明已進(jìn)行了一定程度的描述，明顯地，在不脫離本發(fā)明的精神和范圍的 條件下，可進(jìn)行各個(gè)條件的適當(dāng)變化?？梢岳斫?，本發(fā)明不限于所述實(shí)施方案，而歸于權(quán)利 要求的范圍，其包括所述每個(gè)因素的等同替換。
【主權(quán)項(xiàng)】
1. 一種運(yùn)動(dòng)控制方法，其特征在于，包括： 確定指定目標(biāo)； 通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，識(shí)別出二維圖 像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取 可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的 相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位 置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在 世界坐標(biāo)系中的位置信息； 獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體的姿態(tài)信息和里 程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體周圍的障礙物 在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息， 獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài)； 根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移 動(dòng)載體的世界位置信息為可移動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世界位 置信息和可移動(dòng)載體的世界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度； 根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速 度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程 信息并保存。2. 根據(jù)權(quán)利要求1所述的運(yùn)動(dòng)控制方法，其特征在于，所述通過(guò)可移動(dòng)載體拍攝二維圖 像;根據(jù)指定目標(biāo)的顏色信息和深度信息，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì) 指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取可移動(dòng)載體姿態(tài)信息和里程信息， 根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣 獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的 位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息的步驟， 具體為： 通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，當(dāng)二維圖像中 未發(fā)現(xiàn)指定目標(biāo)時(shí)，調(diào)取指定目標(biāo)之前的世界位置信息，作為指定目標(biāo)的世界位置信息； 當(dāng)二維圖像中發(fā)現(xiàn)指定目標(biāo)時(shí)，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定 目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù) 可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取 指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置 信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息。3. 根據(jù)權(quán)利要求1或2所述的運(yùn)動(dòng)控制方法，其特征在于，所述根據(jù)所述運(yùn)動(dòng)狀態(tài)和所 述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)所 述動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存的步驟，具體為： 根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速 度所需要的動(dòng)力信息;根據(jù)可移動(dòng)載體的速度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整;根據(jù)調(diào)整后的動(dòng) 力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存。4. 一種運(yùn)動(dòng)控制裝置，其特征在于，包括： 指定目標(biāo)確定模塊，用于確定指定目標(biāo)； 指定目標(biāo)定位模塊，用于通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和 深度信息，識(shí)別出二維圖像中的指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目 標(biāo)的相對(duì)位置信息；獲取可移動(dòng)載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和 里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信 息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的 世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置信息； 可移動(dòng)載體解算模塊，用于獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息；根據(jù) 可移動(dòng)載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲 取可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在 世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的姿態(tài)信 息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息為可移 動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體的世界位 置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度;根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài) 下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)所述動(dòng)力信息，控制可移動(dòng)載體 的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信息并保存。5. 根據(jù)權(quán)利要求4所述的運(yùn)動(dòng)控制裝置，其特征在于： 指定目標(biāo)定位模塊，用于通過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和 深度信息，當(dāng)二維圖像中未發(fā)現(xiàn)指定目標(biāo)時(shí)，調(diào)取指定目標(biāo)之前的世界位置信息，作為指定 目標(biāo)的世界位置信息；當(dāng)二維圖像中發(fā)現(xiàn)指定目標(biāo)時(shí)，識(shí)別出二維圖像中的指定目標(biāo)，在二 維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息；獲取可移動(dòng)載體姿態(tài)信息 和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位置信息，通過(guò) 姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息為指定目標(biāo)在 二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐標(biāo)系中的位置 信息。6. 根據(jù)權(quán)利要求4或5所述的運(yùn)動(dòng)控制裝置，其特征在于： 可移動(dòng)載體解算模塊，用于獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息；根據(jù) 可移動(dòng)載體的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲 取可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在 世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的姿態(tài)信 息和里程信息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息為可移 動(dòng)載體在世界坐標(biāo)系中的位置信息;根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體的世界位 置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度;根據(jù)所述運(yùn)動(dòng)狀態(tài)和所述運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài) 下，可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的動(dòng)力信息;根據(jù)可移動(dòng)載體的速度信息，對(duì)動(dòng)力 信息進(jìn)行調(diào)整;根據(jù)調(diào)整后的動(dòng)力信息，控制可移動(dòng)載體的運(yùn)動(dòng)，獲取可移動(dòng)載體的里程信 息并保存。7. -種運(yùn)動(dòng)控制系統(tǒng)，其特征在于，包括： 數(shù)據(jù)采集裝置，包括RGBD攝像頭、激光雷達(dá)、超聲波發(fā)射器、慣性測(cè)量傳感器;RGBD攝像 頭獲取指定目標(biāo)的顏色信息和深度信息并上傳至數(shù)據(jù)分析平臺(tái)；激光雷達(dá)和超聲波發(fā)射器 進(jìn)行障礙信息檢測(cè)，獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息并上傳至數(shù)據(jù)分析 平臺(tái)；慣性測(cè)量傳感器獲取可移動(dòng)載體的姿態(tài)信息并上傳至數(shù)據(jù)分析平臺(tái)； 數(shù)據(jù)分析平臺(tái)，包括運(yùn)動(dòng)解算模塊和上層PID控制器;運(yùn)動(dòng)解算模塊確定指定目標(biāo);通 過(guò)可移動(dòng)載體拍攝二維圖像;根據(jù)指定目標(biāo)的顏色信息和深度信息，識(shí)別出二維圖像中的 指定目標(biāo)，在二維圖像中對(duì)指定目標(biāo)進(jìn)行跟蹤，獲取指定目標(biāo)的相對(duì)位置信息;獲取可移動(dòng) 載體姿態(tài)信息和里程信息，根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信息以及指定目標(biāo)的相對(duì)位 置信息，通過(guò)姿態(tài)變換矩陣獲取指定目標(biāo)的世界位置信息;所述指定目標(biāo)的相對(duì)位置信息 為指定目標(biāo)在二維圖像中的位置信息，所述指定目標(biāo)的世界位置信息為指定目標(biāo)在世界坐 標(biāo)系中的位置信息；獲取環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體 的姿態(tài)信息和里程信息以及環(huán)境中每個(gè)障礙物在世界坐標(biāo)系中的位置信息，獲取可移動(dòng)載 體周圍的障礙物在世界坐標(biāo)系中的位置信息;根據(jù)可移動(dòng)載體周圍的障礙物在世界坐標(biāo)系 中的位置信息，獲取可移動(dòng)載體下一步的運(yùn)動(dòng)狀態(tài);根據(jù)可移動(dòng)載體的姿態(tài)信息和里程信 息，獲取可移動(dòng)載體的世界位置信息，所述可移動(dòng)載體的世界位置信息為可移動(dòng)載體在世 界坐標(biāo)系中的位置信息；上層PID控制器根據(jù)指定目標(biāo)的世界位置信息和可移動(dòng)載體的世 界位置信息，獲取可移動(dòng)載體的運(yùn)動(dòng)速度； 底層控制平臺(tái)，包括底層控制芯片、下層PID控制器、動(dòng)力裝置、運(yùn)動(dòng)模塊、測(cè)速模塊、里 程模塊;底層控制芯片接收數(shù)據(jù)分析平臺(tái)發(fā)送的運(yùn)動(dòng)狀態(tài)和運(yùn)動(dòng)速度，獲取該運(yùn)動(dòng)狀態(tài)下， 可移動(dòng)載體要達(dá)到該運(yùn)動(dòng)速度所需要的預(yù)定動(dòng)力信息；下層PID控制器接收測(cè)速模塊發(fā)送 的速度信息，通過(guò)自反饋機(jī)制，根據(jù)可移動(dòng)載體的速度信息，對(duì)動(dòng)力信息進(jìn)行調(diào)整并將調(diào)整 后的動(dòng)力信息傳輸?shù)絼?dòng)力裝置;動(dòng)力裝置根據(jù)調(diào)整后的動(dòng)力信息，控制運(yùn)動(dòng)模塊實(shí)現(xiàn)可移 動(dòng)載體的運(yùn)動(dòng)，并將調(diào)整后的動(dòng)力信息傳輸?shù)竭\(yùn)動(dòng)模塊、測(cè)速模塊和里程模塊;運(yùn)動(dòng)模塊控 制可移動(dòng)載體的移動(dòng);測(cè)速模塊測(cè)算可移動(dòng)載體的速度信息并發(fā)送到下層PID控制器;里程 模塊測(cè)算可移動(dòng)載體的里程信息并上傳至數(shù)據(jù)分析平臺(tái)。
【文檔編號(hào)】G06T7/00GK106056633SQ201610403662
【公開日】2016年10月26日
【申請(qǐng)日】2016年6月7日
【發(fā)明人】李經(jīng)緯, 陳震, 朱騁, 王皓月, 蔣申璐, 楊永全, 任海波, 杜新鵬
【申請(qǐng)人】速感科技（北京）有限公司