买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

摘要：本发明提供一种基于互补滤波实现加速度校正的坡度估算方法，包括信号采集及预处理、初始坡度角计算、引入四元数、四元数初始值更新、加速度归一化、互补滤波加速度校正等步骤，最终计算获得汽车行驶的坡度，本发明提出了一种受环境因素影响较小的能够实现坡度自动调节的方法，借助纵向加速度的改变，通过一系列的算法缩小误差并估算坡度值，不需要给出经验值计算，提高精确度，使坡度估计结果相较传统坡度计得出结果更加可靠；适用于直线行驶过程中，平直道路上的坡度估计。

主权项：1.一种基于互补滤波实现加速度校正的坡度估算方法，其特征在于：包括以下步骤：一信号采集及预处理：采集陀螺仪和三轴加速度计的信号；对陀螺仪和三轴加速度计的数据值进行预处理：1消除收集数据器材本身存在的误差：多次测量刚通电后陀螺仪所测量的角加速度，取平均值作为零点漂移估计值ω0： 2消除收集数据时存在的噪声误差：运用一阶低通滤波器消除高频噪声的干扰，ak为k次滤波后的加速度信号，ak-1为上一次滤波后的加速度信号，araw为原始采集的加速度信号，α为滤波系数：ak＝αaraw+1-ak-1α2分别对汽车纵向、横向、垂向上的初始采集加速度araw进行滤波处理，获得滤波后的加速度计初始的三轴加速度，分别用ax、ay、az表示加速度计获得的纵向、横向、垂向加速度信号值，多次测量求出当地重力加速度g： 经一阶低通滤波后当地重力加速度g在纵向、横向、垂向加速度的分量分别记为gx，gy，gz；二初始坡度角计算：通过两种方法测量汽车所在初始坡度角：一方面由陀螺仪采集的角加速度减去零漂，再短时积分得到汽车所处初始坡度θ： 另一方面先测量整体车辆的纵向加速度，只留下重力加速度在纵向的分量，由加速度计可知车辆的纵向加速度ax，θ代表车辆起始状态所处坡度： 由汽车整体沿着坡面行驶的速度的纵向分量取一次微分得到；将从gx中去除，得到gsinθ的真实数值g′x，将经过处理后所得数值除以当地重力加速度，接着取其反正弦函数值，所得数值即为加速度计所测的路面和水平面的坡度角θ： 三引入四元数：将陀螺仪短时积分采集到的初始角度转换为四元数，四元数由旋转轴和绕该轴旋转的角度组成，其中α代表绕该轴旋转的角度，cosβx、cosβy、cosβz代表x、y、z方向上旋转轴的分量，用四元数q0、q1、q2、q3描述汽车姿态，引入以下概念：q0＝cosα2q1＝sinα2cosβxq2＝sinα2cosβyq3＝sinα2cosβz6引入四元数作为描述车身姿态的参数，欧拉角转换为四元数公式如下：q＝[q0q1q2q3]T|q|2＝q02+q12+q22+q32＝1 其中，代表翻滚角，θ代表俯仰角即所测量的坡度角，ψ代表航向角，四元数转换为欧拉角公式如下： 四元数与欧拉角之间的变化关系为： θ＝arcsin2q0q2-q1q3 CR为旋转矩阵，四元数与旋转矩阵换公式： 四四元数初始值更新：设初始四元数q0＝1，q1＝0，q2＝0，q3＝0，Wx、Wy、Wz分别为陀螺仪采集的三轴角速率，陀螺仪采集的角速度记为gx、gy、gz； 五加速度归一化：经滤波后的加速度计值归一化，作为机体坐标系下重力向量的测量值a′x,a′y,a′z为归一化后的纵向、横向、垂向加速度，norm为加速度模长合倒数： a′x＝ax*norma′y＝ay*norma′z＝az*norm11六互补滤波加速度校正：将更新后的四元数值q0q1q2q3代入公式10中，得到机体坐标系下的旋转矩阵CR，地理坐标系的中立向量取为[0,0,1]T，将其转换到以汽车为中心的机体坐标系中，用[vx,vy,vz]T表示坐标系旋转变换后加速度矩阵： 两坐标系之间误差向量即为由a′x、ay′、az′所构成的重力向量与机体坐标系测量的向量外积： 将两坐标系误差利用PID控制器来消除，给定Kp、Ki和Kd数值，实现补偿陀螺仪与加速度计的误差，对[gx,gy,gz]T的值更新： 对四元数的更新值进行微分方程的一阶龙格库塔算法，更新四元数的数值： 由四元数计算欧拉角时，先规范化处理更新的四元数，用q″0，q″1，q″2，q″3表示规范化后的四元数值： q″0＝q′0*normq″1＝q′1*normq″2＝q′1*normq″3＝q′1*norm16最终计算获得汽车行驶的坡度，即汽车的俯仰角pitch＝arcsin2q″0q″2-q″1q″3。

全文数据：

权利要求：

百度查询： 吉林大学 一种基于互补滤波实现加速度校正的坡度估算方法