一种基于IMU和高度传感器的自适应车辆姿态及坡度估计方法与流程

本发明涉及汽车主动悬架控制，尤其涉及一种基于imu和高度传感器的自适应车辆姿态及坡度估计方法。

背景技术：

1、在汽车主动悬架控制领域，准确获取车辆姿态与路面坡度信息是实现车身稳定性控制和悬挂系统自适应调节的关键前提，而随着主动悬架系统从被动响应向主动预判方向发展，车辆在不同行驶工况下对姿态估计的精度和实时性提出了更高要求，实际应用中，车辆运行环境复杂多变，包括静态停放、匀速行驶、急加减速、转向以及颠簸路面等多种工况，为了适应这些工况变化，姿态与坡度估计算法需具备静态精度高、动态响应快的特点，并能够在不依赖外部参考信号的条件下，实时输出车身俯仰角、侧倾角及路面纵坡、横坡等信息，从而为主动悬架提供可靠的状态输入。

2、如中国专利申请号：cn202411293389.3的车身姿态估计及其主动控制方法，利用imu和高度传感器设计了一种自适应融合滤波算法来估算车身姿态信息：对imu采集到的俯仰角速度、侧倾角速度和垂向加速度进行积分得到对应的俯仰角、侧倾角和垂向位移；基于7自由度车身动力学模型和高度传感器信号计算车身俯仰角、侧倾角和垂向位移，并基于预设的融合算法自适应律将上述两个姿态信息进行融合，得到车身姿态角。

3、现有技术中，虽然融合了imu积分动态性能较好和车身动力学模型静态角度修正的特性，但在车辆急加减速、急转向或颠簸路面时，加速度计测得的非重力分量会污染绝对姿态角计算结果，而且高度传感器仅能测量车身与车轮的相对位移，难以获得车辆相对于水平地面的绝对姿态角，且缺乏对水平地面绝对姿态角的估算。因此，如何设计一种在静态和动态工况下均能保持较高估算精度，并能同时估算车身相对姿态、绝对姿态及路面坡度的自适应估计算法，是本发明要解决的技术问题。

技术实现思路

1、为了克服背景技术中的缺点，本发明提供了一种基于imu和高度传感器的自适应车辆姿态及坡度估计方法，能够有效解决上述背景技术中涉及的问题。

2、本发明的目的可以通过以下技术方案来实现：本发明提供了一种基于imu和高度传感器的自适应车辆姿态及坡度估计方法，包括以下步骤：

3、步骤1，获取车辆的imu三轴加速度、角速度及四轮高度传感器信号，经低通滤波去除高频路面激励噪声，保留真实车身动态信息，保证输入信号纯净，抑制路面噪声对姿态解算的污染；

4、步骤2，利用加速度计测量重力加速度分量，计算车身绝对俯仰角和侧倾角，反映包含坡度的路面与车身叠加的总体姿态；

5、步骤3，基于四轮高度信号计算前后轴与左右侧相对高度差，推算车身相对俯仰角和侧倾角，确定相对姿态角观测值，有效隔离路面坡度干扰，反映悬架变形引起的姿态变化，独立提取悬架变形姿态；

6、步骤4，构建包含姿态角、角速度及纵坡、横坡的状态向量，并将绝对姿态角、相对姿态角与角速度计测量值重组作为观测向量，引入卡尔曼滤波器融合状态估计值与观测值；

7、步骤5，基于imu测量的三轴加速度总向量实时判断车辆行驶工况，动态切换卡尔曼滤波器的观测噪声协方差矩阵，自适应调整加速度计、高度传感器与角速度计的观测信息权重，确保在平稳与剧烈工况下的姿态与坡度估计均具高精度与强鲁棒性；

8、步骤6，通过卡尔曼更新方程融合绝对与相对观测值，修正预测状态，输出车身俯仰角、侧倾角及路面纵坡、横坡信息，实现自适应高精度估计，兼具动态响应快与静态精度高的优势，供主动悬架控制使用。

9、优选的，所述步骤1具体包括：

10、同步采集车身质心附近六轴imu输出的三轴加速度与三轴角速度原始信号，以及四轮独立高度传感器输出的车身与车轮间相对位移原始信号，保证多源信号在时间基准上的物理一致性；

11、对加速度与高度信号分别进行低通滤波，定向滤除由路面激励产生的高频振动噪声分量，有效抑制路面高频噪声对姿态解算的干扰；

12、保留经滤波后的低频动态信号分量，作为后续姿态与坡度估计的输入信息源，抑制路面噪声对真实车身姿态信息的污染，确保后续姿态解算输入信号的纯净性与真实性。

13、优选的，所述步骤2具体包括：

14、基于重力加速度在三轴上的正交投影关系，利用imu三轴加速度计信号计算车身相对于绝对水平基准面的纵向俯仰角与横向侧倾角；

15、将解算所得绝对姿态角直接作为叠加了路面坡度与车身自身姿态的综合观测信息，用于后续滤波器的静态基准校正，为滤波器提供高精度静态基准，抑制长期漂移。

16、其中，对6轴imu而言，当车辆平稳行驶或静止时，x、y、z三轴加速度计可测得重力加速度在各轴的分量，进而得到相对于水平地面的车身绝对姿态角信息：

17、；

18、；

19、式中，为绝对俯仰角（点头为正），为绝对侧倾角（右倾为正），为重力加速度，绝对角度包含路面坡度和车身相对姿态角信息，

、

、分别为imu测得的x、y、z三轴加速度，该公式仅在车辆平稳行驶或静止时成立，当车辆处于急加减速、急转向工况或者颠簸路面上时，imu测得的

、

、不仅仅为重力加速度分量，计算出的姿态角信息会被污染。

20、优选的，所述步骤3具体包括：

21、采集四个车轮处的高度传感器信号，分别计算前轴左右轮高度均值与后轴左右轮高度均值之间的纵向差值，并基于轴距计算车身由于悬架变形造成的相对俯仰角，实现与路面坡度的精准分离；

22、同步计算左侧前后轮高度均值与右侧前后轮高度均值之间的横向差值，结合轮距计算由于悬架变形造成的相对侧倾角。

23、其中，对高度传感器信号而言，四个轴的相对高度变化可以反映车身相对姿态角的变化：

24、；

25、；

26、则有：

27、；

28、；

29、式中，为前后轴距，为左右轮距，

、

、

、分别为前左、前右、后左、后右的车身高度信号，和分别为相对俯仰角（点头为正）和相对侧倾角（右倾为正）。

30、优选的，所述步骤4具体包括：

31、构建融合俯仰角、侧倾角、俯仰角速度、侧倾角速度、路面纵坡角与路面横坡角的六维耦合状态向量，建立姿态与坡度之间的物理耦合关联，实现姿态与坡度的联合估计，提升状态解耦能力；

32、将绝对姿态角观测值、相对姿态角观测值与角速度计直接测量的角速度值作为卡尔曼滤波器中观测值，将角速度的积分值作为状态估计值，融合了动态响应估计与静态观测信息信息，兼顾静态精度与动态响应，实现优势互补的融合估计。

33、优选的，所述步骤5具体包括：

34、实时计算imu三轴加速度计测量的加速度总向量与重力加速度偏差的动态均方根值，作为判断车辆当前行驶工况的动态特征指标，有效区分车辆动态激励程度，为自适应调整提供可靠依据；

35、当偏差的动态均方根值越大，则判定车辆处于急加减速、急转向或颠簸路面等动态工况，此时按照预设值线性增大与加速度计和高度传感器对应的观测噪声值，降低不可靠观测的融合权重，提升剧烈工况下的估计鲁棒性；

36、当偏差的动态均方根值越大小，则判定车辆处于平稳行驶工况，逐步减小与加速度计和高度传感器对应的观测噪声值，提升静态观测信息的融合权重，使滤波器充分利用加速度计和高度传感器的观测值对状态估计值进行静态修正。

37、其中，动态均方根值采用指数加权移动平均rms，计算方法为：

38、

39、

40、式中，为当前加速度的动态均方根值，为加权系数，为当前时刻imu输出的三轴加速度度总向量与重力加速度偏差值。

41、优选的，所述步骤6具体包括：

42、执行卡尔曼滤波器的预测步骤，基于角速度计测量的角速度值结合上一时刻的姿态角状态，推算当前时刻的姿态角先验估计值，保证动态响应阶段的快速跟踪能力，确保剧烈工况下姿态跟踪的实时性与准确性；

43、执行卡尔曼滤波器的更新步骤，引入绝对姿态角观测值、相对姿态角观测值与角速度观测值，通过卡尔曼增益矩阵动态融合对先验估计值进行修正，实现静动态工况下精度与响应的自适应平衡；

44、从修正后的状态向量中分别提取车身俯仰角、侧倾角、路面纵坡角与横坡角四个独立估计值，输出至主动悬架控制系统，用于悬架作动器的姿态协调控制，提升主动悬架的姿态控制效果与整车稳定性。

45、与现有技术相比，本发明的有益效果是：

46、1、该一种基于imu和高度传感器的自适应车辆姿态及坡度估计方法，通过构建包含车身姿态角、角速度及路面纵坡、横坡的六维耦合状态向量，利用imu的加速度计、角速度计和高度传感器实现车身姿态与路面坡度的联合观测，能够同时输出车身俯仰角、侧倾角及路面纵坡、横坡四个独立估计值，为主动悬架控制提供完整的车体动态与道路几何信息，克服了现有技术仅能估计单一姿态量的局限。

47、2、该一种基于imu和高度传感器的自适应车辆姿态及坡度估计方法，通过将绝对姿态角观测与相对姿态角观测分别作为独立信息来源引入卡尔曼滤波器，在静态工况下利用观测值对状态变量进行基准校正，同时在动态工况下依赖角速度计积分保持快速跟踪能力，兼顾了静态精度与动态响应速度。

48、3、该一种基于imu和高度传感器的自适应车辆姿态及坡度估计方法，通过实时监测imu三轴加速度总向量与重力加速度的动态均方根值，自主识别车辆行驶工况，并自适应调整观测噪声协方差矩阵中加速度计与高度传感器的信任权重，在平稳工况下提升静态观测融合权重以获取高精度基准，在剧烈工况下降低静态观测权重并优先信任角速度计预估值，实现了不同行驶工况下估计精度与鲁棒性的自适应优化。