专利名称：位置计算方法和位置计算装置的制作方法
技术领域：
本发明涉及位置计算方法和位置计算装置。
背景技术：
作为利用测位用信号的测位系统，GPS (Global Positioning System 全球定位系 统)广为人知，应用于在便携型电话机、导航装置等内置的位置计算装置。GPS中，根据多 个GPS卫星的位置和从各GPS卫星到位置计算装置的虚拟距离等的信息，进行求出表示位 置计算装置的位置的3维坐标值和计时器误差的位置计算处理。另外，为了提高位置计算的正确性，考虑了利用加速度传感器、陀螺传感器这样的 惯性导航用的传感器的计测结果来补正GPS计算的位置，进行位置计算的技术(例如，专利 文献1)。专利文献1 日本特表2003-509671号公报。但是，传统的位置计算装置，例如专利文献1所公开，是以搭载GPS的接收电路和 各种传感器为前提的装置，利用两者的计测结果进行预定的运算处理来执行位置计算。因 此，是不利于装置的扩展、变更等的固定系统。

发明内容
本发明鉴于上述课题而提出，其目的是提供用于实现可灵活应对装置的扩展和变 更等的位置计算装置的方法。解决以上的课题的第1形态，是具备惯性计测单元且可装入检测在位置计算中可 利用的物理现象的至少一个传感器单元的位置计算装置所进行的位置计算方法，其包括 在上述传感器单元装入时，识别上述传感器单元的种类；在上述传感器单元未装入时，用上 述惯性计测单元的输出计算位置；以及在上述传感器单元装入时，根据上述识别的上述传 感器单元的种类，用上述传感器单元的输出和上述惯性计测单元的输出计算位置。另外，作为其他形态，可构成位置计算装置，其具备惯性计测单元；可装入检测 在位置计算中可利用的物理现象的传感器单元的装入部；在上述传感器单元装入时识别上 述传感器单元的种类的识别部；以及位置计算部，其在上述传感器单元未装入时，用上述惯 性计测单元的输出计算位置，在上述传感器单元装入时，根据上述识别的上述传感器单元 的种类，用上述传感器单元的输出和上述惯性计测单元的输出计算位置。根据该第1形态等，在装入检测在位置计算中可利用的物理现象的至少一个传感 器单元时，识别装入传感器单元的种类。然后，在传感器单元未装入时，用惯性计测单元的 输出计算位置，在传感器单元装入时，根据识别的传感器单元的种类，用该传感器单元的输 出和惯性计测单元的输出来计算位置。从而，根据传感器单元的装入有无进行适切的运算 处理，可进行位置计算，并可灵活应对装置的扩展、变更等。另外，作为第2形态，可构成第1形态的位置计算方法，其中，还包括取得表示进 行上述传感器单元的校正时采用的校正基准信息的类别的校正基准类别信息；在上述传感器单元装入时，根据上述校正基准类别信息和上述惯性计测单元的输出计算上述校正基准 信息；以及用上述校正基准信息进行上述传感器单元的校正。根据该第2形态，取得表示进行传感器单元校正时采用的校正基准信息的类别的 校正基准类别信息。然后，在传感器单元装入时，根据取得的校正基准类别信息和惯性计测 单元的输出来计算校正基准信息，用该校正基准信息进行传感器单元的校正。在传感器单 元装入时，从该传感器单元取得进行该传感器单元的校正所必要的信息的类别，通过使用 取得的类别信息进行计算/校正，可适切进行装入传感器单元的校正。另外，作为第3形态，可构成第1或第2形态的位置计算方法，其中，在上述传感器 单元装入时，由采用上述传感器单元的输出的过滤处理计算位置。根据该第3形态，在传感器单元装入时，通过采用传感器单元的输出的过滤处理 计算位置，因此可以计算更准确的位置。该场合，作为第4形态，可构成第3形态的位置计算方法，其中，上述传感器单元是 采用来自位置计算用卫星的卫星信号可计算位置的卫星位置计算单元，由上述过滤处理进 行的位置计算包括用上述卫星位置计算单元计算的位置以外的值预测位置；和根据上述 卫星位置计算单元计算的位置补正上述预测的位置。根据该第4形态，在装入卫星位置计算单元作为传感器单元时，作为过滤处理，采 用卫星位置计算单元计算的位置以外的值来预测位置，根据卫星位置计算单元计算的位置 来补正预测位置。通过根据卫星位置计算单元的计算位置来补正用卫星位置计算单元的计 算位置以外的值计算的预测位置，可计算与真实位置更接近的位置。另外，作为第5形态，可构成第4形态的位置计算方法，其中，取得表示上述卫星 位置计算单元计算位置的时间间隔的信息，上述预测包括在上述时间间隔之间反复预测位 置，上述补正包括在上述时间间隔补正上述预测的位置。根据该第5形态，在卫星位置计算单元装入时，取得表示卫星位置计算单元计算 位置的时间间隔的信息。然后，在过滤处理中，在该时间间隔之间反复预测位置，在该时间 间隔补正预测的位置。从而，卫星位置计算单元取得1次位置的期间，可利用卫星位置计算 单元的计算位置以外的值多次进行位置的预测。




图1是位置计算系统的构成的方框图。 图2是识别处理的流向的流程图。 图3是位置计算处理的流向的流程图。 图4是位置计算处理的流向的流程图。 图5是观测值表的表构成的一例示图。 图6是GPA位置汇总处理的流向的流程图。 图7是GPA预测处理的流向的流程图。 图8是GPA补正处理的流向的流程图。 图9是惯性导航运算结果校正处理的流向的流程图。 图10是IMU预测处理的流向的流程图。 图11是IMU补正处理的流向的流程图。
4
图12是变形例中的位置计算系统的构成的方框图。图13是变形例中的观测值表的表构成的一例示图。图14是变形例中的观测值表的表构成的一例示图。符号说明1位置计算系统，10处理部，IlIMU用位置计算部，13插槽用I/F部，15插槽用控制 部，17位置信息汇总(综合)处理部(GPA)，20输入部，30显示部，40通信部，50IMU, 60第 1插槽，70第2插槽，80第1总线，90第2总线，100GPS单元，110方位传感器单元。
具体实施例方式以下，参照附图，说明本发明的优选实施例。另外，当然可适用本发明的实施例不 限于以下说明的实施例。1.系统构成图1是本实施例中的位置计算系统1 (位置计算装置)的功能构成的示图。位置 计算系统1是具备处理部10、输入部20、显示部30、通信部40、IMU (Inertial Measurement Unit 惯性测量单元)50、第1插槽60和第2插槽70而构成的计算机系统。处理部10、输 入部20、显示部30和通信部40经由第1数据总线80连接。另外，处理部10、IMU50、第1 插槽60和第2插槽70经由第2数据总线90连接。位置计算系统1是位置计算装置的一种，是适用于例如包含智能手机在内的便携 型电话机、笔记本电脑、PDA (Personal Digital Assistant 个人数字助理)、车载导航装 置、便携型导航装置这样的可搭载GPStelobalPositioning System 全球定位系统)单元 的各种电子设备的系统(装置)。处理部10是统一控制位置计算系统1的控制装置，作为其功能部，具备IMU用位 置计算部11、插槽用I/F(接口)部13、插槽用控制部15和位置信息汇总处理部(以下，称 为〃 GPA(Generic Position Algorithm 综合位置算法)〃)17。IMU用位置计算部11是利用从IMU50输出的加速度及角速度的数据，进行惯性导 航运算处理，来计算位置、速度的功能部。该功能由例如CPU(Central Processing Unit 中 央处理器)、DSP (Digital Signal Processor 数字信号处理器)等的处理器实现。IMU用 位置计算部11在插槽用控制部15之间进行各种数据的收发。插槽用I/F部13是用于实现装入(嵌入)第1插槽60及第2插槽70的单元(以 下，称为"装入单元")之间的输入输出接口的功能部。作为数据的流向，插槽用I/F部13 起到在装入单元和插槽用控制部15之间连接的作用。插槽用控制部15是进行以下处理的功能部经由插槽用I/F部13从装入单元取 得输出信息；计算IMU用位置计算部11用于进行位置计算的信息并向IMU用位置计算部11 输出；用来自IMU用位置计算部11的输出信息和来自GPA17的输出信息，计算成为装入单 元校正用基准的信息(以下，称为"校正基准信息")。该功能由例如CPU、DSP等的处理 器实现。GPA17是用来自IMU用位置计算部11的输出信息及来自插槽用控制部15的输出 信息，进行规定的过滤处理，来汇总/计算位置、速度的功能部。该功能由例如CPU、DSP等 的处理器实现。本实施例中，GPA17作为过滤处理的一例，进行利用卡尔曼滤波器的位置汇总处理，来计算位置。输入部20是由例如触摸面板、按钮开关等构成的输入装置，将押下的键、按钮的 信号向处理部10输出。通过来自该输入部20的操作输入，输入位置计算请求等的各种指
7J\ ο显示部30由LCD (Liquid Crystal Display 液晶显示器)等构成，是根据从处理 部10输入的显示信号进行各种显示的显示装置。在显示部30显示位置显示画面、时刻信
肩、^^ ο通信部40是用于按照处理部10的控制，在位置计算系统1与外部装置之间进行 通信的通信装置。该功能由例如有线通信用模块、基于IEEE 802. 11的无线LAN、基于扩频 方式的无线通信等的无线通信模块等实现。另外，在将位置计算系统1应用于便携型电话 机时，通信部40也可以是便携电话用的通信装置。IMU50具备加速度传感器51及陀螺传感器53，是可检测正交3轴的各轴向的加速 度及各轴的旋转角速度的惯性计测单元。加速度传感器51及陀螺传感器53分别计测加速 度及角速度。另外，计测计测的加速度包含的误差(以下称为"加速度误差")及角速度 包含的误差(以下称为"角速度误差")，或者，预先设定/存储这些值。IMU50将加速度、角速度、加速度误差及角速度误差和进行这些计测的时刻(计测 时刻)向IMU用位置计算部11输出。本实施例中，IMU50以"0. 01秒"的时间间隔输出 数据。即，IMU50的数据的输出时间间隔是"0.01秒"。另外，加速度传感器51及陀螺传 感器53可以是各自独立的传感器，也可以是一体型的传感器。第1插槽60及第2插槽70也可以说是可装入位置计算用的单元和各种传感器单 元的一种插座。本实施例中，作为检测在位置计算中可利用的物理现象的传感器单元的一 例，在这些插槽中装入GPS单元100或方位传感器单元110。GPS单元100是卫星位置计算单元的一种，是利用从位置计算用卫星的一种GPS卫 星发出的GPS卫星信号进行规定的位置计算运算而计算位置的单元。GPS单元100具有在 装入第1插槽60或第2插槽70时，与位置计算系统1之间进行数据收发的装入时用处理 部 101。方位传感器单元110由例如地磁传感器构成，是通过检测地磁来检测磁方位的单 元。方位传感器单元110也与GPS单元100同样，具有装入时用处理部111。2.处理的流向图2是位置计算系统1进行装入单元的识别处理的流向的流程图。这里，举例说 明第1插槽60装入GPS单元100，第2插槽70装入方位传感器单元110的情况。图的左 侧表示插槽用控制部15进行的处理的流向，图的中央表示GPS单元100的装入时用处理部 101进行的处理的流向，图的右侧表示方位传感器单元110的装入时用处理部111进行的处 理的流向。首先，插槽用控制部15判定有无单元装入第1插槽60或第2插槽70 (步骤Al)， 判定装入时(步骤Al 是)，对装入该插槽的单元进行单元信息的询问(步骤A3)。检测单 元装入第1插槽60及第2插槽70的方法例如可以通过感知单元装入形成的电气连接的方 法等来实现。接受来自插槽用控制部15的询问后，装入第1插槽60的GPS单元100的装入时用处理部101将识别代码、输出规格信息、校正基准类别信息和定时信息作为单元信息通 知插槽用控制部15 (步骤Bl B7)。识别代码是唯一识别各单元的种类的识别信息，预先分配给单元的各个种类。输 出规格信息是表示该单元可输出数据的类别的信息。校正基准类别信息是表示该单元的校 正所需数据的类别的信息。将校正基准类别信息表示的类别数据(该类别参数值)称为" 校正基准信息"，与校正基准类别信息相区别。另外，定时信息是包含该单元输出数据的时 间间隔即输出时间间隔和该单元进行校正的时间间隔即校正时间间隔的信息。本实施例中，GPS单元100的装入时用处理部101作为识别代码，通知〃 GPS"(步 骤Bi)。另外，作为GPS单元100的输出规格信息，通知"位置、速度、时刻、位置误差、速度 误差"(步骤B3)。即，GPS单元100可输出数据的种类是位置、速度、时刻、位置误差及速 度"^差ο另外，GPS单元100的装入时用处理部101作为校正基准类别信息，通知〃多普勒 速度、位置、速度、速度误差"(步骤B5)。即，GPS单元100校正所需要的数据的种类是多 普勒速度、位置、速度及速度误差。另外，GPS单元100的装入时用处理部101作为定时信息，通知〃输出时间间隔1 秒，校正时间间隔0. 1秒"(步骤B7)。即，GPS单元100以1秒间隔输出数据，以0.1秒 间隔进行校正。同样，方位传感器单元110的装入时用处理部111作为识别代码，通知〃方位传 感器"(步骤Cl)。另外，作为方位传感器单元110的输出规格信息，通知"方位"(步骤 C3)，作为校正基准类别信息，通知"位置"(步骤C5)。另外，装入时用处理部111作为定 时信息，通知"输出时间间隔0. 01秒，校正时间间隔1秒"(步骤C7)。本实施例中特征 之一是可以逐个单元地确定输出时间间隔及校正时间间隔。从装入各插槽的单元接受单元信息的通知后，插槽用控制部15判定装入单元的 识别是否结束(步骤A5)，判定未结束的场合(步骤A5 ；否)，返回步骤A3。另外，判定识别 结束的场合(步骤A5 ；是)，结束识别处理。图3及图4是本实施例中的位置计算处理的流向的流程图。这里也举例说明第1 插槽60装入GPS单元100，第2插槽70装入方位传感器单元110的情况。从图的左侧向右 侧，分别表示了 GPA17进行的处理的流向、IMU用位置计算部11进行的处理的流向、插槽用 控制部15进行的处理的流向、IMU50进行的处理的流向、第1插槽60装入的GPS单元100 的装入时用处理部101进行的处理的流向、第2插槽70装入的方位传感器单元110的装入 时用处理部111进行的处理的流向。首先，插槽用控制部15将从各装入单元取得的单元信息向GPA17输出(步骤Fl)。 GPA17从插槽用控制部15取得单元信息后，用该单元信息生成观测值表(步骤Dl)。观测 值表是GPA17在后述的GPA位置汇总处理中，在利用卡尔曼滤波器进行补正处理时作为观 测值(外部观测量)使用的参数而设定的表。图5是观测值表的表构成的一例示图。在观测值表中，与成为观测值的候补的多 个参数的类别对应，逐个单元地设定是否作为该参数的观测值使用。作为观测值使用的参 数设定为"〇"，未作为观测值使用的参数设定为"X"。例如，图5的观测值表中，与GPS单元100对应的位置及速度设定为〃〇〃，意味着由GPS单元100计测的位置及速度在GPA汇总处理中作为观测值使用。本实施例中，将 GPS单元100的计测结果作为观测值使用，IMU50及方位传感器单元110的计测结果不作为 观测值使用。返回图3的说明，GPA17采用步骤Dl设定的观测值表，设定补正定时(步骤D3)。 具体地说，观测值表中，指定观测值设定为"〇"的单元。将用来自指定的单元的计测结果 用作观测值进行补正处理的定时设定成该单元的数据输出的时间间隔的定时。例如，图5的观测值表中，对于GPS单元100，位置及速度为设定〃〇〃。因此，用 来自GPS单元100的计测结果作为观测值进行补正处理。将该补正处理的定时设定成GPS 单元100的数据的输出时间间隔即每"1秒"的定时。然后，插槽用控制部15向GPS单元100输出GPS单元100的初始化用的信息即 GPS初始化信息(步骤F3)。具体地说，将"初始位置、速度、时刻"的信息作为GPS初始化 信息向GPS单元100输出。GPS单元100的装入时用处理部101从插槽用控制部15取得 GPS初始化信息后，用该信息进行初始化GPS单元100的处理(步骤Hl)。另外，插槽用控制部15向方位传感器单元110输出方位传感器单元110的初始化 用的信息即方位传感器初始化信息(步骤F5)。具体地，将"时刻"的信息作为方位传感器 初始化信息向方位传感器单元110输出。方位传感器单元110的装入时用处理部111从插 槽用控制部15取得方位传感器初始化信息后，用该信息进行方位传感器单元110初始化的 处理(步骤Jl)。另外，IMU用位置计算部11向IMU50输出IMU50的初始化用的信息即IMU初始 化信息(步骤E1)。具体地，将"时刻"的信息作为IMU初始化信息向IMU50输出。然后， IMU50的处理部进行IMU50初始化的处理(步骤Gl)。然后，方位传感器单元110的装入时用处理部111将方位传感器的计测结果即方 位和计测时刻作为方位传感器计测结果通过插槽用I/F部13向插槽用控制部15输出(步 骤 J3)。插槽用控制部15从方位传感器单元110取得计测结果后，根据前次取得的方位和 本次取得的方位，进行计算角速度的处理(步骤F7)。例如，通过求出按单位时间的方位变 化量这样的微分运算计算角速度。然后，插槽用控制部15将计算的角速度向IMU用位置计 算部11输出(步骤F8)。另一方面，IMU50将由陀螺传感器53计测的角速度向IMU用位置 计算部11输出(步骤G2)。IMU用位置计算部11从IMU50和插槽用控制部15分别取得角速度后，用取得的角 速度，计算IMU50用的校正基准信息即IMU校正基准信息(步骤E3)。具体地，用从插槽用 控制部15取得的角速度和从IMU50取得的角速度的值，进行公知的误差计算运算，来计算 陀螺传感器53的误差参数的一种即角速度偏差及角速度比例因子。另外，传感器的偏差及比例因子为公知，因此详细的说明省略，但是，偏差意味着 某动作条件下(例如静止时)传感器的输出值，比例因子意味着传感器的敏感度，即输出变 化相对于要计测的物理量的输入变化的比。然后，IMU用位置计算部11将计算的IMU校正基准信息向IMU50输出(步骤E5)。 IMU50从IMU用位置计算部11取得IMU校正基准信息后，进行陀螺传感器53的校正处理 (步骤G3)。
具体地，IMU50将装置内部存储的角速度偏差用从IMU用位置计算部11取得的角 速度偏差更新。另外，将存储的角速度比例因子用取得的角速度比例因子更新。从而，陀螺 传感器53计测的角速度用该更新的角速度偏差及角速度比例因子进行校正。然后，IMU50将加速度传感器51及陀螺传感器53的计测结果即加速度、角速度、 加速度误差及角速度误差和计测时刻作为IMU计测结果向IMU用位置计算部11输出(步 骤G5)。IMU用位置计算部11取得IMU计测结果后，用该IMU计测结果进行公知的惯性导 航运算处理，计算位置及速度和这些包含的误差(步骤E7)。然后，将计算的位置及速度、位 置误差及速度误差和计算时刻作为IMU运算结果向GPA17及插槽用控制部15输出(步骤 E9)。插槽用控制部15从IMU用位置计算部11取得IMU运算结果后，用该IMU运算结 果进行计算GPS校正基准信息的GPS校正基准信息计算处理(步骤F9)。具体地，用IMU运 算结果所包含的位置及速度和GPS卫星的卫星位置及卫星速度，计算位置计算系统1与GPS 卫星的相对速度即多普勒速度。另外，卫星位置及卫星速度可以采用预先取得的卫星轨道信息(历书和星历)和 现在时刻计算。然后，插槽用控制部15将计算的多普勒速度和IMU运算结果所包含的位置、 速度及速度误差作为GPS校正基准信息向GPS单元100输出(步骤Fll)。GPS单元100的装入时用处理部101从插槽用控制部15取得GPS校正基准信息 后，用该GPS校正基准信息进行卫星捕捉处理(步骤H3)。具体地，用GPS校正基准信息所 包含的多普勒速度，确定GPS卫星信号的搜索中心频率，同时确定频率的搜索范围。详细地说，将在GPS卫星信号的载波频率即1. 57542[GHz]加上了多普勒速度量的 频率误差的频率确定为搜索中心频率。从而，通过加上伴随位置计算系统1的移动的多普 勒速度，可以正确设定搜索中心频率，GPS卫星信号的捕捉所需时间可缩短。而且，用GPS校正基准信息所包含的速度及速度误差确定GPS卫星信号的频率的 搜索范围。捕捉GPS卫星信号的场合，在GPS卫星信号的搜索频率变化的同时，进行复制代 码与GPS卫星信号的相关运算，并进行计算相关值的峰值的运算。从而，加上GPS校正基准 信息所包含的位置计算系统1的速度及速度误差，计算频率的搜索宽度，以搜索中心频率 为中心，将具有计算的搜索宽度的频率范围确定为频率的搜索范围。然后，GPS单元100的装入时用处理部101进行GPS位置计算处理(步骤H5)。具 体地，以GPS校正基准信息所包含的位置作为初始位置，进行采用虚拟距离的公知的位置 计算运算(位置收敛运算)，计算位置。此时，求出用卫星轨道信息计算的GPS卫星的卫星 位置和GPS校正基准信息所包含的位置计算系统1的位置间的距离，将求出的距离除以光 速，计算虚拟距离的整数部分。然后，根据进行GPS卫星信号与复制代码的相关运算而获得 的代码相位，求出虚拟距离的尾数部分，将整数部分和尾数部分相加，计算虚拟距离。IMU用位置计算部11将进行惯性导航运算处理而取得的位置作为初始位置来进 行位置计算运算，同时，通过用该位置计算虚拟距离的整数部分，提高位置收敛运算中的收 敛性，且计算的位置变得更准确。另外，详细的说明虽然省略，但是，GPS位置计算处理中， 进行利用多普勒频率的时间变化的公知的速度计算运算，计算位置计算系统1的速度。另 外，进行公知的误差计算运算，将计算的位置及速度包含的误差计算。进行GPS位置计算处理后，GPS单元100的装入时用处理部101将计算的位置、速度、位置误差、速度误差及时刻作为GPS运算结果向插槽用控制部15输出(步骤H7)。然 后，插槽用控制部15将从GPS单元100取得的GPS运算结果向GPA17输出(步骤F13)。GPA17从插槽用控制部15取得GPS运算结果后，进行GPA位置汇总处理(步骤 D5)。GPA位置汇总处理是利用卡尔曼滤波器对IMU位置计算部11计算的位置及速度和GPS 单元100计算的位置及速度进行汇总的处理。另外，图3及图4是图2的处理后续的各功能部的处理流程。从而，图3及图4是 在位置计算系统1装入GPS单元100时的处理的流向。若GPS单元100未装入位置计算系 统1时，不从GPS单元100向GPA17输出位置及速度的信息，IMU用位置计算部11仅仅将 进行惯性导航运算处理而计算的位置及速度向GPA17输出。该场合，GPA17不进行位置汇 总处理，用IMU用位置计算部11的输出计算位置及速度。例如，从IMU用位置计算部11输 出的位置及速度可以直接作为最终计算结果，也可以将从IMU用位置计算部11输出的位置 及速度根据过去的位置及速度的计算结果的履历进行补正，作为最终的计算结果。图6是GPA位置汇总处理的流向的流程图。首先，GPA17设定以位置的变化量及速度为分量的由下式⑴表示的状态矢 量〃 X1"(步骤K1)。
APeasl
^Pnorth Xx =
W/7
north
up
(1)式(1)中，〃APeast"、…n。rth _ —up 天顶方向的位置的变化量。另外，“Veast"、“ Vnorth“及〃 Vui 方向及天顶方向的速度。然后，GPA17设定式⑵表示的状态迁移矩阵"Φ/
APnorth"及"APud"分别表示东西方向、南北方向及
分别表示东西方向、南北 (步骤K3)。状态迁移矩
阵"φ‘是表示状态矢量"X"所包含的各分量的迁移的矩阵。"000At00 _0000At0
Φ、=00000Δ/(2)000100000010000001 _
式(2)中，“At'是来自IMU50的数据的输出时间间隔 K5)。
然后，GPA17进行利用卡尔曼滤波器的预测运算的处理，即GPA预测处理(步骤
图7是GPA预测处理的流向的流程图。
首先，GPA17将本次从IMU用位置计算部11取得的IMU运算结果所包含的速度 和前次从IMU用位置计算部11取得的IMU运算结果所包含的速度的差作为速度的变化 量〃 Av1 〃计算(步骤Li)。
然后，GPA17采用现在的状态矢量〃 X1 "、步骤K3设定的状态迁移矩阵〃 Φ:
步骤Ll计算的速度的变化量"Av1 马聚L3) ο Χ", =φ,Χ;+Χ Μυ
，按照下式(3)计算状态矢量的预测值"F1
和步 / ^
式(3)中，上标的"_ 的数据的输出时间间隔的时刻
r IMU //
t〃是时刻，表示每次来自IMU50
“分别表示东西方向、 中，将东西方向、南
Δ ν
v up // VIMU //
表示是预测值。下标的
另外，“X "表示采用IMU的运算结果设定的状态矢量 的偏差。“Χ·"所包含的” Δ v east 〃、“ Av n。rth〃及〃 南北方向、天顶方向的速度的变化量(八^的各分量)。另外，“X 北方向、天顶方向的位置分量全部设为"0"。即，假定无位置变化，进行预测运算。另外，GPA17根据卡尔曼滤波器的理论，进行预测误差协方差矩阵"Pl"的运算 (步骤L5)。然后，GPA17结束GPA预测处理。返回图6的说明，进行GPA预测处理后，GPA17判定是否为补正定时(步骤K7)。 即，判定步骤D3设定的补正定时是否到来。在判定不是补正定时的场合(步骤K7 ；否)，返 回步骤K5，再次进行GPA预测处理。本实施例中，将GPS单元100的数据的每个输出时间间隔即"1秒"的定时设为 补正定时。另一方面，从IMU用位置计算部11以"0.01秒"的时间间隔输出IMU运算结 果。从而，采用从IMU用位置计算部11以0. 01秒的时间间隔输出的IMU运算结果，可以在 1秒间进行100次的预测运算。因此，本实施例中，在1次的补正定时到来为止的期间，反复 执行GPA预测处理100次。另一方面，步骤K7中判定是补正定时的场合(步骤K7;是)，GPA17进行GPA补正 处理(步骤K9)。图8是GPA补正处理的流向的流程图。首先，GPA17参照步骤Dl设定的观测值表，在该补正定时，生成设定了作为观测值 的参数值的观测矢量"Z1"(步骤Ml)。本实施例中，采用GPS单元100计算的位置及速 度，生成下式(4)所示的观测矢量"Z1"
C Z1 =
l^Ij north
GPS
APup
Vgps east
^ north
VGPS up其中，"Δ ρ'
GPS // east
‘‘ApGPSn。rth〃及〃 Apgpsup〃分别表示GPS单元100计算的东
11
H,=
量"Z
//
西方向、南北方向及天顶方向的位置的变化量。另外，"Vcpseast”、“ Vepsnwth〃及〃 Vepsup‘ 分别表示GPS单元100计算的东西方向、南北方向及天顶方向的速度。
然后，GPA17设定下式(5)所示的观测矩阵〃 H1"(步骤M3)。 "1 0 0 0 0 0" 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1_
观测矩阵"H"是用于通过对状态矢量的预测值"Γ"作用而计算观测矢 所包含的各分量即各观测值的预测值的矩阵。本实施例中，如式(5)所示，用对角 分量全部是〃 1"而其他分量是〃 0〃的观测矩阵〃 H1"进行计算。然后，GPA17按照下式(6)计算卡尔曼增益〃 K1"(步骤Μ5)。K1 = P；Hj [",P~H[ +R} ]"' ( 6 )其中，“R1"是表示观测矢量〃 Z1"的各分量即各观测值包含的误差的大小的测 定误差矩阵。然后，GPA17通过将步骤M3设定的观测矩阵〃 H1"作用于GPA预测处理计算的状 态矢量的预测值"F1"，计算预测的观测矢量即预测观测矢量"H1F1“(步骤M7)。然后，GPA17按照下式(7)补正状态矢量的预测值"F1"的同时(步骤M9)，按照 下式(8)补正误差协方差矩阵的预测值"F1"(步骤Mil)。然后，GPA17结束GPA补正处理。= + K,V, = X「+ K1 [ζ, - H,Χ； ] ( 7 )P； =[l - Κ,Η^' (8)其中，上标的〃 +〃表示为补正值。另外，“1〃是单位矩阵。返回图6的说明，进行GPA补正处理后，GPA17将状态矢量"X1"的补正值〃 X 所包含的位置的变化量"Δρ〃加到从IMU用位置计算部11取得的IMU运算结果所包含的 位置，作为位置的计算结果(步骤Κ11)。另外，GPA17将状态矢量〃 X1"的补正值〃 X+广 所包含的速度"ν"作为速度的计算结果(步骤Κ13)。然后，GPA17结束GPA位置汇总处 理。返回图4的位置计算处理，在进行GPA位置汇总处理后，GPA17将GPA位置汇总处 理的运算结果作为GPA运算结果向IMU用位置计算部11及方位传感器单元110输出(步 骤 D7)。IMU用位置计算部11从GPA17取得GPA运算结果后，进行惯性导航运算结果校正 处理(步骤Ell)。惯性导航运算结果校正处理是采用GPA17进行位置汇总处理而计算的位 置及速度来校正通过步骤Ε7的惯性导航运算处理而计算的位置及速度的处理。图9是惯性导航运算结果校正处理的流向的流程图。首先，IMU用位置计算部11设定以惯性导航运算处理获得的位置及速度作为分量 的状态矢量"X2"(步骤Ni)。即，设定下式(9)所示的状态矢量"X2
//
X2 =
Vup
Pei
(9)
Pnnrlh
_Pup .然后，IMU用位置计算部11进行IMU预测处理(步骤N3)。图10是IMU预测处理的流向的流程图。首先，IMU用位置计算部11对从IMU50取得的角速度进行时间积分，计算姿势角 (步骤Pl)。然后，IMU用位置计算部11用计算的姿势角，将从IMU50取得的本地坐标系中的 加速度变换为ENU坐标系(东北上坐标系)中的加速度(步骤P3)。该加速度的变换可以 采用由方位传感器单元110检测的方位，通过公知的坐标变换运算实现。然后，IMU用位置计算部11对步骤P3中变换的加速度进行时间积分，计算速度 (步骤P5)。然后，IMU用位置计算部11对计算的速度进行时间积分，计算位置(步骤P7)。然后，IMU用位置计算部11将步骤P5计算的位置及速度作为状态矢量的预测 值〃 X_2〃(步骤P9)。另外，IMU用位置计算部11进行基于卡尔曼滤波器的理论的预测运 算，计算误差协方差矩阵的预测值"P—/ (步骤Pll)。然后，IMU用位置计算部11结束 IMU预测处理。返回图9的说明，在进行IMU预测处理后，IMU用位置计算部11进行IMU补正处 理(步骤N5)。图11是IMU补正处理的流向的流程图。首先，IMU用位置计算部11分别计算从GPA17取得的GPA运算结果所包含的位置 及速度与IMU预测处理的步骤P9预测的状态矢量所包含的位置及速度的预测值"X_2"的 差分，生成观测矢量"Z2"(步骤Q1)。即，生成下式(10)所示的观测矢量"Z2"
Z2 =
JMU
GPA
ea
V north^ north
VIMU-Vgpa
upup
P 國—Pgpa
γ eastr east
P northP north
P國-Pgpa
Γ upΓ U,
ψ
(10)其中，上标的〃 IMU〃表示是通过IMU预测处理预测的值，〃 GPA"表示是GPA运 算结果所包含的值。然后，IMU用位置计算部11设定下式(11)所示观测矩阵〃 H2"(步骤Q3)。
权利要求
一种位置计算方法，是具备惯性计测单元且可装入检测在位置计算中可利用的物理现象的至少一个传感器单元的位置计算装置所进行的位置计算方法，其包括在上述传感器单元装入时，识别上述传感器单元的种类；在上述传感器单元未装入时，用上述惯性计测单元的输出计算位置；以及在上述传感器单元装入时，根据上述识别的上述传感器单元的种类，用上述传感器单元的输出和上述惯性计测单元的输出计算位置。
2.根据权利要求1所述的位置计算方法，还包括取得表示进行上述传感器单元的校正时采用的校正基准信息的类别的校正基准类别 fn息；在上述传感器单元装入时，根据上述校正基准类别信息和上述惯性计测单元的输出计 算上述校正基准信息；以及用上述校正基准信息进行上述传感器单元的校正。
3.根据权利要求1或2所述的位置计算方法，还包括在上述传感器单元装入时，由采用上述传感器单元的输出的过滤处理计算位置。
4.根据权利要求3所述的位置计算方法，其中，上述传感器单元是采用来自位置计算用卫星的卫星信号可计算位置的卫星位置计算 单元，由上述过滤处理进行的位置计算包括用上述卫星位置计算单元计算的位置以外的值预测位置；和根据上述卫星位置计算单元计算的位置补正上述预测的位置。
5.根据权利要求4所述的位置计算方法，还包括取得表示上述卫星位置计算单元计算位置的时间间隔的信息， 上述预测包括在上述时间间隔之间反复预测位置， 上述补正包括在上述时间间隔补正上述预测的位置。
6.一种位置计算装置，具备 惯性计测单元；装入部，其可装入检测在位置计算中可利用的物理现象的传感器单元； 识别部，其在上述传感器单元装入时，识别上述传感器单元的种类；以及 位置计算部，其在上述传感器单元未装入时，用上述惯性计测单元的输出计算位置，在 上述传感器单元装入时，根据上述识别的上述传感器单元的种类，用上述传感器单元的输 出和上述惯性计测单元的输出计算位置。
全文摘要
本发明提出用于实现可灵活应对装置的扩展、变更等的位置计算装置的方法。在作为位置计算装置的一种的位置计算系统1中，具备可装入IMU50和GPS单元100的第1插槽60及第2插槽70。任一插槽都未装入GPS单元100时，用IMU50的计测结果计算位置，任一插槽装入GPS单元100时，用IMU50的计测结果和GPS单元100的计测结果计算位置。
文档编号G01S19/47GK101943758SQ20101022339
公开日2011年1月12日 申请日期2010年7月2日 优先权日2009年7月2日
发明者内田周志, 库马尔·阿南德, 恩田健至, 拉马·桑贾伊, 杉本拓也, 水落俊一 申请人:精工爱普生株式会社