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

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

Info

Publication number
WO2016070723A1
WO2016070723A1 PCT/CN2015/092675 CN2015092675W WO2016070723A1 WO 2016070723 A1 WO2016070723 A1 WO 2016070723A1 CN 2015092675 W CN2015092675 W CN 2015092675W WO 2016070723 A1 WO2016070723 A1 WO 2016070723A1
Authority
WO
WIPO (PCT)
Prior art keywords
vehicle
coordinate system
latitude
longitude
rear wheel
Prior art date
Application number
PCT/CN2015/092675
Other languages
English (en)
French (fr)
Inventor
王励扬
何文涛
马成炎
蔺晓龙
翟昆朋
胡晓峰
王浩
Original Assignee
中国科学院嘉兴微电子与系统工程中心
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 中国科学院嘉兴微电子与系统工程中心 filed Critical 中国科学院嘉兴微电子与系统工程中心
Publication of WO2016070723A1 publication Critical patent/WO2016070723A1/zh

Links

Images

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

Definitions

  • the invention belongs to the field of dead reckoning navigation and positioning, and particularly relates to a dead reckoning navigation positioning method and a dead reckoning navigation positioning system for obtaining a latitude and longitude of a vehicle by an odometer.
  • the usual vehicle navigation calculation navigation positioning method has an odometer plus a single-axis gyroscope, a single-axis gyroscope plus a single-axis accelerometer, and two odometer differential three solutions.
  • the first two schemes all involve inertial sensors, and the inertial sensors currently available for vehicle navigation are expensive, and the corresponding data processing methods are complicated, and are not suitable for the vehicle dead reckoning navigation and positioning system.
  • the existing dead reckoning navigation positioning method can only give the vehicle coordinates in the plane rectangular coordinate system, and cannot directly provide the latitude and longitude information.
  • the object of the present invention is to solve the deficiencies in the prior art, and to provide a dead reckoning navigation and positioning method for obtaining a latitude and longitude of a vehicle by an odometer, which only obtains the latitude and longitude of the vehicle by using the vehicle odometer as a data source, thereby directly giving the latitude and longitude of the vehicle.
  • the dead reckoning navigation navigation system is to solve the deficiencies in the prior art, and to provide a dead reckoning navigation and positioning method for obtaining a latitude and longitude of a vehicle by an odometer, which only obtains the latitude and longitude of the vehicle by using the vehicle odometer as a data source, thereby directly giving the latitude and longitude of the vehicle.
  • the invention also provides a dead reckoning navigation and positioning system using a dead reckoning navigation positioning method for obtaining a latitude and longitude of a vehicle by an odometer.
  • the invention provides a dead reckoning navigation positioning method for obtaining a latitude and longitude of a vehicle by an odometer, comprising the following steps:
  • the travel distance obtained by the differential odometer obtains the traveling speed and heading angle of the vehicle
  • the invention provides a dead reckoning navigation and positioning method for obtaining a latitude and longitude of a vehicle by an odometer, which only obtains the latitude and longitude of the vehicle by using the vehicle odometer as a data source, and combines the current traveling speed and the heading angle as the output of the dead reckoning system.
  • a dead reckoning navigation and positioning system that directly gives the latitude and longitude of the vehicle is realized.
  • step (1) of the travel distance obtained by the differential odometer, obtaining the travel speed and the yaw angle change of the vehicle includes the following steps:
  • step (11) the method for obtaining the travel distance of the left rear wheel of the vehicle and the travel distance of the right rear wheel in a calculation period in step (11) is:
  • ⁇ L is the travel distance of the left rear wheel of the vehicle in one calculation period
  • ⁇ R is the travel distance of the right rear wheel of the vehicle in one calculation period
  • TicL is the pulse output of the left rear wheel odometer of the vehicle in one calculation period
  • TicR is the number of pulses output by the right rear wheel odometer of the vehicle during a calculation period
  • d is the travel distance corresponding to one pulse number.
  • step (12) the driving distance of the differential left rear wheel and the driving distance of the right rear wheel are calculated, and the method for calculating the traveling speed of the vehicle in the current calculation period is:
  • ⁇ t is the time interval of the calculation cycle.
  • lenth is the distance between the left rear wheel and the right rear wheel of the vehicle
  • Hd(t) is the heading angle of the current moment of the vehicle
  • Hd(t-1) is the heading angle of the vehicle at a moment.
  • the time interval between the last moment and the current moment is one calculation period.
  • step (2) estimating the latitude and longitude of the vehicle according to the driving speed and the yaw angle change comprises the following steps:
  • step (21) the method for calculating the speed vector of the vehicle in the navigation coordinate system in step (21) is:
  • v n is the velocity vector in the navigation coordinate system N
  • the navigation coordinate system N is in the northeast sky coordinate system
  • v N is a column vector containing three elements, the elements representing the eastward velocity, the northward velocity and the skyward velocity, respectively, due to The vehicle only travels on the road, the sky speed defaults to 0
  • v is the speed of the vehicle in the current calculation cycle
  • Hd is the heading angle of the current time of the vehicle.
  • step (22) calculates a representation of the rotation vector of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, including the following steps:
  • h is the altitude
  • e is the ellipticity of the Earth
  • Rs' is the corrected distance from the center of the Earth to the location of the vehicle.
  • step (23) the method of calculating the cosine matrix of the navigation coordinate system to the coordinate system of the earth coordinate system in step (23) is:
  • step (24) the method for extracting the latitude and longitude of the current moment of the vehicle from the direction cosine matrix in step (24) is:
  • l latitude
  • L longitude
  • D ij the direction cosine matrix of the navigation coordinate system to the earth coordinate system Corresponding element.
  • the invention also provides a dead reckoning navigation and positioning system using a dead reckoning navigation positioning method for obtaining a latitude and longitude of a vehicle by an odometer, comprising:
  • a left rear wheel odometer for providing a travel distance of the left rear wheel of the vehicle
  • a right rear wheel odometer for providing the distance traveled by the right rear wheel of the vehicle
  • the dead reckoning navigation positioning processing module is configured to calculate the speed, the heading angle and the latitude and longitude of the vehicle according to the driving distance of the left rear wheel provided by the left rear wheel odometer and the driving distance of the right rear wheel provided by the right rear wheel odometer, and Output speed, heading angle, and latitude and longitude;
  • the dead reckoning navigation positioning processing module is respectively connected with the left rear wheel odometer and the right rear wheel odometer.
  • the present invention provides a dead reckoning navigation positioning method and a dead reckoning navigation positioning system for obtaining the latitude and longitude of the vehicle by the odometer, and has the following beneficial effects:
  • FIG. 1 is a flow chart of a dead reckoning navigation positioning method for obtaining a latitude and longitude of a vehicle by an odometer according to an embodiment of the present invention
  • FIG. 2 is a block diagram of a dead reckoning navigation and positioning system implemented by the method shown in FIG. 1;
  • FIG. 3 is a simulation result of the dead reckoning navigation and positioning system implemented by the method shown in FIG. 1.
  • FIG. 1 is a flow chart showing a method for calculating a navigational position of a dead reckoning vehicle obtained by an odometer according to an embodiment of the present invention
  • FIG. 2 is a block diagram showing a navigation and positioning navigation system using the method shown in FIG.
  • the system includes a left rear wheel odometer, a right rear wheel odometer and a microprocessor, a left rear wheel odometer, a right rear wheel odometer through an external interruption 1, an external interruption 2, a left rear wheel odometer, and a right rear wheel mileage.
  • the pulse of the meter is sent to the microprocessor.
  • an embodiment of the present invention for obtaining a navigation and positioning navigation method for a latitude and longitude of a vehicle by an odometer includes the following steps:
  • Step 1 The latitude and longitude of the vehicle, the heading angle and the driving speed at the last moment;
  • Step 2 obtaining the current vehicle traveling speed and heading angle from the odometer data
  • ⁇ L is the travel distance of the left rear wheel of the vehicle in one calculation period
  • ⁇ R is the travel distance of the right rear wheel of the vehicle in one calculation period
  • TicL is the pulse output of the left rear wheel odometer of the vehicle in one calculation period
  • TicR is the number of pulses output by the right rear wheel odometer of the vehicle during a calculation period
  • d is the travel distance corresponding to one pulse number.
  • ⁇ t is the time interval of the calculation cycle.
  • lenth is the distance between the left rear wheel and the right rear wheel of the vehicle
  • Hd(t) is the heading angle of the current moment of the vehicle
  • Hd(t-1) is the heading angle of the vehicle at a moment.
  • the time interval between the last moment and the current moment is one calculation period.
  • Step 3 Calculate the speed vector of the vehicle in the navigation coordinate system
  • v n is the velocity vector in the navigation coordinate system N
  • the navigation coordinate system N is in the northeast sky coordinate system
  • v N is a column vector containing three elements, the elements representing the eastward velocity, the northward velocity and the skyward velocity, respectively, due to The vehicle only travels on the road, the sky speed defaults to 0
  • v is the speed of the vehicle in the current calculation cycle
  • Hd is the heading angle of the current time of the vehicle.
  • Step 4 Calculate the rotation vector of the navigation coordinate system relative to the earth coordinate system, and represent in the navigation coordinate system:
  • h is the altitude
  • e is the ellipticity of the Earth
  • Rs' is the corrected distance from the center of the Earth to the location of the vehicle.
  • Step 5 Calculate the cosine matrix from the navigation coordinate system to the earth coordinate system:
  • Step 6 Extract the latitude and longitude of the current moment of the vehicle from the cosine matrix in the direction:
  • l latitude
  • L longitude
  • D ij the direction cosine matrix of the navigation coordinate system to the earth coordinate system Corresponding element.
  • Step 7. Output the current speed, heading angle and latitude and longitude of the current time.
  • the invention provides a dead reckoning navigation and positioning method for obtaining a latitude and longitude of a vehicle by an odometer, which only obtains the latitude and longitude of the vehicle by using the vehicle odometer as a data source, and combines the current traveling speed and the heading angle to serve as a dead reckoning navigation and positioning system. Output, thereby realizing a dead reckoning navigation and positioning system that directly gives the latitude and longitude of the vehicle.
  • the simulator can simultaneously output the standard latitude and longitude of the vehicle at each time, and simultaneously output a pulse signal simulating two odometers.
  • the program of the above steps is run to obtain the latitude and longitude information of the vehicle at each moment.
  • the navigation position estimation navigation information and the standard vehicle position information are plotted on the same picture. As shown in FIG. 3, it can be seen that the method can effectively calculate the latitude and longitude position information of the vehicle.
  • the invention provides a dead reckoning navigation positioning method and a dead reckoning navigation positioning system for obtaining the latitude and longitude of the vehicle by the odometer, and only the vehicle odometer is used as the data source to obtain the latitude and longitude of the vehicle, combined with the current traveling speed and the heading angle, as the navigation
  • the output of the position estimation navigation positioning system is not only the coordinates in the plane rectangular coordinate system, but also realizes the dead reckoning navigation and positioning system directly giving the latitude and longitude of the vehicle; without using inertial devices such as gyroscopes and accelerometers, the direct realization is realized.
  • the dead reckoning navigation and positioning system that gives the vehicle latitude and longitude is relatively low cost.

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的地球曲率矩阵
Figure PCTCN2015092675-appb-000001
Figure PCTCN2015092675-appb-000002
其中，
Figure PCTCN2015092675-appb-000003
Figure PCTCN2015092675-appb-000004
Figure PCTCN2015092675-appb-000005
Figure PCTCN2015092675-appb-000006
其中，r1为车辆所处位置的曲率半径，D2j为导航坐标系N到地球坐标系E的方向余弦矩阵
Figure PCTCN2015092675-appb-000007
第二行的对应元素，
feh＝fe·fh，
其中，
Figure PCTCN2015092675-appb-000008
Figure PCTCN2015092675-appb-000009
其中，h为海拔高度，e为地球椭圆率，Rs′为从地球中心到车辆所处位置的修正距离。
(222)由vN计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示
Figure PCTCN2015092675-appb-000010
Figure PCTCN2015092675-appb-000011
其中，
Figure PCTCN2015092675-appb-000012
为上一时刻
Figure PCTCN2015092675-appb-000013
的第三个元素，
Figure PCTCN2015092675-appb-000014
为导航坐标系N下天向单位向量，
Figure PCTCN2015092675-appb-000015
进一步地，步骤(23)计算导航坐标系到地球坐标系的方向余弦矩阵的方法为：
Figure PCTCN2015092675-appb-000016
其中，
Figure PCTCN2015092675-appb-000017
为当前时刻的
Figure PCTCN2015092675-appb-000018
为上一时刻的
Figure PCTCN2015092675-appb-000019
为导航坐标系N到地球坐标系E的方向余弦矩阵，
Figure PCTCN2015092675-appb-000020
为导航坐标系N相对地球坐标系E在两个时刻自身变化 的方向余弦矩阵，其计算方法为对
Figure PCTCN2015092675-appb-000021
的积分进行泰勒展开：
Figure PCTCN2015092675-appb-000022
其中，
Figure PCTCN2015092675-appb-000023
进一步地，步骤(24)从方向余弦矩阵中提取车辆当前时刻的经纬度的方法为：
Figure PCTCN2015092675-appb-000024
Figure PCTCN2015092675-appb-000025
其中，l为纬度，L为经度，Dij为导航坐标系到地球坐标系的方向余弦矩阵
Figure PCTCN2015092675-appb-000026
的对应元素。
本发明还提供一种采用由里程计获得车辆经纬度的航位推算导航定位方法的航位推算导航定位系统，包括：
左后轮里程计，用于提供车辆的左后轮的行驶距离；
右后轮里程计，用于提供车辆的右后轮的行驶距离；
航位推算导航定位处理模块，用于根据左后轮里程计提供的左后轮的行驶距离以及右后轮里程计提供的右后轮的行驶距离，计算车辆的速度、航向角以及经纬度，并输出速度、航向角以及经纬度；
航位推算导航定位处理模块分别与左后轮里程计、右后轮里程计连接。
与现有技术相比，本发明提供的由里程计获得车辆经纬度的航位推算导航定位方法及航位推算导航定位系统，具有以下有益效果：
(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的地球曲率矩阵
Figure PCTCN2015092675-appb-000027
Figure PCTCN2015092675-appb-000028
其中，
Figure PCTCN2015092675-appb-000029
Figure PCTCN2015092675-appb-000030
Figure PCTCN2015092675-appb-000031
Figure PCTCN2015092675-appb-000032
其中，r1为车辆所处位置的曲率半径，D2j为导航坐标系N到地球坐标系E的方向余弦矩阵
Figure PCTCN2015092675-appb-000033
第二行的对应元素，
feh＝fe·fh，
其中，
Figure PCTCN2015092675-appb-000034
Figure PCTCN2015092675-appb-000035
其中，h为海拔高度，e为地球椭圆率，Rs′为从地球中心到车辆所处位置的修正距离。
(2)由vN计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示
Figure PCTCN2015092675-appb-000036
Figure PCTCN2015092675-appb-000037
其中，
Figure PCTCN2015092675-appb-000038
为上一时刻
Figure PCTCN2015092675-appb-000039
的第三个元素，
Figure PCTCN2015092675-appb-000040
为导航坐标系N下天向单位向量，
Figure PCTCN2015092675-appb-000041
步骤五、计算由导航坐标系到地球坐标系的方向余弦矩阵：
Figure PCTCN2015092675-appb-000042
其中，
Figure PCTCN2015092675-appb-000043
为当前时刻的
Figure PCTCN2015092675-appb-000044
为上一时刻的
Figure PCTCN2015092675-appb-000045
为导航坐标系N到地球坐标系E的方向余弦矩阵，
Figure PCTCN2015092675-appb-000046
为导航坐标系N相对地球坐标系E在两个时刻自身变化的方向余弦矩阵，其计算方法为对
Figure PCTCN2015092675-appb-000047
的积分进行泰勒展开：
Figure PCTCN2015092675-appb-000048
其中，
Figure PCTCN2015092675-appb-000049
步骤六、从该方向余弦矩阵中提取车辆当前时刻的经纬度：
Figure PCTCN2015092675-appb-000050
Figure PCTCN2015092675-appb-000051
其中，l为纬度，L为经度，Dij为导航坐标系到地球坐标系的方向余弦矩阵
Figure PCTCN2015092675-appb-000052
的对应元素。
步骤七、输出当前时刻的行驶速度、航向角以及经纬度。
本发明提供一种由里程计获得车辆经纬度的航位推算导航定位方法，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算导航定位系统的输出，从而实现直接给出车辆经纬度的航位推算导航定位系统。
以下是采用图1所示的方法实现的航位推算导航定位系统的仿真结果：
使用Spirent公司的模拟器，编辑一段车辆行驶轨迹。将车辆的起始点置于东经40度，0纬度的位置，以三十米每秒匀速向西行驶。
模拟器可以同时输出车辆每一时刻标准的经纬度，同时输出模拟两个里程计的脉冲信号。使用该模拟里程计信号，运行上述步骤的程序，得到车辆每一时刻的经纬度信息。同时将航位推算导航定位信息和标准车辆位置信息绘制与同一张图上，如图3所示，可以看出该方法可以有效计算车辆的经纬度位置信息。
本发明提供的由里程计获得车辆经纬度的航位推算导航定位方法及航位推算导航定位系统，仅由车载里程计作为数据源，获得车辆经纬度，结合当前时刻的行驶速度与航向角，作为航位推算导航定位系统的输出，不仅仅是在平面直角坐标系下的坐标，从而实现直接给出车辆经纬度的航位推算导航定位系统；无需使用陀螺仪、加速度计等惯性器件，使得实现的直接给出车辆经纬度的航位推算导航定位系统成本较低。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域的技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

  1. 一种由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，包括以下步骤：
    (1)差分里程计获取的行驶距离，获得所述车辆的行驶速度与航向角；
    (2)根据所述行驶速度与所述航向角推算所述车辆的经纬度；
    (3)输出所述行驶速度、所述航向角以及所述经纬度。
  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所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(2)根据所述行驶速度与所述偏航角度变化推算所述车辆的经纬度包括 以下步骤：
    (21)计算导航坐标系下所述车辆的速度矢量；
    (22)计算所述导航坐标系相对地球坐标系的旋转矢量在所述导航坐标系下的表示；
    (23)计算所述导航坐标系到所述地球坐标系的方向余弦矩阵；
    (24)从所述方向余弦矩阵中提取所述车辆当前时刻的经纬度。
  6. 如权利要求5所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(21)计算导航坐标系下所述车辆的速度矢量的方法为：
    vN＝[v·cos(Hd) v·sin(Hd) 0]T
    其中，vN为导航坐标系N下的速度矢量，导航坐标系N采用东北天坐标系，vN为包含三个元素的列向量，所述元素分别表示东向速度，北向速度和天向速度，由于车辆只在路面上行驶，所述天向速度默认为0，v是车辆在当前计算周期内的行驶速度，Hd是车辆当前时刻的航向角。
  7. 如权利要求6所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(22)计算所述导航坐标系相对地球坐标系的旋转矢量在所述导航坐标系下的表示，包括以下步骤：
    (221)计算3*3的地球曲率矩阵
    Figure PCTCN2015092675-appb-100001
    Figure PCTCN2015092675-appb-100002
    其中，
    Figure PCTCN2015092675-appb-100003
    Figure PCTCN2015092675-appb-100004
    Figure PCTCN2015092675-appb-100005
    Figure PCTCN2015092675-appb-100006
    其中，r1为车辆所处位置的曲率半径，D2j为导航坐标系N到地球坐标系E的方向余弦矩阵
    Figure PCTCN2015092675-appb-100007
    第二行的对应元素，
    feh＝fe·fh，
    其中，
    Figure PCTCN2015092675-appb-100008
    Figure PCTCN2015092675-appb-100009
    其中，h为海拔高度，e为地球椭圆率，Rs′为从地球中心到车辆所处位置的修正距离。
    (222)由vN计算导航坐标系N相对地球坐标系E的旋转矢量在导航坐标系下的表示
    Figure PCTCN2015092675-appb-100010
    Figure PCTCN2015092675-appb-100011
    其中，
    Figure PCTCN2015092675-appb-100012
    为上一时刻
    Figure PCTCN2015092675-appb-100013
    的第三个元素，
    Figure PCTCN2015092675-appb-100014
    为导航坐标系N下天向单位向量，
    Figure PCTCN2015092675-appb-100015
  8. 如权利要求7所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(23)计算所述导航坐标系到所述地球坐标系的方向余弦矩阵的方法为：
    Figure PCTCN2015092675-appb-100016
    其中，
    Figure PCTCN2015092675-appb-100017
    为当前时刻的
    Figure PCTCN2015092675-appb-100018
    Figure PCTCN2015092675-appb-100019
    为上一时刻的
    Figure PCTCN2015092675-appb-100020
    Figure PCTCN2015092675-appb-100021
    为导航坐标系N到地球坐标系E的方向余弦矩阵，
    Figure PCTCN2015092675-appb-100022
    为导航坐标系N相对地球坐标系E在两个时刻自身变化的方向余弦矩阵，其计算方法为对
    Figure PCTCN2015092675-appb-100023
    的积分进行泰勒展开：
    Figure PCTCN2015092675-appb-100024
    其中，
    Figure PCTCN2015092675-appb-100025
  9. 如权利要求8所述的由里程计获得车辆经纬度的航位推算导航定位方法，其特征在于，步骤(24)从所述方向余弦矩阵中提取车辆当前时刻的经纬度的方法为：
    Figure PCTCN2015092675-appb-100026
    Figure PCTCN2015092675-appb-100027
    其中，l为纬度，L为经度，Dij为导航坐标系到地球坐标系的方向余弦矩阵
    Figure PCTCN2015092675-appb-100028
    的对应元素。
  10. 一种采用如权利要求1-9所述的由里程计获得车辆经纬度的航位推算导航定位方法的航位推算导航定位系统，其特征在于，所述航位推算导航定位系统包括：
    左后轮里程计，用于提供车辆的左后轮的行驶距离；
    右后轮里程计，用于提供所述车辆的右后轮的行驶距离；
    航位推算导航定位处理模块，用于根据所述左后轮里程计提供的左后轮的行驶距离以及所述右后轮里程计提供的右后轮的行驶距离，计算所述车辆的速度、航向角以及经纬度，并输出所述速度、所述航向角以及所述经纬度；
    所述航位推算导航定位处理模块分别与所述左后轮里程计、所述右后轮里程计连接。
PCT/CN2015/092675 2014-11-05 2015-10-23 由里程计获得车辆经纬度的航位推算导航定位方法及系统 WO2016070723A1 (zh)

Applications Claiming Priority (2)

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

Publications (1)

Publication Number Publication Date
WO2016070723A1 true WO2016070723A1 (zh) 2016-05-12

Family

ID=52371323

Family Applications (1)

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

Country Status (2)

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

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106646569A (zh) * 2016-11-18 2017-05-10 华为技术有限公司 一种导航定位方法及设备
CN112254724A (zh) * 2020-10-19 2021-01-22 天津津航计算技术研究所 一种提高惯导经纬度分辨率方法

Families Citing this family (12)

* 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 安徽协创物联网技术有限公司 一种基于卫星定位应用的车辆行驶里程算法的优化方法
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 广州汽车集团股份有限公司 一种车辆定位方法、计算机设备、计算机可读存储介质
CN113029201B (zh) * 2021-04-07 2023-04-07 北京布科思科技有限公司 双轮差速模型的里程校准方法和装置、设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09243387A (ja) * 1996-03-04 1997-09-19 Tech Res & Dev Inst Of Japan Def Agency 車両用慣性航法装置
CN1499324A (zh) * 2002-11-06 2004-05-26 中国科学院自动化研究所 一种机器人位置和姿态的推算定位方法及装置
CN101576384A (zh) * 2009-06-18 2009-11-11 北京航空航天大学 一种基于视觉信息校正的室内移动机器人实时导航方法
CN202133404U (zh) * 2011-06-22 2012-02-01 河海大学 无卫星导航装置
US20140288828A1 (en) * 2013-03-22 2014-09-25 Qualcomm Incorporated Estimating an initial position and navigation state using vehicle odometry
CN104316059A (zh) * 2014-11-05 2015-01-28 中国科学院嘉兴微电子与系统工程中心 由里程计获得车辆经纬度的航位推算导航定位方法及系统

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3449240B2 (ja) * 1998-09-24 2003-09-22 株式会社デンソー 車両用現在位置検出装置、車両用現在位置表示装置、ナビゲーション装置および記録媒体
CN1322311C (zh) * 2005-07-13 2007-06-20 李俊峰 车载快速定位定向系统
WO2013059989A1 (zh) * 2011-10-25 2013-05-02 国防科学技术大学 一种惯性导航系统的运动对准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09243387A (ja) * 1996-03-04 1997-09-19 Tech Res & Dev Inst Of Japan Def Agency 車両用慣性航法装置
CN1499324A (zh) * 2002-11-06 2004-05-26 中国科学院自动化研究所 一种机器人位置和姿态的推算定位方法及装置
CN101576384A (zh) * 2009-06-18 2009-11-11 北京航空航天大学 一种基于视觉信息校正的室内移动机器人实时导航方法
CN202133404U (zh) * 2011-06-22 2012-02-01 河海大学 无卫星导航装置
US20140288828A1 (en) * 2013-03-22 2014-09-25 Qualcomm Incorporated Estimating an initial position and navigation state using vehicle odometry
CN104316059A (zh) * 2014-11-05 2015-01-28 中国科学院嘉兴微电子与系统工程中心 由里程计获得车辆经纬度的航位推算导航定位方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
LIU, CHUANCAI ET AL.: "Error Analysis of ''Rear'' and ''Front'' Wheel Dual Odometer System of Mobile Robots with Four Tires", ROBOT, vol. 20, no. 2, 31 March 1998 (1998-03-31), pages 143 - 146, ISSN: 1002-0446 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106646569A (zh) * 2016-11-18 2017-05-10 华为技术有限公司 一种导航定位方法及设备
CN106646569B (zh) * 2016-11-18 2020-04-14 华为技术有限公司 一种导航定位方法及设备
CN112254724A (zh) * 2020-10-19 2021-01-22 天津津航计算技术研究所 一种提高惯导经纬度分辨率方法

Also Published As

Publication number Publication date
CN104316059B (zh) 2017-08-25
CN104316059A (zh) 2015-01-28

Similar Documents

Publication Publication Date Title
WO2016070723A1 (zh) 由里程计获得车辆经纬度的航位推算导航定位方法及系统
CN109931944B (zh) 一种ar导航方法、装置、车端设备、服务端及介质
Karlsson et al. The future of automotive localization algorithms: Available, reliable, and scalable localization: Anywhere and anytime
CN107328410B (zh) 用于定位自动驾驶车辆的方法和汽车电脑
CN107421566B (zh) 一种无人车多源传感器信息仿真平台
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
JP4897542B2 (ja) 自己位置標定装置、自己位置標定方法および自己位置標定プログラム
CN103727941B (zh) 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN104165641B (zh) 一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法
CN106842271B (zh) 导航定位方法及装置
CN106595715B (zh) 基于捷联惯导与卫星组合导航系统里程计标定方法及装置
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
US20180240335A1 (en) Analyzing vehicle sensor data
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN111562603B (zh) 基于航位推算的导航定位方法、设备及存储介质
Jung et al. Monocular visual-inertial-wheel odometry using low-grade IMU in urban areas
CN104061899A (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN111351502A (zh) 用于从透视图生成环境的俯视图的方法，装置和计算机程序产品
CN110940344B (zh) 一种用于自动驾驶的低成本传感器组合定位方法
CN103968844B (zh) 基于低轨平台跟踪测量的大椭圆机动航天器自主导航方法
Gao et al. An integrated land vehicle navigation system based on context awareness
JP4986883B2 (ja) 標定装置、標定方法および標定プログラム
CN105606093A (zh) 基于重力实时补偿的惯性导航方法及装置
US10586393B2 (en) Positioning objects in an augmented reality display
CN106840156B (zh) 一种提高手机惯性导航性能的方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 15856544

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 15856544

Country of ref document: EP

Kind code of ref document: A1