一种校正汽车姿态数据的方法及装置与流程

本发明涉及汽车导航技术领域，尤其涉及一种校正汽车姿态数据的方法及装置。

背景技术：

汽车姿态的准确获取不仅对判断汽车事故具有重要意义，而且对判断汽车上、下立交桥，控制汽车智能行驶具有重要意义，目前已越来越广泛地应用于智能交通系统中。

现有技术中，通常采用低成本的惯性测量组件(三轴微机电系统(MEMS，Micro-Electro-Mechanical Systems)陀螺仪和三轴MEMS加速度计)结合地磁信息对汽车姿态数据进行测量和校正。

具体的，汽车姿态初始数据可以通过对三轴MEMS陀螺仪输出的角速度积分得到，但由于三轴MEMS陀螺仪输出的角速度具有一定的误差，随着时间的增加角速度积分的误差会持续变大，即，汽车姿态初始数据的误差随时间增加而持续变大。由于通过三轴MEMS加速度计与当地重力比较得到的汽车姿态无积分误差累积效应，因此，可以利用三轴MEMS加速度计计算得到的汽车姿态数据校正三轴MEMS陀螺仪计算得到的汽车姿态数据。但是，由于利用三轴MEMS加速度计进行汽车姿态数据计算时在静态条件(汽车静止状态)下精度较高，在动态条件(汽车行驶状态)下利用三轴MEMS加速度计进行汽车姿态数据计算误差较大，因此，在汽车行驶过程中，利用三轴MEMS加速度计计算出的带有误差的汽车姿态数据对三轴MEMS陀螺仪计算得到的汽车姿态数据进行校正，得到的汽车姿态数据也不准确。

技术实现要素：

本发明实施例提供了一种校正汽车姿态数据的方法及装置，用以解决现有技术中使用基于三轴加速度计确定的汽车姿态对基于三轴陀螺仪确定的汽车姿态进行校正准确性低的问题。

基于上述问题，本发明实施例提供的一种校正汽车姿态数据的方法包括：

根据汽车的行驶速度，获取汽车的行驶加速度；

获取三轴加速度计提供的三轴加速度；

根据所述三轴加速度计的安装坐标系与汽车的行驶坐标系之间的坐标系转换关系，从所述三轴加速度中去除所述行驶加速度产生的加速度分量，得到调整后的三轴加速度；

使用基于所述调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。

本发明实施例提供的一种校正汽车姿态数据的装置，包括：

行驶加速度获取模块，用于根据汽车的行驶速度，获取汽车的行驶加速度；

三轴加速度获取模块，用于获取三轴加速度计提供的三轴加速度；

三轴加速度调整模块，用于根据所述三轴加速度计的安装坐标系与汽车的行驶坐标系之间的坐标系转换关系，从所述三轴加速度中去除所述行驶加速度产生的加速度分量，得到调整后的三轴加速度；

汽车姿态数据校正模块，用于使用基于所述调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。

本发明实施例的有益效果包括：

本发明实施例提供的一种校正汽车姿态数据的方法及装置，根据汽车的行驶速度，获取汽车的行驶加速度；获取三轴加速度计提供的三轴加速度；根据三轴加速度计的安装坐标系与汽车的行驶坐标系之间的坐标系转换关系，从三轴加速度中去除行驶加速度产生的加速度分量，得到调整后的三轴加速度；使用基于调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。由于利用三轴MEMS加速度计进行汽车姿态计算时在静态条件(汽车静止状态)下精度较高，而在汽车行驶过程中的动态时，与汽车静态时相比，三轴加速度计提供的三轴加速度中增加了由于汽车行驶产生的加速度分量，本发明提供的汽车姿态的确定方法，从三轴加速度计提供的三轴加速度中，去除由汽车行驶产生的加速度分量，得到的调整后的三轴加速度，可以相当于汽车静态时的三轴加速度，精度较高，使用基于该调整后的三轴加速度确定的汽车姿态数据对基于陀螺仪确定的汽车姿态数据进行校正准确性高。

附图说明

图1为本发明实施例提供的一种校正汽车姿态数据的方法的流程图；

图2为本发明实施例1提供的一种校正汽车姿态数据的方法的流程图；

图3为本发明实施例2提供的一种校正汽车姿态数据的方法的流程图；

图4为本发明实施例在汽车导航系统中应用示意图；

图5为本发明实施例提供的基于三轴加速度计输出的调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正的方法示意图；

图6为本发明实施例提供的一种校正汽车姿态数据的装置的结构示意图。

具体实施方式

本发明实施例提供了一种校正汽车姿态数据的方法及装置，以下结合说明书附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。并且在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明实施例提供一种校正汽车姿态数据的方法，如图1所示，包括：

S101、根据汽车的行驶速度，获取汽车的行驶加速度。

S102、获取三轴加速度计提供的三轴加速度。

S103、根据三轴加速度计的安装坐标系与汽车的行驶坐标系之间的坐标系转换关系，从S102中获取的三轴加速度中去除S101中获取的行驶加速度产生的加速度分量，得到调整后的三轴加速度。

S104、使用基于S103中得到的调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。

进一步地，步骤S101和步骤S102的执行没有严格的先后顺序。

下面结合附图，用具体实施例对本发明提供的方法及相关设备进行详细描述。

实施例1：

本发明实施例1中，提供了一种校正汽车姿态数据的方法，在实施例1中加速度计的安装坐标系与行驶坐标系重合，如图2所示，具体包括如下步骤：

S201、获取三轴加速度计提供的三轴加速度。

本步骤中，三轴加速度计提供的三轴加速度包含了汽车行驶时产生的行驶加速度分量。

S202、按照预设采样规则，从车载诊断系统(OBD，On-Board Diagnostics)获取汽车的总线数据。

本步骤中，预设采样规则可以为OBD按照等时间间隔或者不等时间间隔，主动输出汽车在输出时间点的总线数据，也可以为按照等时间间隔或者不等时间间隔主动向OBD采样，获取采样点的总线数据。

进一步地，OBD可以通过数据传递插接器(DLC，Data Link Connector)，按照OBD相关的标准通信协议(例如：汽车工程师协会J1850总线脉冲宽度调制(SAE J1850PWM，Society of Automotive Engineers J1850Pulse Width Moduleated)、汽车工程师协会J1850总线可变脉冲宽度调制(SAE J1850VPW，Society of Automotive Engineers J1850Variable Pulse Width)、国际标准化组织(ISO 9141-2，International Organization for Standardization 9141-2)(或国际标准化组织/国际标准草案14230-4(ISO/DIS 14230-4，International Organization for Standardization Draft International Standard 14230-4)))输出总线数据。

进一步地，现有技术中，汽车导航系统在进行车速测量时通常利用里程仪或轮速等传感器来进行，这种获取方式可能由于车辆改装等原因无法成功获取到车速信息，而本发明实施例提供的获取车速的方法，通过总线数据获取车速信息，使得获取车速信息时，不会受到车辆改装等的因素影响而导致获取失败，并且获取到的车速信息更加准确，为后续准确判断汽车行驶姿态奠定了基础。

S203、解析S202中获取的总线数据，得到汽车的行驶速度。

进一步地，步骤S202中通过DLC接口的标准通信协议输出总线数据，若需要得到汽车的行驶速度，需要根据该标准通信协议将获取的总线数据进行解析，得到汽车的行驶速度。

进一步地，步骤S202和步骤S203可以单独使用一个装置实现，例如：车载故障诊断装置，将车载故障诊断装置通过DLC接口与OBD系统连接，获取总线数据并解析出车速之后，再通过无线(例如：蓝牙、WIFI、ZigBee、超频波段(UWB)、IrDA、HomeRF、红外传输技术等)发送给用于校正汽车姿态数据的装置。

S204、根据S203中获取的汽车的本次行驶速度、获取本次行驶速度之前最近一次获取的汽车的行驶速度、以及获取本次行驶速度与获取最近一次行驶速度之间的时间差，确定汽车的行驶加速度。

本步骤中，假设当前时刻为ti时刻，步骤S203中确定出汽车在ti时刻行驶速度为vobd,i，那么，在获取本次行驶速度之前获取的汽车的最近一次行驶速度，为ti-1时刻获取的汽车的行驶速度vobd,i-1，根据加速度计算公式，得到汽车的行驶加速度Ai为：

式(1)

式(1)中，i为不小于0的整数。

S205、从S201中获取的三轴加速度在第一坐标轴上的加速度分量中减去S204中确定的汽车的行驶加速度，得到第一坐标轴调整后的加速度分量。

S206、保持S201中获取的三轴加速度中第二和第三坐标轴对应的加速度分量不变，得到调整后的三轴加速度，

其中，将加速度计的安装坐标系中与行驶坐标系中汽车行驶方向的坐标轴重合的坐标轴作为第一坐标轴，其余两个坐标轴分别为第二和第三坐标轴。

首先，介绍一下车载系统中常用到的几种坐标系：导航坐标系、行驶坐标系、以及安装坐标系。导航坐标系可以指地理坐标系(通常用n表示)，其原点位于汽车所在地球表面，可以选取在汽车重心处；行驶坐标系(通常用b表示)原点位于汽车的质量中心，以汽车为参照物，行驶坐标系中三轴中的一轴指向汽车行驶的方向，一轴由驾驶员座位指向副驾驶座位方向，第三轴与汽车底座所在平面垂直指向上方；安装坐标系(通常用a表示)为安装三轴加速度计时，三轴加速度计认定的坐标系，由安装决定，坐标原点可以在汽车质心也可以不在汽车质心。较佳地，行驶坐标系和安装坐标系可以重合。

本实施例中，行驶坐标系和安装坐标系重合，假设三轴分别为汽车行驶的方向oy轴，驾驶员座位指向副驾驶座位方向ox轴，与汽车底座所在平面垂直指向上方的方向oz轴，以行驶坐标系为参考坐标系，汽车在行驶时，只有oy轴的方向具有速度和加速度，因此，当行驶坐标系和安装坐标系重合时，汽车本身的运动加速度完全投影在加速度计的安装坐标系的oy轴方向。进一步地，汽车在ox轴方向(横滚角)和oz轴方向(俯仰角)的加速度分量均由汽车所在地的重力加速度g引起，由于重力加速度g的存在，汽车ox方向和oz方向在汽车横滚角及俯仰角不为零时，汽车ox方向和oz方向的加速度分量也均不为零，并且汽车ox方向和oz方向的加速度分量，在横滚角及俯仰角不变时，不会随时间的变化而变化，因此，此时，三轴加速度计的输出可以由式(2)确定：

式(2)

式(2)中，Ax，Ay，Az分别表征三轴加速度计的X轴加速度计，Y轴加速度计，Z轴加速度计输出的三轴加速度分量；A表征汽车行驶的加速度，Cn^b表征导航坐标系至行驶坐标系的坐标转换矩阵(由于g是以导航坐标系为参考坐标系得到的，因此需要将g转换到行驶坐标系中，并且进行坐标转换之后，会将g映射到行驶坐标系中的三个坐标轴上，对应行驶坐标系中的三轴加速度分量)。

由式(2)可以看出，在三轴加速度计安装坐标系与行驶坐标系重合时，汽车行驶时产生的加速度A给Y轴加速度计的输出带来误差。

步骤S205中，从三轴加速度计提供的三轴加速度中去掉汽车行驶时产生的三轴加速度分量，则可以去掉了Y轴加速度计产生的误差，使得调整后的三轴加速度更加精确。具体如下：

式(3)

式(3)中，Ax，Ay，Az分别表征三轴加速度计的X轴加速度计，Y轴加速度计，Z轴加速度计输出的三轴的加速度分量；A表征汽车行驶的加速度，A'x、A'y、A'z分别表征对三轴加速度计测得的三轴加速度进行调整后的三轴加速度分量，即为从三轴加速度计输出的三轴加速度中，去除汽车行驶产生的加速度分量之后的三轴加速度。

S207、使用基于S206中得到的调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。

实施例2：

本发明实施例2中，提供了一种校正汽车姿态数据的方法，在实施例2中加速度计的安装坐标系与行驶坐标系不重合，实施例2中，与实施例1相似的步骤参见实施例1中对应步骤的解释，这里不再赘述。如图3所示，具体包括如下步骤：

S301、获取三轴加速度计提供的汽车的三轴加速度。

S302、按照预设采样规则，从OBD获取汽车的总线数据。

S303、解析S302中获取的总线数据，得到汽车的行驶速度。

S304、根据S303中获取的汽车的本次行驶速度、获取本次行驶速度之前最近一次获取的汽车的行驶速度、以及获取本次行驶速度与获取最近一次行驶速度之间的时间差，确定汽车的行驶加速度。

S305、根据加速度计的安装坐标系的三轴指向和汽车的行驶坐标系的三轴指向，确定安装坐标系和行驶坐标系之间的坐标转换矩阵。

在行驶坐标系中，汽车行驶时产生的三轴加速度分量中，汽车行驶方向的加速度分量为汽车的当前行驶加速度，汽车横滚方向的加速度分量以及汽车俯仰方向的加速度分量均为零。

S306、使用S305中确定的坐标转换矩阵，将行驶坐标系中与汽车行驶方向重合的坐标轴上的汽车的行驶加速度分量，分别映射到安装坐标系中的三个坐标轴上，得到汽车的行驶加速度在安装坐标系中的三个坐标轴上的映射加速度分量。

本实施例中，由于行驶坐标系与三轴加速度计的安装坐标系不重合，那么，在从三轴加速度计产生的三轴加速度中，去除汽车行驶产生的加速度分量之前，需要统一三轴加速度计产生的三轴加速度和汽车行驶产生的加速度的坐标系。因此，本步骤中，将行驶坐标系中汽车行驶产生的三轴加速度分量，分别旋转投影到加速度计安装坐标系中，得到加速计安装坐标系中汽车行驶产生的三轴加速度分量。

此时，三轴加速度计的输出可以由式(4)确定：

式(4)

式(4)中，Ax，Ay，Az分别表征三轴加速度计的X轴加速度计，Y轴加速度计，Z轴加速度计输出的三轴的加速度分量；A表征汽车行驶的加速度；表征导航坐标系至行驶坐标系的坐标转换矩阵；表征行驶坐标系至三轴加速度计安装坐标系的坐标转换矩阵。

由式(4)可以看出，在三轴加速度计安装坐标系与行驶坐标系不重合时，在将汽车行驶加速度从行驶坐标系转换至三轴加速度计安装坐标系之后，汽车行驶时产生的加速度分量A可能映射到加速度计安装坐标系的各轴加速度计的输出中，也就是给各轴加速度计带来误差。

S307、从所述三轴加速度在所述安装坐标系中的三个坐标轴上的加速度分量中，分别减去对应的映射加速度，得到调整后的三轴加速度。

本步骤中，从加速度计提供的三轴加速度中去掉汽车行驶时产生的三轴加速度分量，则可以去掉了各轴加速计产生的误差，使得调整后的三轴加速度更加精确。具体如下：

式(5)

式(5)中，Ax，Ay，Az分别表征三轴加速度计的X轴加速度计，Y轴加速度计，Z轴加速度计输出的三轴的加速度分量；A表征汽车行驶的加速度；表征行驶坐标系至三轴加速度计安装坐标系的坐标转换矩阵；A'x、A'y、A'z分别表征对三轴加速度计测得的三轴加速度进行调整后的三轴加速度分量，即为从三轴加速度计输出的三轴加速度分量中，去除汽车行驶产生的加速度分量之后的三轴加速度。

S308、使用基于S307中得到的调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。

基于同一发明构思，本发明实施例还提供了一种校正汽车姿态数据的装置，由于这些装置所解决问题的原理与前述一种校正汽车姿态数据的方法相似，因此该装置的实施可以参见前述方法的实施，重复之处不再赘述。

图4为本发明实施例在汽车导航系统中应用示意图，假设本发明实施例应用于汽车导航系统中，汽车速度由车载故障诊断装置提供，如图4所示，车载故障诊断装置401确定汽车当前行驶速度之后，可以通过无线技术将行驶速度发送给汽车导航系统402，汽车导航系统402基于该行驶速度，确定汽车行驶加速度，并且使用该行驶加速度对三轴MEMS加速度计输出的三轴加速度进行调整，去除汽车行驶方向的加速度，并将调整后的三轴加速度和三轴MEMS陀螺仪、三轴MEMS磁力计输出的加速度输入互补滤波姿态解算模块，对基于三轴MEMS陀螺仪和三轴MEMS磁力计确定的汽车姿态数据进行校正。

针对实施例1中的步骤S207以及实施例2中的步骤S308，图5为基于三轴加速度计输出的调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正的方法示意图，如图5所示，加速度计输出的三轴加速度为利用本发明实施例对加速度计的三轴加速度调整后输出的三轴加速度。进一步地，基于陀螺仪输出角速度ω可以转化为四元数，并依次对四元数进行叉乘、积分、归一化之后，由四元数进行姿态解算得到姿态角atti_g，又根据三轴加速度计输出的三轴加速度与当地重力加速度的关系，可以确定出姿态角atti_a，又由于atti_g的输出频率高于atti_a的输出频率，每当得到atti_a时，可以对atti_g和atti_a分别以权值Kg和ka进行加权求和，即可得到由加速度计输出校正后的汽车姿态。需要说明的是，图5并没有表明三轴加速度计输出三轴加速度的时刻，仅表明了当三轴加速度计产生输出时，对陀螺仪输出的汽车姿态数据进行校正的方法。

本发明实施例提供了一种校正汽车姿态数据的装置，如图6所示，包括：

行驶加速度获取模块601，用于根据汽车的行驶速度，获取汽车的行驶加速度；

三轴加速度获取模块602，用于获取三轴加速度计提供的三轴加速度；

三轴加速度调整模块603，用于根据所述三轴加速度计的安装坐标系与汽车的行驶坐标系之间的坐标系转换关系，从所述三轴加速度中去除所述行驶加速度产生的加速度分量，得到调整后的三轴加速度；

汽车姿态数据校正模块604，用于使用基于所述调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。

进一步地，所述三轴加速度调整模块603，具体用于当所述加速度计的安装坐标系与所述行驶坐标系重合时，将加速度计的安装坐标系中与所述行驶坐标系中汽车行驶方向的坐标轴重合的坐标轴作为第一坐标轴，其余两个坐标轴分别为第二和第三坐标轴，从所述三轴加速度在第一坐标轴上的加速度分量中减去汽车的行驶加速度，得到第一坐标轴调整后的加速度分量，保持所述三轴加速度中所述第二和第三坐标轴对应的加速度不变，得到调整后的三轴加速度。

进一步地，所述三轴加速度调整模块603，具体用于当所述加速度计的安装坐标系与行驶坐标系不重合时，根据加速度计的安装坐标系的三轴指向和汽车的行驶坐标系的三轴指向，确定所述安装坐标系和所述行驶坐标系之间的坐标转换矩阵；使用所述坐标转换矩阵，将所述行驶坐标系中与汽车行驶方向重合的坐标轴上的所述汽车的行驶加速度分量，分别映射到所述安装坐标系中的三个坐标轴上，得到所述汽车的行驶加速度在所述安装坐标系中的三个坐标轴上的映射加速度分量；从所述三轴加速度在所述安装坐标系中的三个坐标轴上的加速度分量中，分别减去对应的映射加速度，得到调整后的三轴加速度。

进一步地，所述装置还包括：总线数据获取模块605和行驶速度解析模块606；

所述总线数据获取模块605，用于在获取汽车的行驶加速度之前，按照预设采样规则，从车载诊断系统OBD获取汽车的总线数据；

所述行驶速度解析模块606，用于解析获取的所述总线数据，得到汽车的行驶速度。

进一步地，本发明实施例提供的一种校正汽车姿态数据的装置可以应用于车载设备中，或者具有导航功能的设备中等。

进一步地，由于本发明实施例中可以由车载诊断系统OBD提供车速信息，而OBD提供的接口的位置(例如：方向盘下方)可能不便于车载设备(例如导航设备)直接获取车速信息，因此，可以通过车速获取设备利用OBD提供的接口获取车速信息，并传输给车载设备(例如：通过无线传输)，供车载设备计算加速度，并确定汽车行驶姿态。

上述各单元的功能可对应于图1至图3所示流程中的相应处理步骤，在此不再赘述。

本发明实施例提供的一种校正汽车姿态数据的方法及装置，根据汽车的行驶速度，获取汽车的行驶加速度；获取三轴加速度计提供的三轴加速度；根据三轴加速度计的安装坐标系与汽车的行驶坐标系之间的坐标系转换关系，从三轴加速度中去除行驶加速度产生的加速度分量，得到调整后的三轴加速度；使用基于调整后的三轴加速度确定的汽车姿态数据，对基于陀螺仪确定的汽车姿态数据进行校正。由于利用三轴MEMS加速度计进行汽车姿态计算时在静态条件(汽车静止状态)下精度较高，而在汽车行驶过程中的动态时，与汽车静态时相比，三轴加速度计提供的三轴加速度中增加了由于汽车行驶产生的加速度分量，本发明提供的汽车姿态的确定方法，从三轴加速度计提供的三轴加速度中，去除由汽车行驶产生的加速度分量，得到的调整后的三轴加速度，可以相当于汽车静态时的三轴加速度，精度较高，使用基于该调整后的三轴加速度确定的汽车姿态数据对基于陀螺仪确定的汽车姿态数据进行校正准确性高。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明实施例可以通过硬件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，本发明实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

本领域技术人员可以理解附图只是一个优选实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。

本领域技术人员可以理解实施例中的装置中的模块可以按照实施例描述进行分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。