[發(fā)明專利]一種基于履帶式救援機器人的自主定位導航方法

在審

• 申請?zhí)枺?/td>: 201910804102.1
• 申請日: 2019-08-28
• 公開（公告）號: CN110632919A
• 公開（公告）日: 2019-12-31
• 發(fā)明（設計）人: 梁士流;張學習;盧國昆
• 申請（專利權）人: 廣東工業(yè)大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 44329 廣東廣信君達律師事務所
• 代理人: 楊曉松

1.一種基于履帶式救援機器人的自主定位導航方法，其特征在于，包括以下步驟：

S1.以履帶機器人作為移動機器人平臺，進行必要零件的配置和傳感器擴展；

S2.設置TF變換，采集里程計，激光雷達數(shù)據(jù)；

S3.采用Hector_Slam地圖構建開源包建圖及應用KLD采樣，采用微粒濾波器定位；

S4.利用路由器搭建的WIFI作為履帶機器人與PC上位機之間的無線通訊，實現(xiàn)PC上位機對履帶機器人傳感器數(shù)據(jù)信息的提取以及觀察者對履帶機器人運行狀態(tài)的實時監(jiān)控；

S5.打開自主定位與建圖、導航的工作節(jié)點，對模擬環(huán)境中的任意自定義目標點指明，履帶機器人自動根據(jù)算法規(guī)劃的路徑接收到控制指令，完成自主定位導航任務。

2.根據(jù)權利要求1所述的一種基于履帶式救援機器人的自主定位導航方法，其特征在于，所述步驟S1的具體配置過程如下：

工控機裝配無線網卡發(fā)送熱點到PC上位機，PC上位機的工作是負責向機器人端發(fā)送指令；STM32機器人控制模塊負責編碼器里程計的數(shù)據(jù)采集，同時通過電機控制其上的運行；工控機進行傳感器數(shù)據(jù)的采集，同時IMU的數(shù)據(jù)與激光數(shù)據(jù)進行融合，對基于ROS系統(tǒng)的機器人的姿態(tài)進行調整，最后作一個算法程序的運行處理；PC上位機打開可視化工具可隨時觀察機器人移動進程。

3.根據(jù)權利要求1所述的一種基于履帶式救援機器人的自主定位導航方法，其特征在于，所述步驟S2的具體過程如下：

以TF樹的形式發(fā)布各個相關參考系的坐標變換關系，坐標變化樹即為系統(tǒng)中不同坐標系之間的偏移；履帶機器人平臺擁有移動的底盤、位于底盤正上方并俯視車體中心的激光雷達；TF定義兩個坐標系：一個坐標系原點位于履帶機器人的底盤中心點為base_link，另一個位于激光雷達的中心為base_laser；把激光數(shù)據(jù)從從base_laser變換到base_link；

激光雷達要求能夠通過ROS發(fā)布sensor_msgs/LaserScan格式的消息，即為二維雷達信息；履帶機器人發(fā)布nav_msgs/Odometry格式的里程計信息，同時也要發(fā)布相應的TF變換odom-to-base_link；

里程計包含兩方面信息，一方面是位姿，另一方面是速度；該履帶機器人里程計信息獲取方法采用IMU數(shù)據(jù)積分獲取，IMU獲取三軸線加速度和三軸角速度數(shù)據(jù)，通過數(shù)據(jù)解析節(jié)點封裝成數(shù)據(jù)類型為sensor_msgs/Imu的消息，通過ROS提供的相關包imu_tools進行濾波，得到濾波后的sensor_msgs/Imu的消息；

通過電機的編碼器獲取左右輪的平均速度，而轉向速度根據(jù)左右輪的在給定時間內的弧度差計算得到，位置的獲取根據(jù)上面的前進速度推算出位置；姿態(tài)的獲取根據(jù)轉向速度推算出轉角。

4.根據(jù)權利要求1所述的一種基于履帶式救援機器人的自主定位導航方法，其特征在于，所述步驟S4具體包括：

設置PC上位機操作者筆記本端和履帶機器人本體工控機端之間的無線通信；同一路由下，分別配置好PC上位機、工控機于網段上的域名，然后給PC上位機端系統(tǒng)變量設置工控機端的域名，最后操作者通過SSH連接工控機上的Ubuntu系統(tǒng)上，打開ROS主節(jié)點，從而建立兩臺機的ROS通訊，完成履帶機器人和PC上位機無線通信的配置任務；

配置完成后，操作者在PC上位機上訂閱話題，相應地可以完成PC上位機與工控機的數(shù)據(jù)交互；操作者發(fā)送的位姿指令，工控端可通過無線網絡接收傳送的數(shù)據(jù)并在ROS系統(tǒng)導航框架下進行位姿跟蹤；

操作者在PC上位機端通過打開ROS的可視化工具Rviz上觀察機器人的運動狀態(tài)、運動方向、運動軌跡在一個用激光雷達數(shù)據(jù)信息的點構成的二維柵格地圖展示出來，從而實現(xiàn)PC上位機對履帶機器人傳感器數(shù)據(jù)信息的提取以及觀察者對機器人運行狀態(tài)的實時監(jiān)控。

5.根據(jù)權利要求1所述的一種基于履帶式救援機器人的自主定位導航方法，其特征在于，所述步驟S5包括：

履帶機器人的路徑規(guī)劃算法采用A*算法，公式表示為f(n)＝g(n)+h(n),其中f(n)表示節(jié)點n從初始點到目標點的估價函數(shù)，g(n)表示在狀態(tài)空間中從初始節(jié)點到n節(jié)點的實際代價，即起始節(jié)點到當前節(jié)點的實際代價，h(n)表示從n到目標節(jié)點最佳路徑的估計代價，即當前節(jié)點到目標節(jié)點的估計代價；

g(n)是對g*(n)的一個估計，是當前的搜索圖G中s到n的最優(yōu)路徑費用g(n)≥g*(n)；h(n)是對h*(n)的估計，是從n到目標節(jié)點的估計代價，稱為啟發(fā)函數(shù)；保證找到最短路徑條件，關鍵在于估價函數(shù)h(n)的選?。?/p>

估價值h(n)<＝n到目標節(jié)點的距離實際值，這種情況下，搜索的點數(shù)多，搜索范圍大，效率低，但能得到最優(yōu)解；如果估價值>實際值,搜索的點數(shù)少，搜索范圍小，效率高，但不能保證得到最優(yōu)解；估價值與實際值越接近，估價函數(shù)取得就越好；

廣度優(yōu)先搜索：從起點開始，首先遍歷起點周圍鄰近的點，然后再遍歷已經遍歷過的點鄰近的點，逐步的向外擴散，直到找到終點；在執(zhí)行算法的過程中，每個點需要記錄達到該點的前一個點的位置；一旦到達終點，從終點開始，反過來順著父節(jié)點的順序找到起點，由此構成一條路徑。