CN108873043B - 车辆侧滑角度的计算方法及装置 - Google Patents
车辆侧滑角度的计算方法及装置 Download PDFInfo
- Publication number
- CN108873043B CN108873043B CN201810440521.7A CN201810440521A CN108873043B CN 108873043 B CN108873043 B CN 108873043B CN 201810440521 A CN201810440521 A CN 201810440521A CN 108873043 B CN108873043 B CN 108873043B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- coordinate system
- centroid
- angle
- vehicle body
- 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
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
-
- 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
- G01C21/165—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 combined with non-inertial navigation instruments
Abstract
本发明提供了一种车辆侧滑角度的计算方法及装置,该方法包括:根据检测数据采用欧拉坐标转换方法对主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标;进而确定质心瞬时航向角;采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角;对车身航向数据进行方位变换,得到车身航向角;结合优化的质心航向角和车身航向角计算待测车辆的侧滑角度。本方法通过1次坐标变换和1次卡尔曼滤波可以获取车辆质心高精度的航向角信息,进而通过比对车身航向角计算得到侧滑角度,该方法原理清晰,运算量小,计算误差小,精度高。
Description
技术领域
本发明涉及数据处理的技术领域,尤其是涉及一种车辆侧滑角度的计算方法及装置。
背景技术
行驶的车辆因制动、转动惯性、路况和其他原因,引发某一轴的车轮或两轴的车轮出现横向移动(即向侧面发生甩动)的现象,称为车辆侧滑。车辆侧滑对安全行车威胁较大,常造成碰撞、翻车、掉沟等恶性事故,对驾驶操纵造成干扰,在车辆导航控制领域影响操控精度。在湿滑的路面,极易发生车辆侧滑,例如,在水田作业的农业机械车辆,由于水田环境底层湿滑,泥脚深浅不一,附着力小,极易发生侧向滑移。为了应对车辆侧滑对驾驶带来的干扰,提高驾驶安全性和操纵精度,及时的获取车辆侧滑角度至关重要。
现有技术中,获取侧滑角度的技术路线是通过获取车辆质心横向、纵向速度的基础上获得,该过程涉及多个状态观测器和估计器,流程复杂,计算量大,误差累计大,且在低速条件下实现侧滑角的精确检测困难。
发明内容
有鉴于此,本发明的目的在于提供一种车辆侧滑角度的计算方法及装置,运算量小,计算误差小,精度高。
第一方面,本发明实施例提供了一种车辆侧滑角度的计算方法,所述方法包括:
获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,所述GNSS双卫星天线定位定向传感器和所述车辆姿态与惯性传感器设置于待测车辆上;
基于所述第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标,其中,所述导航坐标系下的主卫星天线位置坐标为对所述第一检测数据进行高斯投影变换得到的;
基于所述待测车辆的质心位置坐标确定所述待测车辆的质心瞬时航向角,其中,所述待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;
采用卡尔曼滤波器优化所述质心瞬时航向角,得到优化的质心航向角;
对所述第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;
结合所述优化的质心航向角和所述车身航向角计算所述待测车辆的侧滑角度。
结合第一方面,本发明实施例提供了第一方面的第一种可能的实施方式,其中,所述第一检测数据包括:所述北东地坐标系下的车身航向数据,大地坐标系下的经度,纬度和高程信息;
所述第二检测数据包括:车身横摆角速度,地理坐标系下的车身的横滚角,俯仰角;
所述GNSS双卫星天线定位定向传感器的双卫星天线间连线与所述待测车辆的车身前进方向垂直,其中,所述主卫星天线设置于车身右侧,从卫星天线设置于车身左侧;
所述车辆姿态与惯性传感器与所述待测车辆的质心位置相邻设置。
结合第一方面,本发明实施例提供了第一方面的第二种可能的实施方式,其中,在获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据之前,所述方法还包括:
构建导航坐标系统,
其中,所述导航坐标系统包括:所述导航坐标系,地理坐标系和车体坐标系,所述导航坐标系为东北天坐标系,所述地理坐标系位于所述主卫星天线中心,始终与所述导航坐标系平行,且随所述主卫星天线移动,所述车体坐标系与所述主卫星天线的中心位置固联,且其初始状态与所述导航坐标系平行,所述地理坐标系根据航姿测量系统的参考坐标系建立。
结合第一方面,本发明实施例提供了第一方面的第三种可能的实施方式,其中,基于所述第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标包括:
对所述大地坐标系下的经度,纬度和高程信息进行所述高斯投影变换,得到所述导航坐标系下的主卫星天线位置坐标;
基于所述地理坐标系下的车身的横滚角,俯仰角,所述导航坐标系下的车身航向角确定欧拉坐标转换的转换矩阵;
基于所述转换矩阵对所述导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标。
结合第一方面,本发明实施例提供了第一方面的第四种可能的实施方式,其中,基于所述转换矩阵对所述导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标包括:
根据欧拉坐标转换公式对所述导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标,其中,表示导航坐标系下的所述待测车辆的质心位置坐标,表示所述导航坐标系下的主卫星天线位置坐标,Rb2n表示所述转换矩阵,表示车体坐标系下的所述主卫星天线位置与所述质心位置的相对关系。
结合第一方面,本发明实施例提供了第一方面的第五种可能的实施方式,其中,基于所述待测车辆的质心位置坐标确定所述待测车辆的质心瞬时航向角包括:
根据所述当前时刻的质心位置坐标和所述前一时刻的质心位置坐标计算所述待测车辆的质心位置矢量;
结合第一方面,本发明实施例提供了第一方面的第六种可能的实施方式,其中,对所述第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角包括:
结合第一方面,本发明实施例提供了第一方面的第七种可能的实施方式,其中,结合所述优化的质心航向角和所述车身航向角计算所述待测车辆的侧滑角度包括:
根据侧滑角度计算公式β=ψ-ψa计算所述待测车辆的侧滑角度,其中,β表示所述侧滑角度,ψ表示所述车身航向角,ψa表示所述优化的质心航向角。
第二方面,本发明实施例还提供了一种车辆侧滑角度的计算装置,所述装置包括:
获取模块,用于获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,所述GNSS双卫星天线定位定向传感器和所述车辆姿态与惯性传感器设置于待测车辆上;
坐标转换模块,用于基于所述第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标,其中,所述导航坐标系下的主卫星天线位置坐标为对所述第一检测数据进行高斯投影变换得到的;
确定模块,用于基于所述待测车辆的质心位置坐标确定所述待测车辆的质心瞬时航向角,其中,所述待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;
优化模块,用于采用卡尔曼滤波器优化所述质心瞬时航向角,得到优化的质心航向角;
方位变换模块,用于对所述第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;
计算模块,用于结合所述优化的质心航向角和所述车身航向角计算所述待测车辆的侧滑角度。
结合第二方面,本发明实施例提供了第二方面的第一种可能的实施方式,其中,所述第一检测数据包括:所述北东地坐标系下的车身航向数据,大地坐标系下的经度,纬度和高程信息;
所述第二检测数据包括:车身横摆角速度,地理坐标系下的车身的横滚角,俯仰角;
所述GNSS双卫星天线定位定向传感器的双卫星天线与所述待测车辆的车身前进方向垂直,其中,所述主卫星天线设置于车身右侧,从卫星天线设置于车身左侧;
所述车辆姿态与惯性传感器与所述待测车辆的质心位置相邻设置。
本发明实施例带来了以下有益效果:本发明实施例提供了一种车辆侧滑角度的计算方法及装置,该方法包括:获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,GNSS双卫星天线定位定向传感器和车辆姿态与惯性传感器设置于待测车辆上;基于第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,其中,导航坐标系下的主卫星天线位置坐标为对第一检测数据进行高斯投影变换得到的;基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,其中,待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角;对第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;结合优化的质心航向角和车身航向角计算待测车辆的侧滑角度。
现有技术中无法对车辆侧滑角度进行计算。与现有技术相比,本发明实施例的车辆侧滑角度的计算方法先获取GNSS双卫星天线定位定向传感器测得的第一检测数据和车辆姿态与惯性传感器测得的第二检测数据,进而采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,进一步基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,然后采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角,同时,对北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角,最终结合优化的质心航向角和车身航向角就能够计算得到待测车辆的侧滑角度。本方法可以获取无漂移的高精度位置、方位角信息,通过1次坐标变换和1次卡尔曼滤波就能够得到优化的质心航向角,进而通过比对车身航向角计算得到待测车辆的侧滑角度,该方法原理清晰,运算量小,计算误差小,精度高,缓解了现有技术中计算车辆侧滑角度时流程复杂,计算量大,误差累计大的技术问题。
本发明的其他特征和优点将在随后的说明书中阐述,并且,部分地从说明书中变得显而易见,或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。
为使本发明的上述目的、特征和优点能更明显易懂,下文特举较佳实施例,并配合所附附图,作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案,下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施方式,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种车辆侧滑角度的计算方法的流程图;
图2为本发明实施例提供的车辆侧滑角度计算的原理图;
图3为本发明实施例提供的基于第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标的方法流程图;
图4为本发明实施例提供的质心瞬时航向角曲线和优化的质心航向角曲线的示意图;
图5为本发明实施例提供的车身航向角曲线与优化的质心航向角曲线的示意图;
图6为本发明实施例提供的待测车辆的侧滑角度曲线的示意图;
图7为本发明实施例提供的一种车辆侧滑角度的计算装置的功能模块图。
图标:
11-获取模块;12-坐标转换模块;13-确定模块;14-优化模块;15-方位变换模块;16-计算模块。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合附图对本发明的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
为便于对本实施例进行理解,首先对本发明实施例所公开的一种车辆侧滑角度的计算方法方法进行详细介绍。
实施例一:
一种车辆侧滑角度的计算方法,参考图1,该方法包括:
S102、获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,GNSS双卫星天线定位定向传感器和车辆姿态与惯性传感器设置于待测车辆上;
在本发明实施例中,该车辆侧滑角度的计算方法的执行主体为嵌入式单片机。GNSS双卫星天线定位定向传感器和车辆姿态与惯性传感器设置于待测车辆上,安装的具体位置将在下文中进行描述,在此不再赘述。在本发明实施例中,车辆定位定向传感器采用Trimble生产的,GNSS双卫星天线接收机模块为BD982,车辆定位定向传感器GNSS双卫星天线接收机模块即为本发明中的GNSS双卫星天线定位定向传感器,车辆姿态与惯性传感器采用Xsens生产的MTi-30。
在具体实现时,嵌入式单片机通过RS232通讯接口分别连接GNSS双卫星天线定位定向传感器和车辆姿态与惯性传感器。以20HZ的频率接收GNSS双卫星天线定位定向传感器的(GNGGA)和(PTNL,AVR),这里的(GNGGA)为数据包的一个报头,(PTNL,AVR)为数据包的另一个报头,这两个报头中含有第一检测数据,同时,以100HZ的频率接收车辆姿态与惯性传感器的第二检测数据,下文中再对第一检测数据和第二检测数据的内容进行具体介绍,在此不再赘述。
S104、基于第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,其中,导航坐标系下的主卫星天线位置坐标为对第一检测数据进行高斯投影变换得到的;
在得到第一检测数据和第二检测数据后,基于第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标。
下文中再对坐标转换的过程以及计算导航坐标系下的主卫星天线位置坐标的过程进行详细描述,在此不再赘述。
S106、基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,其中,待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;
在得到导航坐标系下的待测车辆的质心位置坐标后,基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,下文中再对质心瞬时航向角的确定过程进行详细介绍。
具体的,当前时刻的质心位置坐标是根据当前时刻的第一检测数据和第二检测数据按照上述过程计算得到的,前一时刻的质心位置坐标是根据前一时刻的第一检测数据和第二检测数据按照上述过程计算得到的。
S108、采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角;
在得到质心瞬时航向角后,采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角。
具体的,基于微积分原理,卡尔曼滤波公式具体为:
其中,A=[1],B=[δT],δT为BD982(即GNSS双卫星天线定位定向传感器)信息更新时间,在本发明中因信号传输不可避免的丢包,数据更新时间偶尔出现大于0.05s的间隔,U=gyro_z,也就是横摆角速度,为第二检测数据中的一个参量,是车辆姿态与惯性传感器测得的;离线仿真得到最优Q/R值分别为:Q=0.00001,R=0.01;量测系统参数矩阵H=[1]。上述(3)式中的Z(k)即为质心瞬时航向角(下文中用ψr表示),即为优化的质心航向角(也就是下文中的ψa),其它参量都是中间值,含义固定。
S110、对第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;
继续对第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角。具体变换过程将在下文中进行详细描述。
S112、结合优化的质心航向角和车身航向角计算待测车辆的侧滑角度。
在得到优化的质心航向角和车身航向角,就能够结合优化的质心航向角和车身航向角计算待测车辆的侧滑角度。下文中再对计算过程进行详细描述。
如图2所示,其中,V为质心瞬时速度方向,横轴为车身航向,β表示侧滑角度,CG表示质心,lr表示质心距离后轴的距离,lf表示质心距离前轴的距离,δ表示前轮的转角。
与现有技术相比,本发明实施例的车辆侧滑角度的计算方法先获取GNSS双卫星天线定位定向传感器测得的第一检测数据和车辆姿态与惯性传感器测得的第二检测数据,进而采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,进一步基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,然后采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角,同时,对北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角,最终结合优化的质心航向角和车身航向角就能够计算得到待测车辆的侧滑角度。本方法可以获取无漂移的高精度位置、方位角信息,通过1次坐标变换和1次卡尔曼滤波就能够得到优化的质心航向角,进而通过比对车身航向角计算得到待测车辆的侧滑角度,该方法原理清晰,运算量小,计算误差小,精度高。
上述内容对车辆侧滑角度的计算方法进行了简要描述,下面对其中涉及到的具体内容进行详细介绍。
具体的,第一检测数据包括:北东地坐标系下的车身航向数据,大地坐标系下的经度,纬度和高程信息;
第二检测数据包括:车身横摆角速度,地理坐标系下的车身的横滚角,俯仰角;
GNSS双卫星天线定位定向传感器的双卫星天线间连线与待测车辆的车身前进方向垂直,其中,主卫星天线设置于车身右侧,从卫星天线设置于车身左侧;
车辆姿态与惯性传感器与待测车辆的质心位置相邻设置。该方法中的设备组成简单,安装灵活,使用方便。
在一个可选地实施方式中,在获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据之前,该方法还包括:
构建导航坐标系统,
其中,导航坐标系统包括:导航坐标系,地理坐标系和车体坐标系,导航坐标系为东北天坐标系,地理坐标系位于主卫星天线中心,始终与导航坐标系平行,且随主卫星天线移动;车体坐标系与主卫星天线的中心位置固联,且其初始状态与导航坐标系平行,地理坐标系根据航姿测量系统的参考坐标系建立。
也就是,导航坐标系为东北天坐标系(ENU),根据高斯平面坐标系和航姿测量参考坐标系建立了地理坐标系、车体坐标系,构成导航坐标系统。地理坐标系位于主卫星天线(即坐标系原点位于天线中心位置),始终与导航坐标系平行并跟随主卫星天线移动;车辆初始位置朝向坐标轴东方向,车体坐标系初始位置与导航坐标系平行并固联在主卫星天线位置(即坐标系跟随天线移动而移动,与天线形成“刚体”);航姿坐标系由MTi-30系统定义,安装时与车身固联并与车体坐标系平行。
下面对坐标转换的过程进行详细描述:
在一个可选地实施方式中,参考图3,基于第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标包括:
S301、对大地坐标系下的经度,纬度和高程信息进行高斯投影变换,得到导航坐标系下的主卫星天线位置坐标;
对第一检测数据中的大地坐标系下的经度,纬度和高程信息进行高斯投影变换,得到导航坐标系下的主卫星天线位置坐标。
高斯投影变换为:
式中,l=L-L0,L0为投影带中央子午线经度,卯酉圈曲率半径椭球第一偏心率辅助变量t=tanB,辅助变量η=e'cosB,椭球第二偏心率a、b分别为参考椭球的长半径、短半径;扁率X为赤道至纬度为B的平行圈的子午线弧长,其计算公式为:c为极曲率半径。
本实施例所用投影坐标系的主要参数为:a、投影方式:Gauss-Kruger;b、中央经线:114.000000(3度带);c、水平偏移量:500km;d、地理坐标系:GCS_WGS_1984;e、大地参照系:D_WGS_1984;f、参考椭球体:WGS-84;g、椭球长轴:6378137.000000;h、椭球扁率:0.0033528107。
S302、基于地理坐标系下的车身的横滚角,俯仰角,导航坐标系下的车身航向角确定欧拉坐标转换的转换矩阵;
具体的,欧拉坐标转换的转换矩阵:
其中,ψ表示导航坐标系下的车身航向角,θ表示车身的俯仰角,φ表示车身的横滚角。
S303、基于转换矩阵对导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的待测车辆的质心位置坐标。
具体的,根据欧拉坐标转换公式对导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,其中,表示导航坐标系下的待测车辆的质心位置坐标,表示导航坐标系下的主卫星天线位置坐标,Rb2n表示转换矩阵,表示车体坐标系下的主卫星天线位置与质心位置的相对关系。该值是通过测量得到的,通过米尺就能实现。
下面对质心瞬时航向角的确定过程进行描述:
在一个可选地实施方式中,基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角包括:
(1)根据当前时刻的质心位置坐标和前一时刻的质心位置坐标计算待测车辆的质心位置矢量;
具体的,如果当前时刻的质心位置坐标为P2(xk,yk),前一时刻的质心位置坐标为P2(xk-1,yk-1),那么质心位置矢量:δx=P2(xk)-P2(xk-1);δy=P2(yk)-P2(yk-1)。
(2)根据质心瞬时航向角计算公式
计算待测车辆的质心瞬时航向角,其中,ψr表示待测车辆的质心瞬时航向角,δx表示质心位置矢量中x方向的矢量,δy表示质心位置矢量中y方向的矢量。得到的质心瞬时航向角噪声很大,可以通过卡尔曼滤波器进行信息融合,具体过程已经在步骤S108中进行了详细描述,在此不再赘述。
参考图4,其中一条曲线为质心瞬时航向角的曲线,一条为卡尔曼滤波器对质心瞬时航向角进行优化,优化的质心航向角。
下面对导航坐标系下的车身航向角的计算过程进行介绍:
在一个可选地实施方式中,对第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角包括:
参考图5,其中一条曲线为车身航向角的曲线,一条为优化的质心航向角的曲线。
下面对计算待测车辆的侧滑角度的过程进行详细介绍:
在一个可选地实施方式中,结合优化的质心航向角和车身航向角计算待测车辆的侧滑角度包括:
根据侧滑角度计算公式β=ψ-ψa计算待测车辆的侧滑角度,其中,β表示侧滑角度,ψ表示车身航向角,ψa表示优化的质心航向角。
参考图6,图6为对图5中的两条曲线求差得到的待测车辆的侧滑角度的曲线。
实施例二:
一种车辆侧滑角度的计算装置,参考图7,该装置包括:
获取模块11,用于获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,GNSS双卫星天线定位定向传感器和车辆姿态与惯性传感器设置于待测车辆上;
坐标转换模块12,用于基于第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,其中,导航坐标系下的主卫星天线位置坐标为对第一检测数据进行高斯投影变换得到的;
确定模块13,用于基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,其中,待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;
优化模块14,用于采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角;
方位变换模块15,用于对第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;
计算模块16,用于结合优化的质心航向角和车身航向角计算待测车辆的侧滑角度。
本发明实施例的车辆侧滑角度的计算装置先获取GNSS双卫星天线定位定向传感器测得的第一检测数据和车辆姿态与惯性传感器测得的第二检测数据,进而采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,进一步基于待测车辆的质心位置坐标确定待测车辆的质心瞬时航向角,然后采用卡尔曼滤波器优化质心瞬时航向角,得到优化的质心航向角,同时,对北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角,最终结合优化的质心航向角和车身航向角就能够计算得到待测车辆的侧滑角度。本装置可以获取无漂移的高精度位置、方位角信息,通过1次坐标变换和1次卡尔曼滤波就能够得到优化的质心航向角,进而通过比对车身航向角计算得到待测车辆的侧滑角度,该方法原理清晰,运算量小,计算误差小,精度高。
可选地,第一检测数据包括:北东地坐标系下的车身航向数据,大地坐标系下的经度,纬度和高程信息;
第二检测数据包括:车身横摆角速度,地理坐标系下的车身的横滚角,俯仰角;
GNSS双卫星天线定位定向传感器的双卫星天线间连线与待测车辆的车身前进方向垂直,其中,主卫星天线设置于车身右侧,从卫星天线设置于车身左侧;
车辆姿态与惯性传感器与待测车辆的质心位置相邻设置。
可选地,该装置还包括:
构建模块,用于构建导航坐标系统,
其中,导航坐标系统包括:导航坐标系,地理坐标系和车体坐标系,导航坐标系为东北天坐标系,地理坐标系位于主卫星天线中心,始终与导航坐标系平行,且随主卫星天线移动;车体坐标系与主卫星天线的中心位置固联,且其初始状态与导航坐标系平行,地理坐标系根据航姿测量系统的参考坐标系建立。
可选地,坐标转换模块包括:
高斯投影变换单元,用于对大地坐标系下的经度,纬度和高程信息进行高斯投影变换,得到导航坐标系下的主卫星天线位置坐标;
确定单元,用于基于地理坐标系下的车身的横滚角,俯仰角,导航坐标系下的车身航向角确定欧拉坐标转换的转换矩阵;
欧拉坐标转换单元,用于基于转换矩阵对导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的待测车辆的质心位置坐标。
可选地,欧拉坐标转换单元包括:
欧拉坐标转换子单元,用于根据欧拉坐标转换公式对导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的待测车辆的质心位置坐标,其中,表示导航坐标系下的待测车辆的质心位置坐标,表示导航坐标系下的主卫星天线位置坐标,Rb2n表示转换矩阵,表示车体坐标系下的主卫星天线位置与质心位置的相对关系。
可选地,确定模块包括:
第一计算单元,用于根据当前时刻的质心位置坐标和前一时刻的质心位置坐标计算待测车辆的质心位置矢量;
可选地,方位变换模块包括:
方位变换单元,用于按照方位变换公式对第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角,其中,ψ表示导航坐标系下的车身航向角,ψ0表示北东地坐标系下的车身航向数据。
可选地,计算模块包括:
计算单元,用于根据侧滑角度计算公式β=ψ-ψa计算待测车辆的侧滑角度,其中,β表示侧滑角度,ψ表示车身航向角,ψa表示优化的质心航向角。
该实施例二中的具体内容可参考上述实施例一中的描述,在此不再赘述。
本发明实施例所提供的车辆侧滑角度的计算方法及装置的计算机程序产品,包括存储了程序代码的计算机可读存储介质,所述程序代码包括的指令可用于执行前面方法实施例中所述的方法,具体实现可参见方法实施例,在此不再赘述。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的系统和装置的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
另外,在本发明实施例的描述中,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通。对于本领域的普通技术人员而言,可以具体情况理解上述术语在本发明中的具体含义。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在本发明的描述中,需要说明的是,术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。此外,术语“第一”、“第二”、“第三”仅用于描述目的,而不能理解为指示或暗示相对重要性。
最后应说明的是:以上所述实施例,仅为本发明的具体实施方式,用以说明本发明的技术方案,而非对其限制,本发明的保护范围并不局限于此,尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化,或者对其中部分技术特征进行等同替换;而这些修改、变化或者替换,并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应所述以权利要求的保护范围为准。
Claims (10)
1.一种车辆侧滑角度的计算方法,其特征在于,所述方法包括:
获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,所述GNSS双卫星天线定位定向传感器和所述车辆姿态与惯性传感器设置于待测车辆上;
基于所述第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标,其中,所述导航坐标系下的主卫星天线位置坐标为对所述第一检测数据进行高斯投影变换得到的,主卫星天线为所述GNSS双卫星天线定位定向传感器的双卫星天线中的一个;
基于所述待测车辆的质心位置坐标确定所述待测车辆的质心瞬时航向角,其中,所述待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;
采用卡尔曼滤波器优化所述质心瞬时航向角,得到优化的质心航向角;
对所述第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;
结合所述优化的质心航向角和所述车身航向角计算所述待测车辆的侧滑角度。
2.根据权利要求1所述的方法,其特征在于,
所述第一检测数据包括:所述北东地坐标系下的车身航向数据,大地坐标系下的经度,纬度和高程信息;
所述第二检测数据包括:车身横摆角速度,地理坐标系下的车身的横滚角,俯仰角;
所述GNSS双卫星天线定位定向传感器的双卫星天线间连线与所述待测车辆的车身前进方向垂直,其中,所述主卫星天线设置于车身右侧,从卫星天线设置于车身左侧;
所述车辆姿态与惯性传感器与所述待测车辆的质心位置相邻设置。
3.根据权利要求1所述的方法,其特征在于,在获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据之前,所述方法还包括:
构建导航坐标系统,
其中,所述导航坐标系统包括:所述导航坐标系,地理坐标系和车体坐标系,所述导航坐标系为东北天坐标系,所述地理坐标系位于所述主卫星天线中心,始终与所述导航坐标系平行,且随所述主卫星天线移动;所述车体坐标系与所述主卫星天线的中心位置固联,所述待测车辆的初始位置朝向东向,且所述车体坐标系的初始状态与所述导航坐标系平行,所述地理坐标系根据航姿测量系统的参考坐标系建立。
4.根据权利要求2所述的方法,其特征在于,基于所述第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标包括:
对所述大地坐标系下的经度,纬度和高程信息进行所述高斯投影变换,得到所述导航坐标系下的主卫星天线位置坐标;
基于所述地理坐标系下的车身的横滚角,俯仰角,所述导航坐标系下的车身航向角确定欧拉坐标转换的转换矩阵;
基于所述转换矩阵对所述导航坐标系下的主卫星天线位置坐标进行欧拉坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标。
8.根据权利要求1所述的方法,其特征在于,结合所述优化的质心航向角和所述车身航向角计算所述待测车辆的侧滑角度包括:
根据侧滑角度计算公式β=ψ-ψa计算所述待测车辆的侧滑角度,其中,β表示所述侧滑角度,ψ表示所述车身航向角,ψa表示所述优化的质心航向角。
9.一种车辆侧滑角度的计算装置,其特征在于,所述装置包括:
获取模块,用于获取GNSS双卫星天线定位定向传感器测得的第一检测数据,以及获取车辆姿态与惯性传感器测得的第二检测数据,其中,所述GNSS双卫星天线定位定向传感器和所述车辆姿态与惯性传感器设置于待测车辆上;
坐标转换模块,用于基于所述第二检测数据采用欧拉坐标转换方法对导航坐标系下的主卫星天线位置坐标进行坐标转换,得到导航坐标系下的所述待测车辆的质心位置坐标,其中,所述导航坐标系下的主卫星天线位置坐标为对所述第一检测数据进行高斯投影变换得到的,主卫星天线为所述GNSS双卫星天线定位定向传感器的双卫星天线中的一个;
确定模块,用于基于所述待测车辆的质心位置坐标确定所述待测车辆的质心瞬时航向角,其中,所述待测车辆的质心位置坐标包括:当前时刻的质心位置坐标和前一时刻的质心位置坐标;
优化模块,用于采用卡尔曼滤波器优化所述质心瞬时航向角,得到优化的质心航向角;
方位变换模块,用于对所述第一检测数据中的北东地坐标系下的车身航向数据进行方位变换,得到导航坐标系下的车身航向角;
计算模块,用于结合所述优化的质心航向角和所述车身航向角计算所述待测车辆的侧滑角度。
10.根据权利要求9所述的装置,其特征在于,
所述第一检测数据包括:所述北东地坐标系下的车身航向数据,大地坐标系下的经度,纬度和高程信息;
所述第二检测数据包括:车身横摆角速度,地理坐标系下的车身的横滚角,俯仰角;
所述GNSS双卫星天线定位定向传感器的双卫星天线间连线与所述待测车辆的车身前进方向垂直,其中,所述主卫星天线设置于车身右侧,从卫星天线设置于车身左侧;
所述车辆姿态与惯性传感器与所述待测车辆的质心位置相邻设置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810440521.7A CN108873043B (zh) | 2018-05-09 | 2018-05-09 | 车辆侧滑角度的计算方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810440521.7A CN108873043B (zh) | 2018-05-09 | 2018-05-09 | 车辆侧滑角度的计算方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108873043A CN108873043A (zh) | 2018-11-23 |
CN108873043B true CN108873043B (zh) | 2021-01-15 |
Family
ID=64333218
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810440521.7A Active CN108873043B (zh) | 2018-05-09 | 2018-05-09 | 车辆侧滑角度的计算方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108873043B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109927726B (zh) * | 2019-03-13 | 2021-02-26 | 深兰科技(上海)有限公司 | 一种调整目标车辆运动状态的方法和设备 |
CN110705120B (zh) * | 2019-10-12 | 2023-05-23 | 中国水利水电第七工程局有限公司 | 一种隧道无轨自行式变质量平台车的重心位置动态计算方法 |
CN111103566B (zh) * | 2019-11-28 | 2023-09-05 | 中国科学院上海微系统与信息技术研究所 | 一种航向角确定方法、系统、电子设备及存储介质 |
CN111238471B (zh) * | 2020-01-17 | 2021-08-24 | 青岛农业大学 | 一种适用于农业机械直线导航的侧滑角度估计方法及估计器 |
CN113030504B (zh) * | 2021-03-18 | 2023-03-07 | 北京航迹科技有限公司 | 车辆测速方法、装置、车载计算机设备和存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100599567B1 (ko) * | 2004-08-17 | 2006-07-13 | 현대모비스 주식회사 | 지피에스를 이용한 차량의 횡미끌림각 추정 방법 |
JP2007003461A (ja) * | 2005-06-27 | 2007-01-11 | Churyo Eng Kk | 移動局の横滑角計測装置 |
CN102416956A (zh) * | 2011-09-09 | 2012-04-18 | 中南大学 | 汽车质心侧偏角和轮胎侧偏角软测量方法 |
CN102621570A (zh) * | 2012-04-11 | 2012-08-01 | 清华大学 | 基于双全球定位和惯性测量的汽车动力学参数测量方法 |
CN102954783A (zh) * | 2012-11-05 | 2013-03-06 | 清华大学 | 基于全球定位系统数据采集的车身侧偏角计算方法 |
CN107600073A (zh) * | 2017-08-10 | 2018-01-19 | 同济大学 | 一种基于多源信息融合的车辆质心侧偏角估计系统及方法 |
-
2018
- 2018-05-09 CN CN201810440521.7A patent/CN108873043B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100599567B1 (ko) * | 2004-08-17 | 2006-07-13 | 현대모비스 주식회사 | 지피에스를 이용한 차량의 횡미끌림각 추정 방법 |
JP2007003461A (ja) * | 2005-06-27 | 2007-01-11 | Churyo Eng Kk | 移動局の横滑角計測装置 |
CN102416956A (zh) * | 2011-09-09 | 2012-04-18 | 中南大学 | 汽车质心侧偏角和轮胎侧偏角软测量方法 |
CN102621570A (zh) * | 2012-04-11 | 2012-08-01 | 清华大学 | 基于双全球定位和惯性测量的汽车动力学参数测量方法 |
CN102954783A (zh) * | 2012-11-05 | 2013-03-06 | 清华大学 | 基于全球定位系统数据采集的车身侧偏角计算方法 |
CN107600073A (zh) * | 2017-08-10 | 2018-01-19 | 同济大学 | 一种基于多源信息融合的车辆质心侧偏角估计系统及方法 |
Non-Patent Citations (2)
Title |
---|
Multi-rate Vehicle Side Slip Angle Estimation Using Low-cost GPS/IMU;Sung Bong Hong 等;《2017 17th International Conference on Control, Automation and Systems (ICCAS 2017)》;20171021;全文 * |
车辆质心侧偏角估计综述;陈慧 等;《机械工程学报》;20131231;第49卷(第24期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN108873043A (zh) | 2018-11-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108873043B (zh) | 车辆侧滑角度的计算方法及装置 | |
US6681180B2 (en) | Determination and control of vehicle sideslip using GPS | |
US10761218B2 (en) | System for land vehicle navigation and corresponding method | |
CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
JP5604591B2 (ja) | 車両ナビゲーションシステムの動的パラメータを較正するための装置および方法 | |
US8170726B2 (en) | System and method for road angle estimation | |
US9719786B2 (en) | Systems for navigating using corrected yaw bias values | |
US7711483B2 (en) | Dead reckoning system | |
JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
US7355549B2 (en) | Apparatus and method for carrier phase-based relative positioning | |
CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
JP5419665B2 (ja) | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム | |
US20230358541A1 (en) | Inertial navigation system capable of dead reckoning in vehicles | |
CN110346824B (zh) | 一种车辆导航方法、系统、装置及可读存储介质 | |
CN111949030B (zh) | 一种农机定位方法、农机车辆和存储介质 | |
JP2014077769A (ja) | センサ傾斜判定装置及びプログラム | |
CN112346103A (zh) | 基于v2x的智能网联汽车动态协同定位方法与装置 | |
US11117589B2 (en) | System and method for determining roadway bank angle | |
CN113252048B (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
CN102095424A (zh) | 一种适合车载光纤航姿系统的姿态测量方法 | |
US20160131482A1 (en) | Device for the detection of the attitude of auto-vehicles and corresponding method | |
JP5686703B2 (ja) | 移動体回転半径測定装置及び方法 | |
JP2012098185A (ja) | 方位角推定装置及びプログラム | |
Farroni et al. | A comparison among different methods to estimate vehicle sideslip angle | |
JP6419242B2 (ja) | 移動距離測定装置、移動距離測定方法、及び移動距離測定プログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |