[发明专利]一种改进误差四元数卡尔曼滤波机器人姿态解算方法

权利要求书

1.一种改进误差四元数卡尔曼滤波机器人姿态解算方法，其特征包括如下步骤：

步骤1：选取移动机器人相应的参考坐标系，确定姿态变换矩阵，用非线性指数模型，结合Levenberg-Marquardt迭代最小平方拟合方法建立陀螺仪漂移数学模型，利用陀螺仪输出数据和误差四元数间的关系建立误差四元数微分方程；

步骤2：根据误差四元数微分方程和陀螺仪漂移数学模型建立误差四元数卡尔曼滤波状态方程；

步骤3：根据加速度计输出数据与误差四元数之间的关系建立误差四元数卡尔曼滤波观测方程；

步骤4：根据加速度计输出数据判断移动机器人是否存在非重力加速度，并自适应更新移动机器人非重力加速度的协方差矩阵；

步骤5：利用自适更新的非重力加速度协方差矩阵对量测噪声矩阵进行补偿，并利用误差四元数卡尔曼滤波器获得误差四元数更新值，计算得到真实的姿态四元数，根据四元数法进行姿态解算得到姿态角。

2.根据权利要求1所述的一种改进误差四元数卡尔曼滤波机器人姿态解算方法，其特征在于步骤4所述，根据加速度计输出数据判断移动机器人是否存在非重力加速度，并自适应更新移动机器人非重力加速度的协方差矩阵，其步骤如下：

利用如下公式考虑移动机器人是否存在非重力加速度：

其中，f_x、f_y和f_z表示加速度计三轴输出量，δ_g为常数，与加速度计量测噪声有关；

移动机器人非重力加速度的协方差矩阵为R_acc，R_acc自适应更新如下：

如果上述公式满足，表明移动机器人在静止或稳定状态下：

R_acc＝0

如果公式不满足，则表明移动存在非重力加速度：

R_acc≈max(α||r_a||，0)

其中，α为常数，r_a为残差信息，||r_a||表示r_a的模；

r_a＝z-Hx

其中，z表示系统量测量，H表示系统量测矩阵，x表示系统状态量。

3.根据权利要求1所述的一种改进的误差四元数卡尔曼滤波机器人姿态解算方法，其特征在于步骤5所述，利用自适应更新的非重力加速度协方差矩阵对量测噪声矩阵进行补偿，其步骤如下：

利用更新后的移动机器人非重力加速度的协方差矩阵为R_acc，对量测噪声矩阵R进行补偿；

R＝E(vv^T)＝E(a_ba_b^T+n_an_a^T)＝R_a+R_acc

其中，v表示系统量测噪声，a_b表示非重力加速度，n_a表示加速度计零均值白噪声，R_a为加速度计测量噪声协方差矩阵；利用补偿后的量测噪声矩阵R和建立的误差四元数卡尔曼滤波器算法，得到误差四元数更新值，计算得到真实的姿态四元数，根据四元数法进行姿态解算得到姿态角，减少了非重力加速度对姿态解算的影响。