基于多傳感器的斜坡路面上自主跟隨定位方法

陳長樂, 景嘉寶, 劉永城, 謝雨晴, 何 濤,

(1 湖北工業大學機械工程學院, 湖北 武漢 430068;2 湖北工業大學底特律綠色工業學院, 湖北 武漢 430068)

隨著經濟與科技快速發展,果實采摘機器人[1]、自動跟隨行李箱[2]等自主跟隨機器人的需求日益增多,應用也越發廣泛。自主跟隨機器人應用場景較為自由,環境復雜,因此對移動目標的準確定位是當前研究熱點[3]。

目前定位方法主要分為視覺導航定位[4-5]、GPS導航定位[6]、超聲波導航定位[7]。視覺導航定位通過識別圖像特征實現定位,但圖像算法復雜,且移動目標遇到遮擋物或環境光線過暗、過亮時,傳感器難以識別目標。GPS導航定位通過終端芯片解算衛星坐標及距離實現定位,多用于戶外空曠環境,如車輛路線規劃、行人導航等,而室內環境容易出現GPS信號丟失問題。超聲波導航定位通過解算發射至定位目標的回波時間實現定位,其具有不受環境光照干擾的優點,被廣泛應用于自主跟隨機器人的定位導航系統中。

超聲波導航定位方法當前多使用三角定位法,如文獻[8]和文獻[9] 使用的多傳感器信息融合分析計算出移動目標中心坐標。但當前方法都只適用于移動目標與跟隨機器人在同一水平面情況。在斜坡路面,移動目標與自主跟隨機器人之間出現水平高度差和俯仰角文題,會增大當前方法的定位精度,難以滿足定位系統需求。

基于以上分析,本文提出了一種基于多傳感器信息融合的自動跟隨定位方法。該方法在自主跟隨機器人上加裝兩個信號接收模塊和角度測量模塊。在移動目標上設置多信號源,同時加裝陀螺儀用以檢測移動目標本身前后傾角的改變,建立機器人與移動目標間的冗余信道;設定基于移動機器人的空間坐標系,考慮空間俯仰角的前提下使用三角定位法測量多個信號源的空間三維坐標;根據測量到的樣本點坐標,利用最小二乘逼近[10]準確計算斜坡路面上移動目標中心坐標。

1 移動信號源定位方法

本文采用三角定位法定位移動信號源,定位原理如圖1所示。

圖1 信號源三角定位法

機器人上的對稱信號節點A、B、O在平面的坐標分別為(0, -a, 0)、(0,a, 0)、(0, 0, 0),假設移動目標中心節點坐標為(x,y,z),測得信號源到A和B的距離為l1、l4,俯仰角為β。在移動目標與A、B兩點構成的平面內,由幾何關系可得出:

(1)

(2)

(3)

由式(1)、式(2)和式(3)可得:

(4)

(5)

在空間坐標中,根據幾何關系可得:

(6)

z=l2·sinβ

(7)

由式(3)、式(4)、式(6)與式(7)可得:

(8)

(9)

2 多源信號定位模型及算法

2.1 移動目標多信號源定位模型

本文建立的定位模型在移動目標上將m個信號源以半徑為R圓形排列,等距離布置,便于計算(圖2)。在移動目標移動時,由于地面坡度的變化,信號源與跟隨機器人之間產生俯仰角,移動目標本身的平面與信號源接收平面也會產生傾斜角θ(圖3)。在計算過程中忽略俯仰角度會直接導致移動目標中心點定位計算不準確。因此在考慮移動目標相對于跟隨機器人的俯仰角后,能夠準確分析計算出坡面移動目標的移動方向和速度。

圖2 多源信號定位模型

圖3 坡面上的移動目標

移動目標在移動時,跟隨機器人上的信號接收節點A、B以順時針方向實時接收移動目標上的m個信號源的信號。角度測量模塊O實時測量所有信號源的俯仰角。

將計算出的所有移動目標中心坐標使用最小二乘法逼近來得到準確的中心點空間坐標。由于移動物體保持著運動,每個掃描周期都會得出一個移動目標中心坐標,通過兩個相鄰周期得出的中心點坐標可計算出當前移動物體的運動方向和速度。

2.2 空間多信號源定位算法

為避免坡面傾角影響移動目標中心坐標、目標與跟隨機器人O點的距離d、目標中心位置的俯仰角與方向角、移動目標的運動方向和速度,基于上述模型設計了結合俯仰角的空間多信號源定位算法。該方法結合多信號源的俯仰角來計算分析移動物體的運動方向和速度以及中心位置的特征數據。

(10)

由式(10)計算可得:

A1x+B1y+C1z+D1=0

同理可得:

A2x+B2y+C2z+D2=0

A3x+B3y+C3z+D3=0

圖4中So、Si、Sj和Sk分別是移動目標中心位置和任意已知的3個信號源位置。假設3個信號源確定的移動目標中心位置所在的空間圓半徑為R0,由So、Si、Sj和Sk在空間中的幾何關系可知:

(11)

獲得Si、Sj和Sk3個信號源在空間中關于圓心空間坐標的線性代數方程組后解得

(12)

圖4 中心坐標So計算模型

(13)

(14)

方向角

(15)

由式(14)和式(15)可計算得到移動目標中心位置的方向角和移動速度,其中方向角α和俯仰角β共同決定移動目標中心位置在空間中相對于移動機器人的運動方向。

3 移動目標定位實驗

定位系統需求參數如圖5所示,其中:移動目標定位檢測距離L的范圍需求為300～1500 mm;水平夾角γ需求為45°～135°,用于防止目標轉向速度過快定位系統丟失目標;俯仰角β需求為-60°～60°,用于定位坡面移動目標;移動目標中心點坐標定位精度需求為X軸±50 mm、Y軸±50 mm、Z軸±50 mm。

圖5 定位系統需求參數示意圖

基于以上需求,本文設計了定位系統用于定位實驗。在該系統中,選用了15個超聲波傳感器以24°間隔角在移動目標上以150 mm為半徑的圓上均勻分布安裝(超聲波傳感器測距范圍在500 mm到4500 mm范圍,測距精度為5 mm),移動機器人中心位置O設置兩個超聲波接收模塊于z軸方向上,上下距離為50 mm,用于同時接收移動目標上某一信號源信號,測出該信號源離2個超聲波接收模塊距離以算出俯仰角;移動機器人左右各安裝一個超聲波接收模塊,位于O點位置左右各250 mm,用于接收移動目標上的15個信號源信號。

為滿足系統各參數需求,定位實驗選擇俯仰角β為60°的坡度進行實驗。每次實驗以移動目標相距跟隨機器人水平角夾為γ、距離為500 mm處為起始位置,在坡度上每移動100 mm記錄1次坐標數據,記錄至1500 mm處停止。由于定位系統對稱布置超聲波接收模塊,水平夾角γ為45°～90°,滿足定位精度要求證明90°～135°的水平夾角也滿足,基于此設置水平夾角γ為45°、60°、75°、90°共4組實驗,驗證本文設計的定位系統的有效性。

不同水平夾角下的坡面目標定位實驗中,移動目標中心坐標定位誤差見表1。分析實驗數據可知,相比于傳統移動目標定位方法容易丟失坡面移動目標,本文方法在滿足檢測距離、水平夾角的情況下,能夠對處于60°坡度的移動目標進行有效定位,且目標中心坐標各軸測量誤差處于±50 mm內,滿足定位精度需求。其中,水平夾角為90°、檢測距離為600 mm內時,本文方法檢測精度最佳,目標中心坐標各軸定位精度±10 mm。對于實驗測得移動目標中心坐標誤差波動較大問題,經研究分析,與傳感器標定精度相關,在對傳感器有效標定后,本文方法可進一步提高對斜坡路面移動目標定位精度。

表1 移動目標中心坐標定位誤差

4 結論

本文設計的坡度路面移動目標定位跟隨方法,在機器人跟隨目標時保證定位系統對坡度路面上的移動目標有效定位,避免因為坡度變化導致移動目標中心位置定位偏差,而偏差累積造成移動目標位置丟失問題。實驗表明,該定位方法能夠在具有坡度的路面上保證自動跟隨機器人對移動目標的精準定位,有效降低了坡度對于定位精度的影響,具有一定的工程應用價值。