1.一种基于互补滤波实现加速度校正的坡度估算方法,其特征在于:包括以下步骤:(一)信号采集及预处理:采集陀螺仪和三轴加速度计的信号;
对陀螺仪和三轴加速度计的数据值进行预处理:(1)消除收集数据器材本身存在的误差:多次测量刚通电后陀螺仪所测量的角加速度,取平均值作为零点漂移估计值ω(2)消除收集数据时存在的噪声误差:运用一阶低通滤波器消除高频噪声的干扰,aa
分别对汽车纵向、横向、垂向上的初始采集加速度a经一阶低通滤波后当地重力加速度g在纵向、横向、垂向加速度的分量分别记为g(二)初始坡度角计算:通过两种方法测量汽车所在初始坡度角:一方面由陀螺仪采集的角加速度减去零漂,再短时积分得到汽车所处初始坡度θ:另一方面先测量整体车辆的纵向加速度,只留下重力加速度在纵向的分量,由加速度计可知车辆的纵向加速度a将
(三)引入四元数:将陀螺仪短时积分采集到的初始角度转换为四元数,四元数由旋转轴和绕该轴旋转的角度组成,其中α代表绕该轴旋转的角度,cos(βq
q
q
q
引入四元数作为描述车身姿态的参数,欧拉角转换为四元数公式如下:q=[q
|q|
其中,
四元数与欧拉角之间的变化关系为:θ=arcsin(2(qC
(四)四元数初始值更新:设初始四元数q
(五)加速度归一化:经滤波后的加速度计值归一化,作为机体坐标系下重力向量的测量值a′a′
a′
a′
(六)互补滤波加速度校正:将更新后的四元数值q两坐标系之间误差向量将两坐标系误差利用PID控制器来消除,给定Kp、Ki和Kd数值,实现补偿陀螺仪与加速度计的误差,对[gx,gy,gz]对四元数的更新值进行微分方程的一阶龙格库塔算法,更新四元数的数值:由四元数计算欧拉角时,先规范化处理更新的四元数,用q″q″
q″
q″
q″
(16)
最终计算获得汽车行驶的坡度,即汽车的俯仰角pitch=arcsin(2(q″