機(jī)器人視覺定位系統(tǒng)研究與實(shí)現(xiàn)

鄭紀(jì)陽 張國忠 黃聿榮 方浩堒 劉澤閩

摘要：本文以機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）為研究平臺(tái)，以移動(dòng)機(jī)器人的視覺定位功能為研究目標(biāo)，針對移動(dòng)機(jī)器人在路面上的定位、捕捉圖像及導(dǎo)問題進(jìn)行研究。實(shí)現(xiàn)過程中，使用了開源物理仿真環(huán)境（gazebo）搭建與模擬機(jī)器人視覺定環(huán)境，三維平臺(tái)（Rviz）搭建仿真平臺(tái)完成了多點(diǎn)導(dǎo)航功能。通過實(shí)景測試，驗(yàn)證了本文實(shí)現(xiàn)機(jī)器人視覺定位功能的有效性。

關(guān)鍵詞：機(jī)器人操作系統(tǒng)（ROS）;開源物理仿真環(huán)境（gazebo）;三維平臺(tái)（Rviz）;多點(diǎn)導(dǎo)航。

1.所用系統(tǒng)和算法介紹

1.1 ROS系統(tǒng)源于2007年機(jī)器人技術(shù)公司“Willow Garage”個(gè)人機(jī)器人項(xiàng)目與斯坦福大學(xué)人工智能實(shí)驗(yàn)室項(xiàng)目的合作。2008年后，由柳樹車庫進(jìn)行推廣和維護(hù)。經(jīng)過多年的發(fā)展，ROS 操作系統(tǒng)日趨完善，不僅為操作者提供了實(shí)用性較強(qiáng)的工具，也提供了先進(jìn)的開源算法。ROS具有點(diǎn)對點(diǎn)設(shè)計(jì)、多種編程語言、快速測試和分布式計(jì)算等優(yōu)點(diǎn)，基于 ROS的機(jī)器人應(yīng)用開發(fā)逐漸成為主流[1]。

1.2機(jī)器人SLAM算法介紹

1）SLAM是同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping）的縮寫，最早由Hugh Durant-Whyte和John J.Leonard自1988年提出。主要用于解決移動(dòng)機(jī)器人在未知環(huán)境中運(yùn)行時(shí)即時(shí)定位與地圖構(gòu)建的問題。ROS系統(tǒng)提供了多種SLAM功能包，包括二維SLAM的gmapping、hector、cartographer功能包，以及三維SLAM的rgbslam、ORB_SLAM功能包。這些功能包可分別實(shí)現(xiàn)仿真機(jī)器人和真實(shí)機(jī)器人的SLAM功能[2]。

2）自主導(dǎo)航：ROS提供了移動(dòng)機(jī)器人的導(dǎo)航框架，包括實(shí)現(xiàn)機(jī)器人定位的amcl功能和實(shí)現(xiàn)路徑規(guī)劃的move_base功能包，可幫助快速實(shí)現(xiàn)輪式移動(dòng)機(jī)器人的導(dǎo)航功能。

3）自主探索SLAM：結(jié)合SLAM與自主導(dǎo)航功能，機(jī)器人可以在無人工干預(yù)的情況下自主完成未知環(huán)境的SLAM建圖功能，導(dǎo)航路徑自動(dòng)根據(jù)地圖信息的完善而不斷優(yōu)化。

2.自主導(dǎo)航仿真

2.1 Gazebo開源物理仿真環(huán)境

Gazebo是開源物理仿真環(huán)境，通過它，加上PR2（是Willow Garage公司設(shè)計(jì)的機(jī)器人平臺(tái)，也是目前科研領(lǐng)域常使用到的機(jī)器人之一）功能包嵌入，能實(shí)現(xiàn)移動(dòng)機(jī)器人模型的雛形，PR2功能包機(jī)器人模型如圖1所示。在Gazebo物理環(huán)境下還可實(shí)現(xiàn)任意建模模型。

2.2 Gazebo物理仿真環(huán)境搭建

仿真步驟：（1）配置機(jī)器人模型。（2）創(chuàng)建仿真環(huán)境。（3）開始仿真。

1）機(jī)器人底盤仿真：為各link添加慣性參數(shù)與碰撞屬性和gazebo標(biāo)簽，同時(shí)為joint添加傳動(dòng)裝置（使用transmission標(biāo)簽，小車輪子用速度控制接口）。

2）添加gazebo控制插件（相當(dāng)于作為驅(qū)動(dòng)板）：編寫并運(yùn)行相應(yīng)launch文件得到gazebo中的機(jī)器人仿真模型。

3）創(chuàng)建仿真環(huán)境：選擇insert，在坐標(biāo)系中插入已有的模型。

4）傳感器仿真：攝像頭仿真，為攝像頭link添加碰撞屬性和慣性屬性。同時(shí)宏定義攝像頭，智能車道路運(yùn)行仿真模擬如圖2所示。

用后臺(tái)圖形工具套件Qt查看攝像頭當(dāng)前畫面同時(shí)啟動(dòng)鍵盤控制，鍵盤控制界面及仿真實(shí)例完成效果如圖3所示。

2.4 機(jī)器人視覺定位具體應(yīng)用模塊與組件

2.4.1 Rviz三維平臺(tái)

Rviz 是機(jī)器人操作平臺(tái) ROS 中的一種可視化工具，可以圖形化顯示機(jī)器人的傳感器信息，運(yùn)動(dòng)狀態(tài)以及周圍環(huán)境的變化信息等。同時(shí)Rviz支持豐富的數(shù)據(jù)類型，通過加載不同的Dispalys類型達(dá)到可視化，每一個(gè)Dispaly都有一個(gè)獨(dú)特的名字。下面是幾個(gè)常見的display類型：Axes-顯示坐標(biāo)系;camera-從相機(jī)視角顯示圖像;Grid-顯示網(wǎng)格;Image-顯示圖像;Odomerty-顯示里程計(jì)數(shù)據(jù);RobotModel-顯示機(jī)器人模型。

2.4.2 arbotix_gui 控制器

arbotix是一款控制電機(jī)、舵機(jī)的控制板，并且提供相對應(yīng)的ros功能包。該功能包不僅可以驅(qū)動(dòng)真實(shí)的ArbotiX控制板，還提供了一個(gè)差速控制器。通過接收速度控制指令更新機(jī)器人的joint狀態(tài)，從而實(shí)現(xiàn)在rviz中的運(yùn)動(dòng)。在移動(dòng)機(jī)器人的建圖過程中，可以啟動(dòng)鍵盤節(jié)點(diǎn)控制機(jī)器人進(jìn)行運(yùn)動(dòng)從而實(shí)現(xiàn)對實(shí)驗(yàn)環(huán)境地圖的構(gòu)建。同樣也可以使用 arbotix_gui 控制器控制機(jī)器人完成建圖工作。

2.4.3 室內(nèi)移動(dòng)機(jī)器人軟件結(jié)構(gòu)

基于 ROS 的移動(dòng)機(jī)器人系統(tǒng)軟件設(shè)計(jì)，主要包括底盤驅(qū)動(dòng)的控制、激光雷達(dá)的數(shù)據(jù)采集、SLAM、AMCL 定位以及導(dǎo)航。

1）激光雷達(dá)節(jié)點(diǎn)：將激光雷達(dá)采集的數(shù)據(jù)發(fā)布到scan 話題上，只需要訂閱這個(gè)話題就能使用這些數(shù)據(jù)。

2）底盤驅(qū)動(dòng)控制節(jié)點(diǎn)：樹莓派4B開發(fā)板下達(dá)速度指令傳輸?shù)揭苿?dòng)機(jī)器人底盤控制板，從而控制移動(dòng)機(jī)器人以此速度運(yùn)行。底盤驅(qū)動(dòng)控制節(jié)點(diǎn)通過訂閱/cmd_vel 話題，把消息轉(zhuǎn)換成速度指令，并通過串口發(fā)送到 STM32F405 控制板上，控制機(jī)器人移動(dòng)。

3）SLAM節(jié)點(diǎn)：通過對激光雷達(dá)節(jié)點(diǎn)發(fā)布的/scan 話題的訂閱，再結(jié)合2D SLAM算法實(shí)現(xiàn)地圖構(gòu)建。

4）AMCL節(jié)點(diǎn)：通過訂閱/scan話題獲取激光雷達(dá)數(shù)據(jù)，并輸出機(jī)器人在地圖中的位姿信息。

5）導(dǎo)航節(jié)點(diǎn)：在已知的地圖上根據(jù)激光雷達(dá)測量的數(shù)據(jù)控制移動(dòng)機(jī)器人導(dǎo)航，并躲避障礙物。

2.4.4 通信設(shè)計(jì)

底盤驅(qū)動(dòng)板和 ROS 之間的通信，即 STM32 和樹莓派之間的通信基于串口通信。在進(jìn)行通信之前，需先將格式一致的串口通訊的數(shù)據(jù)包信息從發(fā)送設(shè)備的TXD接口傳輸?shù)浇邮赵O(shè)備的 RXD 接口。在ROS中，控制節(jié)點(diǎn)發(fā)布的命令話題需要通過串口節(jié)點(diǎn)來訂閱，并通過串口設(shè)備發(fā)送到移動(dòng)底盤。同時(shí)串口節(jié)點(diǎn)實(shí)時(shí)接收移動(dòng)底盤發(fā)送的傳感器數(shù)據(jù)，并進(jìn)行封裝發(fā)布 sensor話題，listener節(jié)點(diǎn)實(shí)時(shí)訂閱這個(gè)sensor話題，這樣就實(shí)現(xiàn)了 ROS 和移動(dòng)底座的通信過程。可以在 ROS 中安裝一個(gè)serial串口庫，并進(jìn)行參數(shù)配置，使參數(shù)和 STM32 串口配置一樣，包括起始位、數(shù)據(jù)位和停止位等。再初始化串口，就能實(shí)現(xiàn)樹莓派和 STM32 的串口通信了。

2.4.5 TF坐標(biāo)變換

TF是一個(gè)可以滿足用戶跟隨時(shí)間跟蹤多個(gè)坐標(biāo)系的功能包，其使用樹形數(shù)據(jù)結(jié)構(gòu)，運(yùn)用時(shí)間緩沖并維護(hù)多個(gè)坐標(biāo)系之間的坐標(biāo)變換關(guān)系，它可以幫助開發(fā)者任意時(shí)刻完成點(diǎn)、向量等坐標(biāo)在任意坐標(biāo)系中的變換。

一個(gè)機(jī)器人系統(tǒng)通常配備很多三維坐標(biāo)系，并且它們會(huì)隨著時(shí)間的推移發(fā)生變化，如世界坐標(biāo)系（world frame）、基坐標(biāo)系（base frame）、機(jī)器人頭部坐標(biāo)系（head frame）等。TF可以以時(shí)間為軸跟蹤這些坐標(biāo)系（10s之內(nèi)為默認(rèn)參數(shù)）。常用的變換有2種。

1）廣播TF變換，向系統(tǒng)中廣播坐標(biāo)系間的坐標(biāo)變換關(guān)系。系統(tǒng)中可能會(huì)存在多個(gè)不同部分的TF變換廣播，每個(gè)廣播都可以在無需同步的情況下直接將坐標(biāo)變換關(guān)系插入TF樹中。

2）監(jiān)聽TF變換，接收并緩存系統(tǒng)中發(fā)布的所有坐標(biāo)變換數(shù)據(jù)，并從中查詢所需的坐標(biāo)系變換關(guān)系。

TF常用工具有tf_monitor、tf_echo、static_transform_publisher等。

1）tf_monitor工具的功能是打印TF樹中所有坐標(biāo)系的發(fā)布狀態(tài)，也可以通過輸入?yún)?shù)來查看指定坐標(biāo)系間的發(fā)布狀態(tài)消息。

2）tf_echo工具的功能為查看指定坐標(biāo)系間變換關(guān)系。命令格式如下：

$tf_echo <source_frame> <target_frame>

3）static_transform_publisher功能是發(fā)布兩個(gè)坐標(biāo)系之間的靜態(tài)坐標(biāo)變換，這兩個(gè)坐標(biāo)系不產(chǎn)生相對位置變化。

3.實(shí)驗(yàn)案例

步驟1：將智能車設(shè)備與計(jì)算機(jī)設(shè)備進(jìn)行連接。

步驟2：運(yùn)行Rviz，rosrun rviz rviz。添加add，將上述的機(jī)器人模型RobotModel選擇坐標(biāo)系，畫面出現(xiàn)機(jī)器人。

步驟3：設(shè)置Fixed Frame模式為odom_combined。

步驟4：添加TF，可顯示小車上所搭載的設(shè)備及其零部件。

步驟5：添加LaserScan，其Topic設(shè)置為/scan，可以看到小車搭載雷達(dá)的激光點(diǎn)（紅色區(qū)域），如圖4所示。

步驟6：添加RobotModel，可以看到可視化的小車模型。

步驟7：添加PoseWithCovariance，可以看到小車的車頭方向

步驟8：在終端輸入arbotix_gui開啟虛擬遙控，可以控制小車的移動(dòng)。如圖5所示。

通過arbotix_gui實(shí)現(xiàn)上述小車的虛擬遙控可編寫相關(guān)launch文件并配置相關(guān)arbotix節(jié)點(diǎn)：

<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">

<rosparam file="$（find my_urdf05_rviz）/config/hello.yaml" command="load" />

<param name="sim" value="true" />

</node>

此程序調(diào)用了arbotix_python功能包下的arbotix_diver節(jié)點(diǎn)。arbotix驅(qū)動(dòng)機(jī)器人運(yùn)行時(shí)，需要獲取機(jī)器人信息，可以通過file加載配置文件。在仿真環(huán)境下，需要將sim配置設(shè)為true。最后則啟動(dòng)相應(yīng)launch文件，配置Rviz，將Fixed name這欄選擇并設(shè)置為odom（里程計(jì)坐標(biāo)系），Topic這欄也設(shè)置為odom。但topic中的odom具有不同概念，odom topic是根據(jù)編碼里程計(jì)計(jì)算得到的里程計(jì)信息。完成相應(yīng)配置后調(diào)用rostopic list會(huì)產(chǎn)生一個(gè)話題 /cmd_vel。此時(shí)發(fā)布/cmd_vel話題消息就可以控制小車運(yùn)動(dòng)了。

4.結(jié)論

隨著科技日益發(fā)展，機(jī)器人的視覺定位系統(tǒng)在人類生產(chǎn)生活、人工智能領(lǐng)域扮演著越來重要的腳色。本課題基于Ubuntu系統(tǒng)下的ROS機(jī)器人操作系統(tǒng)為研究平臺(tái)，對輪式機(jī)器人（智能車）在路面上的定位、捕捉圖像及導(dǎo)航問題進(jìn)行研究。通過搭建仿真環(huán)境，控制與實(shí)現(xiàn)智能車的移動(dòng)，雷達(dá)信息接收以及定位與捕捉圖像等功能，測試機(jī)器人仿真環(huán)境下的避障能力，方向控制反應(yīng)能力等性能指標(biāo)均達(dá)到了設(shè)計(jì)的指標(biāo)，具有實(shí)用價(jià)值。

參考文獻(xiàn)

[1] 朱建軍，王明園，張博文.基于ROS的輪式機(jī)器人建模方法與仿真研究[J].吉林化工學(xué)院學(xué)報(bào)，2021，38（3）：61-65.

[2]郭柱梁.基于激光雷達(dá)的SLAM和動(dòng)態(tài)避障機(jī)器人研究[M].哈爾濱：黑龍江大學(xué)出版社，2021.

第一作者簡介：鄭紀(jì)陽 2001年5月1日，性別：男，民族：漢，籍貫：福建省廈門市，學(xué)歷：本科在讀，職稱：學(xué)生，研究方向：ROS機(jī)器人操作系統(tǒng)。

通信作者簡介：張國忠，1951年2月出生，男，漢族，籍貫江西南昌，博士，教授，主要研究方向：系統(tǒng)辨識(shí)，計(jì)算機(jī)控制，電力系統(tǒng)自動(dòng)化，人工智能控制等。

●此文由2021年全國大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目資助，項(xiàng)目名稱：機(jī)器人視覺定位系統(tǒng)研究與實(shí)現(xiàn)項(xiàng)目負(fù)責(zé)人：鄭紀(jì)陽（2021-239）