[發明專利]一種騎行載具姿態識別報警方法和姿態監測報警裝置

在審

• 申請號: 201810401472.6
• 申請日: 2018-04-28
• 公開（公告）號: CN108663045A
• 公開（公告）日: 2018-10-16
• 發明（設計）人: 李忱達;張萌;潘其琦;劉丹妹;成峰;曾鈺;程洪宇;賈璐萌;韓冰;張詠澤;沈松鋒
• 申請（專利權）人: 山東交通學院
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C1/00
• 代理公司: 濟南誠智商標專利事務所有限公司 37105
• 代理人: 鄭憲常

1.一種騎行載具姿態監測報警控制方法，其特征是，包括以下步驟：

S1、獲取陀螺儀檢測到的姿態角在三個軸方向分量，分別采用姿態融合算法修正陀螺儀在三個軸方向融合后的姿態角pitch角agx、yaw角agy、roll角agz和繞三個軸方向的角速度gyrox、gyroy、gyroz；

S2、判斷角速度是否超過設定的猛烈撞擊角速度閾值；如果是，轉到步驟S3，如果否，轉到步驟S4；

S3、啟動基站定位并發送報警信息，采用姿態融合算法修正陀螺儀姿態角，判斷姿態角是否超過設定的扶正閾值，如果否，轉到步驟S3，如果是，返回步驟S1；

S4、判斷姿態角是否超過設定的傾倒姿態角閾值；如果是，轉到步驟S3，如果否，返回步驟S1。

2.如權利要求1所述的一種騎行載具姿態監測報警控制方法，其特征是，步驟S1中，采用姿態融合算法修正陀螺儀在三個軸方向融合后的姿態角具體包括以下步驟：

S11、計算陀螺儀傳感器測量的姿態角預估計值與加速度傳感器的姿態角實測值之間的偏差值，偏差值由公式(7)計算：

error(k)＝angular_w(k)-angular_g(k) (7)

式(7)中：angular_w(k)為k時刻陀螺儀傳感器測量到的姿態角；angular_g(k)為k時刻加速度傳感器測量的姿態角，error(k)為姿態角在k時刻的偏差值，即革新量；

S12、計算卡爾曼增益K，卡爾曼增益K由公式(8)計算：

K(k)＝P(k-1)gC'[CgP(k-1)gC'+R(k)]^-1 (8)

式(8)中：P(k-1)為k-1時刻的濾波協方差，C為系統輸出方程的輸出矩陣；R(k)為測量噪聲協方差，R(k)由公式(10)計算：

R(k)＝E[ω(k)w(k)^T] (10)

式(10)中，w(k)為系統測量噪聲；

S13、利用姿態角革新值error(k)和卡爾曼增益K對陀螺儀傳感器的姿態角進行修正，公式如下：

angular_final(k)＝angular_w(k)+K(k)gerror(k) (11)

式(11)中，angular_final(k)即為修正后的k時刻姿態角的最終輸出值；

S14、計算下一步濾波算法中的濾波協方差：

P(k+1)＝P(k)-K(k)gCgP(k) (12)

當系統采集到k+1時刻陀螺儀傳感器角速度輸出后，回到步驟S1計算，進入k+1時刻的濾波運算。

3.如權利要求2所述的一種騎行載具姿態監測報警控制方法，其特征是，步驟S12中，系統輸出方程的輸出矩陣C計算步驟如下：

建立考慮角速度誤差的陀螺儀姿態角測量的線性模型，如式(1)所示：

θ(k+1)＝θ(k)+[ω(k)-error(k)]gΔt+v(k) (1)

式(1)中：θ(k+1)為k+1時刻陀螺儀傳感器測量的姿態角；θ(k)為k時刻陀螺儀傳感器測量的姿態角；ω(k)為k時刻陀螺儀傳感器測量的姿態角分量相應軸上的角速度；error(k)為陀螺儀傳感器輸出角速度誤差；Δt為采樣周期；v(k)為系統過程噪聲；

建立陀螺儀傳感器輸出角速度誤差與陀螺儀傳感器所測量角速度、上一時刻輸出的角速度誤差線性模型，如式(2)所示：

error(k+1)＝error(k)+δgω(k)+v(k) (2)

式(2)中：δ為陀螺儀傳感器輸出角速度誤差error(k)與其輸入陀螺儀傳感器姿態角ω(k)的線性系數；

聯立式(1)和式(2)得到修正后的陀螺儀傳感器測量姿態角模型，如式(3)所示：

θ(k+1)＝[ω(k)-(error(k)+δgω(k))]Δt+v(k)+θ(k) (3)

選擇θ(k)和error(k)作為系統狀態，以ω(k)作為系統輸入變量，陀螺儀的線性測量模型可以表達如下：

按照以下方式選取系統狀態矩陣：

進一步得到系統狀態方程：

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

系統輸出方程為：

求解出系統輸出方程的輸出矩陣C。