由里程计获得车辆经纬度的航位推算导航定位方法及系统的制作方法
【专利摘要】本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，包括以下步骤：(1)差分里程计获取的行驶距离，获得车辆的行驶速度与航向角；(2)根据行驶速度与航向角推算车辆的经纬度；(3)输出行驶速度、航向角以及经纬度。本发明提供的由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算系统的输出，不仅仅是在平面直角坐标系下的坐标，从而实现直接给出车辆经纬度的航位推算导航定位系统；无需使用陀螺仪、加速度计等惯性器件，使得实现的直接给出车辆经纬度的航位推算导航定位系统成本较低。本发明还提供一种航位推算导航定位系统。
【专利说明】由里程计获得车辆经纬度的航位推算导航定位方法及系统
【技术领域】
[0001] 本发明属于航位推算导航定位领域，具体涉及一种由里程计获得车辆经纬度的航 位推算导航定位方法及航位推算导航定位系统。
【背景技术】
[0002] 目前，通常的车载航位推算导航定位方法有一个里程计加一个单轴陀螺仪，一个 单轴陀螺仪加一个单轴加速度计，两个里程计差分三种方案。其中，前两种方案都涉及到惯 性传感器，而目前可用于车辆导航的惯性传感器价格昂贵，且相应数据处理方法复杂，不适 用于车载航位推算导航定位系统。另外，不论哪一种航位推算导航定位系统方案，现有的 航位推算导航定位方法都只能给出平面直角坐标系下的车辆坐标，无法直接提供经纬度信 肩、。
【发明内容】
[0003] 本发明的目的在于解决现有技术中的不足，提供一种由里程计获得车辆经纬度的 航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，从而实现直接给出 车辆经纬度的航位推算导航定位系统。
[0004] 本发明还提供一种采用由里程计获得车辆经纬度的航位推算导航定位方法的航 位推算导航定位系统。
[0005] 本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，包括以下步 骤：
[0006] (1)差分里程计获取的行驶距离，获得车辆的行驶速度与航向角；
[0007] (2)根据行驶速度与航向角推算车辆的经纬度；
[0008] (3)输出行驶速度、航向角以及经纬度。
[0009] 本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里 程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算系统 的输出，从而实现直接给出车辆经纬度的航位推算导航定位系统。
[0010] 进一步地，步骤（1)差分里程计获取的行驶距离，获得车辆的行驶速度与偏航角 度变化包括以下步骤：
[0011] (11)获取一个计算周期内车辆的左后轮的行驶距离与右后轮的行驶距离；
[0012] (12)差分左后轮的行驶距离与右后轮的行驶距离，计算车辆在当前计算周期内的 行驶速度；
[0013] (13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角。
[0014] 进一步地，步骤（11)获取一个计算周期内车辆的左后轮的行驶距离与右后轮的 行驶距离的方法为：
[0015] AL = TicLX d,
[0016] AR = TicRX d,
[0017] 其中，AL为车辆的左后轮在一个计算周期内的行驶距离，AR为车辆的右后轮在 一个计算周期内的行驶距离，TicL为一个计算周期内车辆的左后轮里程计输出的脉冲数， TicR为一个计算周期内车辆的右后轮里程计输出的脉冲数，d为一个脉冲数对应的行驶距 离。
[0018] 进一步地，步骤（12)差分左后轮的行驶距离与右后轮的行驶距离，计算车辆在当 前计算周期内的行驶速度的方法为：
[0019] (121)计算在当前计算周期内车辆的行驶距离AD :
[0020] AD= (AL+AR) ^-2
[0021] (122)计算车辆在当前计算周期内的行驶速度V :
[0022] V = AD+ At
[0023] 其中，Λ t是计算周期的时间间隔。
[0024] 进一步地，（13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角的 方法为：
[0025] (131)计算当前计算周期的偏航角度变化ω :
[0026] ω = (AR-AL)/Ienth
[0027] 其中，Ienth是车辆左后轮与右后轮的间距；
[0028] (132)计算当前时刻的航向角：
[0029] Hd(t) = Hd(t_l) + c〇
[0030] 其中，Hd(t)为车辆当前时刻的航向角，Hd(t-l)为车辆上一时刻的航向角。
[0031] 这里上一时刻与当前时刻的时间间隔为一个计算周期。
[0032] 进一步地，步骤（2)根据行驶速度与偏航角度变化推算车辆的经纬度包括以下步 骤：
[0033] (21)计算导航坐标系下车辆的速度矢量；
[0034] (22)计算导航坐标系相对地球坐标系的旋转矢量在导航坐标系下的表示；
[0035] (23)计算导航坐标系到地球坐标系的方向余弦矩阵；
[0036] (24)从方向余弦矩阵中提取车辆当前时刻的经纬度。
[0037] 进一步地，步骤（21)计算导航坐标系下车辆的速度矢量的方法为：
[0038] vN = [v · cos (Hd) v · sin (Hd) 0]τ
[0039] 其中，/为导航坐标系N下的速度矢量，导航坐标系N采用东北天坐标系，/为包 含三个元素的列向量，元素分别表示东向速度，北向速度和天向速度，由于车辆只在路面上 行驶，天向速度默认为〇, V是车辆在当前计算周期内的行驶速度，Hd是车辆当前时刻的航 向角。
[0040] 进一步地，步骤（22)计算导航坐标系相对地球坐标系的旋转矢量在导航坐标系 下的表示，包括以下步骤：
[0041] (221)计算3*3的地球曲率矩阵:
【权利要求】
1. 一种由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，包括以下步 骤： (1) 差分里程计获取的行驶距离，获得所述车辆的行驶速度与航向角； (2) 根据所述行驶速度与所述航向角推算所述车辆的经纬度； (3) 输出所述行驶速度、所述航向角以及所述经纬度。
2. 如权利要求1所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，步骤（1)差分里程计获取的行驶距离，获得所述车辆的行驶速度与偏航角度变化包括 以下步骤： (11) 获取一个计算周期内所述车辆的左后轮的行驶距离与右后轮的行驶距离； (12) 差分所述左后轮的行驶距离与所述右后轮的行驶距离，计算所述车辆在当前计算 周期内的行驶速度； (13) 计算当前计算周期的偏航角度变化，并计算当前时刻的航向角。
3. 如权利要求2所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，步骤（12)差分所述左后轮的行驶距离与所述右后轮的行驶距离，计算所述车辆在当前 计算周期内的行驶速度的方法为： (121) 计算在当前计算周期内所述车辆的行驶距离AD: AD= (AL+AR) +2 其中，AL为所述车辆的左后轮在一个计算周期内的行驶距离，AR为所述车辆的右后 轮在一个计算周期内的行驶距离； (122) 计算所述车辆在当前计算周期内的行驶速度V: V=ΔD+Δt 其中，At是计算周期的时间间隔。
4. 如权利要求3所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，（13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角的方法为： (131) 计算当前计算周期的偏航角度变化ω: ω= (AR-AL)/Ienth 其中，Ienth是车辆左后轮与右后轮的间距； (132) 计算当前时刻的航向角： Hd(t) =Hd(t-Ι) +ω 其中，Hd(t)为车辆当前时刻的航向角，Hd(t-l)为车辆上一时刻的航向角。
5. 如权利要求1所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征 在于，步骤（2)根据所述行驶速度与所述偏航角度变化推算所述车辆的经纬度包括以下步 骤： (21) 计算导航坐标系下所述车辆的速度矢量； (22) 计算所述导航坐标系相对地球坐标系的旋转矢量在所述导航坐标系下的表示； (23) 计算所述导航坐标系到所述地球坐标系的方向余弦矩阵； (24) 从所述方向余弦矩阵中提取所述车辆当前时刻的经纬度。
6. 如权利要求5所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，步骤（21)计算导航坐标系下所述车辆的速度矢量的方法为： vN =[v·cos(Hd)v·sin(Hd) 0]T 其中，/为导航坐标系N下的速度矢量，导航坐标系N采用东北天坐标系，/为包含三 个元素的列向量，所述元素分别表示东向速度，北向速度和天向速度，由于车辆只在路面上 行驶，所述天向速度默认为0,V是车辆在当前计算周期内的行驶速度，Hd是车辆当前时刻 的航向角。
7. 如权利要求6所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，步骤（22)计算所述导航坐标系相对地球坐标系的旋转矢量在所述导航坐标系下的表 示，包括以下步骤： (221) 计算3*3的地球曲率矩阵if:
其中，
其中，rl为车辆所处位置的曲率半径,D2j为导航坐标系N到地球坐标系E的方向余弦 矩阵G第二行的对应元素， feh=fe·fh， 其中，
其中，h为海拔高度，e为地球椭圆率，Rsi为从地球中心到车辆所处位置的修正距离。 (222) 由/计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示 ·· ^en(^c(4vX 其中，wfV3(t-l)为上一时刻wl的第三个元素，为导航坐标系N下天向单位向量， ?M〇Mro
8. 如权利要求7所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，步骤（23)计算所述导航坐标系到所述地球坐标系的方向余弦矩阵的方法为： Cn~CL<：' 其中，I为当前时刻的,C^1为上一时刻的G为导航坐标系N到地球坐标系E的方向余弦矩阵，Cf1为导航坐标系N相对地球坐标系E在两个时刻自身变化的方向余弦 矩阵，其计算方法为对的积分进行泰勒展开： 其中，?*j,
O
9. 如权利要求8所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在 于，步骤（24)从所述方向余弦矩阵中提取车辆当前时刻的经纬度的方法为：
其中，1为纬度，L为经度,Dij为导航坐标系到地球坐标系的方向余弦矩阵的对应元 素。
10. -种采用如权利要求1-9所述的由里程计获得车辆经纬度的航位推算导航定位方 法的航位推算导航定位系统，其特征在于，所述航位推算导航定位系统包括： 左后轮里程计，用于提供车辆的左后轮的行驶距离； 右后轮里程计，用于提供所述车辆的右后轮的行驶距离； 航位推算导航定位处理模块，用于根据所述左后轮里程计提供的左后轮的行驶距离以 及所述右后轮里程计提供的右后轮的行驶距离，计算所述车辆的速度、航向角以及经纬度， 并输出所述速度、所述航向角以及所述经纬度； 所述航位推算导航定位处理模块分别与所述左后轮里程计、所述右后轮里程计连接。
【文档编号】G01C22/00GK104316059SQ201410619062
【公开日】2015年1月28日 申请日期:2014年11月5日 优先权日:2014年11月5日
【发明者】王励扬, 何文涛, 马成炎, 蔺晓龙, 翟昆朋, 胡晓峰, 王浩 申请人:中国科学院嘉兴微电子与系统工程中心