CN105865461B - 一种基于多传感器融合算法的汽车定位系统及方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合算法的汽车定位系统及方法，该系统包括主控模块，以及与主控模块均相连的车速检测模块、行驶方向检测模块和GPS模块；车速检测模块，包括与汽车传动轴相连的旋转光电编码器，用于采集车辆行驶的瞬时速度信息；行驶方向检测模块，包括加速度计、陀螺仪和磁力传感器，用于采集车辆的行驶方向信息；GPS模块，用于获取车辆的GPS信息，得到车辆的绝对位置信息；主控模块，用于获取车速检测模块和行驶方向检测模块采集到的信息，对其进行处理得到相对位置信息，并结合GPS模块的绝对位置信息进行数据融合，得到最终的汽车位置信息。本发明以较低的成本实现了汽车定位系统定位精度的有效提升，且降低了设备的成本。

Description

一种基于多传感器融合算法的汽车定位系统及方法

技术领域

本发明涉及汽车导航定位领域，尤其涉及一种基于多传感器融合算法的汽车定位系统及方法。

背景技术

随着汽车电子技术的发展，车载导航定位系统逐渐普及。车辆定位是汽车导航的关键技术之一。对于汽车定位系统，定位精度是一个重要指标。目前的民用GPS技术，其定位精度能够达到20m以内。能够确定汽车的大致位置，但是其精度在某些情况不能满足用户需求。

采用GPS定位系统进行定位，其定位精度有限。有时，用户需要更高的定位精度，例如，当汽车位于某一条道路时，GPS定位系统可以确定车辆在该路段的大致位置，但是如果想要知道车辆位于道路的哪一侧，定位精度就会无法满足要求。如果希望单纯依靠GPS定位装置实现较为精准的定位，对GPS定位模块的性能要求较高，需要较高的成本。

发明内容

本发明要解决的技术问题在于针对现有技术中使用单独GPS定位装置精度较差，且高精度的GPS定位装置成本较高的缺陷，提供一种利用多个传感器进行联合定位的基于多传感器融合算法的汽车定位系统及方法。

本发明解决其技术问题所采用的技术方案是：

本发明提供一种基于多传感器融合算法的汽车定位系统，包括主控模块，以及与主控模块均相连的车速检测模块、行驶方向检测模块和GPS模块；

所述车速检测模块，包括与汽车传动轴相连的旋转光电编码器，用于采集车辆行驶的瞬时速度信息；

所述行驶方向检测模块，包括加速度计、陀螺仪和磁力传感器，用于采集车辆的行驶方向信息；

所述GPS模块，用于获取车辆的GPS信息，得到车辆的绝对位置信息；

所述主控模块，用于获取车速检测模块和行驶方向检测模块采集到的信息，对其进行处理得到相对位置信息，并结合GPS模块的绝对位置信息进行数据融合，得到最终的汽车位置信息。

进一步地，本发明的该系统还包括与主控模块相连的存储模块。

本发明提供一种基于多传感器融合算法的汽车定位方法，包括以下步骤：

S1、获取旋转光电编码器的数据，并对其进行处理得到车辆瞬时速度信息；获取加速度计、陀螺仪和磁力传感器的数据，并对其进行处理得到车辆的行驶方向信息；

S2、获取GPS信息作为绝对位置信息，根据瞬时速度信息和行驶方向信息得到车辆的速度矢量，并对其进行积分运算得到相对位置信息；

S3、对相对位置信息和绝对位置信息进行数据融合，将其分别表示为X、Y轴的分量，对相对速度的X、Y轴分量进行积分得到相对位移的X、Y轴分量，将其经过GPS信息中绝对位置信息的反馈校正，得到车辆的精确位置。

进一步地，本发明的步骤S1中获取瞬时速度信息的方法具体为：通过在单位时间间隔内对旋转光电编码器产生的脉冲进行计数，进而计算得到车辆的瞬时速度信息。

进一步地，本发明的步骤S1中获取行驶方向信息的具体方法为：

通过陀螺仪与加速度计的检测数据进行数据融合得到俯仰角，通过陀螺仪与磁力计的检测数据进行数据融合得到航向角，根据航向角和俯仰角得到车辆的行驶方向信息。

进一步地，本发明的该方法还包括当缺少GPS信号时，通过丢失信号前的初始位置以及车辆相对位置信息推算车辆位置坐标，当GPS信号恢复时，系统自动恢复GPS绝对坐标反馈校正环节。

进一步地，本发明的该方法还包括系统第一次启动时，其定位的初始位置由GPS模块测得，在以后的启动过程，系统自动判断车辆在系统关闭期间是否发生移动，并以此确定初始位置是GPS模块的测量坐标还是系统上次存储的位置坐标。

本发明产生的有益效果是：本发明的基于多传感器融合算法的汽车定位系统，通过结合多个传感器进行数据采集，并通过融合算法计算得到车辆的位置信息；以较低的成本实现了汽车定位系统定位精度的有效提升，利用融合算法，将GPS定位系统不存在累计误差以及车辆自主定位在短时间内精度较高的优点相结合，实现了较为精确的汽车定位，从而有利于提高车辆导航的稳定性与精确性；仅需要多个普通的传感器模块进行数据采集，降低了设备的成本。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的基于多传感器融合算法的汽车定位系统的系统结构框图；

图2(a)是本发明实施例的基于多传感器融合算法的汽车定位系统的俯仰角计算框图；

图2(b)是本发明实施例的基于多传感器融合算法的汽车定位系统的航向角计算框图；

图3是本发明实施例的基于多传感器融合算法的汽车定位系统的融合算法计算框图；

图4是本发明实施例的基于多传感器融合算法的汽车定位算法的流程图。

具体实施方式

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

如图1所示，本发明实施例的基于多传感器融合算法的汽车定位系统，包括主控模块，以及与主控模块均相连的车速检测模块、行驶方向检测模块和GPS模块；

车速检测模块，包括与汽车传动轴相连的旋转光电编码器，用于采集车辆行驶的瞬时速度信息；

行驶方向检测模块，包括加速度计、陀螺仪和磁力传感器，用于采集车辆的行驶方向信息；

GPS模块，用于获取车辆的GPS信息，得到车辆的绝对位置信息；

主控模块，用于获取车速检测模块和行驶方向检测模块采集到的信息，对其进行处理得到相对位置信息，并结合GPS模块的绝对位置信息进行数据融合，得到最终的汽车位置信息。

相对位置信息由车速大小以及车速方向推算得到，即由车辆的速度矢量进行积分运算得到；绝对位置信息由GPS模块直接获得。系统所需的速度矢量由车辆速度大小和行驶方向确定。在车速大小检测上，通过在单位时间间隔内对旋转光电编码器产生的脉冲进行计数，确定汽车的车速；在车辆行驶方向检测上，采用融合算法对车辆的俯仰角和航向角分别进行检测，从而确定车辆行驶方向。其中，俯仰角由陀螺仪与加速度计的检测数据进行数据融合获得；航向角由陀螺仪与磁力计的检测数据进行数据融合获得。

在系统运行时，实时将车辆的位置信息保存在存储芯片中，下次定位装置运行时，系统可以获取到之前车辆的位置坐标。当系统第一次启动时，其定位的初始位置由GPS模块测得，在以后的启动过程，系统自动判断车辆在系统关闭期间是否发生移动，并以此确定初始位置是GPS模块的测量坐标还是系统上次存储的位置坐标。

在相对位置信息与绝对位置信息的数据融合环节，将位置信息表示为X、Y轴的坐标量，将位置信息的融合环节分解为X、Y轴两个维度上的数据融合，在X、Y轴分别进行X坐标与Y坐标的融合。

本发明采用了多传感器融合的方案实现车辆的定位，通过光电编码器，陀螺仪以及加速度计计算出车辆的相对位置信息，并与GPS的绝对位置信息相融合。根据现有的技术规律，车辆自主定位得到的相对位置的精度远远高于GPS的精度，而GPS数据只起起到确定始坐标与修正漂移误差的作用，所占的比重较小，因此GPS的误差对最终融合数据精度影响也较小。综上，本技术方案的定位精度高于单纯使用GPS模块的定位精度。

在本发明的一个具体实施例中，以STM32f407微控制器作为处理器，一方面通过陀螺仪、加速度计、光电编码器确定车辆相对坐标，另一方面通过GPS定位模块确定车辆绝对坐标，并将二者通过数融合算法得到更为精确地车辆位置坐标。

本系统定义X、Y两个坐标轴，其中沿纬度线E方向为X轴正方向，沿经度线N方向为Y轴正方向。

记θ₁为车辆俯仰，θ₂为车辆航向角，其中，俯仰角定义为车辆行驶方向与水平面之间的夹角；航向角定义为车辆行驶方向在水平面上的投影与纬度线E方向的夹角。

如图2(a)和图2(b)所示，为俯仰角与航向角的计算框图。系统运算所需的俯仰角θ₁与航向角θ₂均由融合算法得到，其中，俯仰角由加速度计测得的角度与陀螺仪测得的角速度计算得到；航向角由磁力计测得的方向角与陀螺仪测得的角速度计算得到。

已知处理器以周期T循环运算俯仰角，记ω_H0为陀螺仪所测量的车辆在从垂直方向角速度，θ_H0为加速度计所测量的车辆行驶方向与水平面夹角，则在τ时刻航向角θ₂(τ)运算公式为:

θ₂(τ)＝θ₂(τ-T)+k_p*[(θ_H0(τ)-θ₂(τ-T))+ω_H0(τ)]；

其中，θ₂(0)＝θ_H0(0)；

接上述技术方案，同样地，已知处理器以周期T循环运算航向角，记ω_L0为陀螺仪测量的车辆在水平方向角速度，θ_L0为磁力计测量的车辆与Y轴在水平面上的夹角，则在τ时刻俯仰角θ₁(τ)运算公式为:

θ₁(τ)＝θ₁(τ-T)+k_y*[(θ_L0(τ)-θ₂(τ-T))+ω_L0(τ)]；

其中，θ₁(0)＝θ_L0(0)；

车辆的速度大小由与传动轴连接的光电编码器测定。处理器通过正交解码读取单位时间内光电编码器输出的脉冲数得到传动轴的角速度，进而得到汽车的车速。

车辆在水平方向上的速度矢量由X轴速度Vx以及Y轴速度Vy合成。其中，Vx＝V*COSθ₁*COSθ₂,Vy＝V COSθ₁*SINθ₂。

系统将实时坐标数据通过IIC通信存储到芯片24C02中。当系统再次开始运行，处理器可以获取到车辆之前的位置坐标。

如图3所示，系统通过融合算法将各个传感器所得到的数据进行融合，从而得到车辆的最终坐标。图3为系统由Vx，Vy以及GPS定位模块的坐标数据经过数据融合获得最终位置坐标的计算框图。X_i、Y_i为系统启动时的初始坐标，X_G，YG为GPS定位模块获取的坐标。对Vx，Vy分别积分得到X轴与Y轴的相对位移，该位移与初始坐标相加，并经过GPS所获取的绝对坐标的反馈校正，可以获得较为精准的坐标。其中已知处理器位置坐标运算周期为T，则在τ时刻位置坐标(X(τ),Y(τ))的运算公式为:

ΔX(τ)＝ΔX(τ-T)+k_x*(X_G(τ)-X(τ-T)+V_x(τ))；

ΔY(τ)＝ΔY(τ-T)+k_Y*(Y_G(τ)-Y(τ-T)+V_Y(τ))；

(X(τ),Y(τ))＝(X_i+ΔX(τ),Y_i+ΔY(τ))；

其中，(X(0),Y(0))＝(X_i,Y_i)；

缺少GPS信号时，系统仅通过丢失信号前的初始位置以及车辆相对坐标推算车辆位置坐标。当GPS定位模块信号恢复时，系统自动恢复GPS绝对坐标反馈校正环节。

系统第一次启动运行时，系统的初始坐标为GPS定位模块所获取的坐标。系统再次启动时，处理器对当前系GPS获取坐标与存储芯片的存储坐标进行比较，当二者的差距小于某一阈值时，系统将存储坐标作为初始坐标，否则系统将当前GPS获取坐标作为初始坐标。

如图4所示，本发明实施例的基于多传感器融合算法的汽车定位方法，用于实现本发明实施例的基于多传感器融合算法的汽车定位系统，包括以下步骤：

S1、获取旋转光电编码器的数据，并对其进行处理得到车辆瞬时速度信息；获取加速度计、陀螺仪和磁力传感器的数据，并对其进行处理得到车辆的行驶方向信息；

获取瞬时速度信息的方法具体为：通过在单位时间间隔内对旋转光电编码器产生的脉冲进行计数，进而计算得到车辆的瞬时速度信息。

获取行驶方向信息的具体方法为：

通过陀螺仪与加速度计的检测数据进行数据融合得到俯仰角，通过陀螺仪与磁力计的检测数据进行数据融合得到航向角，根据航向角和俯仰角得到车辆的行驶方向信息。

S2、获取GPS信息作为绝对位置信息，根据瞬时速度信息和行驶方向信息得到车辆的速度矢量，并对其进行积分运算得到相对位置信息；

S3、对相对位置信息和绝对位置信息进行数据融合，将其分别表示为X、Y轴的分量，对相对速度的X、Y轴分量进行积分得到相对位移的X、Y轴分量，将其经过GPS信息中绝对位置信息的反馈校正，得到车辆的精确位置。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (1)

1.一种基于多传感器融合算法的汽车定位方法，其特征在于，该方法通过基于多传感器融合算法的汽车定位系统实现，该系统包括主控模块，以及与主控模块均相连的车速检测模块、行驶方向检测模块和GPS模块；

所述车速检测模块，包括与汽车传动轴相连的旋转光电编码器，用于采集车辆行驶的瞬时速度信息；

所述行驶方向检测模块，包括加速度计、陀螺仪和磁力传感器，用于采集车辆的行驶方向信息；

所述GPS模块，用于获取车辆的GPS信息，得到车辆的绝对位置信息；

所述主控模块，用于获取车速检测模块和行驶方向检测模块采集到的信息，对其进行处理得到相对位置信息，并结合GPS模块的绝对位置信息进行数据融合，得到最终的汽车位置信息；

该系统还包括与主控模块相连的存储模块；

该方法包括以下步骤：

S1、获取旋转光电编码器的数据，并对其进行处理得到车辆瞬时速度信息；获取加速度计、陀螺仪和磁力传感器的数据，并对其进行处理得到车辆的行驶方向信息；

S2、获取GPS信息作为绝对位置信息，根据瞬时速度信息和行驶方向信息得到车辆的速度矢量，并对其进行积分运算得到相对位置信息；

S3、对相对位置信息和绝对位置信息进行数据融合，将其分别表示为X、Y轴的分量，对相对速度的X、Y轴分量进行积分得到相对位移的X、Y轴分量，将其经过GPS信息中绝对位置信息的反馈校正，得到车辆的精确位置；

步骤S1中获取瞬时速度信息的方法具体为：通过在单位时间间隔内对旋转光电编码器产生的脉冲进行计数，进而计算得到车辆的瞬时速度信息；

步骤S1中获取行驶方向信息的具体方法为：

通过陀螺仪与加速度计的检测数据进行数据融合得到俯仰角，通过陀螺仪与磁力计的检测数据进行数据融合得到航向角，根据航向角和俯仰角得到车辆的行驶方向信息；

该方法还包括当缺少GPS信号时，通过丢失信号前的初始位置以及车辆相对位置信息推算车辆位置坐标，当GPS信号恢复时，系统自动恢复GPS绝对坐标反馈校正环节；

该方法还包括系统第一次启动时，其定位的初始位置由GPS模块测得，在以后的启动过程，系统自动判断车辆在系统关闭期间是否发生移动，并以此确定初始位置是GPS模块的测量坐标还是系统上次存储的位置坐标；

通过融合算法得到车辆的精确位置的方法具体为：

定义X、Y两个坐标轴，其中沿纬度线E方向为X轴正方向，沿经度线N方向为Y轴正方向；

记θ₁为车辆俯仰，θ₂为车辆航向角，其中，俯仰角定义为车辆行驶方向与水平面之间的夹角；航向角定义为车辆行驶方向在水平面上的投影与纬度线E方向的夹角；

已知处理器以周期T循环运算俯仰角，记ω_H0为陀螺仪所测量的车辆在从垂直方向角速度，θ_H0为加速度计所测量的车辆行驶方向与水平面夹角，则在τ时刻航向角θ₂(τ)运算公式为:

θ₂(τ)＝θ₂(τ-T)+k_p*[(θ_H0(τ)-θ₂(τ-T))+ω_H0(τ)]；

其中，θ₂(0)＝θ_H0(0)；

已知处理器以周期T循环运算航向角，记ω_L0为陀螺仪测量的车辆在水平方向角速度，θ_L0为磁力计测量的车辆与Y轴在水平面上的夹角，则在τ时刻俯仰角θ₁(τ)运算公式为:

θ₁(τ)＝θ₁(τ-T)+k_y*[(θ_L0(τ)-θ₂(τ-T))+ω_L0(τ)]；

其中，θ₁(0)＝θ_L0(0)；

车辆的速度大小由与传动轴连接的光电编码器测定；处理器通过正交解码读取单位时间内光电编码器输出的脉冲数得到传动轴的角速度，进而得到汽车的车速V；

车辆在水平方向上的速度矢量由X轴速度Vx以及Y轴速度Vy合成；其中，Vx＝V*COSθ₁*COSθ₂,Vy＝V COSθ₁*SINθ₂；

Xi、Yi为系统启动时的初始坐标，XG，YG为GPS定位模块获取的坐标；对Vx，Vy分别积分得到X轴与Y轴的相对位移，该位移与初始坐标相加，并经过GPS所获取的绝对坐标的反馈校正，获得精准的坐标；其中已知处理器位置坐标运算周期为T，则在τ时刻位置坐标(X(τ),Y(τ))的运算公式为:

ΔX(τ)＝ΔX(τ-T)+k_x*(X_G(τ)-X(τ-T)+V_x(τ))；

ΔY(τ)＝ΔY(τ-T)+k_Y*(Y_G(τ)-Y(τ-T)+V_Y(τ))；

(X(τ),Y(τ))＝(X_i+ΔX(τ),Y_i+ΔY(τ))；

其中，(X(0),Y(0))＝(X_i,Y_i)。