JP2733598B2 - Underwater vehicle speed measurement device - Google Patents

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【産業上の利用分野】この発明は水中航走体用速度計測
装置に関し、特に計測誤差の低減、つまり精度の向上に
係わる。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a speed measuring device for underwater vehicles, and more particularly to a method for reducing measurement errors, that is, improving accuracy.

【０００２】[0002]

【従来の技術】従来のこの種の速度計測装置としては、
水中航走体の推進機の回転速度より航走体速度を計測す
るものと、慣性装置を設けて、推進方向の加速度を計測
し、これを積分して速度を求めるものとが存在する。 （ａ）推進機の回転速度より航走体速度を計測する装置 図４に示すように、水中航走体２は尾部に取付けられた
スクリューなどの推進機３をモータ４で回転駆動し、そ
して得られた推進機発生推力Ｔによって航走している。
従来の速度算定過程を図４，図５を参照して説明する。2. Description of the Related Art Conventional speed measuring devices of this type include:
There are a type that measures the speed of a marine vehicle based on the rotation speed of a propulsion unit of an underwater vehicle, and a type that measures the acceleration in the direction of propulsion by providing an inertial device and calculates the speed by integrating the acceleration. (A) Apparatus for measuring the speed of the running vehicle from the rotation speed of the propulsion unit As shown in FIG. 4, the underwater vehicle 2 drives a propulsion unit 3 such as a screw attached to its tail by a motor 4, and The ship is sailing with the obtained thrust T generated by the thruster.
A conventional speed calculation process will be described with reference to FIGS.

【０００３】 はじめに、推進機３の回転速度ＮをＮ
^* に設定して、回転始動すると、航走体２は初期推力Ｔ
＝Ｔo で加速される。 航走体２の速度Ｖが上昇して行くと、速度Ｖに反比
例して推力Ｔが下がる。即ち、 Ｔ∝１／Ｖ …………… （１） 一方、航走抵抗Ｆは速度Ｖの自乗に比例して増加する。
即ち、 Ｆ∝Ｖ² …………… （２） 航走体２の加速度Ａ，質量をＭとすれば Ａ＝（Ｔ−Ｆ）／Ｍ …………… （３） の関係式が成立する。First, the rotation speed N of the propulsion device 3 is set to N
^{* When} the engine starts rotating, the hull 2 has an initial thrust T
= To. As the speed V of the vehicle 2 increases, the thrust T decreases in inverse proportion to the speed V. That is, T∝1 / V (1) On the other hand, the running resistance F increases in proportion to the square of the speed V.
That is, F∝V ² ... (2) Assuming that the acceleration A and mass of the vehicle 2 are M, the relational expression of A = (T−F) / M (3) holds. I do.

【０００４】 図５のＰo 点において発生推力Ｔと航
走抵抗Ｆとが等しくなると、そのとき（３）式より加速
度Ａ＝０となり、航走体２の速度Ｖは一定となる。この
ＴとＦとの平衡点Ｐo におけるＴ，Ｆ，ＶをそれぞれＴ
^* ，Ｆ^* ，Ｖ^* で表すと、 Ｆ^* ＝Ｔ^* …………… （４） Ｆ^* ＝ｋ₁ ^* Ｖ^*2（ｋ₁ ^* ：比例定数） …………… （５） Ｔ^* ＝ｋ₂ ^* Ｎ^*2（ｋ₂ ^* ：比例定数） …………… （６） の各式が成立する。（５），（６）式を（４）に代入し
て、 ｋ₁ ^* Ｖ^*2 ＝ｋ₂ ^* Ｎ^*2 ∴ Ｖ^* ＝（ｋ₂ ^* ／ｋ₁ ^* ）^1/2 Ｎ^* …………… （７） となる。ここで、 （ｋ₂ ^* ／ｋ₁ ^* ）^1/2 ＝Ｋ^* …………… （８） と置けば Ｖ^* ＝Ｋ^* Ｎ^* …………… （９） と表せる。（９）式のように推進機回転速度から求めた
航走体速度（機械的速度と言う）に添字（サフィック
ス）Ｐをつけることにすると、 Ｖ_P ^* ＝Ｋ^* Ｎ^* …………… （９′） 定数Ｋ^* は推力Ｔ^* と抵抗Ｆ^* とが平衡し、加速度Ａ＝
０のとき、つまり航走体速度Ｖが定速度Ｖ_P ^* に等しい
ときの値であり、加減速の有る場合は速度Ｖに応じて変
化する。しかし、加減速の有る場合の機械速度Ｖ_P を求
める際はノミナル値Ｋ（例えばＫ＝Ｋ^* と仮定）を用い
て、近似的に Ｖ_P ≒ＫＮ …………… （１０） より求めている。When the generated thrust T and the running resistance F are equal at the point Po in FIG. 5, the acceleration A = 0 at that time according to the equation (3), and the speed V of the running body 2 is constant. T, F, and V at the equilibrium point Po between T and F are represented by T
^*, F ^*, is represented by ^{V *, F * = T *} ............... (4) F * = k 1 * V * 2 (k 1 *: proportional constant) ............... (5) T ^* = K ₂ ^* N ^{* 2} (k ₂ ^* : proportionality constant) Each equation of (6) holds. (5), (6) by substituting equation in the ^{_{(4), k 1 * V}} * 2 = k 2 * N * 2 ∴ V * = (k 2 * / k 1 *) 1/2 N * ...... ……… (7) Here, if (k ₂ ^* / k ₁ ^* ) ^1/2 = K ^* ... (8), then V ^* = K ^* N ^* ... (9) can be expressed. If a subscript (suffix) P is added to the speed of the sailing body (referred to as mechanical speed) determined from the rotation speed of the propulsion unit as in equation (9), V _P ^* = K ^* N ^* …………… (9 ') The constant K ^* is such that the thrust T ^* and the resistance F ^* are balanced and the acceleration A =
The value is 0 when the vehicle speed V is equal to the constant speed _VP ^* , and changes according to the speed V when there is acceleration / deceleration. However, when obtaining the machine speed V _P of when there is acceleration or deceleration by using the nominal value K (e.g. K = K ^* assuming), with approximately determined from V _P ≒ KN ............... (10) I have.

【０００５】（ｂ−１） プラットフォーム型の慣性装
置 図６Ａに示すように水平安定台（プラットフォーム）５
上に３軸方向の加速度計６とジャイロスコープ（以下ジ
ャイロと言う）７が設置されている。ジャイロ７で航走
体２の回転動揺及び地球レート（自転角速度）を検出
し、それらの検出信号に対応した信号でサーボアンプ８
を介してサーボモータ９を制御して安定水平台５を水平
に保つ。加速度計６で検出された航走体の加速度Ａ（Ａ
_X ，Ａ_Y ，Ａ_Z ）を演算部１０で積分して速度Ｖ_I （Ｖ
_IX，Ｖ_IY，Ｖ_IZ）を得る。即ち、 Ｖ_I （Ｖ_IX，Ｖ_IY，Ｖ_IZ）＝∫Ａ（Ａ_X ，Ａ_Y ，Ａ_Z ）ｄｔ …………… （１１） 図６Ａではピッチ方向の動揺のみを示しているが、実際
には３軸方向の動揺が存在する。(B-1) Platform type inertial device As shown in FIG. 6A, a horizontal stabilizer (platform) 5
A three-axis accelerometer 6 and a gyroscope (hereinafter referred to as a gyroscope) 7 are provided on the top. The gyro 7 detects the rotation fluctuation and the earth rate (rotational angular velocity) of the marine vessel 2 and uses a signal corresponding to the detected signal to generate a servo amplifier 8.
To control the servo motor 9 to keep the stable horizontal base 5 horizontal. The acceleration A of the hull body detected by the accelerometer 6 (A
_X , A _Y , A _Z ) are integrated by the calculation unit 10 and the speed V _I (V
_IX , V _IY , V _IZ ). _{That, V I (V IX, V} IY, V IZ) = ∫A (A X, A Y, A Z) dt ............... (11) shows only Figure 6A the upset in the pitch direction, Actually, there is a three-axis sway.

【０００６】 （ｂ−２） ストラップダウン型の慣性装置 プラットフォーム型では、水平安定台を機械的に水平に
保持して、その上の加速度を検出するのに対して、スト
ラップダウン型の場合は、水平安定台を用いないで、機
体に直接取付けられたジャイロ７で得られた３軸方向の
角速度ω（ω_x,ω_y ，ω_z ）を用いて、演算部１０で機
体座標系（ｘ，ｙ，ｚ）を局地水平座標系（Ｘ，Ｙ，
Ｚ）に変換するための座標変換行列〔Ｃ〕を求め、更に
この行列〔Ｃ〕を加速度計６の検出した機体座標系での
加速度Ａ（Ａ_X ，Ａ_y ，Ａ_Z ）に乗算して、局地水平座
標系での加速度に変換し、その変換した加速度を積分し
て局地水平座標での速度Ｖ_I （Ｖ_IX，Ｖ_IY，Ｖ_IZ）を得
ている。即ち、 Ｖ_I （Ｖ_IX，Ｖ_IY，Ｖ_IZ）＝∫〔Ｃ〕Ａ（Ａ_X ，Ａ_y ，Ａ_Z ）ｄｔ …………… （１２） 慣性装置で求めた速度Ｖ_I を慣性速度と呼び、前記機械
的速度Ｖ_P と区別する。(B-2) Strap-down type inertial device In the platform type, the horizontal stabilizer is mechanically held horizontally to detect the acceleration thereon, whereas in the case of the strap-down type, The arithmetic unit 10 uses the angular velocities ω (ω _x, ω _y , ω _z ) in the three axial directions obtained by the gyro 7 directly attached to the body without using the horizontal stabilizer, and the body coordinate system (x, y, z) in the local horizontal coordinate system (X, Y,
Z), a coordinate conversion matrix [C] is obtained, and this matrix [C] is further multiplied by the acceleration A (A _X , A _y , A _Z ) in the body coordinate system detected by the accelerometer 6. Then, the velocity V _I (V _IX , V _IY , V _IZ ) in the local horizontal coordinate system is obtained by converting the acceleration into the local horizontal coordinate system and integrating the converted acceleration. _{That, V I (V IX, V} IY, V IZ) = ∫ [C] _{A (A X, A y,} A Z) dt ............... (12) inertial velocity the velocity V _I obtained in inertial device and it called to distinguish it from the mechanical velocity V _P.

【０００７】[0007]

【発明が解決しようとする課題】従来の推進機３の回転
速度Ｎから機械的速度Ｖ_P を求める方式では、 発進時などの速度が変化している場合では、図７に
示すよう、航速体２には質量Ｍがあるため推進機回転数
Ｎの立ち上がりより遅れて真の速度Ｖが上がることによ
り、Ｖ≠Ｖ_P ＝ＫＮ（Ｋはノミサル値で、例えばＫ＝Ｋ
^* ）となり、大きな算定誤差となる。In a manner to obtain the mechanical speed V _P from the rotational speed N of a conventional propulsion apparatus 3 THE INVENTION An object will to solve the above-In the case where the rate of such at the start has changed, as shown in FIG. 7, Kosokutai 2 has a mass M, and the true speed V rises later than the rise of the propulsion unit rotation speed N, so that V ≠ V _P = KN (K is a nominal value, for example, K = K
^* ), Resulting in a large calculation error.

【０００８】 推進力Ｔと抗力Ｆが釣り合い、終速度
に安定した期間では、Ｖ≒Ｖ_P ^* ＝ＫＮ^* となるが、Ｋ
はもともと縮小模型での試験等で求められたもので実機
のものと一致しない場合があり、また、製品個々のばら
つきがある。そのため、速度算定誤差が発生する。ま
た、慣性装置により速度を求める方式では、 加速度計信号に含まれる誤差が積分され時間と共に
速度誤差が大きくなる。In a period in which the propulsive force T and the drag F are balanced and stable at the final speed, V ≒ V _P ^* = KN ^*.
Is originally obtained by a test with a reduced model and may not match the actual model, and there are variations among products. Therefore, a speed calculation error occurs. Further, in the method of obtaining the speed by the inertial device, the error included in the accelerometer signal is integrated, and the speed error increases with time.

【０００９】 ジャイロ信号ωが誤差を持つ場合は、
安定水平台または演算部内での仮想的な水平面が真の水
平に保持されないこと、また座標変換行列に誤差をも
ち、重力カップリング分を積分してしまうことにより時
間と共に速度誤差が大きくなる。といった欠点がある。
この発明の目的はこれらの欠点を除去し、従来より誤差
の小さい、つまり精度の高い速度データを得る速度計を
提供することにある。When the gyro signal ω has an error,
The fact that the virtual horizontal plane in the stable horizontal platform or the operation unit is not held true true, and that the coordinate transformation matrix has an error and integrates the gravitational coupling component causes the speed error to increase with time. There are drawbacks.
SUMMARY OF THE INVENTION An object of the present invention is to provide a speedometer which eliminates these drawbacks and obtains speed data with a smaller error, that is, with higher accuracy than in the prior art.

【００１０】[0010]

【課題を解決するための手段】[Means for Solving the Problems]

（１）請求項１の発明は、ジャイロスコープ、加速度計
及び慣性データ演算部より成る慣性装置と、第１，第２
速度補正部及び走行状態判定部より成る速度計測部とか
ら構成される水中航走体用速度計測装置である。前記ジ
ャイロスコープは、３軸方向の回転角速度ωを検出する
ものである。前記加速度計は、前記３軸方向の加速度Ａ
を検出するものである。前記慣性データ演算部は、前記
加速度Ａ及び角速度ωとから水中航走体の速度（慣性速
度と言う）Ｖ_I を演算して、前記第１速度補正部に入力
すると共に、各定常走行期間において、前記第１速度補
正部より入力される補正された慣性速度Ｖ_I ′をリファ
レンス値として更に補正した慣性速度Ｖ_I を演算するも
のである。(1) The invention according to claim 1 is an inertial device comprising a gyroscope, an accelerometer, and an inertial data operation unit, and first and second inertial devices.
This is a speed measuring device for an underwater vehicle, comprising a speed correcting unit and a speed measuring unit including a running state determining unit. The gyroscope detects a rotational angular velocity ω in three axial directions. The accelerometer calculates the acceleration A in the three axial directions.
Is to be detected. The inertial data operation unit, said (referred to as inertial velocity) from the acceleration A and the angular velocity ω speed of underwater vehicle by calculating the V _I, and inputs to the first speed correcting section in each steady running period is for calculating the inertial velocity V _I in which the first speed correcting unit inertial velocity is corrected is input from the V _I 'is further corrected as a reference value.

【００１１】前記走行状態判定部は、前記加速度Ａ及び
角速度ωまたは水中航走体の推進機関部で得られる増減
速信号（ｄＮ／ｄｔ；Ｎは推進機の回転速度）より水中
航走体の増減速期間と定常走行期間とを判別して、その
判別信号を前記第１，第２速度補正部に入力するもので
ある。前記第１速度補正部は、各加減速期間において前
記慣性データ演算部より入力される前記慣性速度Ｖ_I を
装置外部に出力し、各定常走行期間において、前記第２
速度補正部で得られる補正した機械的速度Ｖ_P ′（前記
回転速度Ｎより求めた速度を機械的速度と言い、Ｖ_P で
表す）をリファレンス値として更に補正した慣性速度Ｖ
_I ′を演算して、前記慣性データ演算部に入力するもの
である。[0011] The running state determination unit determines the acceleration A and the angular velocity ω or the acceleration / deceleration signal (dN / dt; N is the rotation speed of the propulsion unit) obtained by the propulsion engine unit of the underwater vehicle. The acceleration / deceleration period and the steady running period are determined, and the determination signal is input to the first and second speed correction units. It said first speed correcting unit outputs the inertial velocity V _I of each deceleration period is input from the inertial data arithmetic unit to the outside of the apparatus, in each normal running period, the second
The inertia speed V further corrected using the corrected mechanical speed V _P ′ obtained by the speed correction unit (the speed obtained from the rotation speed N is referred to as mechanical speed and expressed as V _P ) as a reference value.
_I ′ is calculated and input to the inertial data calculation unit.

【００１２】前記第２速度補正部は、各定常走行期間に
おいて、前記機械的速度Ｖ_P ＝ＫＮ（Ｋは係数）を、前
記慣性速度Ｖ_I を用いて補正した機械的速度Ｖ_P ′を演
算して、装置外部に出力するものである。 （２）請求項２の発明では、前記（１）項記載の水中航
走体用速度計測装置において、前記第２速度補正部がｉ
番目（ｉ＝１，２，３…）の定常走行期間の最初の時点
ｔ_i ^* において、前記慣性速度Ｖ_I （ｔ_i ^* ）と前記回
転速度Ｎ（ｔ_i ^* ）から、補正した係数Ｋ_i ′＝Ｖ
_I （ｔ_i ^* ）／Ｎ（ｔ_i ^* ）を求め、その係数Ｋ_i ′を
用いて各定常走行期間における前記補正した機械的速度
Ｖ_P ′＝Ｋ_i′Ｎを演算する。[0012] The second speed correction unit is provided for each steady running period.
The mechanical speed V_P= KN (K is a coefficient)
Inertia velocity V_IMechanical velocity V corrected using_P′
And outputs it to the outside of the device. (2) According to the second aspect of the invention, the underwater navigation according to the above (1) is provided.
In the speed measurement device for a running body, the second speed correction unit may include i
First time point of the (i = 1,2,3 ...) steady running period
t_i ^*In the above, the inertial velocity V_I(T_i ^*) And said times
Rolling speed N (t_i ^*) From the corrected coefficient K_i'= V
_I(T_i ^*) / N (t_i ^*) And its coefficient K_i′
Using the corrected mechanical speed during each steady driving period
V_P'= K_i'N.

【００１３】（３）請求項３の発明では、前記（１）項
記載の水中航走体用速度計測装置において、前記第２速
度補正部が、１番目の定常走行期間の最初の時点ｔ₁ ^*
において、慣性速度Ｖ_I （ｔ₁ ^* ）と回転速度Ｎ（ｔ₁
^* ）から、補正した係数Ｋ₁′＝Ｖ_I （ｔ₁ ^* ）／Ｎ
（ｔ₁ ^* ）を求め、その係数Ｋ₁ ′を共通に用いて、各
定常走行期間における前記補正した機械的速度Ｖ_P ′＝
Ｋ₁ ′Ｎを演算する。(3) In the invention according to claim 3, in the underwater vehicle speed measuring device according to the above (1), the second speed correction unit is configured to determine the first time point t ₁ of the first steady running period. ^*
, The inertial speed V _I (t ₁ ^* ) and the rotation speed N (t ₁
^* ), The corrected coefficient K ₁ ′ = V _I (t ₁ ^* ) / N
(T ₁ ^* ), and using the coefficient K ₁ ′ in common, the corrected mechanical speed _VP ′ =
Calculate K ₁ 'N.

【００１４】（４）請求項４の発明では、前記（１）乃
至（３）項のいずれかに記載の水中航走体用速度計測装
置において、前記慣性データ演算部が、航走体の姿勢
角、方位角または位置を演算して外部に出力できる。(4) In the invention according to claim 4, in the underwater vehicle speed measurement apparatus according to any one of the above (1) to (3), the inertial data calculation unit includes a posture of the vehicle. The angle, azimuth or position can be calculated and output to the outside.

【００１５】[0015]

【実施例】本発明の一実施例を水中航走体の自動操縦装
置に応用した図１の場合について説明する。慣性装置１
１は、直行３軸のジャイロ７，加速度計６を持ち、これ
らから得られる角速度ω，加速度Ａを演算部１０で計算
することにより姿勢角（ピッチ角θ，ロール角φ）、方
位角ψ，速度Ｖ_I ，位置Ｐ等を計算する。推進機関部１
２は、推進機軸の回転速度Ｎを検出し、またこれを調節
する。DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described with reference to FIG. 1 in which an embodiment of the present invention is applied to an underwater vehicle autopilot. Inertial device 1
1 has a gyro 7 and an accelerometer 6 of three orthogonal axes, and an angular velocity ω and an acceleration A obtained from these are calculated by an arithmetic unit 10 so that an attitude angle (pitch angle θ, roll angle φ), an azimuth angle ψ, The speed V _I , the position P, etc. are calculated. Promotion Agency 1
2 detects and adjusts the rotational speed N of the thruster shaft.

【００１６】速度計測部１３は、以下の１４，１５，１
６により構成され、精度の高い速度データＶ_T を出力す
ることができる。第１速度補正部１４は慣性装置１１よ
り得た慣性速度Ｖ_I を、推進機回転数Ｎより得た機械的
速度Ｖ_P ＝ＫＮ（Ｋはノミサル値）を補正した速度
Ｖ_P ′を基準に更に補正してＶ_I ′を得る。また慣性速
度Ｖ_I をスイッチＳＷ₂ ，ＳＷ₃ に出力する。第２速度
補正部１５は機械的速度Ｖ _P を、慣性速度Ｖ_I を基準に
補正して、Ｖ_P ′＝Ｋ′Ｎを得る。走行状態判定部１６
は慣性装置１１より得られる加速度Ａ及び角速度ωまた
は機関部１２からの回転の増減の変化ｄＮ／ｄｔより増
減速期間と定常走行期間とを判定して判定信号Ｓａを出
力する。The speed measuring unit 13 has the following 14, 15, 1
6 and high-accuracy speed data V_TOutput
Can be The first speed correction unit 14 is connected to the inertial device 11
Inertia velocity V obtained_IIs obtained from the mechanical
Speed V_P= KN (K is the nominal value) corrected speed
V_P'Is further corrected based on_I'. Also inertial speed
Degree V_ISwitch SW_Two, SW_ThreeOutput to 2nd speed
The correction unit 15 calculates the mechanical speed V _PIs the inertial velocity V_IBased on
Correct, V_P'= K'N. Running state determination unit 16
Is the acceleration A and angular velocity ω obtained from the inertial device 11
Is greater than the change dN / dt in the increase / decrease of rotation from the engine unit 12.
A determination signal Sa is output after determining the deceleration period and the steady driving period.
Power.

【００１７】誘導計算部１７はあらかじめセットされた
航走コースもしくは外部よりコントロールされる信号と
自己が得ている姿勢角（ピッチ角θ，ロール角φ）、方
位角ψ，位置Ｐ，速度Ｖ_T により操舵信号Ｓｂを発生す
る。走行状態判定信号Ｓａ（図２Ａ）により制御され
て、スイッチＳＷ₁ 乃至ＳＷ ₃ はそれぞれ図２のＢ，
Ｃ，Ｄに示すようにオン／オフ制御される。定常走行期
間にはスイッチＳＷ₁ がオンとされ、第２速度補正部１
５から補正された機械的速度Ｖ_P ′が第１速度補正部１
４に入力される。第１速度補正部１４ではＶ_P ′をリフ
ァレンス値として慣性装置より入力される慣性速度Ｖ_I
を補正し、その補正した速度Ｖ_I ′を慣性データ演算部
１０に入力する。慣性データ演算部１０では、補正され
た慣性速度Ｖ_I ′をリファレンス値として、更に補正し
た慣性速度Ｖ_I を演算し、これによりＶ_I の積分誤差が
時間と共に増加しないようにしている。The guidance calculation unit 17 is set in advance.
On the course or signals controlled from the outside
Attitude angle (pitch angle θ, roll angle φ), direction
Angle ψ, position P, speed V_TGenerates a steering signal Sb
You. It is controlled by the traveling state determination signal Sa (FIG. 2A).
And switch SW₁Or SW _ThreeAre B in FIG. 2, respectively.
On / off control is performed as shown in C and D. Steady running period
Switch SW between₁Is turned on, and the second speed correction unit 1
Mechanical speed V corrected from 5_P'Is the first speed correction unit 1
4 is input. In the first speed correction unit 14, V_P′ Riff
Inertial velocity V input from inertial device as reference value_I
Is corrected, and the corrected speed V_I′ Is the inertia data operation unit
Enter 10 In the inertia data calculation unit 10, the corrected
Inertia velocity V_I'As a reference value,
Inertia velocity V_ITo calculate V_IThe integration error of
It does not increase over time.

【００１８】加減速期間には、スイッチＳＷ₂ がオンと
され、第１速度補正部１４から慣性速度Ｖ_I （ｔ）が第
２速度補正部１５に入力される。第２速度補正部１５で
は速度Ｖ_I （ｔ）を回転速度検出部１２ａより入力され
る回転速度Ｎ（ｔ）で割算し、走行状態判定信号Ｓａが
オンからオフに立下った時点ｔ＝ｔ^* の商Ｖ_I （ｔ^*）
／Ｎ（ｔ^* ）を補正された定数Ｋ′として得、定常走行
期間でこの定数Ｋ′を用いて速度補正した速度Ｖ_P ′
（ｔ）＝Ｋ′Ｎ（ｔ）を求めて、速度補正部１４及びス
イッチＳＷ₃ の接点ｂ−ｃを通じて誘導計算部１７に入
力する。During the acceleration / deceleration period, the switch SW ₂ is turned on, and the inertia speed V _I (t) is input from the first speed correction unit 14 to the second speed correction unit 15. The second speed correction unit 15 divides the speed V _I (t) by the rotation speed N (t) input from the rotation speed detection unit 12a, and the time t = when the traveling state determination signal Sa falls from on to off. t ^* of the quotient V _I (t ^*)
/ N (t ^* ) is obtained as a corrected constant K ′, and the speed V _P ′ whose speed is corrected using this constant K ′ during the steady running period.
(T) = seeking K'N (t), and inputs to the induction calculation unit 17 through the contact b-c of the speed correcting section 14 and the switch SW _3.

【００１９】スイッチＳＷ₃ は増減速期間には接点ａ側
に切替えられて、計測装置の出力速度Ｖ_T として慣性速
度Ｖ_I が誘導計算部１７へ入力される。なお、慣性デー
タ演算部１０と第１速度補正部１４とを１つに合体する
こともできる。その場合、合体した部にカルマンフィル
タ等の状態推定フィルタを用いて速度データのみなら
ず、姿勢角、方位角、位置等のデータを補正することも
できる。The switch SW ₃ is the acceleration and deceleration time is switched to the contact a side, inertial velocity V _I is input to the induction calculating section 17 as the output speed V _T of the measuring device. In addition, the inertia data calculation unit 10 and the first speed correction unit 14 can be combined into one. In this case, not only velocity data but also data such as an attitude angle, an azimuth angle, and a position can be corrected by using a state estimation filter such as a Kalman filter in the united portion.

【００２０】第１速度補正部１４が判定信号Ｓａに同期
して、加減速期間に慣性速度Ｖ_I を外部に出力し、また
第２速度補正部１５が判定信号Ｓａに同期して、定常走
行期間に、補正された機械的速度Ｖ_P ′を外部に出力す
ることができるので、スイッチＳＷ₃ を省略してもよ
い。また第１，第２速度補正部１４，１５は判定信号Ｓ
ａに基づいて、互いに相手側が必要とする期間に速度Ｖ
_I またはＶ_P ′を供給するようにしたり、或いは常時速
度データを供給し、そのデータを使用する側が必要な期
間に適宜取り込むことができるので、スイッチＳＷ₁ ，
ＳＷ₂ を省略してもよい。[0020] The first speed correcting section 14 in synchronization with the judgment signal Sa, and outputs the inertial velocity V _I outside the acceleration or deceleration period, and the second speed correcting section 15 in synchronization with the judgment signal Sa, steady running the period, the corrected mechanical velocity V _P 'can be output to the outside, it may be omitted switch SW _3. The first and second speed correction units 14 and 15 output the determination signal S
a during the period required by the other party based on the
Or so as to supply the _I or V _P ', or constantly supplies the speed data, so that data can be fetched appropriate period required side to use, the switch SW _1,
The SW ₂ may be omitted.

【００２１】次に図１の動作を図３に示す航走体速度の
時間的な変化特性を例にして更に詳細に説明しよう。 発進時は慣性装置１１による推進軸方向の慣性速度
Ｖ_I を正しいとし、Ｖ _T ＝Ｖ_I を外部に出力する。 スタートしてからの経過時間または加速度の変動ま
たは推進機関部１２より得られる増減速信号ｄＮ／ｄｔ
によって一定の速度となったと判断した時点ｔ ₁ ^* で、 Ｖ_P （ｔ₁ ^* ）＝ＫＮ（ｔ₁ ^* ） …………… （１３） より求めるＶ_P （ｔ₁ ^* ）よりも精度の良い慣性速度Ｖ
_I （ｔ₁ ^* ）を正しいとし、 Ｖ_I （ｔ₁ ^* ）＝Ｋ₁ ′Ｎ（ｔ₁ ^* ）≡Ｖ_P ′（ｔ₁ ^* ） … （１４） と置いて、補正したノミナル値Ｋ₁ ′ Ｋ₁ ′＝Ｖ_I （ｔ₁ ^* ）／Ｎ（ｔ₁ ^* ） …………… （１５） を求める。Next, the operation of FIG. 1 will be described with reference to FIG.
This will be described in more detail with reference to a temporal change characteristic. At the time of start, the inertial speed in the direction of the propulsion axis by the inertial device 11
V_IIs correct and V _T= V_IIs output to the outside. Time elapsed since start or fluctuation of acceleration
Or the acceleration / deceleration signal dN / dt obtained from the propulsion engine unit 12.
T when it is determined that the speed is constant ₁ ^*And V_P(T₁ ^*) = KN (t₁ ^*) ……………………………………… (13)_P(T₁ ^*Inertia velocity V with higher accuracy than
_I(T₁ ^*) Is correct and V_I(T₁ ^*) = K₁'N (t₁ ^*) ≡V_P'(T₁ ^*) ... (14) and corrected nominal value K₁'K₁'= V_I(T₁ ^*) / N (t₁ ^*) …………… (15)

【００２２】 以後、回転数変動の少ない定常走行時
では補正されたＫ₁ ′と推進機回転数Ｎから得られる、 Ｖ_P ′＝Ｋ₁ ′Ｎ …………… （１６） を正しいとして、これを出力する（Ｖ_T ＝Ｖ_P ′）。ま
た慣性装置より得られる速度Ｖ_I はそのまゝでは誤差が
増大するので、Ｖ_P ′をリファレンス値として補正した
Ｖ_I ′を慣性データ演算部１０に入力する。Hereafter, it is assumed that V _P ′ = K ₁ ′ N obtained from the corrected K ₁ ′ and the propulsion unit rotation speed N during steady running with small fluctuations in the rotation speed. This is output ( _VT = _VP '). Further, since the error of the speed V _I obtained from the inertial device increases before that, V _I ′ corrected with V _P ′ as a reference value is input to the inertia data calculation unit 10.

【００２３】 増減速があった場合はＶ_I を正しいと
しＶ_T ＝Ｖ_I を出力する。 ２番目に定常走行になったと判断した時点ｔ₂ ^* で
は、と同様に Ｖ_P （ｔ₂ ^* ）＝ＫＮ（ｔ₂ ^* ） …………… （１７） より求めるＶ_P （ｔ₂ ^* ）よりも精度のよいＶ_I （ｔ₂
^* ）を正しいものとし、 Ｖ_I （ｔ₂ ^* ）＝Ｋ₂ ′Ｎ（ｔ₂ ^* ）＝Ｖ_P ′（ｔ₂ ^* ） …………… （１８） と置いて、補正したノミナル値Ｋ₂ ′ Ｋ₂ ′＝Ｖ_I （ｔ₂ ^* ）／Ｎ（ｔ₂ ^* ） …………… （１９） を求める。If acceleration or deceleration occurs, V _{I is determined} to be correct, and V _T = V _I is output. At time t ₂ ^* is determined that the steady driving Second, similarly to ^{_{V P (t 2 *) =}} KN (t 2 *) obtained from _{............... (17) V P (t} 2 *) V _I (t ₂
^*) As a ^{_{correct, V I (t 2 *)}} = K 2 'N (t 2 *) = V P' (t 2 *) at the ............... (18), corrected nominal value K _{2 'K 2' = V I} (t 2 *) / N (t 2 *) Request ............... (19).

【００２４】しかし、係数Ｋ′は航走体２と推進機３の
寸法、形状が変化しないので、時間的に一定と考えられ
るので、で（１５）式より求めた補正したノミナル値
Ｋ₁′をこれからも使用することにしての処理を省略
し、演算を簡単化することもできる。 （イ）第２速度補正部１５は、回転数変動の少ない
２番目の定常走行時では、（イ）補正されたＫ₂ ′と推
進機回転数Ｎから得られる Ｖ_P ′＝Ｋ₂ ′Ｎ …………… （２０） を正しいとして、これを出力する（Ｖ_T ＝Ｖ_P ′）。However, the coefficient K 'is considered to be constant over time since the size and shape of the hull 2 and the propulsion unit 3 do not change, so the corrected nominal value K ₁ ' obtained by the equation (15) is used. Can be omitted, and the processing can be simplified to simplify the operation. (A) The second speed correction unit 15 is configured to provide, during the second steady running with little rotation speed fluctuation, (A) V _P ′ = K ₂ ′ N obtained from the corrected K ₂ ′ and the propulsion unit rotation speed N. ... (20) is assumed to be correct and is output (V _T = V _P ′).

【００２５】（ロ）或いは、で求めた補正したノミナ
ル値Ｋ₁ ′を毎回使用することにして、 Ｖ_P ′＝Ｋ₁ ′Ｎ …………… （２１） を求めて出力する。また、慣性装置より得られる速度Ｖ
_I は定常走行期間に誤差が増大するので、第１速度補正
部１４はＶ_P ′をリファレンス値として補正したＶ_I ′
を求めて慣性データ演算部１０に入力する。(B) Alternatively, by using the corrected nominal value K ₁ 'obtained every time, V _P ' = K ₁ 'N (21) is obtained and output. Also, the velocity V obtained from the inertial device
_{Since the} error of I increases during the steady running period, the first speed correction unit 14 corrects V _I ′ using V _P ′ as a reference value.
Is input to the inertial data calculation unit 10.

【００２６】[0026]

【発明の効果】 従来は速度出力Ｖ_T として、推進機の回転速度Ｎよ
り算定した機械的速度Ｖ_P ＝ＫＮを用いていたので増減
速時には、航走体２の真の速度Ｖが自身の質量の存在に
よって推進機回転数Ｎの変化よりかなり遅れるために大
きな演算誤差が発生したが、この発明では増減速時には
慣性装置１１の検出速度Ｖ_I を速度出力Ｖ_T としている
ので、算定誤差が大幅に軽減される。As conventionally velocity output V _T, according to the present invention, at the time of acceleration and deceleration so have used mechanical speed V _P = KN which was calculated from the rotational speed N of the propulsion unit, the true velocity V of Kohashikarada 2 is itself a large calculation error to significantly lag the changes in the propulsion speed N by the presence of the mass occurs, so this during acceleration and deceleration is invention has a detection speed V _I a velocity output V _T of the inertial device 11, is computed error Significantly reduced.

【００２７】 従来は、定常走行時における速度出力
Ｖ_T もＶ_P ＝ＫＮより求めていたが、もともと定数Ｋは
縮小模型を試験して求めたものであり、実機との間に誤
差があり、更に各航走体ごとにばらつきが存在する。こ
れらの理由によって定常走行時にもかなりの速度算定誤
算が存在した。しかし、この発明では、慣性装置の検出
した加速度信号Ａまた及び角速度ωは推進機関部１２よ
り得られる増減速信号ｄＮ／ｄｔとから増減速時と定常
走行時とを判別し、前者より後者に切り替わった時点ｔ
^* の慣性速度Ｖ_I （ｔ^* ）と回転速度Ｎ（ｔ^* ）から補
正された係数Ｋ′＝Ｖ_I （ｔ^* ）／Ｎ（ｔ^* ）を各航走
体ごとに求め、このＫ′を用いて定常走行時の速度出力
Ｖ_T ＝Ｖ_P ′＝Ｋ′Ｎを求めている。従って従来の定数
Ｋ自身のもつ誤差に起因する算定誤差をなくすことがで
きる。Conventionally, the speed output V _T at the time of steady running was also obtained from V _P = KN. However, the constant K was originally obtained by testing a reduced model, and there was an error with the actual machine. Further, there is a variation for each vehicle. For these reasons, there was considerable speed calculation error even during steady driving. However, according to the present invention, the acceleration signal A and the angular velocity ω detected by the inertial device determine whether the vehicle is accelerating or decelerating and when the vehicle is in a steady running state based on the acceleration / deceleration signal dN / dt obtained from the propulsion engine unit 12. Switching time t
^The coefficient K ′ = V _I (t ^* ) / N (t ^* ) corrected from the inertia speed V _I (t ^* ) and the rotation speed N (t ^* ) of ^* is obtained for each cruising vehicle. Is used to determine the speed output V _T = V _P '= K'N during steady running. Therefore, it is possible to eliminate the calculation error caused by the error of the conventional constant K itself.

【００２８】慣性装置により速度を求める従来の方式
では、加速度計出力やジャイロ出力に含まれる誤差も一
緒に積分されるため、積分誤差が時間と共に増大する欠
点があったが、この発明では、比較的短時間の増減速時
にのみ慣性速度Ｖ_I を用いるようにしたので、積分誤差
はかなり軽減される。しかも慣性速度Ｖ_I は、比較的長
時間の定常走行時において、補正された速度出力Ｖ_T ＝
Ｖ_P ′＝Ｋ′Ｎをリファレンス値として補正された慣性
速度Ｖ_I ′に一致するように慣性装置が補正演算を実行
しているので、従来のような時間と共に増大する積分誤
差が除去された状態にある。従って、増減速時になれば
いつでも慣性速度Ｖ_I を速度出力Ｖ_T に用いることがで
き、しかもそれ以前の積分誤差の影響のない精度の高い
データが得られる。In the conventional method for obtaining the speed by the inertial device, the errors included in the accelerometer output and the gyro output are integrated together, so that there is a disadvantage that the integration error increases with time. Since the inertial velocity V _I is used only during the acceleration / deceleration in a very short time, the integration error is considerably reduced. In addition, the inertial speed V _I is the corrected speed output V _T = during a relatively long period of steady running.
Since the inertial device performs the correction operation so as to coincide with the corrected inertial velocity V _I ′ using V _P ′ = K′N as a reference value, the conventional integration error that increases with time has been removed. In state. Therefore, it is possible to use the inertial velocity V _I at any time if during acceleration and deceleration to the velocity output V _T, yet is highly affected without the precision of previous integration error data obtained.

【図面の簡単な説明】[Brief description of the drawings]

【図１】この発明の実施例を示すブロック図。FIG. 1 is a block diagram showing an embodiment of the present invention.

【図２】図１の要部の動作波形図。FIG. 2 is an operation waveform diagram of a main part of FIG.

【図３】図１における要部の検出速度の時間的変化を示
す図。FIG. 3 is a diagram showing a temporal change in a detection speed of a main part in FIG. 1;

【図４】水中航走体に作用する推力Ｔと抵抗Ｆの原理的
な説明図。FIG. 4 is a principle explanatory diagram of a thrust T and a resistance F acting on an underwater vehicle.

【図５】図４の推力Ｔ及び抵抗Ｆの走行速度Ｖに対する
変化特性を示す図。FIG. 5 is a diagram showing a change characteristic of a thrust T and a resistance F in FIG. 4 with respect to a traveling speed V;

【図６】慣性装置を水中航走体に搭載して速度を検出す
る従来例の原理的な説明図。FIG. 6 is a principle explanatory diagram of a conventional example in which an inertial device is mounted on an underwater vehicle to detect a speed.

【図７】推進機回転数Ｎ及び航走体の真の速度Ｖの時間
ｔに対する変化の一例を示す図。FIG. 7 is a diagram illustrating an example of a change in the rotation speed N of the propulsion device and the true speed V of the traveling vehicle with respect to time t.

Claims (4)

(57)【特許請求の範囲】(57) [Claims] 【請求項１】 ジャイロスコープ、加速度計及び慣性デ
ータ演算部より成る慣性装置と、第１，第２速度補正部
及び走行状態判定部より成る速度計測部とから構成され
る水中航走体用速度計測装置であって、 前記ジャイロスコープは、３軸方向の回転角速度ωを検
出するものであり、 前記加速度計は、前記３軸方向の加速度Ａを検出するも
のであり、 前記慣性データ演算部は、前記加速度Ａ及び角速度ωと
から水中航走体の速度（慣性速度と言う）Ｖ_I を演算し
て、前記第１速度補正部に入力すると共に、各定常走行
期間において、前記第１速度補正部より入力される補正
された慣性速度Ｖ_I ′をリファレンス値として更に補正
した慣性速度Ｖ_I を演算するものであり、 前記走行状態判定部は、前記加速度Ａ及び角速度ωまた
は水中航走体の推進機関部で得られる増減速信号（ｄＮ
／ｄｔ；Ｎは推進機の回転速度）より水中航走体の増減
速期間と定常走行期間とを判別して、その判別信号を前
記第１，第２速度補正部に入力するものであり、 前記第１速度補正部は、各加減速期間において前記慣性
データ演算部より入力される前記慣性速度Ｖ_I を装置外
部に出力し、各定常走行期間において、前記第２速度補
正部で得られる補正した機械的速度Ｖ_P ′（前記回転速
度Ｎより求めた速度を機械的速度と言い、Ｖ_P で表す）
をリファレンス値として更に補正した慣性速度Ｖ_I ′を
演算して、前記慣性データ演算部に入力するものであ
り、 前記第２速度補正部は、各定常走行期間において、前記
機械的速度Ｖ_P ＝ＫＮ（Ｋは係数）を、前記慣性速度Ｖ
_I を用いて補正した機械的速度Ｖ_P ′を演算して、装置
外部に出力するものであることを特徴とする、 水中航走体用速度計測装置。1. An underwater vehicle speed comprising an inertial device including a gyroscope, an accelerometer, and an inertial data calculation unit, and a speed measurement unit including first and second speed correction units and a traveling state determination unit. A measuring device, wherein the gyroscope detects a rotational angular velocity ω in three axial directions; the accelerometer detects the acceleration A in the three axial directions; the (referred to as inertial velocity) from the acceleration a and the angular velocity ω speed of underwater vehicle by calculating the V _I, and inputs to the first speed correcting section in each normal running period, the first speed correction The corrected inertia speed V _I ′ input from the section is used as a reference value to calculate a further corrected inertia speed V _I , wherein the traveling state determination unit is configured to calculate the acceleration A and the angular velocity ω or the underwater vehicle. Push Acceleration and deceleration signal obtained by the engine unit (dN
/ Dt; N is the rotation speed of the propulsion unit) and determines the acceleration / deceleration period and the steady traveling period of the underwater vehicle, and inputs the determination signal to the first and second speed correction units. said first speed correction unit said inertial velocity V _I input from the inertial data arithmetic unit outputs to the outside of the apparatus at each deceleration period, each normal running period, obtained by the second speed correcting section corrects Mechanical speed V _P ′ (the speed obtained from the rotation speed N is referred to as mechanical speed and is represented by V _P )
Is used as a reference value to calculate the inertia speed V _I ′, which is input to the inertia data calculation unit. The second speed correction unit calculates the mechanical speed V _P = KN (K is a coefficient) is calculated by
_A speed measuring device for underwater vehicles, wherein the device calculates a mechanical speed V _P ′ corrected using _I and outputs it to the outside of the device. 【請求項２】 請求項１記載の水中航走体用速度計測装
置において、前記第２速度補正部がｉ番目（ｉ＝１，
２，３…）の定常走行期間の最初の時点ｔ_i ^*におい
て、前記慣性速度Ｖ_I （ｔ_i ^* ）と前記回転速度Ｎ（ｔ
_i ^* ）から、補正した係数Ｋ_i ′＝Ｖ_I （ｔ_i ^* ）／Ｎ
（ｔ_i ^* ）を求め、その係数Ｋ_i ′を用いて各定常走行
期間における前記補正した機械的速度Ｖ_P ′＝Ｋ_i ′Ｎ
を演算することを特徴とする。2. The underwater vehicle speed measurement device according to claim 1, wherein the second speed correction unit is an ith (i = 1,
2,3 ...) at the first time point t _i ^* of steady running period, the inertial velocity V _I (t _i ^*) and the rotation speed N (t
_i ^* ), the corrected coefficient K _i ′ = V _I (t _i ^* ) / N
(T _i ^* ) is obtained, and the corrected mechanical speed V _P ′ = K _i ′ N in each steady running period is calculated using the coefficient K _i ′.
Is calculated. 【請求項３】 請求項１記載の水中航走体用速度計測装
置において、前記第２速度補正部が、１番目の定常走行
期間の最初の時点ｔ₁ ^* において、慣性速度Ｖ_I （ｔ₁
^* ）と回転速度Ｎ（ｔ₁ ^* ）から補正した係数Ｋ₁ ′＝
Ｖ_I （ｔ₁ ^*）／Ｎ（ｔ₁ ^* ）を求め、その係数Ｋ₁ ′
を共通に用いて、各定常走行期間における前記補正した
機械的速度Ｖ_P ′＝Ｋ₁ ′Ｎを演算することを特徴とす
る。3. The underwater vehicle speed measurement device according to claim 1, wherein the second speed correction unit is configured to control the inertial speed V _I (t ₁₎ at a first time point t ₁ ^* of a first steady running period.
^* ) And the coefficient K ₁ ′ = corrected from the rotation speed N (t ₁ ^* )
V _I (t ₁ ^* ) / N (t ₁ ^* ) is obtained and its coefficient K ₁ ′
Using in common, characterized by calculating the corrected mechanical velocity V _P '= K _1' N in each steady running period. 【請求項４】 請求項１乃至３のいずれかに記載の水中
航走体用速度計測装置において、前記慣性データ演算部
が、航走体の姿勢角、方位角または位置を演算して外部
に出力できることを特徴とする。4. The underwater vehicle speed measurement device according to claim 1, wherein the inertial data calculation unit calculates an attitude angle, an azimuth angle, or a position of the vehicle and externally calculates the attitude angle, the azimuth angle, or the position of the vehicle. It can be output.