CN110888449A - 车辆姿态控制方法、设备和存储介质 - Google Patents

Abstract

本发明公开了车辆姿态控制方法、设备和存储介质，属于车辆技术领域。车辆姿态控制方法包括：获得车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，并根据所述车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，计算加速度旋转矩阵；根据采样率采集n组陀螺仪角速度数据，并根据所述n组陀螺仪角速度数据和所述加速度旋转矩阵，计算车辆坐标系下的姿态角；根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿态。本发明中的车辆实际姿态角、角速度/转弯角度控制方法计算出来的姿态角能够等于车辆实际姿态角度，减小了误差，实现了任意角度安装设备均能够较为准确地计算和控制车辆实际姿态角度。

Description

车辆姿态控制方法、设备和存储介质

技术领域

本发明涉及车辆技术领域，更具体地说，它涉及车辆姿态控制方法、设备 和存储介质。

背景技术

现有的车辆，由于加速度传感器和陀螺仪传感器等设备是需要按照特定角 度安装的，使传感器坐标系和车辆坐标系重合，以保证在实际测量时，加速度 传感器和陀螺仪传感器测量出来的数据能够真实地反映车辆的实际情况。但是， 在实际安装，完完全全地按照设计要求的特定的角度按照设备很难实现，终究 还是会有大大小小的误差，严格意义来讲，这样的安装误差带来的结果是计算 出来的姿态例如姿态角、角速度和转弯角度其实并不等于实际的姿态，也存在 较大的误差。

发明内容

本发明的目的是提供车辆姿态控制方法、设备和存储介质，解决现有的计 算车辆姿态角的方法和装置，无法实现任意角度安装设备均能较为准确地计算 出车辆姿态的技术问题。

本发明的上述技术目的是通过以下技术方案得以实现的：

根据本发明的一个方面，提供一种车辆姿态控制方法，该车辆包括任意角 度安装的加速度传感器和陀螺仪传感器，该方法包括：

获得车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，并根据 所述车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，计算加速度 旋转矩阵；

根据采样率采集n组陀螺仪角速度数据，并根据所述n组陀螺仪角速度数 据和所述加速度旋转矩阵，计算车辆坐标系下的姿态角；

根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿态。

作为进一步优化的，所述获得车辆坐标系下的初始加速度和传感器坐标系 下的初始加速度，并根据所述车辆坐标系下的初始加速度和传感器坐标系下的 初始加速度，计算加速度旋转矩阵包括：

把车停在相对平坦的地面上，得到车辆坐标系的初始加速度

G为 重力加速度；

假定传感器坐标系的初始加速度

则所述加速度旋转矩阵

作为进一步优化的，所述根据采样率采集n组陀螺仪角速度数据，并根据 所述n组陀螺仪角速度数据和所述加速度旋转矩阵，计算车辆坐标系下的姿态 角包括：

每组陀螺仪角速度数据包括x轴的角速度xg、y轴的角速度yg和z轴的角 速度zg；

每组陀螺仪角速度数据，根据采样率f，计算得出单位时间内绕x轴转动的 角度φ＝xg/f，绕y轴转动的角度θ＝yg/f，绕z轴转动的角度ψ＝zg/f， 根据角度φ、θ和ψ得出角度旋转矩阵R_{(φ、θ、ψ)}，所述n组陀螺仪角速度数 据分别计算角度旋转矩阵R_{(φ、θ、ψ)}得到R₁到R_n；

根据所述R₁到R_n和所述加速度旋转矩阵R₀计算得出车辆坐标系下的旋转矩 阵R_E，

根据所述车辆坐标系下的旋转矩阵R_E计算得出车辆坐标系下的姿态角。

作为进一步优化的，所述根据所述车辆坐标系下的旋转矩阵R_E计算得出相 对车辆坐标系的姿态角包括：

R_E的计算结果如下形式：

以上矩阵中，R_E(1,1)表示计算结果中第一行第一列的值，其余类似；

所述车辆坐标系下的姿态角包括α、β和γ，其中α为绕车辆坐标系x轴旋转 的角度，β为绕车辆坐标系y轴旋转的角度，γ为绕车辆坐标系z轴旋转的角度， 则所述车辆坐标系下的姿态角：

作为进一步优化的，所述根据所述计算所得的车辆坐标系下的姿态角调整 车辆的姿态包括：

根据前后车辆坐标系下的姿态角计算车辆坐标系下的角速度，并通过根据 所述计算所得的车辆坐标系下的角速度调整车辆的角速度以实现调整车辆的姿 态。

作为进一步优化的，所述根据所述计算所得的车辆坐标系下的姿态角调整 车辆的姿态包括：

根据前后车辆坐标系下的姿态角计算车辆坐标系下的转弯角度，并通过根 据所述计算所得的车辆坐标系下的转弯角度调整车辆的转弯角度以实现调整车 辆的姿态。

作为进一步优化的，所述通过根据所述计算所得的车辆坐标系下的转弯角 度调整车辆的转弯角度以实现调整车辆的姿态之前包括：

进行车辆转弯方向判断，对所述计算所得的车辆坐标系下的转弯角度进行 方向校准。

作为进一步优化的，所述进行车辆转弯方向判断借助GPS实现。

根据本发明的另一个方面，提供一种车辆实际姿态角度控制设备，该设备 包括存储器、处理器及存储在所述存储器上并可以在所述处理器上运行的计算 机程序，所述计算机程序被所述处理器执行时，实现如前述的车辆实际姿态角 度控制方法的步骤。

根据本发明的又一个方面，提供一种存储介质，所述存储介质上存储有计 算机程序，所述计算机程序被处理器执行时，实现如前述的车辆实际姿态角度 控制方法的步骤。

根据本发明的再一个方面，提供一种车辆实际角速度/转弯角度控制方法， 该方法适用于任意角度安装加速度传感器和陀螺仪传感器，该方法包括：

获得车辆坐标系的初始加速度A和传感器坐标系的初始加速度A′，并根据所 述A和A′计算获得加速度旋转矩阵R₀；

根据采样率采集获得n组陀螺仪角速度数据，每组陀螺仪角速度数据包括x 轴的角速度xg、y轴的角速度yg和z轴的角速度zg，并根据所述n组陀螺仪角 速度数据和所述加速度旋转矩阵R₀计算获得相对车辆坐标系的姿态角；

根据前后相对车辆坐标系的姿态角计算获得车辆实际角速度/转弯角度；

根据所述计算获得的车辆实际角速度/转弯角度调整车辆角速度/转弯角度。

作为进一步优化的，所述根据所述计算获得的实际转弯角度调整车辆转弯 角度之前包括：

进行车辆转弯方向判断，对所述计算获得的实际转弯角度进行方向校准。

根据本发明的又一个方面，提供一种车辆姿态控制设备，其特征在于：该 设备包括存储器、处理器及存储在所述存储器上并可以在所述处理器上运行的 计算机程序，所述计算机程序被所述处理器执行时，实现如前述的车辆姿态控 制方法的步骤。

根据本发明的又一个方面，所述存储介质上存储有计算机程序，所述计算 机程序被处理器执行时，实现如前述的车辆姿态控制方法的步骤。

综上所述，本发明具有以下有益效果：任意角度安装加速度传感器和陀螺 仪，会使得加速度坐标系和车辆坐标系不重合，从而使得获得的加速度信息并 不能直接真实反映车辆的真实情况，本发明中的车辆姿态控制方法先完成坐标 系调整，使得加速度坐标系和车辆坐标系重合，然后计算相对车辆坐标系的姿 态角，计算出来的姿态角能够等于车辆实际姿态角度，减小了误差，实现了任 意角度安装设备均能够较为准确地计算和控制车辆姿态。

附图说明

图1是实施例一中的车辆实际姿态角度控制方法的流程示意图。

具体实施方式

为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚、明白， 以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描 述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例一

本实施例中的一种车辆实际姿态角度控制方法，该方法适用于任意角度安 装加速度传感器进而陀螺仪传感器，如图1所示，该方法包括以下步骤：

S10，获得车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，并 根据所述车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，计算加 速度旋转矩阵。

具体的，在本实施例中，把车停在相对平坦的地面上，这时相对车辆坐标 系，应该得到的车辆坐标系下的初始加速度

其中，G为重力加速度大 小。通过加速度传感器采集传感器坐标系下的初始加速度A′，假定

理 论上，传感器坐标系下的向量通过旋转即可变换为车辆坐标系下的向量，即： A＝R₀×A′ (公式一)，其中R₀为加速度旋转矩阵。又因为旋转矩阵不改变矢量 的大小，因此：

由以上公式一和公式二计算得到 加速度旋转矩阵R₀：

S20，根据采样率采集n组陀螺仪角速度数据，并根据所述n组陀螺仪角速 度数据和所述加速度旋转矩阵，计算车辆坐标系下的姿态角；

具体的，设采样率为f，依据采样率f在前后采样时间段内通过陀螺仪传感 器采集n组陀螺仪角速度数据，每组陀螺仪角速度数据包括x轴的角速度xg、y 轴的角速度yg和z轴的角速度zg。

具体的，每组陀螺仪角速度数据，根据采样率f，计算得出单位时间内绕x 轴转动的角度φ＝xg/f，绕y轴转动的角度θ＝yg/f，绕z轴转动的角度ψ＝ zg/f，再根据角度φ、θ和ψ计算得出每组角速度数据的角度旋转矩阵 R_{(φ、θ、ψ)}。每一次微小的转动，绕x轴的角度旋转矩阵为：

绕y轴的角度旋转矩阵为：

绕z轴的角度旋转矩阵为：

每组陀螺仪角速度数据的总的角度旋转矩阵R_{(φ、θ、ψ)}为：

R_{(φ、θ、ψ)}＝Cx×Cy×Cz (公式七)

将以上公式四、公式五和公式六带入公式七，计算得出：

把每一组陀螺仪角速度数据计算出的φ、θ和ψ分别代入上述公式八，计 算得出R₁到R_n。

每次转动的角度旋转矩阵为R_{(φ、θ、ψ)}，n次连续转动之后，有：

A′₁＝R_n×R_n-1×…×R₁×A′₀ (公式九)

其中，A′₁为n次转动后传感器坐标系下的值，A′₀为转动前传感器坐标系下 的初始值。假设：

R_c＝R_n×R_n-1×…×R₁

则上述公式九简化为：

A′₁＝R_c×A′₀ (公式十)

A′₁和A′₀可以通过以下两个公式转换为车辆坐标系下的值：

A₁＝R₀×A′₁ (公式十一)

A₀＝R₀×A′₀ (公式十二)

其中，A₁为n次转动后车辆坐标系下的值，A₀为转动前车辆坐标系下的初 始值。

将公式十代入公式十一中，得到：

A₁＝R₀×R_c×A′₀ (公式十三)

由公式十二可得：

由于

是正交矩阵，因此上式可化为：

将公式十四代入公式十三，得出：

因此在车辆坐标系中的旋转矩阵R_E为：

以上公式十五中，每一项在检测和计算后均为已知数，因此可以计算出R_E。 经过以上的n次转动后，相对车辆坐标系的姿态角包括α、β和γ，其中α为绕车 辆坐标系x轴旋转的角度，β为绕车辆坐标系y轴旋转的角度，γ为绕车辆坐标 系z轴旋转的角度。以上的n次旋转可以看作是根据相对车辆坐标系的姿态角 经过一次旋转得到，对应的旋转矩阵R_{(α、β、γ)}：

n次旋转和一次旋转的矩阵应该是一样的，即R_{(α、β、γ)}＝R_E。

R_E的计算结果如下形式：

以上矩阵中，R_E(1,1)表示计算结果中第一行第一列的值，其余类似。

因此根据计算得到的R_E和上述公式十六，得出车辆坐标系下的姿态角为：

其中，α为绕车辆坐标系x轴旋转的角度，β为绕车辆坐标系y轴旋转的角 度，γ为绕车辆坐标系z轴旋转的角度。

值得说明的是，上述部分公式为用于阐述详细原理而设立的中间公式，在 实际计算中，并不需要进行中间的转换，而直接将相关角速度数据代入上述公 式三、公式八和公式十五进行相关计算得出结果即可。

S30，根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿态。

具体的，根据上述步骤S10和S20计算获得车辆坐标系下的姿态角后，判 断该车辆的姿态后，根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿 态。

作为优化，所述根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿 态包括：根据前后车辆坐标系下的姿态角计算车辆坐标系下的角速度，并通过 根据所述计算所得的车辆坐标系下的角速度调整车辆的角速度以实现调整车辆 的姿态。在计算获得车辆坐标系下的角速度后，判断该车辆的角速度，然后根 据所述计算所得的车辆坐标系下的角速度调整车辆的角速度以实现调整车辆的 姿态，调整车辆的角速度包括减小、保存不变或增大车辆的角速度。

作为优化，也可以是，所述根据所述计算所得的车辆坐标系下的姿态角调 整车辆的姿态包括：根据前后车辆坐标系下的姿态角计算车辆坐标系下的转弯 角度，并通过根据所述计算所得的车辆坐标系下的转弯角度调整车辆的转弯角 度以实现调整车辆的姿态。在计算获得车辆坐标系下的转弯角度后，判断该车 辆的转弯角度，然后根据所述计算所得的车辆坐标系下的转弯角度调整车辆的 转弯角度以实现调整车辆的姿态，调整车辆的转弯角度包括减小、保存不变或 增大车辆的转弯角度。作为进一步优化，根据所述计算获得的车辆坐标系下的 转弯角度调整车辆的转弯角度以实现调整车辆的姿态之前包括：进行车辆转弯 方向判断，对所述计算所得的车辆坐标系下的转弯角度进行方向校准。具体可借助GPS进行转弯方向判断。

任意角度安装加速度传感器和陀螺仪，会使得加速度坐标系和车辆坐标系 不重合，从而使得获得的加速度信息并不能直接真实反映车辆的真实情况，本 实施例中的车辆姿态控制方法先完成坐标系调整，使得加速度坐标系和车辆坐 标系重合，然后计算相对车辆坐标系的姿态角，计算出来的姿态角能够等于车 辆实际姿态角度，减小了误差，实现了任意角度安装设备均能够较为准确地计 算和控制车辆的姿态。

实施例二

本实施例提供一种车辆姿态控制设备，该设备包括存储器、处理器及存储 在所述存储器上并可在所述处理器上运行的计算机程序，该所述计算机程序被 所述处理器执行时，实现实施例一中的车辆姿态控制方法的步骤。

本发明实施例的车辆姿态控制设备与上述实施例一的方法属于同一构思， 其具体实现过程详细见对应的方法实施例，且方法实施例中的技术特征在本车 辆姿态控制设备实施例中均对应适用，这里不再赘述。

本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、 系统、装置中的功能模块/单元可以被实施为软件、固件、硬件及其适当的组合。

在硬件实施方式中，在以上描述中提及的功能模块/单元之间的划分不一定 对应于物理组件的划分；例如，一个物理组件可以具有多个功能，或者一个功 能或步骤可以由若干物理组件合作执行。某些物理组件或所有物理组件可以被 实施为由处理器，如中央处理器、数字信号处理器或微处理器执行的软件，或 者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以 分布在计算机可读介质上，计算机可读介质可以包括计算机存储介质(或非暂 时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的，术语 计算机存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、程序 模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不 可移除介质。计算机存储介质包括但不限于RAM、ROM、EEPROM、闪存或其他存 储器技术、CD-ROM、数字多功能盘(DVD)或其他光盘存储、磁盒、磁带、磁盘 存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问 的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含 计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调 制数据信号中的其他数据，并且可包括任何信息递送介质。

以上具体实施例仅仅是对本发明的解释，其并不是对本发明的限制，本领 域技术人员在阅读完本说明书后可以根据需要对以上实施例做出没有创造性贡 献的修改，但只要在本发明的权利要求范围内都受到专利法的保护。

Claims (10)

1.一种车辆姿态控制方法，该车辆包括任意角度安装的加速度传感器和陀螺仪传感器，其特征在于：该方法包括：

获得车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，并根据所述车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，计算加速度旋转矩阵；

根据采样率采集n组陀螺仪角速度数据，并根据所述n组陀螺仪角速度数据和所述加速度旋转矩阵，计算车辆坐标系下的姿态角；

根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿态。

2.根据权利要求1所述的方法，其特征在于：所述获得车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，并根据所述车辆坐标系下的初始加速度和传感器坐标系下的初始加速度，计算加速度旋转矩阵包括：

把车停在相对平坦的地面上，得到车辆坐标系下的初始加速度

G为重力加速度；

假定传感器坐标系下的初始加速度

则所述加速度旋转矩阵

3.根据权利要求1所述的方法，其特征在于：所述根据采样率采集n组陀螺仪角速度数据，并根据所述n组陀螺仪角速度数据和所述加速度旋转矩阵，计算车辆坐标系下的姿态角包括：

每组陀螺仪角速度数据包括x轴的角速度xg、y轴的角速度yg和z轴的角速度zg；

每组陀螺仪角速度数据，根据采样率f，计算得出单位时间内绕x轴转动的角度φ＝xg/f，绕y轴转动的角度θ＝yg/f，绕z轴转动的角度ψ＝zg/f，根据角度φ、θ和ψ得出角度旋转矩阵R(φ、θ、ψ)，所述n组陀螺仪角速度数据分别计算角度旋转矩阵R(φ、θ、ψ)得到R₁到R_n；

根据所述R₁到R_n和所述加速度旋转矩阵R₀计算得出车辆坐标系下的旋转矩阵R_E，

根据所述车辆坐标系下的旋转矩阵R_E计算得出车辆坐标系下的姿态角。

4.根据权利要求3所述的方法，其特征在于：所述根据所述车辆坐标系下的旋转矩阵R_E计算得出相对车辆坐标系的姿态角包括：

R_E的计算结果如下形式：

以上矩阵中，R_E(1，1)表示计算结果中第一行第一列的值，其余类似；

所述车辆坐标系下的姿态角包括α、β和γ，其中α为绕车辆坐标系x轴旋转的角度，β为绕车辆坐标系y轴旋转的角度，γ为绕车辆坐标系z轴旋转的角度，则所述车辆坐标系下的姿态角：

5.根据权利要求1所述的方法，其特征在于：所述根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿态包括：

根据前后车辆坐标系下的姿态角计算车辆坐标系下的角速度，并通过根据所述计算所得的车辆坐标系下的角速度调整车辆的角速度以实现调整车辆的姿态。

6.根据权利要求1所述的方法，其特征在于：所述根据所述计算所得的车辆坐标系下的姿态角调整车辆的姿态包括：

根据前后车辆坐标系下的姿态角计算车辆坐标系下的转弯角度，并通过根据所述计算所得的车辆坐标系下的转弯角度调整车辆的转弯角度以实现调整车辆的姿态。

7.根据权利要求6所述的方法，其特征在于：所述通过根据所述计算所得的车辆坐标系下的转弯角度调整车辆的转弯角度以实现调整车辆的姿态之前包括：

进行车辆转弯方向判断，对所述计算所得的车辆坐标系下的转弯角度进行方向校准。

8.根据权利要求7所述的方法，其特征在于：所述进行车辆转弯方向判断借助GPS实现。

9.一种车辆姿态控制设备，其特征在于：该设备包括存储器、处理器及存储在所述存储器上并可以在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时，实现如权利要求1-8任意一项所述的车辆姿态控制方法的步骤。

10.一种存储介质，其特征在于：所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时，实现如权利要求1-8任意一项所述的车辆姿态控制方法的步骤。

Images