JP2012002734A - Position detecting device, method, and program - Google Patents
Position detecting device, method, and program Download PDFInfo
- Publication number
- JP2012002734A JP2012002734A JP2010139237A JP2010139237A JP2012002734A JP 2012002734 A JP2012002734 A JP 2012002734A JP 2010139237 A JP2010139237 A JP 2010139237A JP 2010139237 A JP2010139237 A JP 2010139237A JP 2012002734 A JP2012002734 A JP 2012002734A
- Authority
- JP
- Japan
- Prior art keywords
- value
- distance
- landmark
- abnormal
- landmark device
- 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.)
- Withdrawn
Links
Images
Landscapes
- Navigation (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
Description
本発明は、屋内において現在の位置を検出する位置検出技術に関する。 The present invention relates to a position detection technique for detecting a current position indoors.
GPS(Global Positioning System)信号が受信できない屋内等において、現在位置を測定する方法としてTDOA(Time Difference Of Arrival)システム等が知られている。 A TDOA (Time Difference Of Arrival) system or the like is known as a method of measuring the current position in an indoor area where a GPS (Global Positioning System) signal cannot be received.
TDOAシステムにおいては、適当な間隔にて設置された複数のランドマーク装置が、周期的に距離測定のための信号、例えば、音波信号と無線信号を含むTDOA信号を送信する。なお、各ランドマーク装置は、ほぼ同時にTDOA信号を送信する様に制御されており、TDOA信号に含まれる無線信号には、当該無線信号を送信したランドマーク装置の設置位置についての情報が含まれている。 In the TDOA system, a plurality of landmark devices installed at appropriate intervals periodically transmit a signal for distance measurement, for example, a TDOA signal including a sound wave signal and a radio signal. Each landmark device is controlled to transmit the TDOA signal almost simultaneously, and the radio signal included in the TDOA signal includes information on the installation position of the landmark device that transmitted the radio signal. ing.
位置検出装置が、あるランドマーク装置からの無線信号を時刻t1に、このランドマーク装置から音波信号を時刻t2に受信したものとすると、位置検出装置と当該ランドマーク装置との距離は、音速×(t2−t1)で計算することができる。ランドマーク装置からの無線信号には、当該ランドマークの位置情報が含まれているため、位置検出装置は3つ以上のランドマーク装置との距離を測定することで、自身の現在位置を計算することができる。 Assuming that the position detection device receives a radio signal from a certain landmark device at time t1 and a sound wave signal from the landmark device at time t2, the distance between the position detection device and the landmark device is the speed of sound × It can be calculated by (t2-t1). Since the position information of the landmark is included in the radio signal from the landmark apparatus, the position detection apparatus calculates its current position by measuring the distance from three or more landmark apparatuses. be able to.
ただし、位置検出装置を携帯して移動しながら位置を測定する場合には、測定値をそのまま使用するのではなく、通常、カルマン・フィルタ等を用いて現在位置の推定を行い、推定値により補正した測定値を出力することが行われている(例えば、非特許文献1〜6、参照。)。 However, when measuring the position while carrying the position detection device, the measured value is not used as it is, but the current position is usually estimated using a Kalman filter, etc., and corrected using the estimated value. The measured values are output (for example, see Non-Patent Documents 1 to 6).
しかしながら、移動中には障害物やマルチパス等の影響により、ランドマーク装置からのTDOA信号が不安定となり、測定精度の劣化の原因となっている。 However, during movement, the TDOA signal from the landmark apparatus becomes unstable due to the influence of obstacles, multipaths, etc., which causes the measurement accuracy to deteriorate.
このため、例えば、非特許文献2ではLSQ(Least Squares Minimization)アルゴリズムの利用を開示しているが、引用文献2に記載の方法は、ある種の想定条件に基づくものあり、想定条件から大きく外れた場合には、精度も大きく劣化するという問題がある。
For this reason, for example, Non-Patent
また、非特許文献1及び3〜5は、特殊な統計的仮定に基づき、積分処理や反復計算処理を行うことを開示しているが、計算能力の低い携帯機器への実装には適するものではないという問題がある。
したがって、本発明は、必要な計算量が少なく、信頼性の高い測定結果を得ることができる位置検出装置、方法及びプログラムを提供することを目的とする。 Therefore, an object of the present invention is to provide a position detection apparatus, method, and program that can obtain a highly reliable measurement result with a small amount of calculation.
本発明における位置検出装置によれば、
ランドマーク装置から、該ランドマーク装置の位置情報を受信する受信手段と、ランドマーク装置との距離を測定する測位手段と、各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定する距離異常判定手段と、各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力する位置判定手段とを備えており、位置判定手段は、新たに測定した第1のランドマーク装置との距離が異常である場合、新たに測定した第1のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求めることを特徴とする。
According to the position detection device of the present invention,
Each newly measured land based on the distance history between the receiving device for receiving the position information of the landmark device from the landmark device, the positioning device for measuring the distance from the landmark device, and the distance from each landmark device. Based on the distance anomaly judgment means that determines whether the distance to the mark device is abnormal, the position information of each landmark device, and the distance to each landmark device, the current position is obtained and output by the Kalman filter And a position determining means that, when the distance from the newly measured first landmark device is abnormal, the position determining means determines the Kalman from the distance from the newly measured first landmark device.・ The larger the value obtained by subtracting the predicted value by the filter, the smaller the weighting factor is calculated, the Kalman gain is adjusted by the calculated weighting factor, and the adjusted And it obtains the current position by down gain.
本発明による位置検出装置の他の実施形態によれば、
加速度を測定する手段と、加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定する手段とをさらに備えており、位置判定手段は、新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合、現在位置が測定不可であると判定することも好ましい。
According to another embodiment of the position detection device according to the present invention,
Means for measuring acceleration, and means for determining whether or not the newly measured acceleration is abnormal based on the history of acceleration, wherein the position determining means includes each newly measured landmark device and If any one of the distances is abnormal and the newly measured acceleration is abnormal, it is also preferable to determine that the current position is not measurable.
また、本発明による位置検出装置の他の実施形態によれば、
距離異常判定手段は、異常ではないと判定したときの、測位手段が測定した各ランドマーク装置との距離と、カルマン・フィルタによる、その予測値との差の平均値である第1の値を保持しており、新たに測定した各ランドマーク装置との距離と、その予測値との差である第2の値に基づき、前記平均値を更新し、新たに測定した第1のランドマーク装置との距離と、前記更新後の平均値との差である第3の値を求め、第3の値に基づく値を閾値判定して、新たに測定した第1のランドマーク装置との距離が異常であるか否かを判定することも好ましい。
Further, according to another embodiment of the position detection device according to the present invention,
The distance abnormality determining means determines a first value that is an average value of the difference between the distance from each landmark device measured by the positioning means and the predicted value by the Kalman filter when it is determined that there is no abnormality. A first landmark device that is held and updated based on a second value that is the difference between the distance from each newly measured landmark device and its predicted value, and is newly measured. And a third value that is the difference between the updated average value and the updated average value, a threshold value is determined for the value based on the third value, and the newly measured distance to the first landmark device is It is also preferable to determine whether or not there is an abnormality.
さらに、本発明による位置検出装置の他の実施形態によれば、
距離異常判定手段は、測位手段が測定した各ランドマーク装置との距離と、その予測値との差の標準偏差値である第4の値を、前記第1の値及び前記第2の値に基づき計算し、前記第3の値を、前記第4の値で除した値である第5の値を求め、第5の値に基づく値を閾値判定して、新たに測定した第1のランドマーク装置との距離が異常であるか否かを判定することも好ましい。
Furthermore, according to another embodiment of the position detecting device according to the present invention,
The distance anomaly judgment means sets the fourth value, which is the standard deviation value of the difference between the distance from each landmark device measured by the positioning means and the predicted value, to the first value and the second value. A first value calculated based on the third value is obtained by dividing the third value by the fourth value to obtain a fifth value, and a threshold value is determined for the value based on the fifth value. It is also preferable to determine whether or not the distance from the mark device is abnormal.
さらに、本発明による位置検出装置の他の実施形態によれば、
距離異常判定手段は、平均及び標準偏差が、前記更新後の平均値及び第4の値である正規分布において、その平均値を中心とした積分値が前記第5の値の2倍から1を引いた値に等しくなる位置における確率密度を求め、求めた確率密度にランドマーク装置の数及び距離異常判定手段が異常ではないと判定した回数を乗じた第6の値を求め、第6の値を閾値判定して、新たに測定した第1のランドマーク装置との距離が異常であるか否かを判定することも好ましい。
Furthermore, according to another embodiment of the position detecting device according to the present invention,
In the normal distribution in which the average abnormality and the standard deviation are the updated average value and the fourth value, the distance abnormality determination unit has an integral value centered on the average value that is 2 to 1 from the fifth value. A probability density at a position equal to the subtracted value is obtained, a sixth value obtained by multiplying the obtained probability density by the number of landmark devices and the number of times the distance abnormality determining means determines that it is not abnormal is obtained. It is also preferable to determine whether or not the distance from the newly measured first landmark apparatus is abnormal.
本発明におけるプログラムによれば、
上記位置検出装置としてコンピュータを機能させることを特徴とする。
According to the program of the present invention,
A computer is made to function as the position detection device.
本発明における位置検出方法によれば、
複数のランドマーク装置からの信号を受信する位置検出装置における位置検出方法であって、各ランドマーク装置から、各ランドマーク装置の位置情報を受信し、各ランドマーク装置との距離を測定するステップと、各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定するステップと、各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力するステップとを備えており、新たに測定した第1のランドマーク装置との距離が異常である場合、新たに測定した第1のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求めることを特徴とする。
According to the position detection method of the present invention,
A position detection method in a position detection device that receives signals from a plurality of landmark devices, the step of receiving position information of each landmark device from each landmark device and measuring a distance from each landmark device And determining whether or not the distance to each newly measured landmark device is abnormal based on the history of the distance to each landmark device, position information of each landmark device, and each landmark And a step of obtaining and outputting the current position by a Kalman filter based on the distance to the apparatus, and when the distance from the newly measured first landmark apparatus is abnormal, the newly measured first From the distance to the landmark device, calculate a weighting factor that decreases as the value obtained by subtracting the predicted value by the Kalman filter increases. Adjust the Kalman gain by number, by the Kalman gain after adjustment and obtains the current position.
本発明による位置検出方法の他の実施形態によれば、
加速度を測定するステップと、加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定するステップとをさらに備えており、新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合には、現在位置を測定不可であると判定することも好ましい。
According to another embodiment of the position detection method according to the present invention,
A step of measuring acceleration, and a step of determining whether or not the newly measured acceleration is abnormal based on a history of acceleration, and any one of the distances to each newly measured landmark device Is abnormal and it is also preferable to determine that the current position is not measurable when the newly measured acceleration is abnormal.
移動により、各ランドマーク装置との距離の測定が不安定となったとしても、測定精度を劣化させることなく位置を推定することができる。本発明による位置検出に必要な計算処理は少なく、本発明は、携帯型機器への実装に適したものである。 Even if the distance measurement with each landmark device becomes unstable due to the movement, the position can be estimated without degrading the measurement accuracy. The calculation processing required for position detection according to the present invention is small, and the present invention is suitable for mounting on a portable device.
本発明を実施するための形態について、以下では図面を用いて詳細に説明する。 EMBODIMENT OF THE INVENTION The form for implementing this invention is demonstrated in detail below using drawing.
図2に示す様に、本発明による位置検出システムは、複数のランドマーク装置20−1〜20−nと、位置検出装置10とを含んでおり、各ランドマーク装置20−1〜20−nは、周期的に、音波による信号と無線信号を同時に送信する。つまり、距離測定のためのTDOA信号を送信する。好ましくは、各ランドマーク装置20−1〜20−nからのTDOA信号の送信は、同期、つまりほぼ同時刻に生じる様に制御される。また、各TDOA信号に含まれる無線信号は、送信したランドマーク装置の設置位置についての情報を含んでいる。なお、位置検出装置10が静止している場合において、位置検出装置10の位置を確定させるためにはnは3以上でなければならないが、本発明においては、利用者が位置検出装置10を携帯して移動することを想定しており、現在位置の決定に過去の履歴を使用するため、nは2以上であれば良い。
As shown in FIG. 2, the position detection system according to the present invention includes a plurality of landmark devices 20-1 to 20-n and a
図1は、本発明による位置検出装置の構成図である。図1によると、位置検出装置10は、受信部1と、測距部2と、距離異常判定部3と、加速度異常判定部4と、加速度測定部5と、位置判定部6とを備えている。受信部1は、各ランドマーク装置からのTDOA信号を受信して、TDOA信号に含まれる無線信号から各ランドマーク装置の設置位置情報を取得し、取得した各ランドマーク装置の設置位置情報を距離異常判定部3及び位置判定部6に通知する(図示せず。)。
FIG. 1 is a configuration diagram of a position detection apparatus according to the present invention. According to FIG. 1, the
測距部2は、各ランドマーク装置からTDOA信号を受信する度に、受信したTDOA信号に基づき各ランドマーク装置との距離を測定し、測定した距離を距離異常判定部3と、位置判定部6に出力する。また、加速度測定部5は、2軸又は3軸の加速度センサを有しており、周期的に測定した加速度を加速度異常判定部4に出力する。
Each time the
距離異常判定部3は、各ランドマーク装置との距離の履歴から、各周期において測距部2から入力された距離が異常であるか否かを判定し、判定結果を位置判定部6に出力する。同様に、加速度異常判定部4は、加速度の履歴から、各周期において加速度測定部5から入力された加速度が異常であるか否かを判定し、判定結果を位置判定部6に出力する。
The distance
図3は、位置判定部6における処理フロー図である。なお、以下の説明において、ある時刻において、TDOA信号により測定した各ランドマーク装置との距離を満たす位置を“測定位置”と呼び、当該時点において、位置検出装置10が予測した位置を“予測位置”と呼び、当該時点における測定位置及び予測位置から求めた位置を“最適位置”と呼ぶものとする。なお、位置判定部6は、最適位置を、当該時点における位置として外部に出力する。以下に、図3の処理フローを説明する。
FIG. 3 is a processing flowchart in the position determination unit 6. In the following description, a position that satisfies a distance from each landmark device measured by the TDOA signal at a certain time is referred to as a “measurement position”, and a position predicted by the
位置判定部6は、周期的に、測距部2から各ランドマーク装置との距離を、距離異常判定部3及び加速度異常判定部4から判定結果を受信する(S31)。距離異常判定部3からの判定結果が正常である場合には(S32)、第1の位置判定処理により最適位置を求めて(S34)、求めた最適位置を出力する(S35)。一方、距離異常判定部3からの判定結果が異常である場合には(S32)、加速度異常判定部4からの判定結果が異常であるか否かを確認する(S33)。加速度異常判定部4からの判定結果が正常である場合には、第2の位置判定処理により最適位置を求めて(S36)、求めた最適位置を出力する(S37)。一方、加速度異常判定部4からの判定結果が異常である場合には、測定不可を出力する(S38)。位置判定部6は、S31からS35、S37又はS38までの処理を、位置検出処理の終了指示を外部から受け取るまで、周期的に繰り返し行う。
The position determination unit 6 periodically receives the distances from the
第1の位置判定処理は、ある処理時点における各ランドマーク装置との距離が過去の履歴から異常ではないと判定された場合の処理である。つまり、受信しているTDOA信号が信頼できる場合の処理であり、カルマン・フィルタにより最適位置を算出するものである。第1の位置判定処理を説明する前に、本発明の説明に必要な各パラメータについて説明する。まず、時刻kにおける位置検出装置10の位置を(px(k),py(k))、位置検出装置10の速度を(vx(k),vy(k))とし、
The first position determination process is a process when it is determined that the distance from each landmark device at a certain processing time point is not abnormal from the past history. That is, the process is performed when the received TDOA signal is reliable, and the optimum position is calculated by the Kalman filter. Before describing the first position determination process, each parameter necessary for describing the present invention will be described. First, the position of the
X(k)=AX(k−1)+μ(k)
X (k) = AX (k−1) + μ (k)
なお、上式のμ(k)は時間遷移に関する雑音であり、共分散行列Qかつ正規分布に従う。共分散行列Qは、固定値であってもよいが、非特許文献7に記載されている方法に従い時間により変化させてもよい。 Note that μ (k) in the above equation is noise related to time transition, and follows a covariance matrix Q and a normal distribution. The covariance matrix Q may be a fixed value or may be changed according to time according to the method described in Non-Patent Document 7.
時刻kにおいて、位置検出装置10がn個のランドマーク装置からTDOA信号を受信し、i番目のランドマーク装置の位置を(pxi,pyi)、TDOA信号から計算したi番目のランドマーク装置と位置検出装置10との距離をdiとし、
At time k, the
Z(k)=B(k)+v(k)
Z (k) = B (k) + v (k)
なお、上式において、v(k)は観測雑音であり、共分散行列Rかつ零平均の多変数正規分布に従う。共分散行列Rは、固定値であってもよいが、非特許文献7に記載されている方法に従い時間により変化させてもよい。本発明においては、観測モデルH(k)は、B(k)の各要素を偏微分して得られる以下の行列として定義する。 In the above equation, v (k) is an observation noise and follows a multivariate normal distribution with covariance matrix R and zero mean. The covariance matrix R may be a fixed value or may be changed with time according to the method described in Non-Patent Document 7. In the present invention, the observation model H (k) is defined as the following matrix obtained by partial differentiation of each element of B (k).
(第1の位置判定処理)続いて、第1の位置判定処理について説明する。位置判定部6は、時刻kにおける予測行列Xp(k)を、時刻k−1における最適行列Xo(k−1)から、以下の式により求める。
Xp(k)=AXo(k−1)
なお、予測行列Xp及び最適行列Xoは、4行1列であり、1行目及び3行目の要素がそれぞれ予測位置及び最適位置のx座標及びy座標を示し、2行目及び4行目の要素がそれぞれx方向及びy方向の速度成分を示している。また、最適行列Xoの初期値は、最終的には収束するため任意であり、例えば、総ての要素を0とする。
(First Position Determination Process) Next, the first position determination process will be described. The position determination unit 6 obtains the prediction matrix X p (k) at time k from the optimum matrix X o (k−1) at time k−1 by the following expression.
X p (k) = AX o (k-1)
Note that the prediction matrix X p and the optimal matrix X o are 4 rows and 1 column, and the elements of the first row and the third row indicate the x coordinate and the y coordinate of the predicted position and the optimal position, respectively. The elements in the rows indicate velocity components in the x direction and the y direction, respectively. Further, the initial value of the optimal matrix Xo is arbitrary because it eventually converges. For example, all elements are set to 0.
続いて、時刻kにおける予測誤差行列P(k)と、カルマン・ゲインK(k)を以下の式により求める。
P(k)=AC(k−1)AT+Q
K(k)=P(k)H(k)T(H(k)P(k)H(k)T+R)−1
ここで、C(k−1)は、以下の式で求められる。
C(k−1)=(1−K(k−1)H(k−1))P(k−1)
なお、C(k)の初期値は、最終的には収束するため任意であり、例えば、4行4列の単位行列とする。
Subsequently, a prediction error matrix P (k) and a Kalman gain K (k) at time k are obtained by the following equations.
P (k) = AC (k−1) A T + Q
K (k) = P (k) H (k) T (H (k) P (k) H (k) T + R) −1
Here, C (k−1) is obtained by the following equation.
C (k-1) = (1-K (k-1) H (k-1)) P (k-1)
Note that the initial value of C (k) is arbitrary because it eventually converges, and is, for example, a unit matrix of 4 rows and 4 columns.
最後に、位置判定部6は、時刻kにおける最適行列Xo(k)を以下の式により求め、求めたXo(k)の第1行及び第3行を取り出し、最適位置として出力する。
Xo(k)=Xp(k)+K(k)(Z(k)−H(k)Xp(k))
また、時刻k+1での処理のため、
C(k)=(1−K(k)H(k))P(k)を求めておく。
Finally, the position determination unit 6 obtains the optimum matrix X o (k) at time k by the following equation, extracts the first and third rows of the obtained X o (k), and outputs them as optimum positions.
X o (k) = X p (k) + K (k) (Z (k) -H (k) X p (k))
Also, for processing at time k + 1,
C (k) = (1−K (k) H (k)) P (k) is obtained in advance.
位置検出装置10が各ランドマーク装置からの信号を常に安定して受信できる場合、上述した第1の位置判定処理を繰り返すのみでよいが、現実的には、障害物の影響等により受信状態が不安定になる。この不安定な状態にて検出した異常値をそのまま第1の位置判定処理にて使用するとその精度が劣化するため、本発明においては、測定距離が異常である場合(図3のS32)には、第1の位置判定処理による最適位置の決定は行わず、加速度が異常であるか否かに応じて(図3のS33)、第2の位置判定処理による最適位置の決定又は測定不可の出力を行う。
When the
(距離異常判定処理)以下、距離異常判定部3による距離異常判定処理について説明する。なお、位置判定部6は、時刻k−1の処理終了後、時刻kにおける予測行列Xp(k)を算出して、その1行目及び3行目の要素で表される位置、つまり、時刻kにおける予測位置を、距離異常判定部3に出力しておく。
(Distance abnormality determining process) The distance abnormality determining process by the distance
距離異常判定部3は、時刻kにおける予測位置に基づき、i番目のランドマーク装置との距離dpi(k)を求め(iはn以下の自然数)、Z(k)の各要素di(k)との差γi(k)=di(k)−dpi(k)を求める。 The distance abnormality determination unit 3 obtains a distance d pi (k) from the i-th landmark device based on the predicted position at time k (i is a natural number equal to or less than n), and each element d i (Z (k)) Difference γ i (k) = d i (k) −d pi (k) from k) is obtained.
また、距離異常判定部3は、時刻k−1までの各時刻における距離異常判定処理にて、異常ではないと判断した回数を記録しており、異常では無い時の処理にて求めた各γの総ての平均値
Further, the distance
距離異常判定部3は、各γi(k)について、以下の式によりΦの値を求め、続いて、2Φ−1の値を求める。
For each γ i (k), the distance
最後に、距離異常判定部3は、求めた確率密度の値をnp倍し、この値と所定の閾値、例えば、0.5とを比較し、np倍した値が所定の閾値より小さい場合、異常であると判定し、そうでない場合には正常であると判定する。つまり、図4において符号61及び62で示す位置が、平均から所定の距離より離れている場合に、異常であると判定する。
Finally, the distance
(加速度異常判定処理)続いて、加速度異常判定部4における加速度異常判定処理について説明する。まず、加速度異常判定部4は、加速度測定部5から取得する時刻kにおける直交する2方向の加速度の値ax(k)及びay(k)から、時刻kにおける加速度の値a(k)を以下の式により求める。
(Acceleration abnormality determination process) Next, the acceleration abnormality determination process in the acceleration
なお、ax(k)及びay(k)は、加速度測定部5が加速度センサから取得した時刻kにおける瞬時値であってもよいが、加速度測定部5は、時刻k−1から時刻kまでのある期間における加速度センサが出力する値を積分して、積分した値をax(k)及びay(k)として加速度異常判定部4に出力することが好ましい。
Note that a x (k) and a y (k) may be instantaneous values at the time k acquired by the
続いて、加速度異常判定部4は、時刻kにおける加速度の値a(k−1)から、時刻kにおける加速度の予測値ap(k)を以下の式により求める。
ap(k)=a(k−1)+Δτ
なお、Δτは、以下の式であらわされる値である。
Δτ=sgn(a(k−1)−a(k−2))*Δτ´
ここで、Δτ´は以下の式に示す様に、時刻k−Mから時刻k−1までの加速度の変化の平均値である。
Subsequently, the acceleration
a p (k) = a (k−1) + Δτ
Δτ is a value represented by the following equation.
Δτ = sgn (a (k−1) −a (k−2)) * Δτ ′
Here, Δτ ′ is an average value of changes in acceleration from time k−M to time k−1 as shown in the following equation.
続いて、加速度異常判定部4は、時刻kにおける加速度の予測誤差diff(k)を以下の式により求める。
diff(k)=a(k)−ap(k)
Subsequently, the acceleration
diff (k) = a (k) −a p (k)
加速度異常判定部4は、時刻k−1までの各時刻における加速度異常判定処理にて、異常ではないと判断された回数を記録しており、異常では無い時の処理にて求めた各予測誤差diffの総ての平均値
The acceleration
以上、本発明においては、距離異常判定処理にて測定位置の異常を検出した場合、加速度異常判定処理にて、加速度の異常が発生しているか否かを判定し、加速度の異常が発生していなければ、位置検出装置を携帯している利用者の動作に大きな変化が発生していないものとして、以下に説明する第2の位置判定処理を行う。これに対して、加速度の異常が発生している場合には、位置の判定は不可であると判定して、測定不可を出力する。 As described above, in the present invention, when a measurement position abnormality is detected in the distance abnormality determination process, it is determined whether an acceleration abnormality has occurred in the acceleration abnormality determination process, and an acceleration abnormality has occurred. Otherwise, the second position determination process described below is performed on the assumption that no significant change has occurred in the operation of the user carrying the position detection device. On the other hand, when an abnormality in acceleration occurs, it is determined that the position cannot be determined, and measurement impossible is output.
(第2の位置判定処理)続いて、第2の位置判定処理について説明する。第2の位置判定処理において、位置判定部6は、距離異常判定部3から、距離異常判定部3が判定に使用した各パラメータを取得し、時刻kにおける各γi(k)について、マハラビノス距離Mi(k)を、以下の式により求める。
(Second Position Determination Process) Next, the second position determination process will be described. In the second position determination process, the position determination unit 6 acquires each parameter used for the determination by the distance
続いて、位置判定部6は、重み行列W(k)を、以下の様に求める。 Subsequently, the position determination unit 6 calculates the weight matrix W (k) as follows.
位置判定部6は、以下の通り、重み行列により、Z(k)、B(k)、H(k)及びRを調整する。
Z´(k)=W(k)Z(k)
B´(k)=W(k)B(k)
H´(k)=W(k)H(k)
R´=cov[W(k)v(k)]
The position determination unit 6 adjusts Z (k), B (k), H (k), and R using a weight matrix as follows.
Z ′ (k) = W (k) Z (k)
B ′ (k) = W (k) B (k)
H ′ (k) = W (k) H (k)
R ′ = cov [W (k) v (k)]
さらに、位置判定部6は、重み行列で調整したカルマン・ゲインK´(k)を以下の式により求める。
K´(k)=P(k)H´(k)T(H´(k)P(k)H´(k)T+R´)−1
Further, the position determination unit 6 obtains the Kalman gain K ′ (k) adjusted with the weight matrix by the following equation.
K ′ (k) = P (k) H ′ (k) T (H ′ (k) P (k) H ′ (k) T + R ′) −1
最後に、位置判定部6は、時刻kにおける最適行列Xo(k)を以下の式により求め、求めたXo(k)の第1行及び第3行を取り出し、最適位置として出力する。
Xo(k)=Xp(k)+K´(k)(Z´(k)−B´(k))
また、時刻k+1での処理のため、
C(k)=(1−K´(k)H´(k))P(k)を求めておく。
Finally, the position determination unit 6 obtains the optimum matrix X o (k) at time k by the following equation, extracts the first and third rows of the obtained X o (k), and outputs them as optimum positions.
X o (k) = X p (k) + K ′ (k) (Z ′ (k) −B ′ (k))
Also, for processing at time k + 1,
C (k) = (1−K ′ (k) H ′ (k)) P (k) is obtained in advance.
以上、位置検出装置10は、TDOA信号により求めた距離に異常を検出した場合、測定位置の予測位置との差と、その差の過去の平均との差による重み係数を求め、求めた重み係数でカルマン・ゲインを調整して最適位置を算出することで、異常値を検出した場合であっても測定精度を劣化させることなく位置を推定することができる。本発明による最適位置の算出に必要な計算処理負荷は少なく、本発明は、携帯型機器への実装に適したものである。
As described above, when the
なお、上述した実施形態においては、音波信号及び無線信号を含むTDOA信号により位置検出装置10は各ランドマーク装置との距離を測定していたが、位置検出装置10は、ランドマーク装置までの距離を測定する公知の種々の方法を使用することができる。
In the above-described embodiment, the
また、本発明による位置検出装置10は、コンピュータを図1の各部として機能させるプログラムにより実現することができる。これらコンピュータプログラムは、コンピュータが読み取り可能な記憶媒体に記憶されて、又は、ネットワーク経由で配布が可能なものである。さらに、本発明は、ハードウェア及びソフトウェアの組合せによっても実現可能である。
The
1 受信部
2 測距部
3 距離異常判定部
4 加速度異常判定部
5 加速度測定部
6 位置判定部
10 位置検出装置
20−1〜20−n ランドマーク装置
DESCRIPTION OF SYMBOLS 1
Claims (8)
ランドマーク装置との距離を測定する測位手段と、
各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定する距離異常判定手段と、
各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力する位置判定手段と、
を備えており、
位置判定手段は、新たに測定した第1のランドマーク装置との距離が異常である場合、新たに測定した第1のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求める、
位置検出装置。 Receiving means for receiving position information of the landmark device from the landmark device;
Positioning means for measuring the distance from the landmark device;
Distance abnormality determination means for determining whether or not the distance to each newly measured landmark device is abnormal based on the history of the distance to each landmark device;
Position determining means for obtaining and outputting the current position by a Kalman filter based on the position information of each landmark device and the distance from each landmark device;
With
The position determination means subtracts the predicted value by the Kalman filter from the distance from the newly measured first landmark device when the distance from the newly measured first landmark device is abnormal. The larger the value, the smaller the weighting factor is calculated, the Kalman gain is adjusted by the calculated weighting factor, and the current position is obtained by the adjusted Kalman gain.
Position detection device.
加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定する手段と、
をさらに備えており、
位置判定手段は、新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合、現在位置が測定不可であると判定する、
請求項1に記載の位置検出装置。 Means for measuring acceleration;
Means for determining whether the newly measured acceleration is abnormal based on the history of acceleration;
Further comprising
The position determination means determines that the current position is not measurable if any of the distances to each newly measured landmark device is abnormal and the newly measured acceleration is abnormal.
The position detection device according to claim 1.
請求項1又は2に記載の位置検出装置。 The distance abnormality determining means determines a first value that is an average value of the difference between the distance from each landmark device measured by the positioning means and the predicted value by the Kalman filter when it is determined that there is no abnormality. A first landmark device that is held and updated based on a second value that is the difference between the distance from each newly measured landmark device and its predicted value, and is newly measured. And a third value that is the difference between the updated average value and the updated average value, a threshold value is determined for the value based on the third value, and the newly measured distance to the first landmark device is Determine whether it is abnormal,
The position detection device according to claim 1 or 2.
請求項3に記載の位置検出装置。 The distance anomaly judgment means sets the fourth value, which is the standard deviation value of the difference between the distance from each landmark device measured by the positioning means and the predicted value, to the first value and the second value. A first value calculated based on the third value is obtained by dividing the third value by the fourth value to obtain a fifth value, and a threshold value is determined for the value based on the fifth value. Determine whether the distance from the marking device is abnormal,
The position detection device according to claim 3.
請求項4記載の位置検出装置。 In the normal distribution in which the average abnormality and the standard deviation are the updated average value and the fourth value, the distance abnormality determination unit has an integral value centered on the average value that is 2 to 1 from the fifth value. A probability density at a position equal to the subtracted value is obtained, a sixth value obtained by multiplying the obtained probability density by the number of landmark devices and the number of times the distance abnormality determining means determines that it is not abnormal is obtained. To determine whether or not the distance from the newly measured first landmark device is abnormal,
The position detection device according to claim 4.
各ランドマーク装置から、各ランドマーク装置の位置情報を受信し、各ランドマーク装置との距離を測定するステップと、
各ランドマーク装置との距離の履歴に基づき、新たに測定した各ランドマーク装置との距離が異常であるか否かを判定するステップと、
各ランドマーク装置の位置情報と、各ランドマーク装置との距離に基づき、カルマン・フィルタにより現在位置を求めて出力するステップと、
を備えており、
新たに測定した第1のランドマーク装置との距離が異常である場合、新たに測定した第1のランドマーク装置との距離から、カルマン・フィルタによる、その予測値を引いた値が大きい程、小さくなる重み係数を算出し、算出した重み係数によりカルマン・ゲインを調整し、調整後のカルマン・ゲインにより現在位置を求める、
位置検出方法。 A position detection method in a position detection device for receiving signals from a plurality of landmark devices,
Receiving the position information of each landmark device from each landmark device and measuring the distance to each landmark device;
Determining whether the distance to each newly measured landmark device is abnormal based on the history of distance to each landmark device; and
Based on the position information of each landmark device and the distance to each landmark device, obtaining and outputting the current position by a Kalman filter;
With
When the distance from the newly measured first landmark device is abnormal, the larger the value obtained by subtracting the predicted value by the Kalman filter from the distance from the newly measured first landmark device, Calculate a smaller weighting factor, adjust the Kalman gain with the calculated weighting factor, and obtain the current position with the adjusted Kalman gain.
Position detection method.
加速度の履歴に基づき、新たに測定した加速度が異常であるか否かを判定するステップと、
をさらに備えており、
新たに測定した各ランドマーク装置との距離のいずれかが異常であり、かつ、新たに測定した加速度が異常である場合には、現在位置を測定不可であると判定する、
請求項7に記載の位置検出方法。 Measuring the acceleration;
Determining whether the newly measured acceleration is abnormal based on the acceleration history;
Further comprising
If any of the distances to each newly measured landmark device is abnormal and the newly measured acceleration is abnormal, it is determined that the current position is not measurable,
The position detection method according to claim 7.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010139237A JP2012002734A (en) | 2010-06-18 | 2010-06-18 | Position detecting device, method, and program |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010139237A JP2012002734A (en) | 2010-06-18 | 2010-06-18 | Position detecting device, method, and program |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2012002734A true JP2012002734A (en) | 2012-01-05 |
Family
ID=45534869
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2010139237A Withdrawn JP2012002734A (en) | 2010-06-18 | 2010-06-18 | Position detecting device, method, and program |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2012002734A (en) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101418770B1 (en) | 2012-12-14 | 2014-07-16 | 한국생산기술연구원 | Position estimation system based on rotating type distance measuring device and position estimation method using the same |
WO2015049717A1 (en) * | 2013-10-01 | 2015-04-09 | 株式会社日立製作所 | Device for estimating position of moving body and method for estimating position of moving body |
CN108268428A (en) * | 2018-01-18 | 2018-07-10 | 上海兰宝传感科技股份有限公司 | A kind of data stability Enhancement Method used based on sensor |
CN110657799A (en) * | 2019-09-26 | 2020-01-07 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
JP2020041919A (en) * | 2018-09-11 | 2020-03-19 | パナソニックIpマネジメント株式会社 | Position management system and position management method |
CN112468959A (en) * | 2020-11-24 | 2021-03-09 | 宏景科技股份有限公司 | Position determination method, position determination device, computer equipment and storage medium |
CN114545327A (en) * | 2022-02-07 | 2022-05-27 | 四川中电昆辰科技有限公司 | Motion state information and UWB fusion positioning method and positioning system |
KR102687989B1 (en) | 2019-07-11 | 2024-07-25 | 한화오션 주식회사 | Apparatus and method for forming of position data and computer readable medium storing the same |
-
2010
- 2010-06-18 JP JP2010139237A patent/JP2012002734A/en not_active Withdrawn
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101418770B1 (en) | 2012-12-14 | 2014-07-16 | 한국생산기술연구원 | Position estimation system based on rotating type distance measuring device and position estimation method using the same |
WO2015049717A1 (en) * | 2013-10-01 | 2015-04-09 | 株式会社日立製作所 | Device for estimating position of moving body and method for estimating position of moving body |
JPWO2015049717A1 (en) * | 2013-10-01 | 2017-03-09 | 株式会社日立製作所 | Moving object position estimation apparatus and moving object position estimation method |
CN108268428A (en) * | 2018-01-18 | 2018-07-10 | 上海兰宝传感科技股份有限公司 | A kind of data stability Enhancement Method used based on sensor |
CN108268428B (en) * | 2018-01-18 | 2021-03-16 | 上海兰宝传感科技股份有限公司 | Data stability enhancing method based on sensor use |
JP2020041919A (en) * | 2018-09-11 | 2020-03-19 | パナソニックIpマネジメント株式会社 | Position management system and position management method |
JP7236670B2 (en) | 2018-09-11 | 2023-03-10 | パナソニックIpマネジメント株式会社 | LOCATION MANAGEMENT SYSTEM AND LOCATION MANAGEMENT METHOD |
KR102687989B1 (en) | 2019-07-11 | 2024-07-25 | 한화오션 주식회사 | Apparatus and method for forming of position data and computer readable medium storing the same |
CN110657799B (en) * | 2019-09-26 | 2022-09-09 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
CN110657799A (en) * | 2019-09-26 | 2020-01-07 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
CN112468959A (en) * | 2020-11-24 | 2021-03-09 | 宏景科技股份有限公司 | Position determination method, position determination device, computer equipment and storage medium |
CN112468959B (en) * | 2020-11-24 | 2023-05-16 | 宏景科技股份有限公司 | Position determining method, position determining device, computer equipment and storage medium |
CN114545327A (en) * | 2022-02-07 | 2022-05-27 | 四川中电昆辰科技有限公司 | Motion state information and UWB fusion positioning method and positioning system |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Tian et al. | Human body shadowing effect on UWB-based ranging system for pedestrian tracking | |
JP2012002734A (en) | Position detecting device, method, and program | |
Pak et al. | Accurate and reliable human localization using composite particle/FIR filtering | |
Song et al. | Mobile node localization using fusion prediction-based interacting multiple model in cricket sensor network | |
JP2017534047A (en) | Method and apparatus for real-time mobile-based positioning with sensors and radio frequency measurements | |
US10852386B2 (en) | Method for calibrating a local positioning system based on time-difference-of-arrival measurements | |
WO2017040027A1 (en) | Systems and methods for selecting atmospheric data of reference nodes for use in computing the altitude of a receiver | |
KR101374589B1 (en) | Method of tracking position and apparatus performing the same | |
US20190158982A1 (en) | Radio-location method for locating a target device contained within a region of space | |
CN113108791A (en) | Navigation positioning method and navigation positioning equipment | |
KR100977246B1 (en) | Apparatus and method for estmating positon using forward link angle of arrival | |
Hur et al. | Unknown Input H $ _ {\bm\infty} $ Observer-Based Localization of a Mobile Robot With Sensor Failure | |
KR101390776B1 (en) | Localization device, method and robot using fuzzy extended kalman filter algorithm | |
Zhang et al. | Tracking mobile robot in indoor wireless sensor networks | |
Fang et al. | Robust node position estimation algorithms for wireless sensor networks based on improved adaptive Kalman filters | |
CN109270489B (en) | Real-time continuous positioning method based on UWB (ultra Wide band) under NLOS (non line of sight) tunnel environment | |
Ayabakan et al. | RSSI-based indoor positioning via adaptive federated Kalman filter | |
RU2562616C1 (en) | Method of acquiring radio information and radio system therefor | |
Shue et al. | Reducing the effect of signal multipath fading in RSSI-distance estimation using Kalman filters | |
KR101180825B1 (en) | Apparatus and Method for tracing location of the Mobile based on Sensor Network | |
KR101472690B1 (en) | Relay node for resotring a segmented wireless sensor network, and method for operation of the relay node | |
CN117570980A (en) | UWB and GPS fusion positioning algorithm-based method and system | |
US11979848B2 (en) | Access point based location system for high density WiFi deployments | |
Farmani et al. | A hybrid localization approach in wireless sensor networks using a mobile beacon and inter-node communication | |
Mahardhika et al. | Improving indoor positioning systems accuracy in closed buildings with kalman filter and feedback filter |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20130408 |
|
A300 | Withdrawal of application because of no request for examination |
Free format text: JAPANESE INTERMEDIATE CODE: A300 Effective date: 20130903 |