[發明專利]一種基于改進模型的UWB/IMU/ODOM多傳感器數據融合移動機器人定位方法

有效

• 申請號: 202011464443.8
• 申請日: 2020-12-12
• 公開（公告）號: CN112683263B
• 公開（公告）日: 2022-11-11
• 發明（設計）人: 劉明雍;黃宇軒;石廷超;王宇
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 西安凱多思知識產權代理事務所(普通合伙) 61290
• 代理人: 高凌君

1.一種基于改進模型的UWB/IMU/ODOM多傳感器數據融合移動機器人定位方法，其特征在于：包括以下步驟：

步驟1：機器人建模

建立移動機器人的運動模型：

θ_k＝θ_k-1+ω_k-1*T

v_k＝v_k-1+a_k-1*T

其中T表示離散系統的時間間隔，k表示系統時刻序列，x表示機器人x坐標，y表示機器人y坐標，θ表示機器人當前的偏航角，v表示機器人當前速度，a表示機器人加速度，ω表示機器人的角速度；其中x，y，θ，v組成狀態量X，a，ω組成控制量U；

建立如下狀態方程：

X＝f(X,U+R_imu)

其中R^imu為IMU的隨機觀測噪聲；

步驟2：構建機器人觀測方程

針對ODOM和UWB數據構建兩個觀測方程：

采用UWB作為基準數據完成定位后，只采用其中定位結果的x和y坐標數據；建立UWB觀測矩陣：

觀測方程為：Z^uwb＝H^uwb*X+R^uwb

其中R^uwb是UWB的量測誤差，X為狀態量，Z^uwb為UWB觀測量，包括通過UWB定位得到的機器人的x坐標與y坐標，通過觀測矩陣H^uwb與狀態量X相乘表示，H^uwb為UWB觀測矩陣；

針對ODOM的數據，只采用ODOM的速度v和角度θ作為輸入量；建立ODOM觀測矩陣：

觀測方程：Z^odom＝H^odom*X+R^odom

其中R^odom是ODOM的量測誤差，X為狀態量，Z^odom為ODOM觀測量，包括機器人的速度v和偏航角θ，通過觀測矩陣H^odom與狀態量X相乘表示，H^odom為觀測矩陣；

步驟3：EKF數據融合

首先將狀態方程線性化得到其中A為狀態方程對于狀態量X的偏導數，B為對于控制量U的偏導數，下標k表示系統時刻序列；

接下來進行EKF數據融合：

先完成狀態估計：

其中分別表示在第k時刻，狀態估計后得到的狀態量以及狀態誤差協方差；

進行UWB的觀測校正：

其中X_k′，Σ_k′分別表示在第k時刻，采用UWB進行第一次觀測校正后得到的狀態量以及狀態誤差協方差；

繼續進行odom的觀測二次校正：

Σ_k＝(I-K^odomH^odom)Σ_k′

完成狀態量估計X_k和協方差估計Σ_k，循環迭代就能得到每一時刻的移動機器人運動狀態。

2.根據權利要求1所述一種基于改進模型的UWB/IMU/ODOM多傳感器數據融合移動機器人定位方法，其特征在于：步驟3中對狀態方程線性化的過程為：

求解狀態方程對于狀態量X的偏導數：

以及對于控制量U的偏導數：

對于k時刻，得到線性化的狀態方程為