CN104316059B - 由里程计获得车辆经纬度的航位推算导航定位方法及系统 - Google Patents

由里程计获得车辆经纬度的航位推算导航定位方法及系统 Download PDF

Info

Publication number
CN104316059B
CN104316059B CN201410619062.0A CN201410619062A CN104316059B CN 104316059 B CN104316059 B CN 104316059B CN 201410619062 A CN201410619062 A CN 201410619062A CN 104316059 B CN104316059 B CN 104316059B
Authority
CN
China
Prior art keywords
mrow
msub
vehicle
coordinate system
latitude
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201410619062.0A
Other languages
English (en)
Other versions
CN104316059A (zh
Inventor
王励扬
何文涛
马成炎
蔺晓龙
翟昆朋
胡晓峰
王浩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HANGZHOU ZHONGKE MICROELECTRONICS CO Ltd
Original Assignee
JIAXING MICROELECTRONICS AND SYSTEMS ENGINEERING CENTER CHINESE ACADEMY OF SCIENCES
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by JIAXING MICROELECTRONICS AND SYSTEMS ENGINEERING CENTER CHINESE ACADEMY OF SCIENCES filed Critical JIAXING MICROELECTRONICS AND SYSTEMS ENGINEERING CENTER CHINESE ACADEMY OF SCIENCES
Priority to CN201410619062.0A priority Critical patent/CN104316059B/zh
Publication of CN104316059A publication Critical patent/CN104316059A/zh
Priority to PCT/CN2015/092675 priority patent/WO2016070723A1/zh
Application granted granted Critical
Publication of CN104316059B publication Critical patent/CN104316059B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/14—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by recording the course traversed by the object
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D3/00—Control of position or direction

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，包括以下步骤：(1)差分里程计获取的行驶距离，获得车辆的行驶速度与航向角；(2)根据行驶速度与航向角推算车辆的经纬度；(3)输出行驶速度、航向角以及经纬度。本发明提供的由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算系统的输出，不仅仅是在平面直角坐标系下的坐标，从而实现直接给出车辆经纬度的航位推算导航定位系统；无需使用陀螺仪、加速度计等惯性器件，使得实现的直接给出车辆经纬度的航位推算导航定位系统成本较低。本发明还提供一种航位推算导航定位系统。

Description

由里程计获得车辆经纬度的航位推算导航定位方法及系统
技术领域
本发明属于航位推算导航定位领域，具体涉及一种由里程计获得车辆经纬度的航位推算导航定位方法及航位推算导航定位系统。
背景技术
目前，通常的车载航位推算导航定位方法有一个里程计加一个单轴陀螺仪，一个单轴陀螺仪加一个单轴加速度计，两个里程计差分三种方案。其中，前两种方案都涉及到惯性传感器，而目前可用于车辆导航的惯性传感器价格昂贵，且相应数据处理方法复杂，不适用于车载航位推算导航定位系统。另外，不论哪一种航位推算导航定位系统方案，现有的航位推算导航定位方法都只能给出平面直角坐标系下的车辆坐标，无法直接提供经纬度信息。
发明内容
本发明的目的在于解决现有技术中的不足，提供一种由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，从而实现直接给出车辆经纬度的航位推算导航定位系统。
本发明还提供一种采用由里程计获得车辆经纬度的航位推算导航定位方法的航位推算导航定位系统。
本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，包括以下步骤：
(1)差分里程计获取的行驶距离，获得车辆的行驶速度与航向角；
(2)根据行驶速度与航向角推算车辆的经纬度；
(3)输出行驶速度、航向角以及经纬度。
本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算系统的输出，从而实现直接给出车辆经纬度的航位推算导航定位系统。
进一步地，步骤(1)差分里程计获取的行驶距离，获得车辆的行驶速度与偏航角度变化包括以下步骤：
(11)获取一个计算周期内车辆的左后轮的行驶距离与右后轮的行驶距离；
(12)差分左后轮的行驶距离与右后轮的行驶距离，计算车辆在当前计算周期内的行驶速度；
(13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角。
进一步地，步骤(11)获取一个计算周期内车辆的左后轮的行驶距离与右后轮的行驶距离的方法为：
ΔL＝TicL×d，
ΔR＝TicR×d，
其中，ΔL为车辆的左后轮在一个计算周期内的行驶距离，ΔR为车辆的右后轮在一个计算周期内的行驶距离，TicL为一个计算周期内车辆的左后轮里程计输出的脉冲数，TicR为一个计算周期内车辆的右后轮里程计输出的脉冲数，d为一个脉冲数对应的行驶距离。
进一步地，步骤(12)差分左后轮的行驶距离与右后轮的行驶距离，计算车辆在当前计算周期内的行驶速度的方法为：
(121)计算在当前计算周期内车辆的行驶距离ΔD：
ΔD＝(ΔL+ΔR)÷2
(122)计算车辆在当前计算周期内的行驶速度v：
v＝ΔD÷Δt
其中，Δt是计算周期的时间间隔。
进一步地，(13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角的方法为：
(131)计算当前计算周期的偏航角度变化ω：
ω＝(ΔR-ΔL)/lenth
其中，lenth是车辆左后轮与右后轮的间距；
(132)计算当前时刻的航向角：
Hd(t)＝Hd(t-1)+ω
其中，Hd(t)为车辆当前时刻的航向角，Hd(t-1)为车辆上一时刻的航向角。
这里上一时刻与当前时刻的时间间隔为一个计算周期。
进一步地，步骤(2)根据行驶速度与航向角推算车辆的经纬度包括以下步骤：
(21)计算导航坐标系下车辆的速度矢量；
(22)计算导航坐标系相对地球坐标系的旋转矢量在导航坐标系下的表示；
(23)计算导航坐标系到地球坐标系的方向余弦矩阵；
(24)从方向余弦矩阵中提取车辆当前时刻的经纬度。
进一步地，步骤(21)计算导航坐标系下车辆的速度矢量的方法为：
vN＝[v·cos(Hd) v·sin(Hd) 0]T
其中，vN为导航坐标系N下的速度矢量，导航坐标系N采用东北天坐标系，vN为包含三个元素的列向量，元素分别表示东向速度，北向速度和天向速度，由于车辆只在路面上行驶，天向速度默认为0，v是车辆在当前计算周期内的行驶速度，Hd是车辆当前时刻的航向角。
进一步地，步骤(22)计算导航坐标系相对地球坐标系的旋转矢量在导航坐标系下的表示，包括以下步骤：
(221)计算3*3的地球曲率矩阵
其中，
其中，rl为车辆所处位置的曲率半径，D2j为导航坐标系N到地球坐标系E的方向余弦矩阵第二行的对应元素，
feh＝fe·fh，
其中，
其中，h为海拔高度，e为地球椭圆率，Rs′为从地球中心到车辆所处位置的修正距离。
(222)由vN计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示
其中，为上一时刻的第三个元素，为导航坐标系N下天向单位向量，
进一步地，步骤(23)计算导航坐标系到地球坐标系的方向余弦矩阵的方法为：
其中，为当前时刻的为上一时刻的为导航坐标系N到地球坐标系E的方向余弦矩阵，为导航坐标系N相对地球坐标系E在两个时刻自身变化的方向余弦矩阵，其计算方法为对的积分进行泰勒展开：
其中，
进一步地，步骤(24)从方向余弦矩阵中提取车辆当前时刻的经纬度的方法为：
其中，l为纬度，L为经度，Dij为导航坐标系到地球坐标系的方向余弦矩阵的对应元素。
本发明还提供一种采用由里程计获得车辆经纬度的航位推算导航定位方法的航位推算导航定位系统，包括：
左后轮里程计，用于提供车辆的左后轮的行驶距离；
右后轮里程计，用于提供车辆的右后轮的行驶距离；
航位推算导航定位处理模块，用于根据左后轮里程计提供的左后轮的行驶距离以及右后轮里程计提供的右后轮的行驶距离，计算车辆的速度、航向角以及经纬度，并输出速度、航向角以及经纬度；
航位推算导航定位处理模块分别与左后轮里程计、右后轮里程计连接。
与现有技术相比，本发明提供的由里程计获得车辆经纬度的航位推算导航定位方法及航位推算导航定位系统，具有以下有益效果：
(1)仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算导航定位系统的输出，不仅仅是在平面直角坐标系下的坐标，从而实现直接给出车辆经纬度的航位推算导航定位系统；
(2)无需使用陀螺仪、加速度计等惯性器件，使得实现的直接给出车辆经纬度的航位推算导航定位系统成本较低。
附图说明
图1是本发明的一个实施例的由里程计获得车辆经纬度的航位推算导航定位方法的流程图；
图2是采用图1所示的方法实现的航位推算导航定位系统框图；
图3是采用图1所示的方法实现的航位推算导航定位系统的仿真结果。
具体实施方式
图1示出了本发明的一个实施例的由里程计获得车辆经纬度的航位推算导航定位方法的流程图；图2示出了采用图1所示的方法实现的航位推算导航定位系统框图，该系统包括左后轮里程计、右后轮里程计与微处理器，左后轮里程计、右后轮里程计通过外部中断1、外部中断2将左后轮里程计、右后轮里程计的脉冲发给微处理器。
如图1所示，本发明的一个实施例的由里程计获得车辆经纬度的航位推算导航定位方法，包括以下步骤：
步骤一、上一时刻车辆经纬度，航向角和行驶速度；
步骤二、由里程计数据求得当前车辆行驶速度及航向角；
(1)获取一个计算周期内车辆的左后轮的行驶距离与右后轮的行驶距离；
ΔL＝TicL×d，
ΔR＝TicR×d，
其中，ΔL为车辆的左后轮在一个计算周期内的行驶距离，ΔR为车辆的右后轮在一个计算周期内的行驶距离，TicL为一个计算周期内车辆的左后轮里程计输出的脉冲数，TicR为一个计算周期内车辆的右后轮里程计输出的脉冲数，d为一个脉冲数对应的行驶距离。
(2)差分左后轮的行驶距离与右后轮的行驶距离，计算车辆在当前计算周期内的行驶速度；
(a)计算在当前计算周期内车辆的行驶距离ΔD：
ΔD＝(ΔL+ΔR)÷2
(b)计算车辆在当前计算周期内的行驶速度v：
v＝ΔD÷Δt
其中，Δt是计算周期的时间间隔。
(3)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角。
(a)计算当前计算周期的偏航角度变化ω：
ω＝(ΔR-ΔL)/lenth
其中，lenth是车辆左后轮与右后轮的间距；
(a)计算当前时刻的航向角：
Hd(t)＝Hd(t-1)+ω
其中，Hd(t)为车辆当前时刻的航向角，Hd(t-1)为车辆上一时刻的航向角。
这里上一时刻与当前时刻的时间间隔为一个计算周期。
步骤三、计算导航坐标系下车辆的速度矢量
vN＝[v·cos(Hd) v·sin(Hd) 0]T
其中，vN为导航坐标系N下的速度矢量，导航坐标系N采用东北天坐标系，vN为包含三个元素的列向量，元素分别表示东向速度，北向速度和天向速度，由于车辆只在路面上行驶，天向速度默认为0，v是车辆在当前计算周期内的行驶速度，Hd是车辆当前时刻的航向角。
步骤四、计算导航坐标系相对地球坐标系的旋转矢量，在导航坐标系下的表示：
(1)计算3*3的地球曲率矩阵
其中，
其中，rl为车辆所处位置的曲率半径，D2j为导航坐标系N到地球坐标系E的方向余弦矩阵第二行的对应元素，
feh＝fe·fh，
其中，
其中，h为海拔高度，e为地球椭圆率，Rs′为从地球中心到车辆所处位置的修正距离。
(2)由vN计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示
其中，为上一时刻的第三个元素，为导航坐标系N下天向单位向量，
步骤五、计算由导航坐标系到地球坐标系的方向余弦矩阵：
其中，为当前时刻的为上一时刻的为导航坐标系N到地球坐标系E的方向余弦矩阵，为导航坐标系N相对地球坐标系E在两个时刻自身变化的方向余弦矩阵，其计算方法为对的积分进行泰勒展开：
其中，
步骤六、从该方向余弦矩阵中提取车辆当前时刻的经纬度：
其中，l为纬度，L为经度，Dij为导航坐标系到地球坐标系的方向余弦矩阵的对应元素。
步骤七、输出当前时刻的行驶速度、航向角以及经纬度。
本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算导航定位系统的输出，从而实现直接给出车辆经纬度的航位推算导航定位系统。
以下是采用图1所示的方法实现的航位推算导航定位系统的仿真结果：
使用Spirent公司的模拟器，编辑一段车辆行驶轨迹。将车辆的起始点置于东经40度，0纬度的位置，以三十米每秒匀速向西行驶。
模拟器可以同时输出车辆每一时刻标准的经纬度，同时输出模拟两个里程计的脉冲信号。使用该模拟里程计信号，运行上述步骤的程序，得到车辆每一时刻的经纬度信息。同时将航位推算导航定位信息和标准车辆位置信息绘制与同一张图上，如图3所示，可以看出该方法可以有效计算车辆的经纬度位置信息。
本发明提供的由里程计获得车辆经纬度的航位推算导航定位方法及航位推算导航定位系统，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算导航定位系统的输出，不仅仅是在平面直角坐标系下的坐标，从而实现直接给出车辆经纬度的航位推算导航定位系统；无需使用陀螺仪、加速度计等惯性器件，使得实现的直接给出车辆经纬度的航位推算导航定位系统成本较低。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域的技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (8)

1.一种由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，包括以下步骤：
(1)差分里程计获取的行驶距离，获得所述车辆的行驶速度与航向角；
(2)根据所述行驶速度与所述航向角推算所述车辆的经纬度；
(3)输出所述行驶速度、所述航向角以及所述经纬度；
其中，步骤(2)根据所述行驶速度与所述航向角推算所述车辆的经纬度包括以下步骤：
(21)计算导航坐标系下所述车辆的速度矢量；
(22)计算所述导航坐标系相对地球坐标系的旋转矢量在所述导航坐标系下的表示；
(23)计算所述导航坐标系到所述地球坐标系的方向余弦矩阵；
(24)从所述方向余弦矩阵中提取所述车辆当前时刻的经纬度。
2.如权利要求1所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(1)差分里程计获取的行驶距离，获得所述车辆的行驶速度与偏航角度变化包括以下步骤：
(11)获取一个计算周期内所述车辆的左后轮的行驶距离与右后轮的行驶距离；
(12)差分所述左后轮的行驶距离与所述右后轮的行驶距离，计算所述车辆在当前计算周期内的行驶速度；
(13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角。
3.如权利要求2所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(12)差分所述左后轮的行驶距离与所述右后轮的行驶距离，计算所述车辆在当前计算周期内的行驶速度的方法为：
(121)计算在当前计算周期内所述车辆的行驶距离ΔD：
ΔD＝(ΔL+ΔR)÷2
其中，ΔL为所述车辆的左后轮在一个计算周期内的行驶距离，ΔR为所述车辆的右后轮在一个计算周期内的行驶距离；
(122)计算所述车辆在当前计算周期内的行驶速度v：
v＝ΔD÷Δt
其中，Δt是计算周期的时间间隔。
4.如权利要求3所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，(13)计算当前计算周期的偏航角度变化，并计算当前时刻的航向角的方法为：
(131)计算当前计算周期的偏航角度变化ω：
ω＝(ΔR-ΔL)/lenth
其中，lenth是车辆左后轮与右后轮的间距；
(132)计算当前时刻的航向角：
Hd(t)＝Hd(t-1)+ω
其中，Hd(t)为车辆当前时刻的航向角，Hd(t-1)为车辆上一时刻的航向角。
5.如权利要求1所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(21)计算导航坐标系下所述车辆的速度矢量的方法为：
vN＝[v·cos(Hd) v·sin(Hd) 0]T
其中，vN为导航坐标系N下的速度矢量，导航坐标系N采用东北天坐标系，vN为包含三个元素的列向量，所述元素分别表示东向速度，北向速度和天向速度，由于车辆只在路面上行驶，所述天向速度默认为0，v是车辆在当前计算周期内的行驶速度，Hd是车辆当前时刻的航向角。
6.如权利要求5所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(22)计算所述导航坐标系相对地球坐标系的旋转矢量在所述导航坐标系下的表示，包括以下步骤：
(221)计算3*3的地球曲率矩阵
<mrow> <msubsup> <mi>F</mi> <mi>C</mi> <mi>N</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>Fc</mi> <mn>11</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>Fc</mi> <mn>12</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Fc</mi> <mn>21</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>Fc</mi> <mn>22</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中，
<mrow> <msub> <mi>Fc</mi> <mn>11</mn> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mi>r</mi> <mi>l</mi> </mrow> </mfrac> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <msup> <msub> <mi>D</mi> <mn>21</mn> </msub> <mn>2</mn> </msup> <mi>f</mi> <mi>e</mi> <mi>h</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>Fc</mi> <mn>12</mn> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mi>r</mi> <mi>l</mi> </mrow> </mfrac> <msub> <mi>D</mi> <mn>21</mn> </msub> <msub> <mi>D</mi> <mn>22</mn> </msub> <mi>f</mi> <mi>e</mi> <mi>h</mi> </mrow>
<mrow> <msub> <mi>Fc</mi> <mn>21</mn> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mi>r</mi> <mi>l</mi> </mrow> </mfrac> <msub> <mi>D</mi> <mn>21</mn> </msub> <msub> <mi>D</mi> <mn>22</mn> </msub> <mi>f</mi> <mi>e</mi> <mi>h</mi> </mrow>
<mrow> <msub> <mi>Fc</mi> <mn>22</mn> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mi>r</mi> <mi>l</mi> </mrow> </mfrac> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <msup> <msub> <mi>D</mi> <mn>22</mn> </msub> <mn>2</mn> </msup> <mi>f</mi> <mi>e</mi> <mi>h</mi> <mo>)</mo> </mrow> </mrow>
其中，rl为车辆所处位置的曲率半径，D2j为导航坐标系N到地球坐标系E的方向余弦矩阵第二行的对应元素，
feh＝fe·fh，
其中，
<mrow> <mi>f</mi> <mi>e</mi> <mo>=</mo> <mfrac> <mrow> <msup> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mi>e</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <msup> <msub> <mi>D</mi> <mn>23</mn> </msub> <mn>2</mn> </msup> <mo>&amp;lsqb;</mo> <msup> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mi>e</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mrow> </mfrac> </mrow>
<mrow> <mi>f</mi> <mi>h</mi> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>1</mn> <mo>+</mo> <mi>h</mi> <mo>/</mo> <msup> <mi>Rs</mi> <mo>&amp;prime;</mo> </msup> </mrow> </mfrac> </mrow>
其中，h为海拔高度，e为地球椭圆率，Rs′为从地球中心到车辆所处位置的修正距离；
(222)由vN计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示
<mrow> <msubsup> <mi>w</mi> <mrow> <mi>E</mi> <mi>N</mi> </mrow> <mi>N</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>F</mi> <mi>C</mi> <mi>N</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>u</mi> <mrow> <mi>Z</mi> <mi>N</mi> </mrow> <mi>N</mi> </msubsup> <mo>&amp;times;</mo> <msup> <mi>v</mi> <mi>N</mi> </msup> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>w</mi> <mrow> <mi>E</mi> <mi>N</mi> <mn>3</mn> </mrow> <mi>N</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <msubsup> <mi>u</mi> <mrow> <mi>Z</mi> <mi>N</mi> </mrow> <mi>N</mi> </msubsup> </mrow>
其中，为上一时刻的第三个元素，为导航坐标系N下天向单位向量，
<mrow> <msubsup> <mi>u</mi> <mrow> <mi>Z</mi> <mi>N</mi> </mrow> <mi>N</mi> </msubsup> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>.</mo> </mrow>
7.如权利要求6所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(23)计算所述导航坐标系到所述地球坐标系的方向余弦矩阵的方法为：
<mrow> <msubsup> <mi>C</mi> <msub> <mi>N</mi> <mi>t</mi> </msub> <mi>E</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>N</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mi>E</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msubsup> <mi>C</mi> <msub> <mi>N</mi> <mi>t</mi> </msub> <msub> <mi>N</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msubsup> </mrow>
其中，为当前时刻的 为上一时刻的 为导航坐标系N到地球坐标系E的方向余弦矩阵，为导航坐标系N相对地球坐标系E在两个时刻自身变化的方向余弦矩阵，其计算方法为对的积分进行泰勒展开：
<mrow> <msubsup> <mi>C</mi> <msub> <mi>N</mi> <mi>t</mi> </msub> <msub> <mi>N</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msubsup> <mo>&amp;ap;</mo> <mi>I</mi> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>&amp;xi;</mi> <mi>t</mi> </msub> <mo>&amp;times;</mo> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>&amp;xi;</mi> <mi>t</mi> </msub> <mo>&amp;times;</mo> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>&amp;xi;</mi> <mi>t</mi> </msub> <mo>&amp;times;</mo> <mo>)</mo> </mrow> </mrow>
其中，
8.如权利要求7所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(24)从所述方向余弦矩阵中提取车辆当前时刻的经纬度的方法为：
<mrow> <mi>l</mi> <mo>=</mo> <msup> <mi>tan</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mfrac> <msub> <mi>D</mi> <mn>23</mn> </msub> <msqrt> <mrow> <msup> <msub> <mi>D</mi> <mn>21</mn> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>D</mi> <mn>22</mn> </msub> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>L</mi> <mo>=</mo> <msup> <mi>tan</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mfrac> <msub> <mi>D</mi> <mn>13</mn> </msub> <msub> <mi>D</mi> <mn>33</mn> </msub> </mfrac> <mo>)</mo> </mrow> </mrow>
其中，l为纬度，L为经度，Dij为导航坐标系到地球坐标系的方向余弦矩阵的对应元素。
CN201410619062.0A 2014-11-05 2014-11-05 由里程计获得车辆经纬度的航位推算导航定位方法及系统 Active CN104316059B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201410619062.0A CN104316059B (zh) 2014-11-05 2014-11-05 由里程计获得车辆经纬度的航位推算导航定位方法及系统
PCT/CN2015/092675 WO2016070723A1 (zh) 2014-11-05 2015-10-23 由里程计获得车辆经纬度的航位推算导航定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410619062.0A CN104316059B (zh) 2014-11-05 2014-11-05 由里程计获得车辆经纬度的航位推算导航定位方法及系统

Publications (2)

Publication Number Publication Date
CN104316059A CN104316059A (zh) 2015-01-28
CN104316059B true CN104316059B (zh) 2017-08-25

Family

ID=52371323

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410619062.0A Active CN104316059B (zh) 2014-11-05 2014-11-05 由里程计获得车辆经纬度的航位推算导航定位方法及系统

Country Status (2)

Country Link
CN (1) CN104316059B (zh)
WO (1) WO2016070723A1 (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104316059B (zh) * 2014-11-05 2017-08-25 中国科学院嘉兴微电子与系统工程中心 由里程计获得车辆经纬度的航位推算导航定位方法及系统
CN104977002A (zh) * 2015-06-12 2015-10-14 同济大学 基于sins/双od的惯性组合导航系统及其导航方法
CN105022404A (zh) * 2015-06-18 2015-11-04 北京矿冶研究总院 一种井下无人驾驶铲运机行驶定位系统及方法
CN106017486B (zh) * 2016-05-16 2018-11-09 浙江大学 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法
CN106289307B (zh) * 2016-08-03 2019-05-10 安徽协创物联网技术有限公司 一种基于卫星定位应用的车辆行驶里程算法的优化方法
CN106646569B (zh) * 2016-11-18 2020-04-14 华为技术有限公司 一种导航定位方法及设备
CN106886222B (zh) * 2017-03-29 2021-01-26 北京京东乾石科技有限公司 无人搬运车的控制方法和装置
CN107132563B (zh) * 2017-07-10 2020-04-24 北京理工大学 一种里程计结合双天线差分gnss的组合导航方法
CN108896049A (zh) * 2018-06-01 2018-11-27 重庆锐纳达自动化技术有限公司 一种机器人室内运动定位方法
CN108961337B (zh) * 2018-06-15 2020-11-24 深圳地平线机器人科技有限公司 车载相机航向角标定方法和装置、电子设备以及车辆
CN110077392B (zh) * 2019-03-28 2020-08-25 惠州市德赛西威汽车电子股份有限公司 一种自动泊车定位系统的航迹推算方法
CN114322978B (zh) * 2020-10-10 2024-03-15 广州汽车集团股份有限公司 一种车辆定位方法、计算机设备、计算机可读存储介质
CN112254724B (zh) * 2020-10-19 2023-03-14 天津津航计算技术研究所 一种提高惯导经纬度分辨率方法
CN113029201B (zh) * 2021-04-07 2023-04-07 北京布科思科技有限公司 双轮差速模型的里程校准方法和装置、设备及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6226591B1 (en) * 1998-09-24 2001-05-01 Denso Corporation Vehicle present position detection apparatus, vehicle present position display apparatus, navigation system and recording medium
CN1499324A (zh) * 2002-11-06 2004-05-26 中国科学院自动化研究所 一种机器人位置和姿态的推算定位方法及装置
CN1731091A (zh) * 2005-07-13 2006-02-08 李俊峰 车载快速定位定向方法
CN101576384A (zh) * 2009-06-18 2009-11-11 北京航空航天大学 一种基于视觉信息校正的室内移动机器人实时导航方法
CN103917850A (zh) * 2011-10-25 2014-07-09 中国人民解放军国防科学技术大学 一种惯性导航系统的运动对准方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2843906B2 (ja) * 1996-03-04 1999-01-06 防衛庁技術研究本部長 車両用慣性航法装置
CN202133404U (zh) * 2011-06-22 2012-02-01 河海大学 无卫星导航装置
US9677887B2 (en) * 2013-03-22 2017-06-13 Qualcomm Incorporated Estimating an initial position and navigation state using vehicle odometry
CN104316059B (zh) * 2014-11-05 2017-08-25 中国科学院嘉兴微电子与系统工程中心 由里程计获得车辆经纬度的航位推算导航定位方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6226591B1 (en) * 1998-09-24 2001-05-01 Denso Corporation Vehicle present position detection apparatus, vehicle present position display apparatus, navigation system and recording medium
CN1499324A (zh) * 2002-11-06 2004-05-26 中国科学院自动化研究所 一种机器人位置和姿态的推算定位方法及装置
CN1731091A (zh) * 2005-07-13 2006-02-08 李俊峰 车载快速定位定向方法
CN101576384A (zh) * 2009-06-18 2009-11-11 北京航空航天大学 一种基于视觉信息校正的室内移动机器人实时导航方法
CN103917850A (zh) * 2011-10-25 2014-07-09 中国人民解放军国防科学技术大学 一种惯性导航系统的运动对准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
GPS/DR车载组合导航定位系统研究;岳海波;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20100115(第01期);第42-45页 *
一种车辆航位推算改进方法;贾继超等;《中国惯性技术学报》;20090831;第17卷(第4期);第453-455页 *

Also Published As

Publication number Publication date
CN104316059A (zh) 2015-01-28
WO2016070723A1 (zh) 2016-05-12

Similar Documents

Publication Publication Date Title
CN104316059B (zh) 由里程计获得车辆经纬度的航位推算导航定位方法及系统
Karlsson et al. The future of automotive localization algorithms: Available, reliable, and scalable localization: Anywhere and anytime
CN110631593B (zh) 一种用于自动驾驶场景的多传感器融合定位方法
CN109931944B (zh) 一种ar导航方法、装置、车端设备、服务端及介质
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN104165641B (zh) 一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法
CN107421566B (zh) 一种无人车多源传感器信息仿真平台
CN101360971B (zh) 定位装置及导航系统
CN109946732A (zh) 一种基于多传感器数据融合的无人车定位方法
Barrios et al. Trajectory estimations using smartphones
CN106595715B (zh) 基于捷联惯导与卫星组合导航系统里程计标定方法及装置
Collin MEMS IMU carouseling for ground vehicles
CN103727941B (zh) 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN106842271B (zh) 导航定位方法及装置
CN104360366B (zh) 航位推算和全球定位系统的组合定位方法
CN106092121A (zh) 车辆导航方法和装置
CN105806358B (zh) 一种驾驶提示的方法及装置
Walter et al. Smartphone-based sensor fusion for improved vehicular navigation
Jung et al. Monocular visual-inertial-wheel odometry using low-grade IMU in urban areas
CN103759733B (zh) 基于联邦滤波的ins/vkm/vdm车载导航系统
CN105180933A (zh) 基于直行路口检测的移动机器人航迹推算修正系统及方法
CN104977002A (zh) 基于sins/双od的惯性组合导航系统及其导航方法
CN107247275A (zh) 基于公交车的城市gnss脆弱性监测系统及其方法
CN109470276B (zh) 基于零速修正的里程计标定方法与装置
CN109624973A (zh) 车辆控制装置、车辆控制方法及存储介质

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20230705

Address after: 310053 room 1001, innovation building, 3850 Jiangnan Road, high tech (Binjiang), Hangzhou, Zhejiang

Patentee after: Hangzhou Zhongke Microelectronics Co.,Ltd.

Address before: 314006 Building 2, No. 778, Asia Pacific Road, Jiaxing, Zhejiang Province (Jiaxing Technopole)

Patentee before: JIAXING MICROELECTRONICS AND SYSTEM ENGINEERING CENTER, CHINESE ACADEMY OF SCIENCES

TR01 Transfer of patent right