CN102422166A - 移动状态检测装置 - Google Patents

Abstract

高精度地算出加速度传感器的安装角度，并高精度地校正来自该加速度传感器的加速度。加速度校正部(10)的频率解析部(11)对来自加速度传感器(20)的传感器坐标系加速度进行小波变换，分解成偏置频率成分、重力频率成分、运动加速度频率成分及噪音频率成分。频率解析部(11)只将重力频率成分与运动加速度频率成分的和成分输出给安装角度推算部(12)及校正演算部(13)。安装角度推算部(12)基于只由重力频率成分与运动加速度频率成分的和成分组成的加速度，推算出加速度传感器(20)的安装角度输出给校正演算部(13)。校正演算部(13)通过用推算出的安装角度校正只由重力频率成分与运动加速度频率成分的和成分组成的加速度，算出并输出移动体坐标系加速度。

Description

移动状态检测装置

技术领域

本发明涉及一种移动状态检测装置，该移动状态检测装置设置在移动体上，检测该移动体的速度或俯仰角等移动状态。

背景技术

当前，正在设计各种导航装置，该导航装置安装在汽车等移动体上，检测该移动体的位置、行驶速度及行驶方位，并显示对向目的地的行驶进行援助的信息。并且，在这样的导航装置中，基于来自GPS卫星等定位卫星的定位信号检测自装置位置，且利用移动体原有的速度信息如汽车的车速脉冲或通过陀螺传感器等得到的方位信息一直检测移动体的移动状态。

不过，最近，也正在设计各种与事先设置在上述那样的移动体上、能取得移动体的速度信息等的导航装置不同，能通过导航装置自机取得速度信息等来执行导航的因比较小型而能携带的个人导航装置。然后，通过在移动体上设置这样的个人导航装置，即使原来在移动体上没有安装导航装置，也能利用导航系统。

利用这样的个人导航装置时，因为向移动体的安装角度即个人导航装置内的加速度传感器的向移动体的安装角度，有时不能准确地取得移动体的行驶速度或行驶方位，而难以准确地执行导航。因此，设置如专利文献1、2所示的安装角度算出装置，算出加速度传感器的安装角度，来一直校正加速度。

[现有技术文献]

[专利文献]

[专利文献1]日本特开2007-107951号公报

[专利文献2]日本特开2009-14732号公报

[发明概要]

[发明要解决的课题]

但是，在上述的专利文献1，2示出的安装角度算出装置中，并非只通过加速度传感器的输出值检测安装角度，还利用来自陀螺传感器或角速度传感器的角速度或从上述的移动体得到的车速脉冲等。此外，从/向加速度传感器输出的加速度含有偏置成分或噪音成分等无用成分但并没有考虑这些。从而，因为利用含有偏置成分或噪音成分所引起的误差的加速度来算出安装角度，所以不能高精度地算出安装角度。因此，不能高精度地校正来自加速度传感器的加速度，难以准确地取得移动体的行驶速度或行驶方位。

发明内容

本发明的目的在于实现一种移动状态检测装置，该移动状态检测装置能除去从加速度传感器输出的加速度所含的偏置成分或噪音成分等无用成分引起的影响，能高精度地算出加速度传感器的安装角度，能高精度地校正来自该加速度传感器的加速度。

[解决课题的手段]

本发明的移动状态检测装置具备加速度传感器与加速度校正部。加速度传感器设置在移动体上，检测该移动体的加速度。加速度校正部按照频带分解从该加速度传感器得到的加速度，基于由规定频带成分组成的重力频率成分及运动加速度频率成分推算所述加速度传感器的安装角度，并基于该安装角度校正加速度。

更具体而言，本发明的移动状态检测装置具备加速度传感器与加速度校正部。加速度传感器设置在移动体上，检测该移动体的加速度。加速度校正部从低频侧依次按照频带将从加速度传感器得到的加速度分解成偏置频率成分、重力频率成分、运动加速度频率成分及噪音频率成分，基于重力频率成分及运动加速度频率成分推算加速度传感器的安装角度，并基于该安装角度校正加速度。

在该构成中，按照频率成分分解通过加速度传感器得到的加速度。在此，偏置频率成分是与移动体的移动状态无关而大致稳定地输出的成分，由极低频组成。噪音频率成分是与移动体的移动状态无关而随机持续变化的成分，由高频组成。然后，与这些相别，重力频率成分及运动加速度频率成分依存于移动体的移动状态，因为与偏置频率成分相比变化大，所以与偏置频率成分相比为高频率成分，因为没有噪音频率成分那样的随机性，所以与噪音频率成分相比为低频率成分。因此，通过按照频率成分分解加速度，能除去不依存于移动体的移动状态的、含有偏置频率成分或噪音频率成分的无用成分。据此，能取得只由依存于移动体的移动状态的重力频率成分及运动加速度频率成分组成的准确的加速度。

此外，本发明的移动状态检测装置的加速度校正部通过小波变换对所述加速度进行频率分解。

在该构成中，为取得加速度的各频率成分采用小波变换，所以不仅能得到在频率轴上展开的各频率成分，也能直观地取得各频率成分在时间轴上的变化状态等。

此外，本发明的移动状态检测装置的加速度传感器按照分别正交的前后方向加速度成分、横方向加速度成分及上下方向加速度成分检测加速度成分。加速度校正部执行以下推算中的至少一个：通过前后方向加速度成分与横方向加速度成分推算方位方向安装角度，通过前后方向加速度成分与上下方向加速度成分推算俯仰方向安装角度，通过前后方向加速度成分、横方向加速度成分及上下方向加速度成分与俯仰方向安装角度推算横滚方向安装角度。

此外，本发明的移动状态检测装置的加速度传感器按照分别正交的前后方向加速度成分、横方向加速度成分及上下方向加速度成分检测加速度成分。加速度校正部通过前后方向加速度成分与横方向加速度成分推算方位方向安装角度，通过前后方向加速度成分与上下方向加速度成分推算俯仰方向安装角度，通过前后方向加速度成分、横方向加速度成分及上下方向加速度成分与俯仰方向安装角度推算横滚方向安装角度。

在这些构成中，示出具体地取得正交的3轴的加速度成分来推算算出加速度传感器的立体的安装角度的情况。

此外，横方向加速度成分不足规定阈值时，本发明的移动状态检测装置的加速度校正部推算方位方向安装角度、俯仰方向安装角度中的至少一个。

此外，横方向加速度成分不足规定阈值时，本发明的移动状态检测装置的加速度校正部推算方位方向安装角度及俯仰方向安装角度。

此外，横方向加速度成分是规定阈值以上时，本发明的移动状态检测装置的加速度校正部推算横滚方向安装角度。

在这些构成中，示出了安装角度算出的具体的方法，根据基于横方向加速度成分的移动体的移动状态的变化，推算方位方向安装角度、俯仰方向安装角度、横滚方向安装角度。

此外，本发明的移动状态检测装置的加速度校正部依次存储推算的安装角度，且通过时间平均处理算出安装角度。此时，本次推算的安装角度相对于紧前推算的安装角度变化规定角度以上时，加速度校正部降低时间平均处理中的过去的安装角度的权重。

在该构成中，通过执行时间平均处理，抑制推算的安装角度的误差成分。比如，若是在普通的城镇地面等行驶的车辆，大部分的行驶路径是平坦地面，所以通过时间平均处理抑制上坡或下坡时推算的受倾斜角的影响的安装角度引起的误差成分。据此，能更高精度地计算安装角度。此外，因用户强制地变更安装角度的情况等，若安装角度变化大，则能抑制过去推算出的安装角度的影响。

此外，本发明的移动状态检测装置具备速度/行驶角算出部，该速度/行驶角算出部基于通过加速度校正部校正的加速度，计算移动体的速度、上下方向倾斜角即行驶俯仰角中的至少一个。

在该构成中，示出了移动状态检测装置的具体的构成例，根据如上述高精度地取得的移动体的加速度，能高精度地算出移动体的速度或行驶角。

此外，本发明的移动状态检测装置的速度/行驶角算出部算出行驶俯仰角时，算出用重力频率成分除以重力加速度而得到的值，通过该值校正算出了的行驶俯仰角。

在该构成中，采用上述的时间平均处理时，有可能产生该时间平均处理引起的累计误差。因此，通过以只根据加速度传感器的加速度的重力频率成分与重力加速度得到的行驶俯仰角，校正根据时间平均处理得到的行驶俯仰角，能抑制上述的累计误差，持续地算出高精度的行驶俯仰角。

此外，本发明的移动状态检测装置具备停止检测部，噪音频率成分被算出时，该停止检测部检测出运动加速度频率成分及噪音频率成分的和不足规定阈值来检测到移动体的停止。

在该构成中，作为上述的移动体的移动状态的一状态能准确地检测停止状态。

此外，本发明的一种导航装置，具备上述的移动状态检测装置，基于该移动状态检测装置算出的与所述移动体的移动相关的信息执行自装置的位置或导航处理。

在该构成中，因能如上述通过移动状态检测装置取得高精度的加速度或速度、行驶俯仰角，利用它们能执行准确的导航处理。

[发明的效果]

根据本发明，通过按照频率成分来分解，能高精度地取得加速度，所以能高精度地算出加速度传感器的安装角度，高精度地校正加速度。据此，能高精度地检测移动体的速度或行驶角。

附图说明

[图1]是表示第1实施方式的移动状态检测装置1的主要构成的框图。

[图2]是说明加速度传感器20相对于移动体的安装角度的图。

[图3]是表示本实施方式的构成与以往的构成引起的上下方向的加速度的变化的坐标图。

[图4]是表示第2实施方式的移动状态检测装置1’的主要构成的框图。

[图5]是表示第3实施方式的移动状态检测装置100的主要构成的框图。

具体实施方式

对照附图说明本发明的第1实施方式所涉及的移动状态检测装置。本实施方式所涉及的移动状态检测装置利用在车载用导航装置或PND(Personal Navigation Device)等各种导航装置上。

图1是表示本实施方式的移动状态检测装置1的主要构成的框图。

如图1所示，移动状态检测装置1具备加速度传感器20、加速度校正部10、速度/行驶角算出部30，固定设置在车辆等移动体上。另外，本实施方式的移动状态检测装置1检测移动体的速度及行驶角(行驶俯仰角、行驶横滚角、行驶方位角(偏航角))，但例如，若是只输出移动体的加速度，则可省略速度/行驶角算出部30。

加速度传感器20在自身的坐标系(传感器坐标系)中，检测由与移动体的前后方向即x轴方向对应的加速度成分

与移动体的横方向即y轴方向对应的加速度成分

与移动体的上下方向即z轴方向对应的加速度成分

组成的传感器坐标系加速度

在此，设为相对于移动体，按照由方位方向安装角度Δψ、俯仰方向安装角度Δθ、横滚方向安装角度Δφ构成的安装角度[Δψ，Δθ，Δφ]来安装加速度传感器20。而且，安装角度[Δψ，Δθ，Δφ]以图2所示的坐标系为基准。图2是说明移动体的坐标系的图。

如图2所示，移动体的坐标系是由以移动体的前后方向为轴方向的x轴、以移动体的横方向为轴方向的y轴、以移动体的上下方向为轴方向的z轴构成的正交3轴的坐标系。而且，将以x轴为中心轴进行转动的方向设为横滚角(φ)方向，将以y轴为中心轴进行转动的方向设为俯仰角(θ)方向，将以z轴为中心轴进行转动的方向设为方位角(ψ)方向。

如此，相对于移动体按照安装角度[Δψ，Δθ，Δφ]来安装加速度传感器20，所以从加速度传感器20输出的传感器坐标系加速度

的各成分与移动体坐标系加速度

的各成分之间，产生与基于安装角度[Δψ，Δθ，Δφ]的立体角相应的差。

加速度校正部10具备频率解析部11、安装角度推算部12、校正演算部13。

频率解析部11通过小波变换将传感器坐标系加速度

变换成频率轴上的加速度成分组。更具体而言，频率解析部11例如每1秒取得传感器坐标系加速度

并存储64秒量，基于这64秒量的数据执行小波变换。于是，频率解析部11取得与64秒的采样期间对应的大致稳定成分(DC成分)、与32秒的采样期间对应的变化成分(AC成分)、与16秒的采样期间对应的变化成分(AC成分)、与8秒的采样期间对应的变化成分(AC成分)、与4秒的采样期间对应的变化成分(AC成分)、与2秒的采样期间对应的变化成分(AC成分)、与1秒的采样期间对应的变化成分(AC成分)。

频率解析部11设定取得的极低频带频率的64秒宽DC成分为偏置频率成分，设定中频带频率的32秒宽AC成分及16秒宽AC成分为重力频率成分，设定中频带频率的8秒宽AC成分、4秒宽AC成分及2秒宽AC成分为运动加速度频率成分，设定1秒宽AC成分为噪音频率成分。通过下面所示原理能设定这些。

首先，因为在64秒宽得到的DC成分能看做是不依存于移动体的移动状态的、加速度传感器20大致稳定地输出的成分。接着，在32秒宽或16秒宽得到的AC成分虽依存于移动体的移动状态，但影响较低，能看做是与移动体的运动加速度相比易依存于在移动体上所产生的重力的成分。接着，在8秒宽、4秒宽及2秒宽得到的AC成分是易带给移动体的移动状态大影响的成分，能看做是与稳定地一直产生的重力加速度相比易依存于移动体的运动加速度的成分。接着，在1秒宽得到的AC成分能看做是与移动体的运动加速度相比包含更多随机性的成分。

频率解析部11将通过小波变换得到的重力频率成分与运动加速度频率成分的和作为检测用加速度

向安装角度推算部12及校正演算部13输出。

安装角度推算部12根据检测用加速度

推算出安装角度[Δψ，Δθ，Δφ]。该安装角度的推算可根据上述的传感器坐标系加速度的取得定时，例如可按照每1秒来进行。此外，可边缓冲传感器坐标系加速度的各频率成分，边按照适当地设定的定时进行。

在此，就安装角度[Δψ，Δθ，Δφ]的推算算出原理加以说明。设定传感器坐标系加速度

移动体坐标系加速度

安装角度[Δψ，Δθ，Δφ]的情况下，下式成立。

[数式1]

$\left[\begin{array}{c}{a}_x^s\\ a_y^s\\ a_z^s\end{array}\right]=C_b^s\left[\begin{array}{c}{a}_x^b\\ a_y^b\\ a_z^b\end{array}\right]-\left(1\right)$

是将移动体坐标系变换成加速度传感器坐标系的旋转矩阵，利用安装角度[Δψ，Δθ，Δφ]能用下式表示。

[数式2]

$C_b^s=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\Δ\φ}\right)& \sin \left(\mathrm{\Δ\φ}\right)\\ 0& -\sin \left(\mathrm{\Δ\φ}\right)& \cos \left(\mathrm{\Δ\φ}\right)\end{array}\right)\left(\begin{array}{ccc}\cos \left(\mathrm{\Δ\θ}\right)& 0& -\sin \left(\mathrm{\Δ\θ}\right)\\ 0& 1& 0\\ \sin \left(\mathrm{\Δ\θ}\right)& 0& \cos \left(\mathrm{\Δ\θ}\right)\end{array}\right)\left(\begin{array}{ccc}\cos \left(\mathrm{\Δ\ψ}\right)& \sin \left(\mathrm{\Δ\ψ}\right)& 0\\ -\sin \left(\mathrm{\Δ\ψ}\right)& \cos \left(\mathrm{\Δ\ψ}\right)& 0\\ 0& 0& 1\end{array}\right)$

$=\left(\begin{array}{ccc}\cos \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\θ}\right)& \sin \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\θ}\right)& -\sin \left(\mathrm{\Δ\θ}\right)\\ -\sin \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\φ}\right)+\cos \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\sin \left(\mathrm{\Δ\φ}\right)& \cos \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\φ}\right)+\sin \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\sin \left(\mathrm{\Δ\φ}\right)& \cos \left(\mathrm{\Δ\θ}\right)\sin \left(\mathrm{\Δ\φ}\right)\\ \sin \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\φ}\right)+\cos \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\cos \left(\mathrm{\Δ\φ}\right)& -\cos \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\φ}\right)+\sin \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\cos \left(\mathrm{\Δ\φ}\right)& \cos \left(\mathrm{\Δ\θ}\right)\cos \left(\mathrm{\Δ\φ}\right)\end{array}\right)-\left(2\right)$

在此，安装角度[Δψ，Δθ，Δφ]的每一项是Δψ＜＜1[r a d]、Δθ＜＜1[r a d]、Δφ＜＜1[r a d]时，旋转矩阵能近似如下式。

[数式3]

$C_b^s=\left[\begin{array}{ccc}1& \mathrm{\Δ\ψ}& -\mathrm{\Δ\θ}\\ -\mathrm{\Δ\ψ}& 1& \mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}& -\mathrm{\Δ\φ}& 1\end{array}\right]-\left(3\right)$

因而，式(1)能用下式表示。

[数式4]

$\left[\begin{array}{c}{a}_x^s\\ a_y^s\\ a_z^s\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\Δ\ψ}& -\mathrm{\Δ\θ}\\ -\mathrm{\Δ\ψ}& 1& \mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}& -\mathrm{\Δ\φ}& 1\end{array}\right]\left[\begin{array}{c}{a}_x^b\\ a_y^b\\ a_z^b\end{array}\right]-\left(4\right)$

在此，若利用通过频率解析部11除去偏置频率成分及噪音频率成分而得到的检测用加速度，则上述式(4)表示成下式。

[数式5]

$\left[\begin{array}{c}{a}_{x\left(\mathrm{AG}\right)}^s\\ a_{y\left(\mathrm{AG}\right)}^s\\ a_{z\left(\mathrm{AG})\right)}^s\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\Δ\ψ}& -\mathrm{\Δ\θ}\\ -\mathrm{\Δ\ψ}& 1& \mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}& -\mathrm{\Δ\φ}& 1\end{array}\right]\left[\begin{array}{c}{a}_x^b\\ a_y^b\\ a_z^b\end{array}\right]-\left(5\right)$

可是，移动体在平坦地面直进行驶时，移动体坐标系加速度

的横方向成分上下方向成分可看做是“0”。即，

因而，式(5)如下式表示。

[数式6]

$\left[\begin{array}{c}{a}_{x\left(\mathrm{AG}\right)}^s\\ a_{y\left(\mathrm{AG}\right)}^s\\ a_{z\left(\mathrm{AG}\right))}^s\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\Δ\ψ}& -\mathrm{\Δ\θ}\\ -\mathrm{\Δ\ψ}& 1& \mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}& -\mathrm{\Δ\φ}& 1\end{array}\right]\left[\begin{array}{c}{a}_x^b\\ 0\\ 0\end{array}\right]=\begin{array}{c}\left[\begin{array}{c}{a}_x^b\\ -a_x^b\·\mathrm{\Δ\ψ}\\ a_x^b\·\mathrm{\Δ\θ}\end{array}\right]-\left(6\right)\end{array}$

据此，前后方向成分

非“0”时，通过下式能算出方位方向安装角度Δψ、俯仰方向安装角度Δθ。

[数式7]

$\mathrm{\Δ\ψ}=-\frac{a_{y\left(\mathrm{AG}\right)}^s}{a_{x\left(\mathrm{AG}\right)}^s}-\left(7A\right)$

$\mathrm{\Δ\θ}=+\frac{a_{z\mathrm{AG})}^s}{a_{x\left(\mathrm{AG}\right)}^s}-\left(7B\right)$

此外，移动体在平坦地面一直旋转行驶时，移动体坐标系加速度

的上下方向成分

能看做是“0”，前后方向成分与横方向成分能看做成大致同样大小。而且，通过利用上述的近似条件Δψ＜＜1[r a d]，式(5)能如下式表示。

[数式8]

$\left[\begin{array}{c}{a}_{x\left(\mathrm{AG}\right)}^s\\ a_{y\left(\mathrm{AG}\right)}^s\\ a_{z\left(\mathrm{AG}\right))}^s\end{array}\right]=\begin{array}{}\end{array}\left[\begin{array}{ccc}1& \mathrm{\Δ\ψ}& -\mathrm{\Δ\θ}\\ -\mathrm{\Δ\ψ}& 1& \mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}& -\mathrm{\Δ\φ}& 1\end{array}\right]\left[\begin{array}{c}{a}_x^b\\ a_y^b\\ 0\end{array}\right]=\left[\begin{array}{c}{a}_x^b+a_y^b\·\mathrm{\Δ\ψ}\\ a_y^b-a_x^b\·\mathrm{\Δ\ψ}\\ a_x^b\·\mathrm{\Δ\θ}-a_y^b\·\mathrm{\Δ\φ}\end{array}\right]\≈\left[\begin{array}{c}{a}_x^b\\ a_y^b\\ a_x^b\·\mathrm{\Δ\θ}-a_y^b\·\mathrm{\Δ\φ}\end{array}\right]-\left(8\right)$

据此，也利用通过上述的式(7)算出的俯仰方向方位角Δθ根据下式算出横滚方向安装角度Δφ。

[数式9]

$\mathrm{\Δ\φ}=-\frac{a_{z\left(\mathrm{AG}\right)}^s-a_{x\left(\mathrm{AG}\right)}^s\·\mathrm{\Δ\θ}}{a_{y\left(\mathrm{AG}\right)}^s}-\left(9\right)$

如以上，能只通过检测用加速度

推算出安装角度[Δψ，Δθ，Δφ]。

可是，如上述，平坦地面是算出处理的前提，但通过如下所示的时间平均处理，能解除平坦地面这个条件。这是因为在通常的城镇地面行驶中，与在具有上坡或下坡的倾斜地面行驶的期间相比，在平坦地面行驶的期间非常长。根据该设定，根据下式计算方位方向安装角度Δψ、俯仰方向安装角度Δθ及横滚方向安装角度Δφ。另外，在下式中，E[演算式]是表示时间平均处理的算子。

[数式10]

$\mathrm{\Δ\ψ}=E_{\mathrm{\Δ\ψ}}[-\frac{a_{y\left(\mathrm{AG}\right)}^s}{a_{x\left(\mathrm{AG}\right)}^s}]-\left(10A\right)$

$\mathrm{\Δ\θ}=E_{\mathrm{\Δ\θ}}\left[\frac{a_{z\mathrm{AG})}^s}{a_{x\left(\mathrm{AG}\right)}^s}\right]-\left(10B\right)$

$\mathrm{\Δ\φ}=E_{\mathrm{\Δ\φ}}[-\frac{a_{z\left(\mathrm{AG}\right)}^s-a_{x\left(\mathrm{AG}\right)}^s\·\mathrm{\Δ\θ}}{a_{y\left(\mathrm{AG}\right)}^s}]-\left(10C\right)$

此外，在上述的算出处理中，设定了是直进行驶还是旋转行驶的选择条件，这对横方向加速度

事先设定旋转检测用的阈值，该横方向加速度是该阈值以上时判定为旋转行驶，不足阈值时判定直进行驶即可。或者，对由后述的校正演算部13算出的移动体坐标系的横方向加速度

事先设定旋转检测用的阈值，该横方向加速度

是该阈值以上时判定为旋转行驶，不足阈值时判定为直进行驶即可。

基于如上述的原理，安装角度推算部12，根据上述的式(7A)、式(7B)、式(10)按照规定定时算出安装角度[Δψ，Δθ，Δφ]，利用后述的卡尔曼滤波进行与时间平均处理相当的处理后，向校正演算部13输出推算出的安装角度[Δψ，Δθ，Δφ]。

在此，以对方位方向安装角度Δψ利用1次LPF进行卡尔曼滤波处理的情况为例，但对其他的俯仰方向安装角度Δθ及横滚方向安装角度Δφ也同样进行卡尔曼滤波处理。

通过假定某定时t的方位方向安装角度Δψ为Δψ[t]，假定下一定时的方位方向安装角度Δψ为Δψ[t+1]，可设定下式。

[数11]

$\mathrm{\Δ\ψ}[t+1]=\mathrm{\Δ\ψ}\left[t\right]+\mathrm{\α}\·(-\frac{a_{y\left(\mathrm{AG}\right)}^s}{a_{x\left(\mathrm{AG}\right)}^s}-\mathrm{\Δ\ψ}[t\left]\right)$

其中，当

或旋转时，Δψ[t+1]＝Δψ[t]。

通过利用这样的演算式，对方位方向安装角度Δψ、或俯仰方向安装角度Δθ及横滚方向安装角度Δφ进行时间平均处理，解除平坦地面行驶的条件，能推算出安装角度[Δψ，Δθ，Δφ]。此时，如上述，将除去了偏置频率成分及噪音频率成分而得到的加速度提供给安装角度推算部12，所以能高精度地推算出安装角度[Δψ，Δθ，Δφ]。

另外，在此，α是等待值，能适当设定，若基于下示的条件设定等待值α，则能得到更好的效果。在此，也只对方位方向安装角度Δψ加以说明，关于俯仰方向安装角度Δθ及横滚方向安装角度Δφ同样也能取得效果。

对等待值α，传感器坐标系的前后方向加速度非“0”，在直进状态，且

不足安装角度变更检测用的阈值β时，设定为α₁。此外，对等待值α，传感器坐标系的前后方向加速度

非“0”，在直进状态，且

是安装角度变更检测用的阈值β以上时，设定为α₂。在此，α₁，α₂设定为0＜α₁≤α₂＜1。

通过进行这样的等待值α的设定，若本次算出的安装角度相对前次推算出并进行时间平均处理后的安装角度变化不大，则进行时间平均处理以使受基于过去的计算结果的安装角度的影响大。另一方面，若本次算出的安装角度相对前次推算出并进行时间平均处理的安装角度变化大，则进行时间平均处理以使难以受到基于过去的计算结果的安装角度的影响。据此，若未变更移动状态检测装置1即加速度传感器20的安装角度，则还是利用过去的稳定的安装角度，能进行更高精度的安装角度的推算算出，即使用户等强制地变更安装角度时，也能边抑制该变更引起的影响边不断地进行安装角度的推算算出。

校正演算部13通过基于在安装角度推算部12推算出的安装角度[Δψ，Δθ，Δφ]，校正从频率解析部11输出的检测用加速度

计算并输出移动体坐标系加速度

具体而言，该校正基于下面的原理。设定检测用加速度

移动体坐标系加速度

时，下式成立。

[数式12]

$\left[\begin{array}{c}{a}_x^b\\ a_y^b\\ a_z^b\end{array}\right]=C_b^s\left[\begin{array}{c}{a}_{x\left(\mathrm{AG}\right)}^s\\ a_{y\left(\mathrm{AG}\right)}^s\\ a_{z\left(\mathrm{AG}\right)}^s\end{array}\right]-\left(12\right)$

是将移动体坐标系变换成加速度传感器坐标系的旋转矩阵，利用推算出的安装角度[Δψ，Δθ，Δφ]可用下式表示。

[数式13]

$C_s^b=\left(\begin{array}{ccc}\cos \left(\mathrm{\Δ\ψ}\right)& -\sin \left(\mathrm{\Δ\ψ}\right)& 0\\ \sin \left(\mathrm{\Δ\ψ}\right)& \cos \left(\mathrm{\Δ\ψ}\right)& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}\cos \left(\mathrm{\Δ\θ}\right)& 0& \sin \left(\mathrm{\Δ\θ}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\Δ\θ}\right)& 0& \cos \left(\mathrm{\Δ\θ}\right)\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\Δ\φ}\right)& -\sin \left(\mathrm{\Δ\φ}\right)\\ 0& \sin \left(\mathrm{\Δ\φ}\right)& \cos \left(\mathrm{\Δ\φ}\right)\end{array}\right)$

$=\left(\begin{array}{ccc}\cos \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\θ}\right)& -\sin \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\φ}\right)+\cos \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\sin \left(\mathrm{\Δ\φ}\right)& \sin \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\φ}\right)+\cos \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\cos \left(\mathrm{\Δ\φ}\right)\\ \sin \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\θ}\right)& \cos \left(\mathrm{\Δ\ψ}\right)\cos \left(\mathrm{\Δ\φ}\right)+\sin \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\sin \left(\mathrm{\Δ\φ}\right)& -\cos \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\φ}\right)+\sin \left(\mathrm{\Δ\ψ}\right)\sin \left(\mathrm{\Δ\θ}\right)\cos \left(\mathrm{\Δ\φ}\right)\\ -\sin \left(\mathrm{\Δ\θ}\right)& \cos \left(\mathrm{\Δ\θ}\right)\sin \left(\mathrm{\Δ\φ}\right)& \cos \left(\mathrm{\Δ\θ}\right)\cos \left(\mathrm{\Δ\φ}\right)\end{array}\right)-\left(13\right)$

尤其，安装角度[Δψ，Δθ，Δφ]的每一项是Δψ＜＜1[r a d]、Δθ＜＜1[r a d]、Δφ＜＜1[r a d]时，旋转矩阵

可近似为如下式。

[数式14]

$C_s^b=\left[\begin{array}{ccc}1& -\mathrm{\Δ\ψ}& \mathrm{\Δ\θ}\\ \mathrm{\Δ\ψ}& 1& -\mathrm{\Δ\φ}\\ -\mathrm{\Δ\θ}& \mathrm{\Δ\φ}& 1\end{array}\right]-\left(14\right)$

校正演算部13利用这样的旋转矩阵

将检测用加速度

变换成移动体坐标系加速度

并输出。

进行这样的校正处理时，来自频率解析部11的检测用加速度除去了偏置频率成分及噪音成分，能高精度地推算出安装角度，因此算出的移动体坐标系加速度

也能为高精度的值。

速度/行驶角算出部30基于下面所示原理，算出移动体的行驶速度及行驶角。尤其，在以下的说明中，示出了只算出移动体的前后方向的行驶速度v_x及上下方向的行驶角即行驶俯仰角θ的例子，但关于其他方向的行驶速度或行驶角也能通过采用同样的原理算出。

移动体坐标系的加速度的前后方向成分

上下方向成分

前后方向的行驶速度v_x及行驶俯仰角θ在将重力加速度设为g时，成以下的关系。

[数式15]

$a_x^b=\frac{d{v}_x}{\mathrm{dt}}+g\·\mathrm{\θ}-\left(15A\right)$

$a_z^b=-v_x\·\frac{\mathrm{d\θ}}{\mathrm{dt}}-g-\left(15B\right)$

在此，通过如上述根据小波变换除去偏置频率成分而得到的检测用加速度

算出移动体坐标系的加速度的上下方向成分所以式(15B)能用下式表示。

[数式16]

$\frac{\mathrm{d\θ}}{\mathrm{dt}}=\left\{\begin{array}{c}-\frac{a_z^b}{v_x}\·\·\·v_x\≠0\\ 0\·\·\·\·\·\·v_x=0\end{array}\right.-\left(16\right)$

此外，同样地，通过如上述根据小波变换除去偏置频率成分而得到的检测用加速度

也能算出移动体坐标系的加速度的上下方向成分

所以式(15A)能用下式表示。

[数式17]

$\frac{d{v}_x}{\mathrm{dt}}=a_x^b-g\·\mathrm{\θ}-\left(17\right)$

因而，通过演算由式(16)、式(17)组成的联立微分方程式，能算出前后方向的行驶速度v_x、行驶俯仰角θ。

为实现它，速度/行驶角算出部30执行下式所示的卡尔曼滤波处理。另外，下式的t是时刻(演算定时)，Δt表示更新时间间隔。

[数式18]

$v_x[t+1]=a_x^b\left[t\right]-g\·\mathrm{\θ}\left[t\right]\·\mathrm{\Δt}-\left(18A\right)$

$\mathrm{\θ}[t+1]=\left\{\begin{array}{c}\mathrm{\θ}\left[t\right]-\frac{{\hat{a}}_z^b\left[t\right]}{v_x\left[t\right]}\·\mathrm{\Δt}\·\·\·\·\·\·\·\·\·v_x\left[t\right]\≠0\\ \mathrm{\θ}\left[t\right]\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·\·v_x\left[t\right]=0\end{array}\right.-\left(18B\right)$

通过进行这样的算出处理，能算出移动体的行驶速度及行驶角。此时，通过利用如上述高精度地取得的移动体坐标系的加速度，也能高精度地算出行驶速度及行驶角。

通过进行这样的处理，如下述的实验结果所示，移动体在平坦地面及有坡的倾斜地面行驶时，能准确地识别在平坦地面行驶中还是在倾斜地面行驶中。图3是表示上下方向的加速度的变化的坐标图，图3(A)表示利用本实施方式的构成时的上下方向加速度的变化，图3(B)表示利用以往的构成时的上下方向加速度的变化。尤其，本坐标图中，约240秒～约350秒的区间及约440秒～约550秒的区间是在倾斜地面行驶的时间带。

在以往的构成中，图3(B)如所示不能明确地检测出倾斜地面的行驶，但通过采用本实施方式的构成，如图3(A)所示，与以往相比能明确地识别倾斜地面的行驶期间。

另外，在上述的说明中，示出了根据式(18A)、(18B)算出前后方向的行驶速度v_x、行驶俯仰角θ的例子，在该方法中，有可能产生累计误差而精度劣化。因此，速度/行驶角算出部30从频率解析部11取得传感器坐标系加速度

的前后方向加速度

中的重力频率成分

然后，速度/行驶角算出部30通过用重力频率成分除以重力加速度g，算出校正用行驶俯仰角θc。

速度/行驶角算出部30通过对在上述的式(18A)、(18B)得到的行驶俯仰角θ、校正用行驶俯仰角θc进行加权平均处理，算出输出的行驶俯仰角。通过进行这样的处理，能防止积累累计误差，能抑制长期的演算处理所引起的精度的劣化。尤其，进行该加权平均处理的定时，可按照行驶俯仰角θ的算出定时，但可适当设定为更长的规定定时间隔。

下面，对照附图，说明第2实施方式所涉及的移动状态检测装置。

图4是表示本实施方式的移动状态检测装置1’的主要构成的框图。

本实施方式的移动状态检测装置1’是对第1实施方式所示的移动状态检测装置1追加停止检测部40的实施方式，在以下，只说明该停止检测部40及与之关联的部分。

停止检测部40从频率解析部11取得由传感器坐标系加速度

中的运动加速度频率成分与噪音频率成分的和组成的停止检测用加速度

停止检测部40对停止检测用加速度

的前后方向成分

及横方向成分

事先存储停止检测用的阈值(≠0)。停止检测部40若检测出停止检测用加速度的前后方向成分及横方向成分

都是停止检测用的阈值以下，则判断移动体停止着，将停止检测数据向速度/行驶角算出部30输出。速度/行驶角算出部30一取得停止检测数据，就设定前后方向的行驶速度v_x＝0，且相应于该设定基于上述的式(18B)算出行驶俯仰角θ。

通过采用这样的构成及处理，能通过简单的处理识别移动体的停止。

下面对照附图说明第3实施方式所涉及的移动状态检测装置。

图5表示本实施方式的移动状态检测装置100的主要构成的框图。

本实施方式的移动状态检测装置100不但基于由第1实施方式所示的移动状态检测装置1的构成得到的速度或行驶角，还基于来自GPS接收机102的各种数据或来自陀螺传感器101的角速度数据，算出移动体的位置及移动方位。因而，在以下的说明中，关于与第1实施方式所示的移动状态检测装置1的构成相同的部分省略其说明，只说明不同的部分。

移动状态检测装置100具备移动状态检测装置1的构成以及方位算出部50和位置算出部60，与陀螺传感器101与GPS接收机102相连接。

陀螺传感器101至少能检测方位方向的转动，检测方位方向的角速度，输出给方位检测部60。

GPS接收机102接收来自GPS卫星的GPS信号，根据接收的GPS信号通过已知的方法进行定位，算出GPS位置数据、GPS速度数据及GPS方位数据。GPS接收机102向位置算出部60输出GPS位置数据，向速度/行驶角算出部30’输出GPS速度数据，向方位算出部50输出GPS方位数据。

速度/行驶角算出部30’在一直取得GPS速度数据的期间，利用GPS速度数据的微分值，通过上述的式(18A)，(18B)算出行驶角。或者，速度/行驶角算出部30’利用GPS速度数据的微分值与从加速度校正部10输出的移动体坐标系加速度的加权平均值等，通过上述的式(18A)，(18B)算出行驶角。另一方面，速度/行驶角算出部30’若不能取得GPS速度数据，则将利用GPS速度数据最后输出的速度数据设成初始值，通过上述的式(18A)，(18B)算出速度及行驶角。

方位算出部50在一直取得GPS方位数据期间，直接输出GPS方位数据。或者，方位算出部50也可算出对来自陀螺传感器101的角速度数据进行积分并与前次输出的方位数据积算后的值与GPS方位数据的加权平均值并输出。另一方面，方位算出部50若不能取得GPS方位数据，则将利用GPS方位数据最后输出的方位数据设为初始值，通过对角速度数据进行积分并积算来算出方位数据。

位置算出部60在一直取得GPS位置数据期间，直接输出GPS位置数据。或者，位置算出部60也可算出对根据速度/行驶角算出部30的速度数据、行驶角数据及方位算出部50的方位数据演算后的速度向量进行积分并与前次输出的位置数据相加而得到的值与GPS位置数据的加权平均值并输出。

另一方面，位置算出部60若不能取得GPS位置数据，则将利用GPS位置数据最后输出的位置数据设成初始值，通过对根据速度/行驶角算出部30的速度数据、行驶角数据及方位算出部50的方位数据演算后的速度向量进行积分并积算来算出位置数据。

通过设定这样的构成，能高精度地算出移动体的位置、速度、行驶角、方位。

如此，高精度地算出的移动体的各种移动信息利用到安装移动状态检测装置1的导航装置中的导航处理等。该导航装置至少具备执行路线导航处理的导航处理部、显示部以及操作部，显示部也可兼用为操作部，例如，按照来自操作部的操作输入通过导航部，根据移动体的当前位置及目的位置算出最佳路线，将该路线显示到显示部上。并且，如上述，能高精度地取得移动体的移动信息，因此导航装置能实现准确的导航处理。

另外，在本实施方式中，示出了将本实施方式的构成适用到第1实施方式的构成的例子，对第2实施方式的构成也同样能适用本实施方式的构成。

此外，在上述的说明中，示出了通过频率解析部11进行小波变换的例子。优选利用小波变换，但也可执行其他的频率变换处理，比如傅里叶变换处理等，进一步，也可为用通带不同的多个滤波器分解成各频率成分。

此外，在上述的说明中，示出了按照功能将加速度校正部10分割成频率解析部11、安装角度推算部12、校正演算部13的例子，但也可通过一个演算元件和执行程序实现这些。进一步，也可以通过一个演算元件和执行程序实现加速度校正部10和速度/行驶角算出部30、30’，也可以在此基础上还包括停止检测部40，也通过一个演算元件和执行程序来实现。

此外，在上述的说明中，示出了推算出三维的安装角度[Δψ，Δθ，Δφ]的例子，但根据需求，也可推算出安装角度[Δψ，Δθ，Δφ]的各要素(方位方向安装角度Δψ、俯仰方向安装角度Δθ、横滚方向安装角度Δφ)的至少一个。

此外，在上述的说明中，示出了算出行驶速度v_x、行驶俯仰角θ的例子，但关于它们，也可根据需求算出至少一个。

此外，在上述的说明中，示出了将传感器坐标系加速度

分解成偏置频率成分、重力频率成分、运动加速度频率成分及噪音频率成分的四个成分，而利用于推算算出的例子，但提取传感器坐标系加速度

中的至少重力频率成分与运动加速度频率成分利用到安装角度的推算算出即可。此时，利用例如以与重力频率成分、运动加速度频率成分相对应的频带为通带的带通滤波器即可。而且，利用该带通滤波器时，可进行与行驶速度等移动体的移动状态相应改变通带等的处理。

[符号的说明]

1，1’，1”，100-移动状态检测装置、10-加速度校正部、11-频率解析部、12-安装角度推算部、13-校正演算部、20-加速度传感器、30-速度/行驶角算出部、40-停止检测部、50-方位算出部、60-位置算出部、101-陀螺传感器、102-GPS接收机

Claims (13)

1.一种移动状态检测装置，具备，

加速度传感器，该加速度传感器设置在移动体上；

加速度校正部，该加速度校正部按照频带分解从该加速度传感器得到的加速度，基于由规定频带成分组成的重力频率成分及运动加速度频率成分推算所述加速度传感器的安装角度，并基于该安装角度校正所述加速度。

2.如权利要求1所述的移动状态检测装置，

所述加速度校正部将从该加速度传感器得到的加速度按照频带分解成偏置频率成分、所述重力频率成分、所述运动加速度频率成分及噪音频率成分，基于所述重力频率成分及所述运动加速度频率成分推算所述加速度传感器的安装角度，并基于该安装角度校正所述加速度。

3.如权利要求1或2所述的移动状态检测装置，

所述加速度校正部通过小波变换对所述加速度进行频率分解。

4.如权利要求1或3所述的移动状态检测装置，

所述加速度传感器按照分别正交的前后方向加速度成分、横方向加速度成分及上下方向加速度成分检测加速度成分；

所述加速度校正部执行以下推算中的至少一个：通过所述前后方向加速度成分与所述横方向加速度成分推算方位方向安装角度，通过所述前后方向加速度成分与所述上下方向加速度成分推算俯仰方向安装角度，通过所述前后方向加速度成分、所述横方向加速度成分及所述上下方向加速度成分与所述俯仰方向安装角度推算横滚方向安装角度。

5.如权利要求1～3中任一项所述的移动状态检测装置，

所述加速度传感器按照分别正交的前后方向加速度成分、横方向加速度成分及上下方向加速度成分检测加速度成分；

所述加速度校正部通过所述前后方向加速度成分与所述横方向加速度成分推算方位方向安装角度，通过所述前后方向加速度成分与所述上下方向加速度成分推算俯仰方向安装角度，通过所述前后方向加速度成分、所述横方向加速度成分及所述上下方向加速度成分与所述俯仰方向安装角度推算横滚方向安装角度。

6.如权利要求4所述的移动状态检测装置，

所述横方向加速度成分不足规定阈值时，所述加速度校正部推算所述方位方向安装角度、所述俯仰方向安装角度中的至少一个。

7.如权利要求5所述的移动状态检测装置，

所述横方向加速度成分不足规定阈值时，所述加速度校正部推算所述方位方向安装角度及所述俯仰方向安装角度。

8.如权利要求4～7中任一项所述的移动状态检测装置，

所述横方向加速度成分是所述规定阈值以上时，所述加速度校正部推算所述横滚方向安装角度。

9.如权利要求1～8中任一项所述的移动状态检测装置，

所述加速度校正部依次存储推算的所述安装角度，且通过时间平均处理算出所述安装角度；

本次推算的安装角度相对于紧前推算的安装角度变化规定角度以上时，降低所述时间平均处理中的过去的安装角度的权重。

10.如权利要求1～9中任一项所述的移动状态检测装置，

具备速度/行驶角算出部，该速度/行驶角算出部基于通过所述加速度校正部校正的加速度，算出所述移动体的速度、上下方向倾斜角即行驶俯仰角中的至少一个。

11.如权利要求10所述的移动状态检测装置，

所述速度/行驶角算出部算出所述行驶俯仰角时，算出用所述重力频率成分除以重力加速度而得到的值，通过该值校正算出了的行驶俯仰角。

12.如权利要求2～11中任一项所述的移动状态检测装置，

具备停止检测部，所述噪音频率成分被算出时，该停止检测部检测出所述运动加速度频率成分及所述噪音频率成分的和不足规定阈值来检测到所述移动体的停止。

13.一种导航装置，

具备权利要求1～权利要求12的任一项所述的移动状态检测装置，

该导航装置基于该移动状态检测装置算出的与所述移动体的移动相关的信息执行自装置的位置或导航处理。

Images