车载道路坡度角测量系统及车载道路坡度角测量方法

专利名称：车载道路坡度角测量系统及车载道路坡度角测量方法
技术领域：
本发明涉及一种用于机动车上坡时对道路的坡度角进行测量的车载道路坡度角测量系统及车载道路坡度角测量方法。
背景技术：
为了使机动车在坡路上行驶时能获得更好的驱动性能，需要在车身上安装对道路坡度角进行计算的测量装置。专利号为6714851的美国专利中公开了一种能够对道路坡度角进行测量的装置，但这种装置结构比较复杂，需要在整个机动车系统中安装多个传感器如转向轮转角传感器、加速度传感器和车轮转速传感器，其造价高，并且所测量得出的道路坡度角中包含有机动车的俯仰角。俯仰角是指将机动车在同一条直线上的前后两个轮子各看成一个质点，连接两质点所得到的直线与坡面的夹角即为俯仰角。由于俯仰角不可忽略，因此这种装置所测量得出的道路坡度值不准确，其精度不高。

发明内容
本发明的一个目的是提供一种结构简单、能够对道路坡度角进行较精确测量的车载道路坡度角测量系统。本发明的另一目的是提供一种通过所述测量系统对道路坡度角进行测量的方法。
根据本发明的第一个方面，提供一种车载道路坡度角测量系统，其包括GPS速度测量仪，其用于测量行驶中的机动车在某一瞬间的垂直速度Vv及水平速度Vh；安装在机动车前悬架处的前悬架位移传感器，用于测量所述瞬间时的前悬架位置处的车身沿垂直于道路平面方向的位移Zf；
安装在机动车后悬架处的后悬架位移传感器，用于测量所述瞬间时的后悬架位置处的车身沿垂直于道路平面方向的位移Zr；电子控制单(ECU)，其与所述GPS速度测量仪电连接以获取所述垂直速度Vv和水平速度Vh，与所述前悬架位移传感器电连接以获取所述前悬架位置处的车身沿垂直于道路平面方向的位移Zf，与所述后悬架位移传感器电连接以获取所述后悬架位置处的车身沿垂直于道路平面方向的位移Zr，并根据下列公式(1)、(2)和(3)计算出道路坡度角θαGPS＝arctan(Vv/Vh)(1)γ＝arctan((Zf-Zr)/L)(2)θ＝αGPS-γ(3)式中，γ为机动车行驶在坡度路面时的俯仰角，θ为需要测定的道路坡度角，αGPS为由GPS速度测量仪测定的速度通过公式(1)计算得出的俯仰角与道路坡度角之和，L为前悬架位移传感器与后悬架位移传感器之间的距离。
所述GPS速度测量仪优选安装在机动车的车顶上。
所述电子控制单元包括数据接收模块和数据处理模块；数据接收模块用于从所述GPS速度测量仪接收所述垂直速度Vv和水平速度Vh，并从所述前、后悬架位移传感器分别接收所述前悬架位置处的车身沿垂直于道路平面方向的位移Zf和所述后悬架位置处的车身沿垂直于道路平面方向的位移Zr；所述数据处理模块用于根据接收的Vv、Vh、Zf、Zr、以及由前、后悬架位移传感器的安装位置所确定的L的值，由公式(1)、(2)和(3)计算出道路坡度角θ。
通过测得的道路坡度角，就可以通过机动车的驱动控制模块对机动车在坡路上的行驶进行有效控制。
在实际的测量过程中，由于GPS速度测量仪可能存在测量偏差(噪声)，因此测得的αGPS和实际的α之间的关系可以表达为下列公式(4)αGPS＝α+η (4)式中η为GPS测量过程中的噪声。
为了更精确地测量道路坡度角θ，根据本发明的一种优选的实施方式，在机动车的质心处还安装了一个陀螺仪，其与所述电子控制单元(ECU)电连接，用来测量机动车的俯仰角速度q。记陀螺仪的俯仰角速度测量值为qgyro，则存在下列关系式(5)qgyro=α·+qbias+ϵ---(5)]]>式中 表示实际的机动车俯仰角速度，qbias是陀螺仪的偏差，ε是陀螺仪测量过程中的噪声(高斯白噪声)。
将(5)式写成矩阵的形式得α·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>式中α为实际的道路坡度角与机动车俯仰角之和， 表示实际的机动车俯仰角速度，qbias是陀螺仪的测量偏差， 表示陀螺仪偏差随时间的变化率，qgyro是机动车俯仰角速度的陀螺仪测量值，ε是高斯白噪声。
同样也可以把(4)式写成矩阵形式αGPS=10αqbias+η---(7)]]>由式(6)和式(7)可以构造一个俯仰卡尔曼滤波器算法，其状态向量x＝[αqbias]T，量测向量为y＝αGPS，状态转移矩阵φ＝，控制矩阵ψ＝[10]T，量测矩阵H＝[10]。这样，就可以较为准确地估计出α角和陀螺仪的偏差qbias。通过由此测得的α角替换公式(3)中的αGPS，同时通过公式(2)算出俯仰角γ，就可以通过公式(3)算出道路坡度角θ。
另外，根据该优选的实施方式，当GPS速度测量仪不能正常工作时，可以设公式(5)中的qbias和ε均为0，这样就可以通过由陀螺仪测量的俯仰角速度值qgyro粗略地估算出α角，通过将该α角替换公式(3)中的αGPS，同时通过公式(2)算出俯仰角γ，就可以通过公式(3)估算出道路坡度角θ。因此，即使GPS速度测量仪不能正常工作，通过本发明的该优选的实施方式，也可以对道路坡度角进行估算，从而对机动车在坡路上的行驶进行有效控制。
根据本发明的第二个方面，提供一种通过所述测量系统对道路坡度角进行测量的方法，该方法包括由GPS速度测量仪测得在坡路上行驶的机动车在某一瞬间的垂直速度Vv及水平速度Vh；由安装在机动车前悬架处的前悬架位移传感器测得所述瞬间时的前悬架位置处的车身沿垂直于道路平面方向的位移Zf；由安装在机动车后悬架处的后悬架位移传感器测得所述瞬间时的后悬架位置处的车身沿垂直于道路平面方向的位移Zr；由电子控制单元(ECU)接收所述Vv、Vh、Zf、Zr的值，并由下列公式(1)、(2)和(3)计算出道路坡度角θαGPS＝arctan(Vv/Vh) (1)γ＝arctan((Zf-Zr)/L) (2)θ＝αGPS-γ(3)在本发明方法的一种优选的实施方式中，由安装在机动车质心处的陀螺仪测得机动车的俯仰角速度qgyro，由下列公式(6)和(7)通过俯仰卡尔曼滤波器算法计算出实际的α和陀螺仪的偏差qbias，同时通过公式(2)算出俯仰角γ，再通过将该α替换公式(3)中的αGPS而由公式(3)算出道路坡度角θ。
α·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>αGPS=10αqbias+η---(7)]]>当GPS速度测量仪不能正常工作时，可以设公式(5)中的qbias和ε均为0，这样就可以通过由陀螺仪测量的俯仰角速度值qgyro粗略地估算出α角，通过将该α角替换公式(3)中的αGPS，同时通过公式(2)算出俯仰角γ，就可以通过公式(3)估算出道路坡度角θ。因此，即使GPS速度测量仪不能正常工作，通过本发明的该优选的实施方式，也可以对道路坡度角进行估算，从而对机动车在坡路上的行驶进行有效控制。


图1为根据本发明的坡度角测量系统的结构框图；图2为机动车在坡路上的状况及根据本发明的坡度角测量系统各部件的安装位置示意图；图3为电子控制单元(ECU)的工作流程图；图4为在任一时刻n时的卡尔曼滤波的处理框图。
具体实施例方式
以下结合附图对本发明的具体实施方式
进行更详细的说明。
如图1和图2所示，本发明的车载道路坡度角测量系统包括一个位于机动车车顶的车载GPS速度测量仪20、安装在机动车质心(GC)处的陀螺仪50、安装在机动车前悬架上的前悬架位移传感器30和安装在机动车后悬架上的后悬架位移传感器40，以及电子控制单元(ECU)60和驱动控制模块70。位移传感器30、40以及陀螺仪50均与电子控制单元(ECU)60电连接。驱动控制模块70与电子控制单元电连接，用于根据电子控制单元(ECU)60提供的道路坡度角对机动车在坡路上的行驶进行有效控制。所述电子控制单元(ECU)60和驱动控制模块70可以是集成在机动车主控板上的一部分。
所述GPS速度测量仪以及所述前、后悬架位移传感器和陀螺仪是现有技术已有的常规设备，其可以从市售商品获得，例如Thales Navigation公司的Aquarius 5002 unit或Racelogic公司的VBOX等。
本发明的工作原理如下如图2所示，在坡度角为θ的道路上，在行驶方向为u的机动车10的车顶置有GPS速度测量仪20，由该GPS速度测量仪20可以获得在坡路上行驶的机动车在某一瞬间的垂直速度Vv及水平速度Vh；电子控制单元(ECU)60的数据接收模块获取该速度后，由其数据处理模块根据公式(1)计算出GPS速度测量仪的测量值αGPS，该αGPS应该是道路坡度角θ与机动车俯仰角η之和。
αGPS＝arctan(Vv/Vh) (1)式中Vv和Vh分别是GPS速度测量仪测量得到的垂直速度和水平速度。
同时，安装在机动车前悬架处的前悬架位移传感器30测得所述瞬间时的前悬架位置处的车身沿垂直于道路平面方向的位移Zf，安装在机动车后悬架处的后悬架位移传感器40测得所述瞬间时的后悬架位置处的车身沿垂直于道路平面方向的位移Zr，电子控制单元(ECU)60的数据接收模块接收所述Zf和Zr的值，并且由于由前、后悬架位移传感器的安装位置所确定的L的值是已知的，因此电子控制单元(ECU)60的数据处理模块可以根据下列公式(2)计算出机动车行驶在坡度路面时的俯仰角γγ＝arctan((Zf-Zr)/L) (2)由于αGPS是道路坡度角θ与机动车俯仰角η之和，因此电子控制单元(ECU)60的数据处理模块可以根据下列公式(3)计算出道路坡度角θ。
θ＝αGPA-γ (3)在实际的测量过程中，由于GPS速度测量仪20可能存在测量偏差(噪声)，因此测得的αGPS和实际的α之间的关系可以表达为下列公式(4)αGPS＝α+η (4)式中η为GPS测量过程中的噪声。
为了更精确地测量道路坡度角θ，在机动车的质心(GC)处还安装了一个陀螺仪50，其与所述电子控制单元(ECU)60电连接，用来测量机动车的俯仰角速度q。记陀螺仪的俯仰角速度测量值为qgyro，则存在下列关系式(5)qgyro=α·+qbias+ϵ---(5)]]>式中qbias是陀螺仪的偏差，ε是陀螺仪测量过程中的噪声。
将(5)式写成矩阵的形式得α·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>同样也可以把(4)式写成矩阵形式得αGPS=10αqbias+η---(7)]]>公式(6)和(7)可以简写成如下形式得x·=Φx+Ψu+ϵ---(8)]]>y＝Hx+η (9)将公式(8)和(9)写成离散化形式得x[n+1]＝Φx[n]+Ψu[n]+ε[n] (10)y[n]＝Hx[n]+η[n] (11)式中x＝[αqbias]T，y＝[αGPS]，Φ=0-100,]]>Ψ＝[10]T，H＝[10]，u＝[qgyro]，ε和η是相互独立的高斯随机向量，也就是高斯白噪声。
对于形如公式(10)与公式(11)构成的单输入-单输出的系统状态模型，可以用卡尔曼滤波算法对向量x进行最优估计。
卡尔曼滤波是一种公知的数学算法，其数学原理如下形如公式(10)和(11)的状态模型，其卡尔曼滤波满足以下递推方程A.测量更新[n|n]＝[n|n-1]+K[n](y[n]-H[n|n-1])(12)K[n]＝P[n|n-1]HT(HP[n|n-1]HT+R[n])-1(13)P[n|n]＝(I-K[n]H)P[n|n-1] (14)B.时间更新[n+1|n]＝Φ[n|n]+Ψu[n] (15)P[n+1|n]＝ΦP[n|n]ΦT+Q[n] (16)以上5个公式中[n|n]表示利用n时刻的量测值y[n]进行更新的x的更新值，也就是在n时刻对x进行的最佳估计，如图4所示；[n+1|n]表示由y
直到y[n]的量测值估计的n+1时刻的x值；I是单位矩阵；K[n]是卡尔曼增益矩阵；Q[n]是模型噪声ε[n]的协方差矩阵，为Q[n]＝E(ε[n]ε[n]T)；R[n]是量测噪声η[n]的协方差矩阵，为R[n]＝E(η[n]η[n]T)；P[n|n]和P[n|n-1]是误差协方差矩阵，他们的定义为P[n|n]＝E({x[n]-[n|n]}{x[n]-[n|n]T})P[n|n-1]＝E({x[n]-[n|n-1]}{x[n]-[n|n-1]T})其中x[n]表示在n时刻x的实际值，符号E()的含义是求括号中表达式的期望值。
当给定0时刻的初始值x[1|0]和P[1|0]后，就可以反复迭代卡尔曼滤波算法的5个公式，即公式(12)～公式(16)，从而在任一n时刻都可以估计出最佳的x值，如图4所示。
这样，就可以经过卡尔曼滤波后较为准确地估计出x，即α角和陀螺仪的偏差qbias。通过由此测得的α角替换公式(3)中的αGPS，同时通过公式(2)算出俯仰角γ，就可以通过公式(3)算出道路坡度角θ。
另外，当GPS速度测量仪不能正常工作时，可以设公式(5)中的qbias和ε均为0，这样就可以通过由陀螺仪测量的俯仰角速度值qgyro粗略地估算出α角，通过将该α角替换公式(3)中的αGPS，同时通过公式(2)算出俯仰角γ，就可以通过公式(3)估算出道路坡度角θ。因此，即使GPS速度测量仪不能正常工作，通过本发明提供的陀螺仪，也可以对道路坡度角进行估算。
通过得到的道路坡度角θ，驱动控制模块70可以根据机动车动力学方程调节驱动力，使机动车能够在坡道上也能够获得很好的驱动性能。
图3为电子控制单元(ECU)的工作流程图。在图3的步骤S200中，ECU 60从GPS速度测量仪20、陀螺仪50、前悬架位移传感器30和后悬架位移传感器40获取信号；在步骤S300中根据公式(1)计算α角的GPS测量值αGPS；在步骤S400中，利用步骤S300中的结果和俯仰卡尔曼滤波器算法由公式(6)和公式(7)计算α角的估计值；在步骤S500中，根据公式(2)计算汽车的俯仰角γ；在步骤S600中，根据公式(3)计算出道路的坡度角θ。
权利要求
1.一种车载道路坡度角测量系统，其包括GPS速度测量仪，其用于测量行驶中的机动车在某一瞬间的垂直速度Vv及水平速度Vh；安装在机动车前悬架处的前悬架位移传感器，用于测量所述瞬间时的前悬架位置处的车身沿垂直于道路平面方向的位移Zf；安装在机动车后悬架处的后悬架位移传感器，用于测量所述瞬间时的后悬架位置处的车身沿垂直于道路平面方向的位移Zr；电子控制单元，其与所述GPS速度测量仪电连接以获取所述垂直速度Vv和水平速度Vh，与所述前悬架位移传感器电连接以获取所述前悬架位置处的车身沿垂直于道路平面方向的位移Zf，与所述后悬架位移传感器电连接以获取所述后悬架位置处的车身沿垂直于道路平面方向的位移Zr，并根据下列公式(1)、(2)和(3)计算出道路坡度角θαGPS＝arctan(Vv/Vh)(1)γ＝arctan((Zf-Zr)/L)(2)θ＝αGPS-γ(3)式中，γ为机动车行驶在坡度路面时的俯仰角，θ为需要测定的道路坡度角，αGPS为由GPS速度测量仪测定的所述速度通过公式(1)计算得出的机动车俯仰角与道路坡度角之和，L为前悬架位移传感器与后悬架位移传感器之间的距离。
2.根据权利要求1所述的车载道路坡度角测量系统，其中所述GPS速度测量仪安装在机动车的车顶上。
3.根据权利要求1所述的车载道路坡度角测量系统，其中所述电子控制单元包括数据接收模块和数据处理模块；所述数据接收模块用于从所述GPS速度测量仪接收所述垂直速度Vv和水平速度Vh，并从所述前、后悬架位移传感器分别接收所述前悬架位置处的车身沿垂直于道路平面方向的位移Zf和所述后悬架位置处的车身沿垂直于道路平面方向的位移Zr；所述数据处理模块用于根据接收的Vv、Vh、Zf、Zr、以及由前、后悬架位移传感器的安装位置所确定的L的值，由公式(1)、(2)和(3)计算出道路坡度角θ。
4.根据权利要求1所述的车载道路坡度角测量系统，还包括安装在机动车质心处的陀螺仪，其与所述电子控制单元电连接，用于测量机动车的俯仰角速度qgyro，由下列公式(6)和(7)通过俯仰卡尔曼滤波器算法计算出实际的α和陀螺仪的偏差qbias，同时通过公式(2)算出俯仰角γ，再通过将该α替换公式(3)中的αGPS而由公式(3)算出道路坡度角θα·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>式中α为实际的道路坡度角与机动车俯仰角之和， 表示实际的机动车俯仰角速度，qbias是陀螺仪的测量偏差， 表示陀螺仪偏差随时间的变化率，qgyro是机动车俯仰角速度的陀螺仪测量值，ε是高斯白噪声；αGPS=10αqbias+η---(7)]]>式中η是1×1高斯白噪声。
5.一种通过权利要求1所述测量系统对道路坡度角进行测量的方法，该方法包括由GPS速度测量仪测得在坡路上行驶的机动车在某一瞬间的垂直速度Vv及水平速度Vh；由安装在机动车前悬架处的前悬架位移传感器测得所述瞬间时的前悬架位置处的车身沿垂直于道路平面方向的位移Zf；由安装在机动车后悬架处的后悬架位移传感器测得所述瞬间时的后悬架位置处的车身沿垂直于道路平面方向的位移Zr；由电子控制单元接收所述Vv、Vh、Zf、Zr的值，并由下列公式(1)、(2)和(3)计算出道路坡度角θ。αGPS＝arctan(Vv/Vh)(1)γ＝arctan((Zf-Zr)/L)(2)θ＝αGPS-γ (3)
6.根据权利要求5所述的对道路坡度角进行测量的方法，其特征在于，该方法还包括由安装在机动车质心处的陀螺仪测得机动车的俯仰角速度qgyro，由下列公式(6)和(7)通过俯仰卡尔曼滤波器算法计算出实际的α和陀螺仪的偏差qbias，同时通过公式(2)算出俯仰角γ，再通过将该α替换公式(3)中的αGPS而由公式(3)算出道路坡度角θα·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>式中α为实际的道路坡度角与机动车俯仰角之和， 表示实际的机动车俯仰角速度，qbias是陀螺仪的测量偏差， 表示陀螺仪偏差随时间的变化率，qgyro是机动车俯仰角速度的陀螺仪测量值，ε是高斯白噪声；αGPS=10αqbias+η---(7)]]>式中η是1×1高斯白噪声。
7.根据权利要求6所述的对道路坡度角进行测量的方法，其特征在于，当GPS速度测量仪不能正常工作时，由公式qgyro=α·]]>估算出α角，同时通过公式(2)算出俯仰角γ，再通过将该α角替换公式(3)中的αGPS而由公式(3)估算出道路坡度角θ。
全文摘要
本发明提供一种车载道路坡度角测量系统及车载道路坡度角测量方法。其中所述车载道路坡度角测量系统包括GPS速度测量仪，其用于测量行驶中的机动车在某一瞬间的垂直速度V
文档编号B60R21/01GK1800780SQ20041010403
公开日2006年7月12日 申请日期2004年12月31日 优先权日2004年12月31日
发明者宋佑川 申请人:比亚迪股份有限公司