移动机器人姿态角解算方法技术

本发明专利技术的移动机器人姿态角解算方法，包括：步骤1、传感器原始数据采集；步骤2、传感器原始数据预处理；步骤3、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的过程模型；步骤4、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的观测模型；步骤5、移动机器人姿态角求解。本发明专利技术提供了一种基于四元数融合扩展卡尔曼滤波器和显性互补滤波器的移动机器人姿态角解算方法，解决了现有的移动机器人姿态角解算精度不高的问题，该方法能够得到更高精度的姿态角，从而可以更好地控制移动机器人的姿态。

A Method for Solving Attitude Angle of Mobile Robot

【技术实现步骤摘要】
移动机器人姿态角解算方法
本专利技术涉及包括无人机在内的移动机器人，具体涉及基于四元数融合扩展卡尔曼滤波器和显性互补滤波器的移动机器人姿态角的解算方法。
技术介绍
目前，机器人姿态估计主要利用姿态矩阵进行计算，姿态矩阵可以采用不同的数学方式定义。而姿态矩阵更新方法主要有方向余弦法、欧拉角法以及四元数法。移动机器人在翻越障碍时会出现俯仰角接近90°的情况，采用四元数法进行移动机器人载体姿态更新，可以避免移动机器人在翻越障碍过程中出现的“奇点”问题，并且相对欧拉角法计算量更小，算法更加简单。现有的移动机器人姿态计算滤波器有显性互补滤波器和扩展卡尔曼滤波器。显性互补滤波器计算方法简单，运算量小，适用于实时性要求高的移动机器人进行姿态解算。由于显性互补滤波器参数固定,因此并不适用于运动状态变化较大情况下的姿态求解。在运用扩展卡尔曼滤波器求解移动机器人四元数姿态的过程中，采用加速度计和磁力计数据作为系统观测量，而加速度计输出为移动机器人载体的比力，即移动机器人载体相对惯性空间的绝对加速度和引力加速度之和，这会造成移动机器人载体在高加速度运动过程中通过加速度计解算的姿态存在很大的误差。
技术实现思路
为克服现有技术中所存在的上述不足，本专利技术提供了一种基于四元数融合扩展卡尔曼滤波器和显性互补滤波器的移动机器人姿态角解算方法，以解决现有的移动机器人姿态角解算精度不高的问题，该方法能够得到更高精度的姿态角，从而可以更好地控制移动机器人的姿态。本专利技术的技术方案：移动机器人姿态角解算方法，所述移动机器人由直流电机驱动；所述移动机器人上设有主控芯片和惯性测量单元，所述惯性测量单元具有九轴传感器；所述姿态角解算方法包括：步骤1、传感器原始数据采集：主控芯片通过I2C总线采集惯性测量单元的九轴传感器信息，即三轴陀螺仪、三轴加速度计和三轴磁力计的数据；步骤2、传感器原始数据预处理：采用均值滤波去除数据信息中掺杂的高频噪声；步骤3、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的过程模型：取四元数与陀螺仪零漂误差7个参数作为系统状态变量，建立非线性系统状态方程；步骤4、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的观测模型：利用显性互补滤波器解算的四元数作为扩展卡尔曼滤波器的观测量，再利用融合滤波器估算出最优四元数；步骤5、移动机器人姿态角求解：将步骤4中得到的最优四元数代入四元数和欧拉角的转换关系式，计算出移动机器人的三个姿态角：横滚角、俯仰角和偏航角。作为优化，前述的移动机器人姿态角解算方法中，所述步骤2具体包括如下子步骤：2.1、对陀螺仪、加速度计和磁力计原始数据进行加窗处理，取滑动窗口长度n＝10，步长l＝1；2.2、对窗口中的数据进行均值处理，输出结果作为当前时刻的数值，公式如下所示：2.3、子步骤2.2结束后I(n-9)出窗，I(n+1)进窗；2.4、子步骤2.2与子步骤2.3循环，直到最后一个数据进窗，并输出最终结果。作为优化，前述的移动机器人姿态角解算方法中，所述步骤3中：系统状态向量为：x(k)＝(q0(k)q1(k)q2(k)q3(k)bgx(k)bgy(k)bgz(k))；上式中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻四元数姿态，bgx(k)、bgy(k)、bgz(k)分别为陀螺仪三轴的零漂误差；由系统状态向量建立系统状态方程为：对f(x(k-1),k-1)求Jacobi矩阵可得状态转移矩阵为：其中：Г(k)为噪声驱动矩阵，W(k-1)为零均值白噪声序列；分别表示陀螺仪三轴测量值；表示陀螺仪估计值，由陀螺仪测量值和零漂误差值相减得到；I3×3为三维单位矩阵；作为优化，前述的移动机器人姿态角解算方法中，所述步骤4按照如下子步骤实现：4.1、数据归一化：当移动机器人处于非加速状态时，加速度计在世界坐标系下的重力向量为归一化后重力向量为移动机器人载体坐标系下加速度计测得的重力分量记为4.2、根据测得的和计算得到的载体坐标系下的重力向量，计算出误差补偿值加速度计在参考坐标系下归一化后的重力向量为通过姿态矩阵可以将gn转换为载体坐标系下的重力向量：对载体坐标系下的重力矢量和载体坐标系下的重力矢量估计做向量叉乘，得到对陀螺仪角速度数据的校正补偿值：4.3、根据磁力计数据和计算得到的移动机器人载体坐标系下的磁场强度，计算出误差补偿值磁力计测量地球磁场的大小和方向，载体坐标系内磁力计输出为mb＝(mximyimzi)T，通过姿态矩阵转换得到世界坐标系下的投影为hn＝(hxihyihzi)T，当磁地理坐标系和移动机器人载体坐标系重合时，磁力计输出为bb＝(0byibzi)T，在XOY平面中，bb的投影为byi，hn的投影为(hxi+hyi)；得到磁场强度的估计值是：由上式的磁场强度的估计值得到导航坐标系下的磁场数值，然后再利用姿态矩阵转换推算出移动机器人载体坐标系下的磁场投影为wb＝(wxiwyiwzi)T；根据测得的和推算得到的移动机器人载体坐标系下的磁场强度，将两者做向量叉乘，得修正补偿值：4.4、误差补偿：误差补偿值4.5、修正陀螺仪角速度误差。利用误差补偿值修正陀螺仪角速度数值：4.6、利用修正的陀螺仪角速度数值更新四元数，采用一阶龙格库塔法求解微分方程，并更新四元数；t时刻的四元数微分方程为：得到(t+T)时刻更新后的四元数：系统观测量设为：z(k)＝(q0(k)q1(k)q2(k)q3(k))；其中：q0(k)、q1(k)、q2(k)、q3(k)为显性互补滤波算法解算后得到的移动机器人载体四元数姿态；系统观测方程为：根据上式对h(x(k),k)求雅克比矩阵可得系统量测矩阵为：将(k-1)时刻的四元数姿态代入系统状态方程更新得到k时刻移动机器人载体四元数姿态状态量；然后将k时刻的四元数姿态状态量和通过显性互补滤波器求解得到的系统观测量代入状态向量估计方程，最终得到k时刻移动机器人载体最优四元数姿态估计：作为优化，前述的移动机器人姿态角解算方法中，所述步骤5中，姿态角计算式为：作为优化，前述的移动机器人姿态角解算方法中，所述主控芯片为STM32F103。作为优化，前述的移动机器人姿态角解算方法中，所述惯量单元为MPU9250。作为优化，前述的移动机器人姿态角解算方法中，所述I2C总线的数据传输速率为400kbit/s，所述九轴传感器的数据采样频率为500HZ。作为优化，前述的移动机器人姿态角解算方法中，所述主控单元每2ms进行周期性的数据采集和预处理1次。与现有技术相比，本专利技术的移动机器人姿态角解算方法取得了如下显著的进步：通过基于四元数融合扩展卡尔曼滤波器和显性互补滤波器的算法解算移动机器人的姿态角，解决了现有的移动机器人姿态角解算精度不高的问题，能够得到更高精度的姿态角，从而可以更好地控制移动机器人的姿态。实验结果表明：经融合滤波器得到的姿态角波动更小，平滑性更好，并且对偏航角的误差控制更强，姿态角跟踪效果更好；当移动机器人快速运动时，利用融合滤波器响应速度更高，并且姿态角波动范围较小。通过对比实验结果发现，经过融合扩展卡尔曼滤波器和显性互补滤波器得到的静态定姿精度相较于扩展卡尔曼滤波器，横滚角、俯仰角和偏航角分别可以提高71％、74％和66％；动态定姿精度相较于扩展卡本文档来自技高网...

【技术保护点】
1.移动机器人姿态角解算方法，所述移动机器人由直流电机驱动；其特征在于：所述移动机器人上设有主控芯片和惯性测量单元，所述惯性测量单元具有九轴传感器；所述姿态角解算方法包括：步骤1、传感器原始数据采集：主控芯片通过I2C总线采集惯性测量单元的九轴传感器信息，即三轴陀螺仪、三轴加速度计和三轴磁力计的数据；步骤2、传感器原始数据预处理：采用均值滤波去除数据信息中掺杂的高频噪声；步骤3、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的过程模型：取四元数与陀螺仪零漂误差7个参数作为系统状态变量，建立非线性系统状态方程；步骤4、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的观测模型：利用显性互补滤波器解算的四元数作为扩展卡尔曼滤波器的观测量，再利用融合滤波器估算出最优四元数；步骤5、移动机器人姿态角求解：将步骤4中得到的最优四元数代入四元数和欧拉角的转换关系式，计算出移动机器人的三个姿态角：横滚角、俯仰角和偏航角。

【技术特征摘要】
1.移动机器人姿态角解算方法，所述移动机器人由直流电机驱动；其特征在于：所述移动机器人上设有主控芯片和惯性测量单元，所述惯性测量单元具有九轴传感器；所述姿态角解算方法包括：步骤1、传感器原始数据采集：主控芯片通过I2C总线采集惯性测量单元的九轴传感器信息，即三轴陀螺仪、三轴加速度计和三轴磁力计的数据；步骤2、传感器原始数据预处理：采用均值滤波去除数据信息中掺杂的高频噪声；步骤3、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的过程模型：取四元数与陀螺仪零漂误差7个参数作为系统状态变量，建立非线性系统状态方程；步骤4、建立基于四元数融合扩展卡尔曼滤波器和显性互补滤波器融合算法的观测模型：利用显性互补滤波器解算的四元数作为扩展卡尔曼滤波器的观测量，再利用融合滤波器估算出最优四元数；步骤5、移动机器人姿态角求解：将步骤4中得到的最优四元数代入四元数和欧拉角的转换关系式，计算出移动机器人的三个姿态角：横滚角、俯仰角和偏航角。2.根据权利要求1所述的移动机器人姿态角解算方法，其特征在于，所述步骤2具体包括如下子步骤：2.1、对陀螺仪、加速度计和磁力计原始数据进行加窗处理，取滑动窗口长度n＝10，步长l＝1；2.2、对窗口中的数据进行均值处理，输出结果作为当前时刻的数值，公式如下所示：2.3、子步骤2.2结束后I(n-9)出窗，I(n+1)进窗；2.4、子步骤2.2与子步骤2.3循环，直到最后一个数据进窗，并输出最终结果。3.根据权利要求2所述的移动机器人姿态角解算方法，其特征在于，所述步骤3中：系统状态向量为：x(k)＝(q0(k)q1(k)q2(k)q3(k)bgx(k)bgy(k)bgz(k))；上式中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻四元数姿态，bgx(k)、bgy(k)、bgz(k)分别为陀螺仪三轴的零漂误差；由系统状态向量建立系统状态方程为：对f(x(k-1),k-1)求Jacobi矩阵可得状态转移矩阵为：其中：Г(k)为噪声驱动矩阵，W(k-1)为零均值白噪声序列；分别表示陀螺仪三轴测量值；表示陀螺仪估计值，由陀螺仪测量值和零漂误差值相减得到；I3×3为三维单位矩阵。4.根据权利要求3所述的移动机器人姿态角解算方法，其特征在于，所述步骤4按照如下子步骤实现：4.1、数据归一化：当移动机器人处于非加速状态时，加速度计在世界坐标系下的重力向量为归一化后重力向量为移动机器人载体坐标系下加速度计测得的重力分量记为4.2、根据测得的和计算得到的载体坐标系下的重力向量，计算出误差补偿值...

【专利技术属性】
技术研发人员：郑凯，郑力达，魏丽娜，陈英米，
申请(专利权)人：台州知通科技有限公司，
类型：发明
国别省市：浙江,33