[發明專利]用慣導、壓力傳感器修正下坡懸空后輪里程計誤差的方法

有效

• 申請號: 201710229436.1
• 申請日: 2017-04-10
• 公開（公告）號: CN107063305B
• 公開（公告）日: 2020-03-27
• 發明（設計）人: 王景川;曹金龍;馬葉;柳澤宇
• 申請（專利權）人: 江蘇東方金鈺智能機器人有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 錢麗

1.用慣導、壓力傳感器修正下坡懸空后輪里程計誤差的方法，其特征在于：采用陀螺儀和加速度傳感器實時檢測機器人的姿態，以判斷機器人是否正在下坡，當由機器人的姿態判斷出機器人處于下坡的過程中時，再利用壓力傳感器檢測機器人的后輪是否懸空；

當由姿態判斷出機器人處于平面行駛時，或者雖然處于下坡狀態，但后輪均未懸空時，采用下式進行行走距離增量(△x,△y,△θ)的計算：

式中：(△x,△y,△θ)＝前一次采樣間隔走過的路徑；△x為機器人x方向位置增量，△y為機器人y方向位置增量，△s為機器人的位置增量，△s_r,△s_l分別為右輪、左輪行走的距離；b為差動驅動機器人兩個輪子之間的距離；θ為機器人的朝向；

當檢測出一個后輪已懸空時，忽略兩個影響：

(1)后輪轉速的強閉環控制使速度不受重力加速度的影響；

(2)忽略前輪的方向對于單后輪驅動直線行駛的影響；這樣機器人只受一個后輪的驅動按直線行駛；

左輪懸空時，機器人行走距離增量(△x,△y,△θ)按照下面的修正公式進行計算：

右輪同理。

2.根據權利要求1所述的用慣導、壓力傳感器修正下坡懸空后輪里程計誤差的方法，其特征在于：陀螺儀和加速度傳感器均設于機器人底盤的中央位置，陀螺儀用于檢測機器人的角速度變化；加速度傳感器用于檢測機器人的速度變化；壓力傳感器有兩個，兩個壓力傳感器分別用于檢測兩個后輪對于機器人支架的壓力；機器人上還設有兩個光電編碼器、激光傳感器和工控機；兩個光電編碼器分別安裝在兩個后輪上作為里程計使用；激光傳感器用于檢測周邊環境的信息進行定位導航；工控機作為車載控制器，用于接收陀螺儀、加速度傳感器、壓力傳感器、光電編碼器和激光傳感器的數據，進行定位導航的運算。

3.根據權利要求1所述的用慣導、壓力傳感器修正下坡懸空后輪里程計誤差的方法，其特征在于：利用陀螺儀和加速度傳感器數據計算機器人姿態角，判斷機器人是否在下坡的步驟如下：

步驟(1)：系統建模，系統的狀態方程為：

X(k+1)＝AX(k)+Bu(k)+v(k)； (1)

式中，k為時刻，X(k+1)為機器人在(k+1)時刻狀態，A為狀態轉移矩陣，X(k)為機器人在k時刻狀態，B為系統控制矩陣，u(k)為k時刻系統輸入量，v(k)為系統過程噪聲，

輸出方程為：

Y(k)＝CX(k)+w(k)； (2)

式中，Y(k)為k時刻系統輸出量，C為觀測矩陣，w(k)為觀測噪聲，

步驟(2)：計算陀螺儀測量到的姿態角預估計值與加速度傳感器之間的偏差：

error(k)＝angle_g(k)-angle_a(k)； (3)

式中，error(k)為在k時刻的姿態角偏差，angle_g(k)為k時刻陀螺儀計算到的姿態角，angle_a(k)為k時刻加速度傳感器測得的姿態角，

步驟(3)：計算Kalman增益K(k)：

K(k)＝P(k-1)·C^T/[C·P(k-1)·C^T+R(k)]； (4)

式中，P(k-1)為狀態估計，R(k)為旋轉矩陣，

步驟(4)：計算k時刻機器人姿態角的最終輸出值angle_final(k)：

angle_final(k)＝angle_g(k)+K(k)·error(k)； (5)

步驟(5)：計算k+1時刻的濾波協方差P(k+1)：

P(k|k)＝(I-K(k)·C)P(k|k-1)； (6)

式中，P(k|k)為k時刻狀態估計，I為單位矩陣，P(k|k-1)為(k-1)時刻狀態估計，至此就完成了k時刻對機器人姿態所有的Kalman濾波計算，當k+1時刻的陀螺儀角速度數據輸入到機器人中，回到步驟(1)進行計算，這樣不停的迭代運行以輸出機器人的姿態信息，

在得到機器人的姿態信息后，判斷angle_final(k)和根據斜坡斜率得出的可判斷機器人正在下坡的臨界角度critical_angle，下坡的臨界角度critical_angle根據機器人結構設計由人工人為設定，當angle_final(k)>critical_angle時，判斷機器人正在下坡，不然判斷機器人在正常的行駛。

4.根據權利要求3所述的用慣導、壓力傳感器修正下坡懸空后輪里程計誤差的方法，其特征在于：檢測機器人的后輪是否懸空的方法如下：在k時刻判斷機器人正在下坡時，此時不停的讀取i、i≥k時刻兩個后輪壓力傳感器的數據p_right(i)和p_left(i)來判斷是否有后輪處于懸空的狀態，記k-1時刻機器人仍在正常行駛時的兩個后輪壓力傳感器的數據是p_right(k-1)與p_left(k-1)，通過壓力傳感器變化的臨界值critical_p來判斷后輪是否懸空，即當p_right(k-1)-p_right(i)>critical_p時，判斷機器人的右后輪已處于懸空的狀態中，當p_left(k-1)-p_left(i)>critical_p時，判斷機器人的左后輪已處于懸空的狀態中。