﻿ 基于卡尔曼滤波数据融合算法的智能钓鱼竿系统
 计算机系统应用  2020, Vol. 29 Issue (2): 83-93 PDF

Intelligent Fishing Rod System Based on Kalman Filter Data Fusion
CHEN Xiao-Lei, YUE Jun-Feng, LI Xiu-Mei
School of Information Science and Engineering, Hangzhou Normal University, Hangzhou 311121, China
Foundation item: National Natural Science Foundation of China (61571174)
Abstract: In order to cope with the problem of insufficient function of intelligent fishing rod in domestic market, this study designs an intelligent fishing rod system, which has the functions of fishing region selection, intelligent alarm and automatic walking fish. The study mainly solves the attitude of fishing rodby using quaternion method, adopts a data fusion algorithm based on Kalman filter, and builds the uC/OS-II operating system with the core microprocessor STM32, combined with various environmental sensors and cascade PID method. The results indicate that this system can not only reduce the probability that fish escape from the short line, but also provide the chance of automatic fishing in harsh environment.
Key words: quaternion method     attitude algorithm     Kalman filter     data fusion     cascade PID

1 智能钓鱼竿系统总体方案设计

 图 1 智能钓鱼竿示意图

2 智能钓鱼竿系统硬件部分设计

 图 2 智能钓鱼竿系统硬件部分设计框图

3 智能钓鱼竿系统软件部分设计

 图 3 鱼竿电机控制流程图

3.1 四元数姿态解算

 图 4 四元数姿态解算流程图

3.1.1 四元数定义

 $Q = {q_0} + {q_1} \cdot i + {q_2} \cdot j + {q_3} \cdot k$ (1)

 ${i^2} = {j^2} = {k^2} = i \cdot j \cdot k = - 1$ (2)
 \left\{ {\begin{aligned} & i \cdot j = k \\ & j \cdot k = i \\ & k \cdot i = j \end{aligned}} \right. (3)
 \left\{ {\begin{aligned} & j \cdot i = - k\\ & k \cdot j = - i\\ & i \cdot k = - j \end{aligned}} \right. (4)

 $Q = \left( {{q_0},{q_1},{q_2},{q_3}} \right)$ (5)

 \left\{ {\begin{aligned} & {{q_0} = \cos \left( {\frac{\alpha }{2}} \right)} \\ & {{q_1} = \sin \left( {\frac{\alpha }{2}} \right)\cos (\beta x)} \\ & {{q_2} = \sin \left( {\frac{\alpha }{2}} \right)\cos (\beta y)} \\ & {{q_3} = \sin \left( {\frac{\alpha }{2}} \right)\cos (\beta z)} \end{aligned}} \right. (6)
3.1.2 四元数微分方程

 $Q = \frac{1}{2}Q \cdot \omega$ (7)

 $\left[ {\begin{array}{*{20}{l}} {{q_0}} \\ {{q_1}} \\ {{q_2}} \\ {{q_3}} \end{array}} \right] = \frac{1}{2} \cdot \left[ {\begin{array}{*{20}{c}} 0&{ - \omega x}&{ - \omega y}&{ - \omega z} \\ {\omega x}&0&{\omega z}&{ - \omega y} \\ {\omega y}&{ - \omega z}&0&{\omega x} \\ {\omega z}&{\omega y}&{ - \omega x}&0 \end{array}} \right] \cdot \left[ {\begin{array}{*{20}{c}} {{q_0}} \\ {{q_1}} \\ {{q_2}} \\ {{q_3}} \end{array}} \right]$ (8)

3.1.3 四元数姿态矩阵关系

 $\begin{array}{l} C_b^e = \\ \!\!\!\!\left[\!\!\!\!\! {\begin{array}{*{20}{c}} {1 - 2 \cdot \left( {q_2^2 + q_3^2} \right)}\!\!\!&\!\!\!{2 \cdot \left( {{q_1} \cdot {q_2} \!-\! {q_0} \cdot {q_3}} \right)}\!\!\!&\!\!\!{2 \cdot \left( {{q_1} \cdot {q_3} \!+\!{q_0} \cdot {q_2}} \right)} \\ {2 \cdot \left( {{q_1} \cdot {q_2} \!+\! {q_0} \cdot {q_3}} \right)}\!\!\!&\!\!\!{1 - 2 \cdot \left( {q_1^2 + q_3^2} \right)}\!\!\!&\!\!\!{2 \cdot \left( {{q_2} \cdot {q_3}\! -\! {q_0} \cdot {q_1}} \right)} \\ {2 \cdot \left( {{q_1} \cdot {q_3}\! -\! {q_0} \cdot {q_2}} \right)}\!\!\!&\!\!\!{2 \cdot \left( {{q_2} \cdot {q_3} \!+\! {q_0} \cdot {q_1}} \right)}\!\!\!&\!\!\!{1 - 2 \cdot \left( {q_1^2 + q_2^2} \right)} \end{array}} \!\!\!\!\right] \end{array}$ (9)

 $\left[ {\begin{array}{*{20}{l}} {{X_e}} \\ {{Y_e}} \\ {{Z_e}} \end{array}} \right] = C_b^e \cdot \left[ {\begin{array}{*{20}{l}} {{X_b}} \\ {{Y_b}} \\ {{Z_b}} \end{array}} \right]$ (10)

 $C_e^1 = C_g^1 = \left[ {\begin{array}{*{20}{c}} {\cos \psi }&{ - \sin \psi }&0 \\ {\sin \psi }&{\cos \psi }&0 \\ 0&0&1 \end{array}} \right]$ (11)
 \begin{aligned} C_1^b & = C_2^b \cdot C_1^2\\ &=\! \left[ {\begin{array}{*{20}{c}} {\cos \phi }&0&{ - \sin \phi } \\ 0&1&0 \\ {\sin \phi }&0&{\cos \phi } \end{array}} \right] \cdot \left[ {\begin{array}{*{20}{c}} 1&0&1 \\ 0&{\cos \theta }&{\sin \theta } \\ 0&{ - \sin \theta }&{\cos \theta } \end{array}} \right] \\ &=\left[ {\begin{array}{*{20}{c}} {\cos \phi }&{\sin \theta \cdot \sin \phi }&{ - \cos \theta \cdot \sin \phi } \\ 0&{\cos \theta }&{\sin \theta } \\ {\sin \phi }&{ - \sin \theta \cos \phi }&{\cos \theta \cos \phi } \end{array}} \right] \end{aligned} (12)
 $c_e^b = c_1^b \cdot c_e^1 = \left[ {\begin{array}{*{20}{c}} {\cos \psi \cos \phi + \sin \theta \sin \phi \sin \psi }&{ - \cos \phi \sin \psi + \sin \phi \cos \psi \sin \theta }&{ - \cos \theta \sin \phi } \\ {\sin \psi \cos \theta }&{\cos \psi \cos \theta }&{\sin \theta } \\ {\sin \phi \cos \psi - \cos \phi \sin \psi \sin \theta }&{ - \sin \phi \sin \psi - \cos \phi \cos \psi \sin \theta }&{\cos \phi \cos \theta } \end{array}} \right]$ (13)

$C_e^b = \left[ {\begin{array}{*{20}{l}} {{T_{11}}}&{{T_{12}}}&{{T_{13}}} \\ {{T_{21}}}&{{T_{22}}}&{{T_{32}}} \\ {{T_{31}}}&{{T_{32}}}&{{T_{33}}} \end{array}} \right]$

 $C_b^e = \left( {C_e^b} \right)T = \left[ {\begin{array}{*{20}{c}} {{T_{11}}}&{{T_{21}}}&{{T_{31}}} \\ {{T_{12}}}&{{T_{22}}}&{{T_{32}}} \\ {{T_{13}}}&{{T_{23}}}&{{T_{33}}} \end{array}} \right]$ (14)

 \left\{ \begin{aligned} & \theta = \arcsin \left( {{T_{32}}} \right) \\ & \phi = \arctan \left( { - \frac{{{T_{31}}}}{{{T_{33}}}}} \right) \\ & \psi = \arctan \left( {\frac{{{T_{12}}}}{{{T_{22}}}}} \right) \end{aligned} \right. (15)

 \left\{ {\begin{aligned} & {\theta = \arcsin \left( {2 \cdot \left( {{q_2} \cdot {q_3} + {q_0} \cdot {q_1}} \right)} \right)} \\ & {\phi = \arctan \left( { - \frac{{2 \cdot \left( {{q_1} \cdot {q_3} + {q_0} \cdot {q_2}} \right)}}{{1 - 2 \cdot \left( {q_1^2 + q_2^2} \right)}}} \right)} \\ & {\psi = \arctan \cdot \left( {\frac{{2 \cdot \left( {{q_1} \cdot {q_2} - {q_0} \cdot {q_3}} \right)}}{{1 - 2 \cdot \left( {q_1^2 + q_3^2} \right)}}} \right)} \end{aligned}} \right. (16)
3.1.4 初始四元数

 \left\{ {\begin{aligned} & {q_0^2 + q_1^2 - q_2^2 + q_3^2 = {T_{11}}} \\ &{q_0^2 - q_1^2 + q_2^2 - q_3^2 = {T_{22}}} \\ & {q_0^2 - q_1^2 - q_2^2 + q_3^2 = {T_{33}}} \\ & {q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1} \end{aligned}} \right. (17)

 \left\{ {\begin{aligned} & {{q_1} = \pm \frac{1}{2}\sqrt {1 + {T_{11}} + {T_{22}} + {T_{33}}} } \\ & {{q_1} = \pm \frac{1}{2}\sqrt {1 + {T_{11}} - {T_{22}} - {T_{33}}} } \\ & {{q_2} = \pm \frac{1}{2}\sqrt {1 - {T_{11}} + {T_{22}} - {T_{33}}} } \\ & {{q_3} = \pm \frac{1}{2}\sqrt {1 - {T_{11}} - {T_{22}} + {T_{33}}} } \end{aligned}} \right. (18)

3.1.5 更新四元数

 $Q[t + \Delta t] = Q[t] + \Delta t\frac{{{\rm{d}}Q}}{{{\rm{d}}t}}$ (19)

 ${\left[ {\begin{array}{*{20}{l}} {{q_0}} \\ {{q_1}} \\ {{q_2}} \\ {{q_3}} \end{array}} \right]_{t + \Delta t}} = {\left[ {\begin{array}{*{20}{l}} {{q_0}} \\ {{q_1}} \\ {{q_2}} \\ {{q_3}} \end{array}} \right]_t} + \frac{{\Delta t}}{2}\left[ {\begin{array}{*{20}{r}} { - {w_x}{q_1} - {w_y}{q_2} - {w_z}{q_3}} \\ {{w_x}{q_0} + {q_2} - {w_y}{q_3}} \\ {{w_y}{q_0} - {w_z}{q_1} - {w_x}{q_3}} \\ {{w_z}{q_0} + {w_y}{q_1} - {w_x}{q_2}} \end{array}} \right]$ (20)

${g_x}$ ${g_y}$ ${g_z}$ 为陀螺仪传感器测量的角速度, 设T为姿态更新时间, 则由四元数微分方程的求解公式可得更新后的四元数:

 $\left\{ {\begin{array}{*{20}{l}} {{q_0} = {q_0} + ( - {q_1}*{g_x} - {q_2}*{g_y} - {q_3}*{g_z})*{T / 2}} \\ {{q_1} = {q_1} + ({q_0}*{g_x} + {q_2}*{g_z} - {q_3}*{g_y})*{T / 2}} \\ {{q_2} = {q_2} + ({q_0}*{g_y} - {q_1}*{g_z} + {q_3}*{g_x})*{T / 2}} \\ {{q_3} = {q_3} + ({q_0}*{g_z} + {q_1}*{g_y} - {q_2}*{g_x})*{T / 2}} \end{array}} \right.$ (21)
3.1.6 四元数转换欧拉角

 $Q(t + T) = \frac{{Q(t + T)}}{{\left\| {Q(t + T)} \right\|}}$ (22)

 $mode = \sqrt {q_0^2 + q_1^2 + q_2^2 + q_3^2}$ (23)
 $\left\{ {\begin{array}{*{20}{l}} {{q_0} = {{{q_0}} / {mode}}} \\ {{q_1} = {{{q_1}} / {mode}}} \\ {{q_2} = {{{q_2}} / {mode}}} \\ {{q_3} = {{{q_3}} / {mode}}} \end{array}} \right.$ (24)

 \left\{ \begin{aligned} & \psi = \arcsin (2 \cdot ({q_2} \cdot {q_3} + {q_0} \cdot {q_1})) \\ & \theta = \arctan \left( { - \frac{{2 \cdot ({q_1} \cdot {q_3} - {q_0} \cdot {q_2})}}{{1 - 2 \cdot (q_1^2 + q_2^2)}}} \right) \\ & \phi = \arctan \left( {\frac{{2 \cdot ({q_1} \cdot {q_2} - {q_0} \cdot {q_3})}}{{1 - 2 \cdot (q_1^2 + q_3^2)}}} \right) \end{aligned} \right. (25)
3.2 卡尔曼滤波算法

3.2.1 状态方程和观测方程

 ${x_k} = {F_{k - 1}}{x_{k - 1}} + {G_{k - 1}}{u_{k - 1}} + {w_{k - 1}}$ (26)

 ${z_k} = H{x_k} + {v_k}$ (27)

 $p(w)\sim N(0,Q)$ (28)
 $p(v)\sim N(0,R)$ (29)

3.2.2 卡尔曼滤波算法流程

 ${\hat x_{\bar k}} = {A_{{{\hat x}_{k - 1}}}} + B{u_{k - 1}}$ (30)
 ${P_{\bar k}} = A{P_{k - 1}}{A^{\rm{T}}} + Q$ (31)

 $K = {P_{\bar k}}{H^{\rm{T}}}{(H{P_{\bar k}}{H^{\rm{T}}} + R)^{ - 1}},$ (32)
 ${\hat x_k} = {\hat x_{\bar k}} + K\{ {z_k} - H{x_{\bar k}}\} ,$ (33)
 ${P_k} = (I - KH){P_{\bar k}}.$ (34)

3.3 基于卡尔曼滤波的数据融合算法

3.3.1 状态方程的建立

 $Q = \frac{1}{2}Q \cdot \omega$ (35)

 $\left[ {\begin{array}{*{20}{l}} {{q_0}} \\ {{q_1}} \\ {{q_2}} \\ {{q_3}} \end{array}} \right] = \frac{1}{2} \cdot \left[ {\begin{array}{*{20}{c}} 0&{ - \omega x}&{ - \omega y}&{ - \omega z} \\ {\omega x}&0&{\omega z}&{ - \omega y} \\ {\omega y}&{ - \omega z}&0&{\omega x} \\ {\omega z}&{\omega y}&{ - \omega x}&0 \end{array}} \right] \cdot \left[ {\begin{array}{*{20}{c}} {{q_0}} \\ {{q_1}} \\ {{q_2}} \\ {{q_3}} \end{array}} \right]$ (36)

 $\phi _G^k = \phi _T^k + \Delta \phi _G^k.$ (37)

3.3.2 观测方程的建立

 $\theta _{AM}^k = \theta _T^k + \Delta \theta _{AM}^k$ (38)

3.3.3 姿态角噪声估计计算方法

 ${\Delta _G} =\phi _G^{k + 1} - \phi_G^k = (\phi_T^{k + 1} - \phi _T^k) + (\Delta \phi_G^{k + 1} - \Delta \phi_G^k)$ (39)
 ${\Delta _{AM}} = \theta _{AM}^{k + 1} - \theta _{AM}^k = (\theta _T^{k + 1} - \theta _T^k) + (\Delta \theta _{AM}^{k + 1} - \Delta \theta _{AM}^k)$ (40)

 $f = {\Delta _{AM}} - {\Delta _G} = (\Delta \theta _{AM}^{k + 1} - \Delta \theta _{AM}^k) - (\Delta \phi _G^{k + 1} - \Delta \phi _G^k)$ (41)

 $\operatorname{cov} (f,f) = E[(f - Ef)(f - Ef)]$ (42)

 $E[\Delta \phi _G^{k + 1} \cdot \Delta \phi_G^k] = 0,E[\Delta \theta _{AM}^{k + 1} \cdot \Delta \theta _{AM}^k] = 0$ (43)

 $\operatorname{cov} (f,f) = 2E\left[ {{{(\Delta \theta _{AM}^k)}^2}} \right]$ (44)

 $R = 1/2\operatorname{cov} (f,f) = E\left[ {{{(\Delta \theta _{AM}^k)}^2}} \right]$ (45)

3.3.4 算法流程

(1)初始化参数, 记初始状态k=1, 在六轴MPU6050传感器处于静止状态下, 通过陀螺仪、加速度计和磁力计获取初始角度.

(2)当k=2时, 根据陀螺仪的角速度数据, 通过四元数法进行姿态解算预测当前角度, 根据前期陀螺仪数据实验求得先验误差协方差 ${P_{\bar k}}$ .

(3)按照式(32)求出卡尔曼增益K.

(4)根据k=2时刻的加速度计和磁力计求出的姿态角作为观测值, 按式(33), 式(34)实现预测值更新.

(5)计算k+1时刻的预测值. 实现方法: 通过采集k+1时刻的陀螺仪输出的角速度, 代入四元数微分方程求出此时刻的姿态角, 并作为预测值. 然后循环执行步骤(3)和步骤(4).

3.3.5 卡尔曼滤波数据融合算法的适用性

 图 5 算法流程图

3.4 基于PID控制算法的智能遛鱼控制方案

 图 6 鱼竿倾角示意图

3.4.1 串级PID控制模型的建立

 \left\{ {\begin{aligned} & {G1(s) = \frac{{38.5}}{{0.1{s^2} + 1}}} \\ & {G2(s) = \frac{{38.5}}{{0.1{s^2} + 1}}} \\ & {G3(s) = \frac{{48.6}}{{0.1{s^2} + 1}}} \end{aligned}} \right. (46)

 图 7 串级PID控制流程图

 图 8 单级和串级PID下俯仰角和翻滚角仿真结构图

3.4.3 PID参数整定

 图 9 单级和串级PID下偏航角仿真结构图

 图 10 俯仰角和翻滚角仿真结果对比图

 图 11 偏航角仿真结果对比图

 图 12 俯仰角PID最终仿真结果图

4 智能钓鱼竿系统测试结果

4.1 姿态角测试结果

 图 13 智能钓鱼竿整体实物图

 图 14 静止时解算的姿态角波形图

 图 15 角度突变时解算的姿态角波形图

4.2 智能鱼竿报警功能测试

5 结论与展望

 [1] 张荣辉, 贾宏光, 陈涛, 等. 基于四元数法的捷联式惯性导航系统的姿态解算. 光学 精密工程, 2008, 16(10): 1963-1970. [2] 姚佳乐, 沈宏君, 张虹波. 变桨距四旋翼飞行器控制系统设计. 微型机与应用, 2017, 36(6): 77-79, 83. [3] 刘志军, 吕强, 王东来. 小型四旋翼直升机的建模与仿真控制. 计算机仿真, 2010, 27(7): 18-20, 69. DOI:10.3969/j.issn.1006-9348.2010.07.005 [4] 许震, 毛丽民, 刘同连, 等. 四轴飞行器控制系统设计. 常熟理工学院学报, 2013, 27(2): 109-113. DOI:10.3969/j.issn.1008-2794.2013.02.024 [5] 吴明洋. 主动雷达目标状态估计算法研究[硕士学位论文]. 上海: 上海交通大学, 2012. [6] 邹波, 张华, 姜军. 多传感信息融合的改进扩展卡尔曼滤波定姿. 计算机应用研究, 2014, 31(4): 1035-1038, 1042. DOI:10.3969/j.issn.1001-3695.2014.04.019 [7] 陈伟. 基于四元数和卡尔曼滤波的姿态角估计算法研究与应用[硕士学位论文]. 秦皇岛: 燕山大学, 2015.