CN117705128A - 自主导航系统 - Google Patents

自主导航系统 Download PDF

Info

Publication number
CN117705128A
CN117705128A CN202311181877.0A CN202311181877A CN117705128A CN 117705128 A CN117705128 A CN 117705128A CN 202311181877 A CN202311181877 A CN 202311181877A CN 117705128 A CN117705128 A CN 117705128A
Authority
CN
China
Prior art keywords
angle range
yaw angle
inertial sensor
yaw
detection unit
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.)
Pending
Application number
CN202311181877.0A
Other languages
English (en)
Inventor
清野裕之
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Alps Alpine Co Ltd
Original Assignee
Alps Electric Co Ltd
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 Alps Electric Co Ltd filed Critical Alps Electric Co Ltd
Publication of CN117705128A publication Critical patent/CN117705128A/zh
Pending legal-status Critical Current

Links

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/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明提供即使在惯性传感器的安装角的偏摆角大的情况下也能够良好地进行惯性传感器的安装角的推定的“自主导航系统”。在传感器安装角检测部(13)中，检测6轴惯性传感器(11)的大致安装角的偏摆角A3，在传感器输出变换部(15)中，根据检测出的安装角A3所属的角度范围，将6轴惯性传感器11的坐标系的Xs、Ys、Zs的3轴的加速度输出(ax，ay，az)和角速度输出(ωx，ωy，ωz)变换为偏摆角为±45以内的坐标系的Xs’，Ys’，Zs的3轴的加速度(ax’，ay’，az)和角速度(ωx’，ωy’，ωz)，并输入到卡尔曼滤波器(16)。

Description

自主导航系统
技术领域
本发明涉及使用了卡尔曼滤波器的自主导航系统。
背景技术
作为使用了卡尔曼滤波器的自主导航系统，已知有搭载于汽车的自主导航系统(例如，专利文献1)。
在该自主导航系统中，利用反复扩展卡尔曼滤波器对车速传感器的输出、检测3轴方向的加速度的加速度传感器的输出、检测绕3轴的角速度的陀螺仪的输出、GPS接收机的输出进行处理，来推定汽车的三维位置矢量等的状态。
另外，作为与本发明有关的技术，已知有如下技术：使用检测Xs轴方向的加速度的加速度传感器的输出、检测绕与Xs轴垂直的Zs轴的角速度的陀螺仪的输出、GPS接收机的输出，检测包含该加速度传感器、陀螺仪的装置向汽车的安装角(例如，专利文献2)。
在此，在该技术中，将朝向汽车的前方方向的轴设为Xm，将朝向汽车的上方方向的轴设为Zm，将Xs-Zs坐标系相对于Xm-Zm坐标系的俯仰角θpitch、偏摆角θyow、翻滚角θroll作为向汽车的安装角，并如以下那样求出。
即，对车辆的行进方向的加速度分量为零时的加速度传感器输出的加速度As进行累计，计算平均化后的值Aave。然后，如式1所示，将值Aave除以重力加速度g而求出sin(θpitch)，根据sin(θpitch)的值得到俯仰角θpitch。
[数式1]
接下来，如式2所示，设从加速度As减去g×sin(θpitch)而得到值Asx。然后，基于GPS接收机的输出求出汽车的X轴方向的加速度Ar后，如式3所示除以加速度Ar的余弦值Arcos(θpitch)，求出cos(θyaw)，根据cos(θyaw)的值得到偏摆角θyaw。
[数式2]
Asx＝As-g×sin(θpitch)····式2
另外，基于GPS接收机的输出求出汽车的绕Z轴的角速度ωr，并且如式4所示，取角速度ωr与陀螺仪输出的角速度ωs之比，求出陀螺仪的灵敏度Sg。然后，如式5所示，通过将灵敏度Sg除以cos(θpitch)来求出cos(θroll)，根据cos(θroll)的值得到翻滚角θroll。
[数式3]
[现有技术文献]
[专利文献]
[专利文献1]日本专利第5164645号公报
[专利文献2]日本特开2004-239613号公报
发明内容
在使用具备检测3轴方向的加速度的加速度传感器和检测绕3轴的角速度的陀螺仪的6轴惯性传感器和卡尔曼滤波器构建自主导航系统的情况下，需要将与汽车的三维位置矢量的计算有关的6轴惯性传感器向汽车的安装角作为状态进行推定。
但是，若6轴惯性传感器向汽车的安装角偏离作为初始值而设定的安装角的标准值，则有时在安装角的推定值收敛之前需要长时间，或者安装角的推定值收敛于错误的值。
另外，若6轴惯性传感器向汽车安装的安装角的偏摆角超过±90°，则有时会超过卡尔曼滤波器的推定能力的容许范围，安装角的推定值不收敛而发散。
并且，该情况在使用了惯性传感器和卡尔曼滤波器的自主导航系统中，会妨碍适当地计算三维位置矢量等汽车的状态。
因此，本发明的技术问题在于，在搭载于移动体的、使用了惯性传感器和卡尔曼滤波器的自主导航系统中，与惯性传感器向移动体的安装角无关地适当地计算移动体的状态。
为了解决上述技术问题，本发明在搭载于移动体的自主导航系统中具备：惯性传感器；偏摆角检测单元，检测所述惯性传感器相对于所述移动体的安装角的偏摆角；变换单元，对所述惯性传感器的输出进行变换并输出；以及卡尔曼滤波器，使用所述变换单元的输出来推定所述移动体的状态。其中，所述卡尔曼滤波器推定的状态包含所述惯性传感器的安装角，所述变换单元根据所述偏摆角检测单元检测出的偏摆角，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±90°以内的虚拟的惯性传感器的输出并输出。
另外，为了达成上述技术问题，本发明提供一种自主导航系统，搭载于移动体，具备：惯性传感器；偏摆角角度范围检测单元，将所述惯性传感器相对于所述移动体的安装角的偏摆角所属的角度范围检测为偏摆角角度范围；变换单元，对所述惯性传感器的输出进行变换并输出；以及卡尔曼滤波器，使用所述变换单元的输出来推定所述移动体的状态。其中，所述卡尔曼滤波器推定的状态包括所述惯性传感器的安装角，所述变换单元根据所述偏摆角角度范围检测单元检测出的偏摆角角度范围，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±90°以内的虚拟的惯性传感器的输出并输出。
在此，该自主导航系统也可以构成为，在所述偏摆角角度范围检测单元中，将-45°与+45°之间的角度范围、+45°与+135°之间的角度范围、+135°与+180°之间的角度范围、-45°与-135°之间的角度范围、-135°与-180°之间的角度范围中的、所述惯性传感器相对于所述移动体的安装角的偏摆角所属的角度范围检测为所述偏摆角角度范围，所述变换单元根据所述偏摆角角度范围检测单元检测出的偏摆角角度范围，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±45°以内的虚拟的惯性传感器的输出并输出。
另外，在这样构成的情况下，也可以将所述惯性传感器作为输出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)。
另外，在这样构成的情况下，也可以将所述惯性传感器作为输出绕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)。
另外，在这样构成的情况下，也可以使所述惯性传感器输出加速度(ax，ay，az)和角速度(ωx，ωy，ωz)，在所述变换单元中，作为所述变换，进行上述的加速度(ax，ay，az)向加速度(ax’，ay’，az’)的变换和角速度(ωx，ωy，ωz)向角速度(ωx’，ωy’，ωz’)的变换。
这样，在卡尔曼滤波器中，通过利用变换单元对惯性传感器的输出进行变换而生成安装角的偏摆角为±90°以内、±45°以内的虚拟的惯性传感器的输出，并代替惯性传感器的输出而输入至卡尔曼滤波器，从而无论惯性传感器向移动体的安装角如何，在卡尔曼滤波器中，能够抑制安装角的推定发散，并且能够期待能够将安装角的推定迅速地收敛于正确的值。
在此，也可以是，以上的具备偏摆角角度范围检测单元的自主导航系统中，所述偏摆角角度范围检测单元构成为检测所述惯性传感器相对于所述移动体的安装角的偏摆角，并且该自主导航系统中具备初始值设定单元，该初始值设定单元基于所述偏摆角角度范围检测单元检测出的偏摆角和偏摆角角度范围，计算所述虚拟的惯性传感器的偏摆角，并将其作为所述卡尔曼滤波器推定的安装角的偏摆角的初始值对该卡尔曼滤波器进行设定。
另外，在该情况下，还可以是，在所述偏摆角角度范围检测单元中，检测所述惯性传感器相对于所述移动体的安装角的翻滚角和俯仰角，在所述初始值设定单元中，基于所述偏摆角角度范围检测单元检测出的翻滚角、俯仰角和偏摆角角度范围，计算所述虚拟的惯性传感器的俯仰角，并将其作为所述卡尔曼滤波器推定的安装角的俯仰角的初始值对该卡尔曼滤波器进行设定。
另外，也可以是，在偏摆角角度范围检测单元中从-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°。
如上所述，通过设定由卡尔曼滤波器推定的安装角的初始值，能够期待6轴惯性传感器的安装角的推定不发散而迅速收敛。
[发明效果]
如上所述，根据本发明，在搭载于移动体的、使用了惯性传感器和卡尔曼滤波器的自主导航系统中，能够与惯性传感器向移动体的安装角无关地适当地计算移动体的状态。
附图说明
图1是表示本发明的实施方式的自主导航系统的结构的框图。
图2是表示在本发明的实施方式中卡尔曼滤波器推定的6轴惯性传感器向汽车安装的安装角的偏摆角的图。
图3是表示本发明的实施方式中的从6轴惯性传感器的坐标系到虚拟6轴惯性传感器的坐标系的变换例的图。
具体实施方式
以下，对本发明的实施方式进行说明。
图1表示本实施方式的自主导航系统的结构。
自主导航系统1是搭载于汽车的、计算汽车的位置矢量等汽车的状态的系统，自主导航系统1计算出的汽车的位置矢量等汽车的状态由状态利用系统2利用，该状态利用系统2是同样被搭载于汽车的自动驾驶系统、导航系统等。
如图所示，自主导航系统1具备6轴惯性传感器11、进行卫星定位的GNSS接收机12、传感器安装角检测部13、控制部14、传感器输出变换部15、卡尔曼滤波器16。
另外，6轴惯性传感器11具备：加速度传感器111，对相对于6轴惯性传感器11固定的正交坐标系即传感器坐标系的Xs、Ys、Zs这3轴方向的各加速度ax，ay，az进行检测；以及陀螺仪112，对绕Xs、Ys、Zs这3轴的各角速度ωx、ωy、ωz进行检测。
另外，卡尔曼滤波器16具备预测步骤执行部161和滤波步骤执行部162。
传感器输出变换部15按照从控制部14设定的加速度变换函数Ta，将加速度传感器111输出的加速度ax、ay、az变换为加速度ax’、ay’、az’并输出给卡尔曼滤波器16，并且按照从控制部14设定的角速度变换函数Tω，将陀螺仪112输出的角速度ωx、ωy、ωz变换为角速度ωx’、ωy’、ωz’并输出给卡尔曼滤波器16。
接下来，卡尔曼滤波器16根据式6所示的状态方程式和式7所示的观测方程式，推定各时刻k的汽车的状态Xk。
[数式4]
Xk＝f(xk-1，uk)+qk····式6
zk＝h(xk)+rk····式7
推定的汽车的状态Xk中包含汽车的三维位置矢量(三维移动距离)、6轴惯性传感器11的三维速度矢量、6轴惯性传感器11的翻滚、俯仰、偏摆的姿态角、6轴惯性传感器11向汽车的安装角的俯仰角和偏摆角、加速度传感器111和陀螺仪112的灵敏度、偏置等。
在式6中，uk表示从传感器输出变换部15输入的包含加速度ax’、ay’、az’、角速度ωx’、ωy’、ωz’的输入矢量，qk表示输入矢量的误差。另外，f是在设为没有误差qk的情况下根据前时刻k-1的状态Xk-1和输入向量uk导出状态Xk的非线性函数。
在式7中，Zk表示包含经由信号处理部输入到卡尔曼滤波器16的GNSS接收器的测定值、从传感器输出变换部15输入的角速度ωx’、ωy’、ωz’等的测定值，rk表示测定值的误差。另外，h是在设为没有误差rk的情况下根据状态Xk导出测定值Zk的非线性函数。
卡尔曼滤波器16的预测步骤执行部161和滤波步骤执行部162例如进行以下的处理。
即，卡尔曼滤波器16的预测步骤执行部161在时刻k，按照式8计算状态Xk+1的先验推定值X-k+1。在此，式8中的X+ k是在滤波步骤执行部162中在前时刻k计算出的事后推定值。
[数式5]
另外，预测步骤执行部161按照式9，计算先验误差协方差P- k+1。在式9中，Φk、Γk分别是用非线性函数f的雅可比行列式定义的矩阵、ΦT k、ΓT k分别是Φk、Γk的转置矩阵、P+ k是在滤波步骤执行部162中在前时刻k计算出的事后误差协方差，Qk是表示误差qk的协方差矩阵。
[数式6]
接着，滤波步骤执行部162当在时刻k得到了测定值的情况下，执行一边使式10、式11、式12的i从0逐次前进1一边反复进行1次或多次的规定次数、或者规定的停止条件成立为止的滤波步骤。
[数式7]
式10是卡尔曼增益Kk，i的计算式，式中P-k是在前时刻k-1在预测步骤中计算出的先验误差协方差。另外，x+ k，0设为在前时刻k-1在预测步骤中计算出的先验推定值x- k。
另外，Hk是由非线性函数h的雅可比行列式定义的矩阵，HT k是表示Hk的转置矩阵，Rk是表示误差rk的协方差矩阵。
接下来，式11是计算事后推定值X+ k，i+1的式子，式12是计算事后误差协方差P+ k，i+1的式子，式中的I是单位矩阵。
并且，如果滤波步骤执行部162使式10、式11、式12的重复在n次停止，则将最终得到的事后推定值X+ k，n、事后误差协方差P+ k，n作为事后推定值X+ k、事后误差协方差P+ k。
然后，将事后推定值X+ k作为汽车的状态Xk+1的推定值，在需要的情况下，与事后误差协方差P+ k，n一起输出到状态利用系统2。
此外，在时刻k得不到测定值而无法执行滤波步骤的情况下，将先验推定值X- k作为汽车的状态Xk的推定值，在需要的情况下，与事后误差协方差P- k，n一起输出到状态利用系统2。
另外，这样的卡尔曼滤波器16的预测步骤执行部161的处理从设定了先验推定值X-k的初始值X- 0、先验误差协方差P- k的初始值P- 0之后的时刻k＝1开始。
另外，滤波步骤执行部162的处理由预测步骤执行部161从计算出最初的先验推定值x- 1和先验误差协方差P- 1的时刻k＝1开始。
控制部14进行先验推定值X- k的初始值X- 0中包含的6轴惯性传感器11向汽车的安装角的俯仰角的初始值SA2和偏摆角的初始值SA3的设定。关于其他的初始值，设定期望值等。
以下，对控制部14进行的、向所述的传感器输出变换部15的加速度变换函数Ta和角速度变换函数Tω的设定、作为向卡尔曼滤波器16的先验推定值X- k的初始值X- 0的一部分的、6轴惯性传感器11的安装角的俯仰角的初始值SA2和偏摆角的初始值SA3的设定进行说明。
首先，对加速度变换函数Ta和角速度变换函数Tω的设定进行说明。
传感器安装角检测部13在自主导航系统1的初始设定时，检测6轴惯性传感器11向汽车的大致的安装角(翻滚角A1、俯仰角A2、偏摆角A3)。
在此，预先定义了相对于汽车固定的正交坐标系即车辆坐标系，车辆坐标系的3轴是朝向汽车的前方向的Xm轴、朝向汽车的右方向的Ym轴、朝向汽车的上方向的Zm轴。
并且，6轴惯性传感器11向汽车的安装角的翻滚角A1是绕Xm轴的传感器坐标系相对于车辆坐标系的倾角，俯仰角A2是绕Ym轴的传感器坐标系相对于车辆坐标系的倾角，偏摆角A3如图2的a、b所示是绕Zm轴的传感器坐标系相对于车辆坐标系的倾角。在此，在图2是a、b中，偏摆角A3以顺时针方向为正方向。
另外，传感器安装角检测部13对这样的6轴惯性传感器11的汽车的大致安装角(翻滚角A1、俯仰角A2、偏摆角A3)的检测例如适用所述的专利文献2的技术，能够根据加速度传感器111的输出、陀螺仪112的输出、GNSS接收机12的输出通过上述的式1～式5进行推定。
但是，传感器安装角检测部13的6轴惯性传感器11向汽车的大致安装角(翻滚角A1、俯仰角A2、偏摆角A3)的检测可以通过其他方法来进行，也可以将由操作员设定的翻滚角A1、俯仰角A2、偏摆角A3直接推定为翻滚角A1、俯仰角A2、偏摆角A3。
在任一情况下，翻滚角A1、俯仰角A2、偏摆角A3的推定精度即使最低也设为能够判别各角度属于-45°～+45度、+45°～+135°、-45°～-135°、+135°～+180°、-135°～-180°中的某一个的精度。
接着，控制部14根据传感器安装角检测部13检测出的偏摆角A3所属的角度范围，将在传感器输出变换部15中将加速度传感器111输出的加速度ax、ay、az变换为加速度ax’、ay’、az’的加速度变换函数Ta、和在传感器输出变换部15中将陀螺仪112输出的角速度ωx、ωy、ωz变换为角速度ωx’、ωy’、ωz’的角速度变换函数Tω如下所述那样设定。
-45°＜A3＜+45°时，
(ax’，ay’，az’)＝Ta(ax，ay，az)＝(ax，ay，az)
(ωx’，ωy’，ωz’)＝Tω(ωx，ωy，ωz)＝(ωx，ωy，ωz)
+45°≤A3＜+135°时，
(ax’，ay’，az’)＝Ta(ax，ay，az)＝(-ay，ax，az)
(ωx’，ωy’，ωz’)＝Tω(ωx，ωy，ωz)＝(-ωy，ωx，ωz)
+135°≤A3＜+180°时，
(ax’，ay’，az’)＝Ta(ax，ay，az)＝(-ax，-ay，az)
(ωx’，ωy’，ωz’)＝Tω(ωx，ωy，ωz)＝(-ωx，-ωy，ωz)
-45°≥A3＞-135°时，
(ax’，ay’，az’)＝Ta(ax，ay，az)＝(ay，-ax，az)
(ωx’，ωy’，ωz’)＝Tω(ωx，ωy，ωz)＝(ωy，-ωx，ωz)
-135°≥A3≥-180°时，
(ax’，ay’，az’)＝Ta(ax，ay，az)＝(-ax，-ay，az)
(ωx’，ωy’，ωz’)＝Tω(ωx，ωy，ωz)＝(-ωx，-ωy，ωz)
通过这样设定加速度变换函数Ta和角速度变换函数Tω，加速度传感器111输出的加速度ax、ay、az被变换为表示图3的a～e中偏摆角A3所属的每个角度范围所示的Xs’轴方向的加速度ax’和表示Ys’轴方向的加速度的ay’、表示Zs轴方向的加速度的az’，陀螺仪112输出的角速度ωx、ωy、ωz被变换为表示绕Xs’轴的角速度的ωx’、表示绕Ys’轴的角速度的ωy’、表示绕Zs轴的角速度的ωz’。
即，6轴惯性传感器11的输出被变换为具有以Xs’轴、Ys’轴、Zs轴为正交三轴的虚拟的传感器坐标系的虚拟6轴惯性传感器的输出。
另外，该虚拟6轴惯性传感器向汽车安装的安装角的偏摆角A3’，因为Xs’轴和ys’轴如图所示那样成为将Xs轴和ys轴在偏摆方向上根据偏摆角A3所属的角度范围以如下方式旋转后的轴：
在-45°＜A3＜+45°时为0°，
在+45°≤A3＜+135°时为-90°，
在+135°≤A3＜+180°时为-180°，
在-45°≥A3＞-135°时为+90°，
在-135°≥A3≥-180°时为+180°，
而成为：
在-45°＜A3＜+45°时A3’＝A3，
在+45°≤A3＜+135°时A3’＝A3-90°，
在+135°≤A3＜+180°时A3’＝A3-180°，
在-45°≥A3＞-135°时A3’＝A3+90°，
在-135°≥A3≥-180°时A3’＝A3+180°，
无论偏摆角A3如何，偏摆角A3’都收于图中灰色所示的±45°的范围内。
因此，在卡尔曼滤波器16中，使用通过传感器输出变换部15的变换而生成的虚拟6轴惯性传感器的输出来代替6轴惯性传感器11的输出等价于使用向汽车安装的安装角的偏摆角为±45°的范围内的A3’的6轴惯性传感器的输出。
因此，无论6轴惯性传感器11向汽车的安装角如何，都能够抑制安装角的推定发散，并且能够期待能够使安装角的推定迅速地收敛于正确的值。
接着，对控制部14进行的6轴惯性传感器11的安装角的俯仰角的初始值SA2和偏摆角的初始值SA3的设定进行说明。
在此，卡尔曼滤波器16推定虚拟6轴惯性传感器坐标系相对于车辆坐标系的安装角的俯仰角和偏摆角，以使以Xs轴、Ys轴、Zs轴为正交三轴的六轴惯性传感器11的坐标系绕Zs轴旋转了根据传感器安装角检测部13检测出的偏摆角A3所属的角度范围而确定的角度θ而得到的Xs’轴、Ys’轴、Zs’轴为正交三轴。
因此，控制部14根据传感器安装角检测部13检测出的俯仰角A2和偏摆角A3、偏摆角A3所属的角度范围，如下所述那样设定向卡尔曼滤波器16的、安装角的俯仰角的初始值SA2、偏摆角的初始值SA3。
即，
在-45°＜A3＜+45°时，SA2＝A2，θ＝0°，SA3＝A3+θ；
在+135°≤A3＜+180°时，SA2＝A2，θ＝-180°，SA3＝A3+θ；
在-135°≥A3≥-180°时，SA2＝A2，θ＝+180°，SA3＝A3+θ。
另外，在传感器安装角检测部13检测出的偏摆角A3所属的角度范围处于下一范围、能够期待传感器坐标系的Zs轴与车辆坐标系的Zm轴近似的情况下，根据传感器安装角检测部13检测出的翻滚角A1、偏摆角A3、偏摆角A3所属的角度范围，如下所述那样设定安装角的俯仰角的初始值SA2、偏摆角的初始值SA3。
在+45°≤A3＜+135°时，SA2＝A1，θ＝-90°，SA3＝A3+θ；
在-45°≥A3≥-135°时，SA2＝-A1，θ＝+90°，SA3＝A3+θ。
在控制部14中，通过如以上那样设定安装角的俯仰角的初始值SA2和偏摆角的初始值SA3，能够期待六轴惯性传感器11的安装角的俯仰角和偏摆角的推定不发散而迅速地收敛。
此外，在以上的实施方式中，使虚拟6轴惯性传感器的偏摆角A3’收敛于±45°的范围内，但收敛偏摆角A3’的范围也可以不必须为±45°的范围，可以为小于±90°的任意的范围。
[附图标记说明]
1…自主导航系统、2…状态利用系统、11…6轴惯性传感器、12…GNSS接收机、13…传感器安装角检测部、14…控制部、15…传感器输出变换部、16…卡尔曼滤波器、111…加速度传感器、112…陀螺仪、161…预测步骤执行部、162…滤波步骤执行部。

Claims (9)

1.一种自主导航系统，搭载于移动体，其特征在于，具备：
惯性传感器；
偏摆角检测单元，检测所述惯性传感器相对于所述移动体的安装角的偏摆角；
变换单元，对所述惯性传感器的输出进行变换并输出；以及
卡尔曼滤波器，使用所述变换单元的输出来推定所述移动体的状态，
所述卡尔曼滤波器推定的状态包含所述惯性传感器的安装角，
所述变换单元根据所述偏摆角检测单元检测出的偏摆角，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±90°以内的虚拟的惯性传感器的输出并输出。
2.一种自主导航系统，搭载于移动体，其特征在于，具备：
惯性传感器；
偏摆角角度范围检测单元，将所述惯性传感器相对于所述移动体的安装角的偏摆角所属的角度范围检测为偏摆角角度范围；
变换单元，对所述惯性传感器的输出进行变换并输出；以及
卡尔曼滤波器，使用所述变换单元的输出来推定所述移动体的状态，
所述卡尔曼滤波器推定的状态包括所述惯性传感器的安装角，
所述变换单元根据所述偏摆角角度范围检测单元检测出的偏摆角角度范围，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±90°以内的虚拟的惯性传感器的输出并输出。
3.根据权利要求2所述的自主导航系统，其特征在于，
所述偏摆角角度范围检测单元检测-45°与+45°之间的角度范围、+45°与+135°之间的角度范围、+135°与+180°之间的角度范围、-45°与-135°之间的角度范围、-135°与-180°之间的角度范围中的、所述惯性传感器相对于所述移动体的安装角的偏摆角所属的角度范围作为所述偏摆角角度范围，
所述变换单元根据所述偏摆角角度范围检测单元检测出的偏摆角角度范围，将所述惯性传感器的输出变换为相对于所述移动体的安装角的偏摆角至少为±45°以内的虚拟的惯性传感器的输出并输出。
4.根据权利要求3所述的自主导航系统，其特征在于，
所述惯性传感器是输出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)。
5.根据权利要求3所述的自主导航系统，其特征在于，
所述惯性传感器是输出绕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)。
6.根据权利要求4所述的自主导航系统，其特征在于，
所述惯性传感器除了输出加速度(ax，ay，az)以外还输出所述绕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)。
7.根据权利要求6所述的自主导航系统，其特征在于，
所述偏摆角角度范围检测单元检测所述惯性传感器相对于所述移动体的安装角的偏摆角，
该自主导航系统中具备初始值设定单元，该初始值设定单元基于所述偏摆角角度范围检测单元检测出的偏摆角和偏摆角角度范围，计算所述虚拟的惯性传感器的偏摆角，并将其作为所述卡尔曼滤波器推定的安装角的偏摆角的初始值对该卡尔曼滤波器进行设定。
8.根据权利要求7所述的自主导航系统，其特征在于，
所述偏摆角角度范围检测单元检测所述惯性传感器相对于所述移动体的安装角的翻滚角和俯仰角，
所述初始值设定单元基于所述偏摆角角度范围检测单元检测出的翻滚角、俯仰角和偏摆角角度范围，计算所述虚拟的惯性传感器的俯仰角，并将其作为所述卡尔曼滤波器推定的安装角的俯仰角的初始值对该卡尔曼滤波器进行设定。
9.根据权利要求6所述的自主导航系统，其特征在于，
所述偏摆角角度范围检测单元检测所述惯性传感器相对于所述移动体的安装角的翻滚角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°。
CN202311181877.0A 2022-09-14 2023-09-13 自主导航系统 Pending CN117705128A (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022146423A JP2024041548A (ja) 2022-09-14 2022-09-14 自律航法システム
JP2022-146423 2022-09-14

Publications (1)

Publication Number Publication Date
CN117705128A true CN117705128A (zh) 2024-03-15

Family

ID=87933679

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311181877.0A Pending CN117705128A (zh) 2022-09-14 2023-09-13 自主导航系统

Country Status (4)

Country Link
US (1) US20240085183A1 (zh)
EP (1) EP4339558A1 (zh)
JP (1) JP2024041548A (zh)
CN (1) CN117705128A (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4191499B2 (ja) 2003-02-03 2008-12-03 パイオニア株式会社 車載装置
JP4443621B2 (ja) * 2007-01-18 2010-03-31 三菱電機株式会社 カーナビゲーション装置
JP5164645B2 (ja) 2008-04-07 2013-03-21 アルパイン株式会社 カルマンフィルタ処理における繰り返し演算制御方法及び装置
JP5328252B2 (ja) * 2008-07-30 2013-10-30 アルパイン株式会社 ナビゲーションシステムの位置検出装置および位置検出方法
JP6094026B2 (ja) * 2011-03-02 2017-03-15 セイコーエプソン株式会社 姿勢判定方法、位置算出方法及び姿勢判定装置
US10746551B2 (en) * 2015-10-15 2020-08-18 Mitsubishi Electric Corporation Positioning apparatus and positioning method

Also Published As

Publication number Publication date
EP4339558A1 (en) 2024-03-20
US20240085183A1 (en) 2024-03-14
JP2024041548A (ja) 2024-03-27

Similar Documents

Publication Publication Date Title
JP7036080B2 (ja) 慣性航法装置
US7168177B2 (en) Method and apparatus for determining a geomagnetic field by using a compass and method and apparatus for determining an azimuth angle of a moving object using the same
EP3388787B1 (en) Polar region operating attitude and heading reference system
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
US4347573A (en) Land-vehicle navigation system
CN109405837A (zh) 物体定位方法、应用和车辆
JP5022747B2 (ja) 移動体の姿勢及び方位検出装置
CN108873043A (zh) 车辆侧滑角度的计算方法及装置
Emami et al. A low complexity integrated navigation system for underwater vehicles
JPH095104A (ja) 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置
JPH09189564A (ja) 移動体位置速度算出装置
CN117705128A (zh) 自主导航系统
CN114897942B (zh) 点云地图的生成方法、设备及相关存储介质
WO2014171227A1 (ja) 姿勢角推定装置およびそれを備える移動状態検出装置
JP6981459B2 (ja) センサ誤差補正装置
Damerius et al. A generic inertial navigation system
Onunka et al. USV attitude estimation: an approach using quaternion in direction cosine matrix
JP2007155365A (ja) 方位センサの補正係数演算装置及び演算プログラム
JP6409625B2 (ja) 車両位置算出装置
JP5939395B2 (ja) 移動体の相対姿勢計測方法と装置
Hunter et al. GNSS-Assisted System Identification of Autonomous Ground Vehicle Model and Sensor Parameters
Ji et al. Study on underwater navigation for underwater track vehicle
Bucci et al. EKF on Lie Groups for Autonomous Underwater Vehicles orientation initialization in presence of magnetic disturbances
CN113030504B (zh) 车辆测速方法、装置、车载计算机设备和存储介质
EP4336145A1 (en) Method and system for determining initial heading angle

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication