自主导航系统的制作方法

本发明涉及使用了卡尔曼滤波器的自主导航系统。

背景技术：

1、作为使用了卡尔曼滤波器的自主导航系统，已知有搭载于汽车的自主导航系统(例如，专利文献1)。

2、在该自主导航系统中，利用反复扩展卡尔曼滤波器对车速传感器的输出、检测3轴方向的加速度的加速度传感器的输出、检测绕3轴的角速度的陀螺仪的输出、gps接收机的输出进行处理，来推定汽车的三维位置矢量等的状态。

3、另外，作为与本发明有关的技术，已知有如下技术：使用检测xs轴方向的加速度的加速度传感器的输出、检测绕与xs轴垂直的zs轴的角速度的陀螺仪的输出、gps接收机的输出，检测包含该加速度传感器、陀螺仪的装置向汽车的安装角(例如，专利文献2)。

4、在此，在该技术中，将朝向汽车的前方方向的轴设为xm，将朝向汽车的上方方向的轴设为zm，将xs-zs坐标系相对于xm-zm坐标系的俯仰角θpitch、偏摆角θyow、翻滚角θroll作为向汽车的安装角，并如以下那样求出。

5、即，对车辆的行进方向的加速度分量为零时的加速度传感器输出的加速度as进行累计，计算平均化后的值aave。然后，如式1所示，将值aave除以重力加速度g而求出sin(θpitch)，根据sin(θpitch)的值得到俯仰角θpitch。

6、[数式1]

7、

8、接下来，如式2所示，设从加速度as减去g×sin(θpitch)而得到值asx。然后，基于gps接收机的输出求出汽车的x轴方向的加速度ar后，如式3所示除以加速度ar的余弦值arcos(θpitch)，求出cos(θyaw)，根据cos(θyaw)的值得到偏摆角θyaw。

9、[数式2]

10、asx＝as-g×sin(θpitch)····式2

11、

12、另外，基于gps接收机的输出求出汽车的绕z轴的角速度ωr，并且如式4所示，取角速度ωr与陀螺仪输出的角速度ωs之比，求出陀螺仪的灵敏度sg。然后，如式5所示，通过将灵敏度sg除以cos(θpitch)来求出cos(θroll)，根据cos(θroll)的值得到翻滚角θroll。

13、[数式3]

14、

15、

16、[现有技术文献]

17、[专利文献]

18、[专利文献1]日本专利第5164645号公报

19、[专利文献2]日本特开2004-239613号公报

技术实现思路

1、在使用具备检测3轴方向的加速度的加速度传感器和检测绕3轴的角速度的陀螺仪的6轴惯性传感器和卡尔曼滤波器构建自主导航系统的情况下，需要将与汽车的三维位置矢量的计算有关的6轴惯性传感器向汽车的安装角作为状态进行推定。

2、但是，若6轴惯性传感器向汽车的安装角偏离作为初始值而设定的安装角的标准值，则有时在安装角的推定值收敛之前需要长时间，或者安装角的推定值收敛于错误的值。

3、另外，若6轴惯性传感器向汽车安装的安装角的偏摆角超过±90°，则有时会超过卡尔曼滤波器的推定能力的容许范围，安装角的推定值不收敛而发散。

4、并且，该情况在使用了惯性传感器和卡尔曼滤波器的自主导航系统中，会妨碍适当地计算三维位置矢量等汽车的状态。

5、因此，本发明的技术问题在于，在搭载于移动体的、使用了惯性传感器和卡尔曼滤波器的自主导航系统中，与惯性传感器向移动体的安装角无关地适当地计算移动体的状态。

6、为了解决上述技术问题，本发明在搭载于移动体的自主导航系统中具备：惯性传感器；偏摆角检测单元，检测所述惯性传感器相对于所述移动体的安装角的偏摆角；变换单元，对所述惯性传感器的输出进行变换并输出；以及卡尔曼滤波器，使用所述变换单元的输出来推定所述移动体的状态。其中，所述卡尔曼滤波器推定的状态包含所述惯性传感器的安装角，所述变换单元根据所述偏摆角检测单元检测出的偏摆角，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±90°以内的虚拟的惯性传感器的输出并输出。

7、另外，为了达成上述技术问题，本发明提供一种自主导航系统，搭载于移动体，具备：惯性传感器；偏摆角角度范围检测单元，将所述惯性传感器相对于所述移动体的安装角的偏摆角所属的角度范围检测为偏摆角角度范围；变换单元，对所述惯性传感器的输出进行变换并输出；以及卡尔曼滤波器，使用所述变换单元的输出来推定所述移动体的状态。其中，所述卡尔曼滤波器推定的状态包括所述惯性传感器的安装角，所述变换单元根据所述偏摆角角度范围检测单元检测出的偏摆角角度范围，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±90°以内的虚拟的惯性传感器的输出并输出。

8、在此，该自主导航系统也可以构成为，在所述偏摆角角度范围检测单元中，将-45°与+45°之间的角度范围、+45°与+135°之间的角度范围、+135°与+180°之间的角度范围、-45°与-135°之间的角度范围、-135°与-180°之间的角度范围中的、所述惯性传感器相对于所述移动体的安装角的偏摆角所属的角度范围检测为所述偏摆角角度范围，所述变换单元根据所述偏摆角角度范围检测单元检测出的偏摆角角度范围，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±45°以内的虚拟的惯性传感器的输出并输出。

9、另外，在这样构成的情况下，也可以将所述惯性传感器作为输出x、y、z这正交的3轴方向的加速度(ax，ay，az)的惯性传感器，在所述变换单元中，作为所述变换进行加速度(ax，ay，az)向加速度(ax’，ay’，az’)的变换，并且通过如下方式进行该向加速度(ax’，ay’，az’)的变换：在偏摆角角度范围检测单元检测出的偏摆角角度范围为-45°与+45°之间的角度范围时，(ax’，ay’，az’)＝(ax，ay，az)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为+45°与+135°之间的角度范围时，(ax’，ay’，az’)＝(-ay，ax，az)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为+135°与+180°之间的角度范围时，(ax’，ay’，az’)＝(-ax，-ay，az)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为-45°与-135°之间的角度范围时，(ax’，ay’，az’)＝(ay，-ax，az)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为-135°与-180°之间的角度范围时，(ax’，ay’，az’)＝(-ax，-ay，az)。

10、另外，在这样构成的情况下，也可以将所述惯性传感器作为输出绕x、y、z这正交的3轴的角速度(ωx、ωy、ωz)的惯性传感器，在所述变换单元中，作为所述变换进行角速度(ωx，ωy，ωz)向角速度(ωx’，ωy’，ωz’)的变换，并且通过如下方式进行该向角速度(ωx’，ωy’，ωz’)的变换：在偏摆角角度范围检测单元检测出的偏摆角角度范围为-45°与+45°之间的角度范围时，(ωx’，ωy’，ωz’)＝(ωx，ωy，ωz)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为+45°与+135°之间的角度范围时，(ωx’，ωy’，ωz’)＝(-ωy，ωx，ωz)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为+135°与+180°之间的角度范围时，(ωx’，ωy’，ωz’)＝(-ωx，-ωy，ωz)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为-45°与-135°之间的角度范围时，(ωx’，ωy’，ωz’)＝(ωy，-ωx，ωz)；在偏摆角角度范围检测单元检测出的偏摆角角度范围为-135°与-180°之间的角度范围时，(ωx’，ωy’，ωz’)＝(-ωx，-ωy，ωz)。

11、另外，在这样构成的情况下，也可以使所述惯性传感器输出加速度(ax，ay，az)和角速度(ωx，ωy，ωz)，在所述变换单元中，作为所述变换，进行上述的加速度(ax，ay，az)向加速度(ax’，ay’，az’)的变换和角速度(ωx，ωy，ωz)向角速度(ωx’，ωy’，ωz’)的变换。

12、这样，在卡尔曼滤波器中，通过利用变换单元对惯性传感器的输出进行变换而生成安装角的偏摆角为±90°以内、±45°以内的虚拟的惯性传感器的输出，并代替惯性传感器的输出而输入至卡尔曼滤波器，从而无论惯性传感器向移动体的安装角如何，在卡尔曼滤波器中，能够抑制安装角的推定发散，并且能够期待能够将安装角的推定迅速地收敛于正确的值。

13、在此，也可以是，以上的具备偏摆角角度范围检测单元的自主导航系统中，所述偏摆角角度范围检测单元构成为检测所述惯性传感器相对于所述移动体的安装角的偏摆角，并且该自主导航系统中具备初始值设定单元，该初始值设定单元基于所述偏摆角角度范围检测单元检测出的偏摆角和偏摆角角度范围，计算所述虚拟的惯性传感器的偏摆角，并将其作为所述卡尔曼滤波器推定的安装角的偏摆角的初始值对该卡尔曼滤波器进行设定。

14、另外，在该情况下，还可以是，在所述偏摆角角度范围检测单元中，检测所述惯性传感器相对于所述移动体的安装角的翻滚角和俯仰角，在所述初始值设定单元中，基于所述偏摆角角度范围检测单元检测出的翻滚角、俯仰角和偏摆角角度范围，计算所述虚拟的惯性传感器的俯仰角，并将其作为所述卡尔曼滤波器推定的安装角的俯仰角的初始值对该卡尔曼滤波器进行设定。

15、另外，也可以是，在偏摆角角度范围检测单元中从-45°与+45°之间的角度范围、+45°与+135°之间的角度范围、+135°与+180°之间的角度范围、-45°与-135°之间的角度范围、-135°与-180°之间的角度范围中检测偏摆角角度范围的自主导航系统中，所述偏摆角角度范围检测单元构成为检测所述惯性传感器相对于所述移动体的安装角的翻滚角a1、俯仰角a2和偏摆角a3，并且在该自主导航系统中具备初始值设定单元，该初始值设定单元设定所述卡尔曼滤波器推定的安装角的俯仰角的初始值sa2和偏摆角的初始值sa3。在该情况下，该初始值设定单元通过如下方式来计算初始值sa2和偏摆角的初始值sa3：在偏摆角角度范围检测单元检测出的偏摆角角度范围为-45°与+45°之间的角度范围时，sa2＝a2，sa3＝a3；在偏摆角角度范围检测单元检测出的偏摆角角度范围为+45°与+135°之间的角度范围时，sa2＝a1，sa3＝a3-90°；在偏摆角角度范围检测单元检测出的偏摆角角度范围为+135°与+180°之间的角度范围时，sa2＝-a2，sa3＝a3-180°；在偏摆角角度范围检测单元检测出的偏摆角角度范围为-45°与-135°之间的角度范围时，sa2＝-a1，sa3＝a3+90°；在偏摆角角度范围检测单元检测出的偏摆角角度范围为-135°与-180°之间的角度范围时，sa2＝-a2，sa3＝a3+180°。

16、如上所述，通过设定由卡尔曼滤波器推定的安装角的初始值，能够期待6轴惯性传感器的安装角的推定不发散而迅速收敛。

17、[发明效果]

18、如上所述，根据本发明，在搭载于移动体的、使用了惯性传感器和卡尔曼滤波器的自主导航系统中，能够与惯性传感器向移动体的安装角无关地适当地计算移动体的状态。