[发明专利]一种基于传感器mpu6050采集信号的手柄摇杆系统和方法

权利要求书

1.一种基于传感器mpu6050采集信号获得摇杆位置的方法，其特征在于，步骤如下：

S1、在摇杆装置上建立右手笛卡尔坐标系为绝对坐标系，用芯片B的几何中心为原点，x、y和z轴表示固定参考坐标系F_(x，y，z)，用芯片A的几何中心为原点，n、o和a轴表示运动坐标系F_(n，o，a)；

S2、手指拨动摇杆，芯片B进行数据采集，获得绕x轴方向上旋转α角度，绕y轴方向上旋转β角度；芯片A进行数据采集，获得绕n轴方向上旋转γ角度，绕o轴方向上旋转δ角度；

S3、微处理器K60通过IIC协议读取数据，并采用扩展卡尔曼滤波将传感器收集的角度数据进行融合并对误差进行补偿,得到手柄摇杆的最优估计值；

S4、在固定参考坐标系中使用位置向量P建立数学模型为：

式中，n、o和a表示方向向量；p表示位置向量；x、y和z为坐标轴；

S5、使用手柄摇杆补偿后得到的最优估计值建立向量p_xyz相对坐标方程式为：

p_xyz＝Rot(y,β)Rot(x,α)Rot(n,(γ-α))Rot(o,(δ-β))p_noa

p_xyz为手柄摇杆几何中心变换后的坐标点；

S6、建立向量p_xyz的矩阵方程：

S7、进一步处理向量得：

S8、获得在平面xy内的坐标：

横坐标为：

p_x＝180(sin(δ-β)cosβ-sin(γ-α)cos(δ-β)sinβsinα+cos(γ-α)sin(δ-β)sinβcosα)

纵坐标为：

p_y＝-180(sin(γ-α)cos(δ-β)cosα+cos(γ-α)sin(δ-β)sinα)。

2.如权利要求1所述的基于传感器mpu6050采集信号获得摇杆位置的方法，其特征在于，S2中，通过扩展卡尔曼滤波建立系统状态方程和观测方程，如下：

X_K+1＝f(X_K)+w_K+1

Y_K＝h(X_K)+v_K

式中，X_K+1为系统在k+1时刻的状态变量，Y_K为系统在k时刻的测量变量，w、v为零均值高斯白噪声。

通过非线性函数泰勒级数展开线性化方法，可以近似的表示初始系统的状态方程和观测方程，如下：

式中，F_X(K)和F_w分别是对X、v的偏导数；H_X(K)和H_v分别是对X、w的偏导数。

预测前的状态估计^[：

预测前的方差估计：

式中，Q(K)为K时刻的过程激励噪声协方差矩阵。系统通过实时调整修正后，

得出状态更新方程。其增益矩阵、状态估计和方差估计分别为：