CN104198765A - 车辆运动加速度检测的坐标系转换方法 - Google Patents

Abstract

本发明涉及一种能够实现车辆运动加速度检测的坐标系转换的方法。包括：利用加速度计和磁力计，建立MEMS坐标与地理坐标系之间的相对关系；在导航过程中，根据陀螺的输出，获得姿态矩阵；根据所述姿态矩阵，获得MEMS坐标系与地理坐标系之间的转换矩阵利用MEMS坐标系与地理坐标系之间的转换矩阵将MEMS加速度计输出的加速度转换至地理坐标的加速度；根据全球定位系统GPS输出的GPS的东向速度和车辆速度V_R，获得相对航向角根据所述相对航向角，地理坐标系向车体坐标系的转换矩阵在得到和的计算方法后，可通过矩阵连乘的方法得到MEMS坐标与车体坐标之间的转换矩阵根据所述MEMS坐标与车体坐标之间的转换矩阵将所有MEMS陀螺和加速度计的输出转换到车体坐标系中。

Description

车辆运动加速度检测的坐标系转换方法

技术领域

本发明涉及计算机领域，尤其涉及一种车辆运动加速度检测的坐标系转换方法。 

背景技术

目前，随着传感器技术和信息处理技术的发展，针对车辆运动加速度的检测在汽车保险领域应用程度不断深化，对车辆运动状态下的加速度矢量测量精度需求不断提升。 

为了实现对车辆运动加速度的检测，一般采用即插即用的车载设备，该设备中包括MEMS加速度计和陀螺仪，受安装位置尺寸等方面的限制，该车载设备的安装具有较大的随机性安装问题。目前市场上的现有产品没有对随机安装位置进行处理，直接采集MEMS三轴加速度计测量的合成标量信息，无法识别出在车辆前进方向、横向、天向三维坐标系统中各轴向加速度，对于驾驶者的驾驶行为无法准确进行评估。 

针对现有技术在检测车辆运动加速度的不足，即不管OBD接口位置和方向如何，都能保证MEMS组件的测量轴与车辆方向保持一致，否则就会产生误差，而影响车辆姿态判定结果。 

有鉴于上述的缺陷，本设计人，积极加以研究创新，以期创设一种车辆运动加速度检测的坐标系转换方法，使其更具有产业上的利用价值。 

发明内容

为解决上述技术问题，本发明的目的是提供一种能够车辆运动加速度检测的坐标系转换方法。 

本发明的车辆运动加速度检测的坐标系转换方法，包括： 

利用加速度计和磁力计，建立MEMS坐标与地理坐标系之间的相对关系； 

在导航过程中，根据陀螺的输出，对姿态进行更新，获得姿态矩阵； 

根据所述姿态矩阵，获得MEMS坐标系与地理坐标系之间的转换矩阵

利用MEMS坐标系与地理坐标系之间的转换矩阵将MEMS加速度计输出的加速度转换至地理坐标的加速度； 

根据全球定位系统GPS输出的GPS的东向速度和车辆速度V_R，获得相对航向角

根据所述相对航向角，地理坐标系向车体坐标系的转换矩阵

在得到和的计算方法后，可通过矩阵连乘的方法得到MEMS坐标与车体坐标之间的转换矩阵

根据所述MEMS坐标与车体坐标之间的转换矩阵将所有MEMS陀螺和加速度计的输出转换到车体坐标系中。 

进一步的，所述在导航过程中，根据陀螺的输出，对姿态进行更新，获得姿态矩阵，具体包括：根据陀螺的输出，对姿态进行更新，获得姿态矩阵 

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]={\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]}_{-1}\left[\begin{array}{c}\cos \frac{\mathrm{\δ}{\mathrm{\θ}}_0}{2}\\ \frac{\sin \frac{{\mathrm{\δ\θ}}_0}{2}}{{\mathrm{\δ\θ}}_0}{\mathrm{\δ\θ}}_x\\ \frac{\sin \frac{{\mathrm{\δ\θ}}_0}{2}}{{\mathrm{\δ\θ}}_0}{\mathrm{\δ\θ}}_y\\ \frac{\sin \frac{{\mathrm{\δ\θ}}_0}{2}}{{\mathrm{\δ\θ}}_0}{\mathrm{\δ\θ}}_y\end{array}\right]$

其中，初始姿态四元数q⁰，q¹，q²，q³， 

q₀＝cos(ψ/2)cos(θ/2)cos(γ/2)+sin(ψ/2)sin(θ/2)sin(γ/2)， 

q₁＝cos(ψ/2)sin(θ/2)cos(γ/2)+sin(ψ/2)cos(θ/2)sin(γ/2)， 

q₂＝cos(ψ/2)cos(θ/2)sin(γ/2)-sin(ψ/2)sin(θ/2)cos(γ/2)， 

q₃＝cos(ψ/2)sin(θ/2)sin(γ/2)-sin(ψ/2)cos(θ/2)cos(γ/2)， 

其中，θ,γ,ψ分别表示俯仰角、横滚角和航向角，δθ₀表示俯仰角的平均值，δθ_x，δθ_y，δθ_z分别表示俯仰角的x轴，y轴和z轴的值。 

进一步的，所述MEMS坐标系与地理坐标系之间的转换矩阵具体为： 

$C_M^N=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right].$

进一步的，所述利用MEMS坐标系与地理坐标系之间的转换矩阵将MEMS加速度计输出的加速度转换至地理坐标的加速度，具体包括： 

通过下述公式 $\left[\begin{array}{c}{w}_x^N\\ w_y^N\\ w_z^N\end{array}\right]=C_M^N\left[\begin{array}{c}{w}_x^M\\ w_y^M\\ w_z^M\end{array}\right],$ 将MEMS加速度计输出的加速度 转换至地理坐标的加速度。 

进一步的，所述相对航向角具体为其中，表示GPS的东向速度，V_R表示车辆。 

进一步的，所述根据所述相对航向角，地理坐标系向车体坐标系的转换矩阵

$C_{N1}^B=M_3[-{\mathrm{\ψ}}_{\mathrm{NB}}^N]=\left[\begin{array}{ccc}\cos (-{\mathrm{\ψ}}_{\mathrm{NB}}^N)& -\sin (-{\mathrm{\ψ}}_{\mathrm{NB}}^N)& 0\\ \sin (-{\mathrm{\ψ}}_{\mathrm{NB}}^N)& \cos (-{\mathrm{\ψ}}_{\mathrm{NB}}^N)& 0\\ 0& 0& 1\end{array}\right].$

进一步的，转换矩阵具体为其中，为地理坐标系向车体坐标系的转换矩阵；表示MEMS坐标系与地理坐标系之间的转换矩阵，表示MEMS坐标系向车体坐标系的转换矩阵。 

进一步的，所述根据所述转换矩阵将所有MEMS陀螺和加速度计的输出转换到车体坐标系，具体为 $\left[\begin{array}{c}{w}_x^B\\ w_y^B\\ w_z^B\end{array}\right]=C_M^B\left[\begin{array}{c}{w}_x^M\\ w_y^M\\ w_z^M\end{array}\right],$ 其中，分别表示车体横向、前向、天向加速度。 

借由上述方案，本发明至少具有以下优点： 

本发明通过在采集MEMS、GPS、车辆速度表数据，分别构建各自所代表的坐标系统，通过相应的算法运算，实现各坐标系之间的信息转换，可将加速度分别表示在车体坐标系或地理坐标系中。通过本发明的实施，可为评价车辆运动加速度提供定量分析，对于评价驾驶者的驾驶习惯以及后续车辆保险服务提供客观数据支撑。 

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例并配合附图详细说明如后。 

附图说明

图1是本发明车辆运动加速度检测的坐标系转换方法的方法流程图； 

图2为本发明实施例的车辆运动时的坐标方向划分示意图 

图3为本发明实施例的车体坐标与导航坐标之间的相对关系图。 

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。 

本发明的一种车辆运动加速度检测的坐标系转换系统，包括：MEMS微机电系统(Micro-Electro-MechanicalSystem,MEMS)传感器和车辆速度表和全球定位系统GPS。其中，MEMS传感器包括MEMS三轴加速度计、MEMS三轴陀螺仪和MEMS三轴磁力计，MEMS传感器所代表的为MEMS坐标系(以字母M表示)，车辆速度表所代表的为车体坐标系(以字母B表示)以及GPS所代表的为地理坐标系(以字母N表示)。 

GPS位置、速度信息进行相关坐标系变换，将加速度计敏感到的运动参数通过坐标分解的方式转换至车辆速度表所对应的车体坐标系中，实现对车辆运动的三轴精确检测。 

MEMS坐标系是指MEMS敏感单元安装到车辆上后，由于各类车型不同其安装方式、安装位置均会有所不同，无法保证与车体坐标系重合，即无法直 接提供车辆前进方向、横向、天向三个轴向的加速度数值。通过导航相关算法，可以在MEMS坐标系内建立姿态方程，并辅以三轴磁力计测量，得到地理坐标系与MEMS坐标系之间的相对转换关系车辆速度表可以提供车体前向速度的大小，在车体右向-前向-天向坐标系内车辆速度矢量可以表示为  $V^B=\left[\begin{array}{c}{V}_X^B\\ V_Y^B\\ V_Z^B\end{array}\right]=\left[\begin{array}{c}0\\ V_R\\ 0\end{array}\right],$

其中V_R表示速度表输出的速度标量。将车体坐标系下的速度与GPS速度矢量进行比较，可以得到车体坐标系与地理坐标系之间的转换关系

参见图1所示，一种车辆运动加速度检测的坐标系转换方法，包括： 

步骤101：利用加速度计和磁力计建立MEMS坐标与地理坐标系之间的相对关系，可以得到俯仰角、横滚角和航向角，俯仰角、横滚角和航向角分别表示为θ,γ,ψ，并以此建立初始姿态四元数q⁰，q¹，q²，q³，具体以下式表示： 

q₀＝cos(ψ/2)cos(θ/2)cos(γ/2)+sin(ψ/2)sin(θ/2)sin(γ/2) 

q₁＝cos(ψ/2)sin(θ/2)cos(γ/2)+sin(ψ/2)cos(θ/2)sin(γ/2) 

q₂＝cos(ψ/2)cos(θ/2)sin(γ/2)-sin(ψ/2)sin(θ/2)cos(γ/2) 

q₃＝cos(ψ/2)sin(θ/2)sin(γ/2)-sin(ψ/2)cos(θ/2)cos(γ/2)。 

在后续导航过程中，根据陀螺的输出进行，对姿态进行更新，获得姿态矩阵如下： 

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]={\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]}_{-1}\left[\begin{array}{c}\cos \frac{\mathrm{\δ}{\mathrm{\θ}}_0}{2}\\ \frac{\sin \frac{{\mathrm{\δ\θ}}_0}{2}}{{\mathrm{\δ\θ}}_0}{\mathrm{\δ\θ}}_x\\ \frac{\sin \frac{{\mathrm{\δ\θ}}_0}{2}}{{\mathrm{\δ\θ}}_0}{\mathrm{\δ\θ}}_y\\ \frac{\sin \frac{{\mathrm{\δ\θ}}_0}{2}}{{\mathrm{\δ\θ}}_0}{\mathrm{\δ\θ}}_y\end{array}\right]$

其中 ${\mathrm{\&delta;\&theta;}}_0=\sqrt{{{\mathrm{\&delta;\&theta;}}_x}^2+{{\mathrm{\&delta;\&theta;}}_y}^2+{{\mathrm{\&delta;\&theta;}}_z}^2}.$ 如果|δθ₀|<1e-5，则 $\frac{1}{{\mathrm{\&delta;\&theta;}}_0}\sin \frac{{\mathrm{\&delta;\&theta;}}_0}{2}=0.5$

MEMS坐标系与地理坐标系之间的转换矩阵可以表示为： 

$C_M^N=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right].$

步骤102，根据姿态矩阵，从零速起，结合加速度计的输出的值，进行MEMS惯性速度计算。 

利用将MEMS加速度计输出的加速度转换至地理坐标系下，即： 

$\left[\begin{array}{c}{w}_x^N\\ w_y^N\\ w_z^N\end{array}\right]=C_M^N\left[\begin{array}{c}{w}_x^M\\ w_y^M\\ w_z^M\end{array}\right]$

步骤103，设GPS输出的水平方向合成速度为在|(V^GPS-V_R)/V_R|<0.01时，即可认为当前车辆在近似水平面内行驶。将GPS输出的东向速度北向速度与V_R之间的相对关系表达在图3所示。由于GPS提供的天向速度误差较大，因此利用车辆速度进行相对航向角的求解。图3中表示在导航坐标系下车体坐标相对地理坐标的相对航向角。 

则此时可以计算如下： 

${\mathrm{\&psi;}}_{\mathrm{NB}}^N=\arcsin \left(\frac{V_E^{\mathrm{GPS}}}{V_R}\right)$

地理坐标系向车体坐标系的转换矩阵可以表示为： 

$C_{N1}^B=M_3[-{\mathrm{\&psi;}}_{\mathrm{NB}}^N]=\left[\begin{array}{ccc}\cos (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& -\sin (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& 0\\ \sin (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& \cos (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& 0\\ 0& 0& 1\end{array}\right].$

步骤104：可选的，为了进一步提高修正的计算精度，在的初步计算基础上，进一步引入卡尔曼滤波进行精度提高计算。构建如下式所示的滤波状态向量： 

包括相对俯仰偏差角和相对航向偏差角。 

观测方程为： 

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_x\\ {\mathrm{\&delta;V}}_y\end{array}\right]=\left[\begin{array}{c}{V}_E^{\mathrm{GPS}}\\ V_N^{\mathrm{GPS}}\end{array}\right]-C_B^N\left[\begin{array}{c}0\\ V_R\\ 0\end{array}\right],$ 该时仅作第1,2行计算。 

利用卡尔曼滤波进行误差角估计，将得到的估计结果可表示为： 

则修正后的 $C_N^B=C_{N1}^B\&CenterDot;C_N^{N1}.$

步骤105，在得到和的计算方法后，可通过矩阵连乘的方法得到MEMS坐标与车体坐标之间的转换矩阵

$C_M^B=C_N^B\&CenterDot;C_M^N.$

所有MEMS陀螺和加速度计的输出可通过上式转换到车体坐标系下： 

$\left[\begin{array}{c}{w}_x^B\\ w_y^B\\ w_z^B\end{array}\right]=C_M^B\left[\begin{array}{c}{w}_x^M\\ w_y^M\\ w_z^M\end{array}\right],$

上式中分别表示车体横向、前向、天向加速度。 

本发明的有益效果在于：本发明通过在采集MEMS、GPS、车辆速度表数据，分别构建各自所代表的坐标系统，通过相应的算法运算，实现各坐标系之间的信息转换，可将加速度分别表示在车体坐标系或地理坐标系中。通过本发明的实施，可为评价车辆运动加速度提供定量分析，对于评价驾驶者的驾驶习惯以及后续车辆保险服务提供客观数据支撑。 

以上所述仅是本发明的优选实施方式，并不用于限制本发明，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。 

Claims (8)

1.一种车辆运动加速度检测的坐标系转换方法，其特征在于，包括：

利用加速度计和磁力计，建立MEMS坐标与地理坐标系之间的相对关系；

在导航过程中，根据陀螺的输出，对姿态进行更新，获得姿态矩阵；

根据所述姿态矩阵，获得MEMS坐标系与地理坐标系之间的转换矩阵

利用MEMS坐标系与地理坐标系之间的转换矩阵将MEMS加速度计输出的加速度转换至地理坐标的加速度；

根据全球定位系统GPS输出的GPS的东向速度和车辆速度V_R，获得相对航向角

根据所述相对航向角，地理坐标系向车体坐标系的转换矩阵

在得到和的计算方法后，可通过矩阵连乘的方法得到MEMS坐标与车体坐标之间的转换矩阵

根据所述MEMS坐标与车体坐标之间的转换矩阵将所有MEMS陀螺和加速度计的输出转换到车体坐标系中。

2.根据权利要求1的车辆运动加速度检测的坐标系转换方法，其特征在于，所述在导航过程中，根据陀螺的输出，对姿态进行更新，获得姿态矩阵，具体包括：根据陀螺的输出，对姿态进行更新，获得姿态矩阵：

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]={\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]}_{-1}\left[\begin{array}{c}\cos \frac{\mathrm{\&delta;}{\mathrm{\&theta;}}_0}{2}\\ \frac{\sin \frac{{\mathrm{\&delta;\&theta;}}_0}{2}}{{\mathrm{\&delta;\&theta;}}_0}{\mathrm{\&delta;\&theta;}}_x\\ \frac{\sin \frac{{\mathrm{\&delta;\&theta;}}_0}{2}}{{\mathrm{\&delta;\&theta;}}_0}{\mathrm{\&delta;\&theta;}}_y\\ \frac{\sin \frac{{\mathrm{\&delta;\&theta;}}_0}{2}}{{\mathrm{\&delta;\&theta;}}_0}{\mathrm{\&delta;\&theta;}}_y\end{array}\right]$

其中，初始姿态四元数q⁰，q¹，q²，q³，

q₀＝cos(ψ/2)cos(θ/2)cos(γ/2)+sin(ψ/2)sin(θ/2)sin(γ/2)，

q₁＝cos(ψ/2)sin(θ/2)cos(γ/2)+sin(ψ/2)cos(θ/2)sin(γ/2)，

q₂＝cos(ψ/2)cos(θ/2)sin(γ/2)-sin(ψ/2)sin(θ/2)cos(γ/2)，

q₃＝cos(ψ/2)sin(θ/2)sin(γ/2)-sin(ψ/2)cos(θ/2)cos(γ/2)，

其中，θ,γ,ψ分别表示俯仰角、横滚角和航向角，δθ₀表示俯仰角的平均值，δθ_x，δθ_y，δθ_z分别表示俯仰角的x轴，y轴和z轴的值。

3.根据权利要求2的车辆运动加速度检测的坐标系转换方法，其特征在于，所述MEMS坐标系与地理坐标系之间的转换矩阵具体为：

$C_M^N=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right].$

4.根据权利要求1的车辆运动加速度检测的坐标系转换方法，其特征在于，所述利用MEMS坐标系与地理坐标系之间的转换矩阵将MEMS加速度计输出的加速度转换至地理坐标的加速度，具体包括：

通过下述公式 $\left[\begin{array}{c}{w}_x^N\\ w_y^N\\ w_z^N\end{array}\right]=C_M^N\left[\begin{array}{c}{w}_x^M\\ w_y^M\\ w_z^M\end{array}\right],$ 将MEMS加速度计输出的加速度转换至地理坐标的加速度。

5.根据权利要求1的车辆运动加速度检测的坐标系转换方法，其特征在于，所述相对航向角具体为其中，表示GPS的东向速度，V_R表示车辆。

6.根据权利要求1的车辆运动加速度检测的坐标系转换方法，其特征在于，所述根据所述相对航向角，地理坐标系向车体坐标系的转换矩阵

$C_{N1}^B=M_3[-{\mathrm{\&psi;}}_{\mathrm{NB}}^N]=\left[\begin{array}{ccc}\cos (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& -\sin (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& 0\\ \sin (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& \cos (-{\mathrm{\&psi;}}_{\mathrm{NB}}^N)& 0\\ 0& 0& 1\end{array}\right].$

7.根据权利要求1的车辆运动加速度检测的坐标系转换方法，其特征在于，所述转换矩阵具体为其中，为地理坐标系向车体坐标系的转换矩阵；表示MEMS坐标系与地理坐标系之间的转换矩阵，表示MEMS坐标系向车体坐标系的转换矩阵。

8.根据权利要求1的车辆运动加速度检测的坐标系转换方法，其特征在于，所述根据所述转换矩阵将所有MEMS陀螺和加速度计的输出转换到车体坐标系，具体为 $\left[\begin{array}{c}{w}_x^B\\ w_y^B\\ w_z^B\end{array}\right]=C_M^B\left[\begin{array}{c}{w}_x^M\\ w_y^M\\ w_z^M\end{array}\right],$ 其中，分别表示车体横向、前向、天向加速度。