Method and device for determining the relative position, velocity and/or acceleration of a body

Abstract

In a method for determining the relative position, velocity, acceleration, and/or the rotation center of a body displaceable in a three dimensional space, at least twelve linear acceleration sensors are provided and in each case arranged on a position which is stationarily fixed with respect to the body. At least one acceleration measurement signal is captured by the linear acceleration sensors. A position, velocity, acceleration, and/or rotation center signal is generated for the body from the acceleration measurement signal and data describing the position and orientation of the linear acceleration sensors in the body-fixed coordinate system.

Description

BACKGROUND OF THE INVENTION

The invention relates to a method for determining the relative position, velocity, and/or acceleration of a body displaceable in a one to three dimensional space, wherein a plurality of linear acceleration sensors, which in each case have a sensitive measurement axis, is arranged and wherein the individual linear acceleration sensors are always arranged on a position P_i which is stationary with respect to the body. Furthermore, the invention relates to a device for determining the relative position, velocity, and/or acceleration of a body in a one to three dimensional space, wherein the device has a plurality of linear acceleration sensors stationarily arranged with respect to the body for capturing at least one acceleration measurement signal {right arrow over (a)}=(a₁, a₂, a₃, a₄, . . . a_n).

A relative position is to be understood as a position relative to a reference position, for example, a starting position. A linear acceleration sensor is to be understood as a unidirectional acceleration sensor, which is sensitive to accelerations that have at least one component that lies on the measurement axis of said linear acceleration sensor. The linear acceleration sensors can be structurally separated from each other, each one being arranged in its own housing. However, it is also possible for at least two and especially three linear acceleration accelerators having different measurement axes to be integrated as a multidirectional acceleration sensor in a common electric or electronic component.

A method and a device for determining the angular velocity of a turnable body, such as a vehicle, are disclosed in DE 199 62 687 A1. For measuring an acceleration measurement signal, a total of nine linear acceleration sensors, in each case having a sensitive measurement axis passing through the corresponding position, are fix mounted on the body in four non-adjacent positions. Three linear acceleration sensors, which in each case are oriented with their sensitive measurement axis toward one of the x, y, z axes of the body-fixed coordinate system, are provided in a first position arranged in the origin of the body-fixed coordinate system. Two other linear sensors, of which one is oriented with its measurement axis in z-direction and the other of which is oriented with its measurement axis in x-direction, are provided in a second position arranged at a distance r₁ from the origin on the y-axis. Two other linear sensors, of which one is oriented with its measurement axis in y-direction and the other of which is oriented with its measurement axis in z-direction, are provided in a third position arranged at a distance r₂ from the origin on the x-axis. In a corresponding manner, two linear sensors, which are oriented in x- or y-direction, are provided in a fourth position arranged on the z-axis at a distance r₃ from the origin. The acceleration {right arrow over (a)} at a given point P is expressed as:{right arrow over (a)}=A(t)+{right arrow over ({dot over (ω)}x{right arrow over (r)}+{right arrow over (ω)}x({right arrow over (ω)}x{right arrow over (r)})+2·{right arrow over (ω)}x{right arrow over ({dot over (r)}+{right arrow over ({umlaut over (r)}  Equation (1)

Where:

A is the acceleration of the origin of the body-fixed coordinate system,

t is the time,

{right arrow over (ω)} is the angular velocity of the body, and

{right arrow over (r)} is the position vector that indicates the point P from the origin of the body-fixed coordinate system.

Assuming that a linear acceleration sensor is fix mounted on the point P of the body, the terms {right arrow over ({dot over (r)} and {right arrow over ({umlaut over (r)} become equal to zero. With this assumption, one obtains:{right arrow over (a)}=A(t)+{right arrow over ({dot over (ω)}x{right arrow over (r)}+{right arrow over (ω)}x({right arrow over (ω)}x{right arrow over (r)})  Equation (2)

This is a three dimensional, non-linear differential equation system, which as a rule cannot be solved analytically. The determination of the angular velocity of the turnable body therefore requires a relative complicated calculation. The device operating according to the procedure is therefore correspondingly complicated, expensive, and this notwithstanding, usually inaccurate.

DE 199 62 687 A1 discloses another prior art procedure in which the angular acceleration {right arrow over (ω)}_x of the body about the x-axis is measured by only four linear acceleration sensors fix mounted on the body. In this arrangement, two linear acceleration sensors are arranged in the origin of the body-fixed coordinate system, wherein a first linear acceleration sensor is oriented with its measurement axis toward the y-axis and a second linear acceleration sensor is oriented with its measurement axis toward the z-axis. A third linear acceleration sensor is arranged on the y-axis at a distance r₁ from the origin and oriented toward the z-axis. A fourth linear acceleration sensor is arranged on the z-axis at a distance r₃ from the origin with its sensitive measurement axis oriented toward the y-axis. If one plugs the positions of the four linear acceleration sensors into equation (2), algebraic transformation gives:

$\begin{array}{cc}{\stackrel{.}{\omega }}_x=\frac{a_{z1}-a_{z0}}{2·r_1}-\frac{a_{y3}-a_{y0}}{2·r_3}& \mathrm{Equation}\left(3\right)\end{array}$

wherein a_z0, a_z1, a_y0, a_y1 represent the measurement values of the four linear acceleration sensors. Equation (3), however, only applies to the special case where the first and third linear acceleration sensors are always oriented with their measurement axes exactly in y-direction and where both of the other linear acceleration sensors are oriented with their measurement axes perpendicular thereto in z-direction. The perpendicular arrangement of the linear acceleration sensors, however, is problematic in practice, as manufacturing and positioning tolerances arise in the manufacture and attachment of the linear acceleration sensors on the body. Deviations in positioning and alignment are almost inevitable, especially when mounting the linear acceleration sensors on the body. Even small deviations of the position of the linear acceleration sensors from the sensor arrangement on which equation (3) is based can lead to relatively large errors in the calculation of the angular acceleration {right arrow over (ω)}_x. This is especially disadvantageous when the angular acceleration signal for determining the position of the body integrates and the errors add up without limit. This last point in particular was the decisive reason why the use of position determining devices that employ linear sensors only never caught on.

It is therefore the object of the invention to develop a method and a device as mentioned in the introduction that enable a simple, exact determination of the position, velocity, and/or acceleration of one of the bodies.

SUMMARY OF THE INVENTION

This object is achieved according to the method by provision of a plurality of n linear acceleration sensors, in each case having a sensitive measurement axis, wherein the plurality n is at least twelve, wherein the individual linear acceleration sensors are always arranged on a fixed position P_i with respect to the body and which can be described by a stationary position vector {right arrow over (r)}_i=(r_xi, r_yi, r_zi) indicating the respective position P_i from a body-fixed reference point, wherein the linear acceleration sensors are aligned so that in each case their measurement axes are oriented toward a direction vector {right arrow over (Θ)}_i=(Θ_xi, Θ_yi, Θ_zi), wherein iε[1, 2, 3, 4, . . . n] in each case designate the respective linear acceleration sensor and the x, y, z axes of a body-fixed coordinate system that pass through the reference point, and wherein the position vectors {right arrow over (r)}_i and the direction vectors {right arrow over (Θ)}_i are selected so that the rank of a twelve column matrix formed from the column vectors

${\overrightarrow{s}}_1=\left(\begin{array}{c}{\Theta }_{x1}\\ {\Theta }_{x2}\\ {\Theta }_{x3}\\ {\Theta }_{x4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}\end{array}\right),{\overrightarrow{s}}_2=\left(\begin{array}{c}{\Theta }_{y1}\\ {\Theta }_{y2}\\ {\Theta }_{y3}\\ {\Theta }_{y4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_3=\left(\begin{array}{c}{\Theta }_{z1}\\ {\Theta }_{z2}\\ {\Theta }_{z3}\\ {\Theta }_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}\end{array}\right),{\overrightarrow{s}}_4=\left(\begin{array}{c}{\Theta }_{z1}{r}_{y1}-{\Theta }_{y1}{r}_{z1}\\ {\Theta }_{z2}{r}_{y2}-{\Theta }_{y2}{r}_{z2}\\ {\Theta }_{z3}{r}_{y3}-{\Theta }_{y3}{r}_{z3}\\ {\Theta }_{z4}{r}_{y4}-{\Theta }_{y4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}{r}_{\mathrm{yn}}-{\Theta }_{\mathrm{yn}}{r}_{\mathrm{zn}}\end{array}\right),{\overrightarrow{s}}_5=\left(\begin{array}{c}{\Theta }_{x1}{r}_{z1}-{\Theta }_{z1}{r}_{x1}\\ {\Theta }_{x2}{r}_{z2}-{\Theta }_{z2}{r}_{x2}\\ {\Theta }_{x3}{r}_{z3}-{\Theta }_{z3}{r}_{x3}\\ {\Theta }_{x4}{r}_{z4}-{\Theta }_{z4}{r}_{x4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}{r}_{\mathrm{zn}}-{\Theta }_{\mathrm{zn}}{r}_{\mathrm{xn}}\end{array}\right),{\overrightarrow{s}}_6=\left(\begin{array}{c}{\Theta }_{y1}{r}_{x1}-{\Theta }_{x1}{r}_{y1}\\ {\Theta }_{y2}{r}_{x2}-{\Theta }_{x2}{r}_{y2}\\ {\Theta }_{y3}{r}_{x3}-{\Theta }_{x3}{r}_{y3}\\ {\Theta }_{y4}{r}_{x4}-{\Theta }_{x4}{r}_{y4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}{r}_{\mathrm{xn}}-{\Theta }_{\mathrm{xn}}{r}_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_7=\left(\begin{array}{c}{\Theta }_{y1}{r}_{y1}-{\Theta }_{z1}{r}_{z1}\\ {\Theta }_{y2}{r}_{y2}-{\Theta }_{z2}{r}_{z2}\\ {\Theta }_{y3}{r}_{y3}-{\Theta }_{z3}{r}_{z3}\\ {\Theta }_{y4}{r}_{y4}-{\Theta }_{z4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}{r}_{\mathrm{yn}}-{\Theta }_{\mathrm{zn}}{r}_{\mathrm{zn}}\end{array}\right),{\overrightarrow{s}}_8=\left(\begin{array}{c}{\Theta }_{x1}{r}_{x1}-{\Theta }_{z1}{r}_{z1}\\ {\Theta }_{x2}{r}_{x2}-{\Theta }_{z2}{r}_{z2}\\ {\Theta }_{x3}{r}_{x3}-{\Theta }_{z3}{r}_{z3}\\ {\Theta }_{x4}{r}_{x4}-{\Theta }_{z4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}{r}_{\mathrm{xn}}-{\Theta }_{\mathrm{zn}}{r}_{\mathrm{zn}}\end{array}\right)$ ${\overrightarrow{s}}_9=\left(\begin{array}{c}{\Theta }_{x1}{r}_{x1}-{\Theta }_{y1}{r}_{y1}\\ {\Theta }_{x2}{r}_{x2}-{\Theta }_{y2}{r}_{y2}\\ {\Theta }_{x3}{r}_{x3}-{\Theta }_{y3}{r}_{y3}\\ {\Theta }_{x4}{r}_{x4}-{\Theta }_{y4}{r}_{y4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}{r}_{\mathrm{xn}}-{\Theta }_{\mathrm{yn}}{r}_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_{10}=\left(\begin{array}{c}{\Theta }_{y1}{r}_{x1}+{\Theta }_{x1}{r}_{y1}\\ {\Theta }_{y2}{r}_{x2}+{\Theta }_{x2}{r}_{y2}\\ {\Theta }_{y3}{r}_{x3}+{\Theta }_{x3}{r}_{y3}\\ {\Theta }_{y4}{r}_{x4}+{\Theta }_{x4}{r}_{y4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}{r}_{\mathrm{xn}}+{\Theta }_{\mathrm{xn}}{r}_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_{11}=\left(\begin{array}{c}{\Theta }_{z1}{r}_{x1}+{\Theta }_{x1}{r}_{z1}\\ {\Theta }_{z2}{r}_{x2}+{\Theta }_{x2}{r}_{z2}\\ {\Theta }_{z3}{r}_{x3}+{\Theta }_{x3}{r}_{z3}\\ {\Theta }_{z4}{r}_{x4}+{\Theta }_{x4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}{r}_{\mathrm{xn}}+{\Theta }_{\mathrm{xn}}{r}_{\mathrm{zn}}\end{array}\right)\mathrm{and}{\overrightarrow{s}}_{12}=\left(\begin{array}{c}{\Theta }_{z1}{r}_{y1}+{\Theta }_{y1}{r}_{z1}\\ {\Theta }_{z2}{r}_{y2}+{\Theta }_{y2}{r}_{z2}\\ {\Theta }_{z3}{r}_{y3}+{\Theta }_{y3}{r}_{z3}\\ {\Theta }_{z4}{r}_{y4}+{\Theta }_{y4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}{r}_{\mathrm{yn}}+{\Theta }_{\mathrm{yn}}{r}_{\mathrm{zn}}\end{array}\right)$ equals twelve, wherein by means of the linear acceleration sensors at least one acceleration measurement signal {right arrow over (a)}=(a₁, a₂, a₃, a₄, . . . a_n) is captured, and wherein a position, velocity, and/or acceleration signal for the body is generated from the acceleration measurement signal and data that describe the relative position and orientation of the linear acceleration sensors in the body-fixed coordinate system.

The invention is based on equation (2), which can be written in vector notation in terms of its fractions in the body-fixed coordinates x, y, z, as follows:

$\begin{array}{cc}\left(\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right)=\left(\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right)+\left(\begin{array}{c}{r}_z{\stackrel{.}{\omega }}_y-r_z{\stackrel{.}{\omega }}_y\\ -r_z{\stackrel{.}{\omega }}_x+r_x{\stackrel{.}{\omega }}_z\\ r_y{\stackrel{.}{\omega }}_x-r_z{\stackrel{.}{\omega }}_y\end{array}\right)+\left(\begin{array}{c}{r}_y{\omega }_x{\omega }_y-r_y{\omega }_y^2+r_z{\omega }_x{\omega }_z-r_x{\omega }_z^2\\ r_x{\omega }_x{\omega }_y-r_y{\omega }_x^2+r_z{\omega }_y{\omega }_z-r_y{\omega }_z^2\\ r_x{\omega }_x{\omega }_y-r_x{\omega }_y^2+r_y{\omega }_y{\omega }_z-r_z{\omega }_x^2\end{array}\right)& \mathrm{Equation}\left(4\right)\end{array}$

Because the individual linear acceleration sensors in each case only have one sensitive measurement axis for the direction vector {right arrow over (Θ)}_i, the scalar product must be derived from equation (4) and {right arrow over (Θ)}_i. This gives equation (5), which describes the acceleration a_i measured by the linear acceleration sensor i in point P_i:

$\begin{array}{cc}{a}_i={\theta }_z\left(\begin{array}{c}{A}_z-r_x{\omega }_x^2-r_z{\omega }_y^2+r_x{\omega }_x{\omega }_z+\\ r_y{\omega }_y{\omega }_z+r_y{\stackrel{.}{\omega }}_x-r_x{\stackrel{.}{\omega }}_y\end{array}\right)+{\theta }_y\left(\begin{array}{c}{A}_y-r_y{\omega }_x^2-r_y{\omega }_z^2+r_x{\omega }_x{\omega }_y+\\ r_z{\omega }_y{\omega }_z+r_y{\stackrel{.}{\omega }}_z-r_z{\stackrel{.}{\omega }}_x\end{array}\right)+{\theta }_x\left(\begin{array}{c}{A}_x-r_x{\omega }_y^2-r_x{\omega }_z^2+r_y{\omega }_x{\omega }_y+\\ r_z{\omega }_x{\omega }_z+r_x{\stackrel{.}{\omega }}_y-r_y{\stackrel{.}{\omega }}_z\end{array}\right)& \mathrm{Equation}\left(5\right)\end{array}$

If one considers the right-hand side of equation (5), it is evident that it is formed from the following twelve values, of which six are independent:w=(A_x,A_y,A_z,{dot over (ω)}_x,{dot over (ω)}_y,{dot over (ω)}_z,ω_x²,ω_y²,ω_z²,ω_x,ω_y,ω_x,ω_z,ω_y,ω_z)

The remaining values describe the spatial position {right arrow over (r)}_i or the spatial orientation {right arrow over (Θ)}_i of the linear acceleration sensors.

If a plurality n of at least twelve linear acceleration sensors is employed, the measurement values a₁ through a_n are obtained. If one writes equation (5) for the n sensors one below the other, the following equation system is obtained, which is only partially shown due to lack of space:

$\begin{array}{cc}{a}_1={\theta }_{z1}\left(\begin{array}{c}{A}_z-r_{z1}{\omega }_x^2-r_{z1}{\omega }_y^2+r_{x1}{\omega }_x{\omega }_z+\\ r_{y1}{\omega }_y{\omega }_z+r_{y1}{\stackrel{.}{\omega }}_x-r_{x1}{\stackrel{.}{\omega }}_y\end{array}\right)+\dots a_2={\theta }_{z2}\left(\begin{array}{c}{A}_z-r_{z2}{\omega }_x^2-r_{z2}{\omega }_y^2+r_{x2}{\omega }_x{\omega }_z+\\ r_{y2}{\omega }_y{\omega }_z+r_{y2}{\stackrel{.}{\omega }}_x-r_{x2}{\stackrel{.}{\omega }}_y\end{array}\right)+\dots \dots \dots a_n={\theta }_{z12}\left(\begin{array}{c}{A}_z-r_{z12}{\omega }_x^2-r_{z12}{\omega }_y^2+r_{x12}{\omega }_x{\omega }_z+\\ r_{y12}{\omega }_y{\omega }_z+r_{y12}{\stackrel{.}{\omega }}_x-r_{x12}{\stackrel{.}{\omega }}_y\end{array}\right)+\dots & \mathrm{Equation}\left(6\right)\end{array}$

If the above-mentioned twelve values are isolated from the right side of equation (6), the equation system shown in FIG. 1a through 1c is obtained, which can be simplified as:{right arrow over (a)}=A·{right arrow over (w)}  Equation (7)

In order to be able to define the motion of the body from the acceleration measurement signal {right arrow over (a)}, equation (7) must be solved for {right arrow over (w)}. To this end, the 12×n matrix A must be invertible, which it is if the linear acceleration sensors are arranged in such a way that the matrix A formed from the column vectors {right arrow over (s)}₁, {right arrow over (s)}₂, {right arrow over (s)}₃, {right arrow over (s)}₄, {right arrow over (s)}₅, {right arrow over (s)}₆, {right arrow over (s)}₇, {right arrow over (s)}₈, {right arrow over (s)}₉, {right arrow over (s)}₁₀, {right arrow over (s)}₁₁ and {right arrow over (s)}₁₂ mentioned in claim 1 has the rank Rg(A)=12 or is linearly independent and therefore the determinant of A is not equal to 0. According to the algorithm:{right arrow over (w)}=A⁻¹·{right arrow over (a)}  Equation (8)

The method of the invention advantageously enables a simple, precise determination of the relative position, velocity, and/or acceleration of the body even with measurement axes obliquely aligned with each other, without having to solve a differential equation system numerically.

After arrangement of the linear acceleration sensors on the positions P_i in a preferred embodiment of the invention, at least five measurements are taken, in which the body is linearly accelerated and/or accelerated about at least one known center of rotation in various known directions with respect to an earth-fixed coordinate system, wherein with each measurement an acceleration measurement signal {right arrow over (a)}_k=(a₁, a₂, a₃, a₄, . . . a_n) is captured in each case, and wherein the data describing the position and orientation of the linear acceleration sensors are determined from the acceleration measurement signals {right arrow over (a)}_k and the directions and/or the at least one rotation center. A calibration step in which the data are determined in relation to the respective position and alignment of the linear acceleration sensors is thus carried out so that any potential tolerances arising in the manufacture and/or positioning of the linear acceleration sensors on the body are considered in the capture of the position, velocity, acceleration, and or rotation center signal. It is thus possible to achieve great measurement accuracy even with relatively high manufacturing and positioning tolerances of the linear acceleration sensors.

In an advantageous embodiment of the invention, the relative position of a rotation center about which the body spatially rotates is calculated from the velocity signal. For determining the rotation center of the rotary motion, the point is sought at which the velocity equals zero. In a vehicle, for example, the triggering of a safety mechanism for protecting the passengers can be controlled with respect to the position of the rotation center relative to the center of the vehicle and relative to the angular velocity and/or angular acceleration of the vehicle.

With regard to the device, the aforementioned objective is achieved in that said device for the capture of at least one acceleration measurement signal {right arrow over (a)}=(a₁, a₂, a₃, a₄, . . . a_n) has a plurality of n linear acceleration sensors stationarily arranged with respect to the body, wherein said plurality n is at least twelve, wherein the positions P_i on which said linear acceleration sensors are arranged in each case can be described by a stationary position vector {right arrow over (r)}_i=(r_xi, r_yi, r_zi), which indicates the respective position P_i from a body-fixed reference point, wherein the linear acceleration sensors in each case have a sensitive measurement axis oriented toward a direction vector {right arrow over (Θ)}_i=(Θ_xi, Θ_yi, Θ_zi), wherein the respective linear acceleration sensor and the x, y, z axes of a body-fixed coordinate system passing through the reference point are in each case designated by iε[1, 2, 3, 4, . . . n], wherein the position vectors it and the direction vectors {right arrow over (Θ)}_i are selected so that the rank of a twelve column matrix formed from the column vectors

${\overrightarrow{s}}_1=\left(\begin{array}{c}{\Theta }_{x1}\\ {\Theta }_{x2}\\ {\Theta }_{x3}\\ {\Theta }_{x4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}\end{array}\right),{\overrightarrow{s}}_2=\left(\begin{array}{c}{\Theta }_{y1}\\ {\Theta }_{y2}\\ {\Theta }_{y3}\\ {\Theta }_{y4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_3=\left(\begin{array}{c}{\Theta }_{z1}\\ {\Theta }_{z2}\\ {\Theta }_{z3}\\ {\Theta }_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}\end{array}\right),{\overrightarrow{s}}_4=\left(\begin{array}{c}{\Theta }_{z1}{r}_{y1}-{\Theta }_{y1}{r}_{z1}\\ {\Theta }_{z2}{r}_{y2}-{\Theta }_{y2}{r}_{z2}\\ {\Theta }_{z3}{r}_{y3}-{\Theta }_{y3}{r}_{z3}\\ {\Theta }_{z4}{r}_{y4}-{\Theta }_{y4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}{r}_{\mathrm{yn}}-{\Theta }_{\mathrm{yn}}{r}_{\mathrm{zn}}\end{array}\right),{\overrightarrow{s}}_5=\left(\begin{array}{c}{\Theta }_{x1}{r}_{z1}-{\Theta }_{z1}{r}_{x1}\\ {\Theta }_{x2}{r}_{z2}-{\Theta }_{z2}{r}_{x2}\\ {\Theta }_{x3}{r}_{z3}-{\Theta }_{z3}{r}_{x3}\\ {\Theta }_{x4}{r}_{z4}-{\Theta }_{z4}{r}_{x4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}{r}_{\mathrm{zn}}-{\Theta }_{\mathrm{zn}}{r}_{\mathrm{xn}}\end{array}\right),{\overrightarrow{s}}_6=\left(\begin{array}{c}{\Theta }_{y1}{r}_{x1}-{\Theta }_{x1}{r}_{y1}\\ {\Theta }_{y2}{r}_{x2}-{\Theta }_{x2}{r}_{y2}\\ {\Theta }_{y3}{r}_{x3}-{\Theta }_{x3}{r}_{y3}\\ {\Theta }_{y4}{r}_{x4}-{\Theta }_{x4}{r}_{y4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}{r}_{\mathrm{xn}}-{\Theta }_{\mathrm{xn}}{r}_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_7=\left(\begin{array}{c}{\Theta }_{y1}{r}_{y1}-{\Theta }_{z1}{r}_{z1}\\ {\Theta }_{y2}{r}_{y2}-{\Theta }_{z2}{r}_{z2}\\ {\Theta }_{y3}{r}_{y3}-{\Theta }_{z3}{r}_{z3}\\ {\Theta }_{y4}{r}_{y4}-{\Theta }_{z4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}{r}_{\mathrm{yn}}-{\Theta }_{\mathrm{zn}}{r}_{\mathrm{zn}}\end{array}\right),{\overrightarrow{s}}_8=\left(\begin{array}{c}{\Theta }_{x1}{r}_{x1}-{\Theta }_{z1}{r}_{z1}\\ {\Theta }_{x2}{r}_{x2}-{\Theta }_{z2}{r}_{z2}\\ {\Theta }_{x3}{r}_{x3}-{\Theta }_{z3}{r}_{z3}\\ {\Theta }_{x4}{r}_{x4}-{\Theta }_{z4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}{r}_{\mathrm{xn}}-{\Theta }_{\mathrm{zn}}{r}_{\mathrm{zn}}\end{array}\right)$ ${\overrightarrow{s}}_9=\left(\begin{array}{c}{\Theta }_{x1}{r}_{x1}-{\Theta }_{y1}{r}_{y1}\\ {\Theta }_{x2}{r}_{x2}-{\Theta }_{y2}{r}_{y2}\\ {\Theta }_{x3}{r}_{x3}-{\Theta }_{y3}{r}_{y3}\\ {\Theta }_{x4}{r}_{x4}-{\Theta }_{y4}{r}_{y4}\\ ⋮\\ {\Theta }_{\mathrm{xn}}{r}_{\mathrm{xn}}-{\Theta }_{\mathrm{yn}}{r}_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_{10}=\left(\begin{array}{c}{\Theta }_{y1}{r}_{x1}+{\Theta }_{x1}{r}_{y1}\\ {\Theta }_{y2}{r}_{x2}+{\Theta }_{x2}{r}_{y2}\\ {\Theta }_{y3}{r}_{x3}+{\Theta }_{x3}{r}_{y3}\\ {\Theta }_{y4}{r}_{x4}+{\Theta }_{x4}{r}_{y4}\\ ⋮\\ {\Theta }_{\mathrm{yn}}{r}_{\mathrm{xn}}+{\Theta }_{\mathrm{xn}}{r}_{\mathrm{yn}}\end{array}\right),{\overrightarrow{s}}_{11}=\left(\begin{array}{c}{\Theta }_{z1}{r}_{x1}+{\Theta }_{x1}{r}_{z1}\\ {\Theta }_{z2}{r}_{x2}+{\Theta }_{x2}{r}_{z2}\\ {\Theta }_{z3}{r}_{x3}+{\Theta }_{x3}{r}_{z3}\\ {\Theta }_{z4}{r}_{x4}+{\Theta }_{x4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}{r}_{\mathrm{xn}}+{\Theta }_{\mathrm{xn}}{r}_{\mathrm{zn}}\end{array}\right)\mathrm{and}{\overrightarrow{s}}_{12}=\left(\begin{array}{c}{\Theta }_{z1}{r}_{y1}+{\Theta }_{y1}{r}_{z1}\\ {\Theta }_{z2}{r}_{y2}+{\Theta }_{y2}{r}_{z2}\\ {\Theta }_{z3}{r}_{y3}+{\Theta }_{y3}{r}_{z3}\\ {\Theta }_{z4}{r}_{y4}+{\Theta }_{y4}{r}_{z4}\\ ⋮\\ {\Theta }_{\mathrm{zn}}{r}_{\mathrm{yn}}+{\Theta }_{\mathrm{yn}}{r}_{\mathrm{zn}}\end{array}\right)$ equals twelve, and wherein the linear acceleration sensors are connected to an evaluator configured to generate a position, velocity, and/or acceleration signal for the body out of the acceleration measurement signal {right arrow over (a)} and data that describe the relative position and orientation of the linear acceleration sensors in the body-fixed coordinate system.

As explained previously for the method, the measurement axes of the at least twelve linear acceleration sensors can also be discretionarily obliquely aligned with each other as long as the matrix formed from the twelve column vectors {right arrow over (s)}₁, {right arrow over (s)}₂, {right arrow over (s)}₃, {right arrow over (s)}₄, {right arrow over (s)}₅, {right arrow over (s)}₆, {right arrow over (s)}₇, {right arrow over (s)}₈, {right arrow over (s)}₉, {right arrow over (s)}₁₀, {right arrow over (s)}₁₁, {right arrow over (s)}₁₂ is linearly independent. Nevertheless, the relative position, velocity, and/or acceleration signal can be determined with great precision by means of the device. This is especially advantageous when the body is a vehicle body, as the linear acceleration sensors can now be installed on suitable sites in the vehicle. The employment of linear acceleration sensors already present in the vehicle, for example airbag sensors, for the device of the invention is even conceivable.

The device can be a component in an inertial navigation system (INS). Furthermore, the navigation system can also have a global positioning system (GPS), which shows the positions of a plurality of satellites orbiting the earth. Preference is given to employment of the inertial navigation system when there is temporary interference with the GPS positioning, for example, when the path to the satellites is blocked by an obstruction. The dynamic but long-term unstable INS can be coupled with the sluggish but long-term stable GPS via a suitable mathematical filter, especially a Kaiman filter, an extended Kaiman filter, and/or a particle filter. This enables the achievement of a dynamic and long-term stable system with better characteristics than the corresponding single system. Furthermore, an online auto-calibration is also possible.

Applications of the device of the invention are not limited to the automotive field. The device is also especially suited to applications in medical technology, in an aircraft or watercraft, as well as to all other applications that require measurement of the spatial position and motion of a body.

in an advantageous embodiment of the invention, the evaluator has a data storage in which at least one data signal for a line vector of a data matrix is filed, said matrix being the inverse of a twelve column matrix comprising the column vectors {right arrow over (s)}₁, {right arrow over (s)}₂, {right arrow over (s)}₃, {right arrow over (s)}₄, {right arrow over (s)}₅, {right arrow over (s)}₆, {right arrow over (s)}₇, {right arrow over (s)}₈, {right arrow over (s)}₉, {right arrow over (s)}₁₀, {right arrow over (s)}₁₁, {right arrow over (s)}₁₂. To this end, the device can have a microprocessor, in which the calculation of the relative position, velocity, and/or acceleration signal is performed, connected to the data storage. Preference is given to calculation of the data matrix on the basis of measurement values for the position and alignment of the individual linear acceleration sensors relative to the body-fixed coordinate system. Obviously the data matrix can also be specified, for example, when the position and alignment of the individual linear acceleration sensors are known by means other than measurement.

It is advantageous if the evaluator has means for generating at least one first scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, and if the at least one data signal is selected so that at least a first scalar product signal corresponds to a vectorial component of the translatory acceleration of the body. To this end, preference is given to configuration of these means wherein in each case a component for the translatory acceleration of the body with respect to the body-fixed coordinate system is determined by means of the device for three directions running diagonally to each other, say, the directions of the axes of a Cartesian coordinate system.

In a preferred embodiment of the invention, the evaluator has means for generating at least one second scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, wherein the at least one data signal is selected so that the second scalar product signal corresponds to a vectorial component of the angular acceleration of the body. To this end, preference is given to configuration of these means wherein in each case a component for the angular acceleration of the body with respect to the body-fixed coordinate system is determined by means of the device for three directions running diagonally to each other, say, the directions of the axes of a Cartesian coordinate system.

In a functional embodiment of the invention, the evaluator has means, should the situation arise, for the two-fold integration of the at least one second scalar product signal. The angular velocity and/or the rotary position of the body with respect to a starting position can then be determined from the angular acceleration. The device can thus be employed especially for controlling a safety mechanism to protect the passengers of a vehicle from injury in the event said vehicle is rolled. To this end, the device can serve in particular for determining the angle which a specified axis, say, the lengthwise axis, of a vehicle assumes with respect to the direction of the acceleration due to gravity, in order to trigger the safety mechanism at the right time in the event the vehicle is rolled. The critical angle at which a rollover is unavoidable shifts depending on whether the vehicle is on level ground or a hill. The spatial position of the rotary axis is also of great importance for deciding what measures should be taken in the event of a probable rollover. If the rotation axis is located, say, inside the vehicle, a different safety mechanism can be employed than if the rotation axis is located outside the vehicle. The sturdiness of the linear acceleration sensors is especially advantageous in a vehicle as a safety device, because said sensors must not be destroyed during a collision before the actual rollover.

In an advantageous embodiment of the invention, the evaluator has means for generating at least one third scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, wherein the at least one data signal is selected so that the third scalar product signal corresponds to the square of a vectorial component of the angular velocity of the body, and wherein the evaluator has means for generating a square root signal from the scalar product signal. The device can then provide a measurement signal for the magnitude of the angular velocity of the body.

It is advantageous if the evaluator has means for generating at least one fourth scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, if the at least one data signal is selected so that the fourth scalar product signal corresponds to the product of two different vectorial components of the angular velocity of the body, if the evaluator has means for the integration of the second scalar product signals detected in each case for the respective vectorial components and for generating at least one product signal from the results of these integrations, if the evaluator has a comparison mechanism for comparing the strength of this product signal with the fourth scalar product signal, and if the comparison mechanism has an output for an error status signal dependent on the result of the comparison. The second and fourth scalar product signals are thus subjected to a plausibility test. Should it be established that the corresponding scalar product signals do not match, the relative position, velocity, and/or acceleration signal detected by the device can be deleted and/or the triggering of a safety mechanism control-connected to the device can be blocked.

The comparison mechanism is functionally configured for the comparison of the square root signal with the signal formed from the integration of the at least one second scalar product signal and has an output for an error status signal dependent on the result of the comparison. The plausibility of the measurement results can then be checked even more effectively.

It is advantageous if the evaluator has means for integrating the second scalar product signal and for forming the square from the result of this integration, if said means for comparing the signal thus obtained with the third scalar product signal is connected to the comparison mechanism, and if the comparison mechanism has an output for an error status signal dependent on the result of the comparison. The plausibility of the second and third scalar product signals is thus checked.

In a preferred embodiment of the invention, the evaluator has a differentiating mechanism for differentiating the square root signal, wherein the differentiating mechanism for comparison of the differentiated signal with the second scalar product signal is connected to the comparison mechanism, and wherein the comparison mechanism has an output for an error status signal dependent on the result of the comparison. This means also enables a simple and rapid plausibility test of the measurement results.

BRIEF DESCRIPTION OF THE DRAWINGS

In the following, an illustrative embodiment of the invention is explained in more detail with reference to the drawing, wherein:

FIG. 1 a through 1c show an equation system comprising two vectors and a matrix for twelve linear acceleration sensors,

FIG. 2 shows a schematic illustration of a body which is displaceable in a three dimensional space toward the three axes u, v, w of a Cartesian earth-fixed coordinate system and which is pivotable about these axes,

FIG. 3 shows a schematic illustration of the measurement axes of linear acceleration sensors affixed to and detached from the body in a body-fixed coordinate system with the axes x, y, and z,

FIG. 4 shows a table in which the positions of the linear acceleration sensors in the coordinates of the body-fixed Cartesian coordinate system are given,

FIG. 5 shows a table in which the directions in which the linear acceleration sensors are sensitive are given in the coordinates of the body-fixed Cartesian coordinate system,

FIG. 6 shows the matrix from the equation system shown in FIG. 2 for the linear acceleration sensor arrangement shown in FIG. 3,

FIG. 7 shows a matrix which is the inverse of the matrix shown in FIG. 6,

FIG. 8 shows a vector field that contains velocity vectors for a motion of a body, said motion having a translatory and a rotary component, wherein the rotary center of the rotary motion is designated with C, and

FIG. 9 shows a block diagram of a device for determining the position, velocity, acceleration, and/or the rotary center of a body in a three dimensional space.

A device for determining the relative position, velocity, and should the need arise, the rotary center of a body 1 situated in a three dimensional space (FIG. 2) described by the coordinates u, v, w of a space- or earth-fixed coordinate system has at least twelve linear acceleration sensors 2, which are stationarily arranged with respect to the body 1 and which in each case are sensitive along a sensitive measurement axis 3, for the capture of at least one acceleration measurement signal {right arrow over (a)}=(a₁, a₂, a₃, a₄, a₅, a₆, a₇, a₈, a₉, a₁₀, a₁₁, a₁₂). The body 1 is freely spatially displaceable, in other words, displaceable along the axes u, v, w and in each case capable of being rotated about the axes u, v, w.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

It can be discerned in FIG. 2 that a body-fixed Cartesian coordinate system with the axes x, y, and z and the origin 4 is allocated to the body. The origin 5 of the space-fixed coordinate system u, v, w is connected to the origin 4 of the body-fixed coordinate system x, y, z by a vector {right arrow over (R)}. A path 6 along which the origin 4 of the body-fixed coordinate system x, y, z was displaced is illustratively shown in FIG. 2. A point P remote from the origin 4 of the body 1 is also shown. The origin 4 is connected to the point P by a vector {right arrow over (r)}.

The measurement axes 3 of the linear acceleration sensors 2 are shown in the body-fixed coordinate system x, y, z in FIG. 3. The coordinates r_xi, r_yi, r_zi of the position vectors {right arrow over (r)}_i and the coordinates Θ_xi, Θ_yi, Θ_zi of the direction vectors {right arrow over (Θ)}_i of the measurement axes 3 are listed in each case in FIG. 4. The index iε[1, 2, 3, 4, . . . 12] corresponds to the number of the respective linear acceleration sensor 2. Preference is given to calibration for determining the position vectors 4 and the direction vectors {right arrow over (Θ)}_i.

In can be discerned from FIG. 3 and the table in FIG. 4, that in each case three linear acceleration sensors 2, of which the measurement axes 3 are oriented in directions running orthogonally to each other, which are not arranged in a plane, are arranged on the same position and form a multidirectional acceleration sensor 7 (FIG. 9).

If the coordinates r_xi, r_yi, r_zi of the twelve position vectors 4, and the coordinates Θ_xi, Θ_yi, Θ_zi of the twelve direction vectors {right arrow over (Θ)}_i are plugged into the matrix A of the equation system shown in FIG. 1a through 1c, the 12×12 matrix shown in FIG. 6 is obtained. The determinant of this matrix is −16 and the rank Rg(A)=12. The condition that the column vectors of the matrix A are linearly independent and that the matrix A is therefore invertible in a manner known per se is thus satisfied. The inverted matrix A⁻¹ rounded to three decimal places, which henceforth shall be known as the data matrix, is shown in FIG. 7.

With the data matrix A-1 the vector {right arrow over (w)}=(A_x, A_y, A_z, {dot over (ω)}_x, {dot over (ω)}_y, {dot over (ω)}_z, ω_x², ω_y², ω_z², ω_x, ω_y, ω_x, ω_z, ω_y, ω_z) can now be calculated according to equation (8), and the translatory acceleration {right arrow over (A)}=(Ax, Ay, Az), the rotary acceleration {right arrow over ({dot over (ω)}=({dot over (ω)}_x, {dot over (ω)}_y, {dot over (ω)}_z), the squares of the angular velocities ω_x², ω_y², ω_x² of the body 1 relative to the body-fixed axes x, y, z, and mixed terms, which in each case correspond to the product of two different vectorial components ω_x², ω_y², ω_x² of the angular velocity of the body 1, can be determined easily.

In order to be able to determine the rotation point of a body, the translation velocity {right arrow over (A)} and the rotation {right arrow over (ω)} must be known. The rotation point is then determined by seeking the point at which the velocity quantity disappears. The derivation of the velocity is achieved by superimposing the translation velocity {dot over (R)}=v and the rotation velocity ω=r. In order for the velocity to disappear, the equation ω×q=−v in which q represents the rotation point must be satisfied. The rotation point can then expressed as q=ω×v¹/∥ω∥₂² with v¹=−(ωv)ω/∥ω∥₂². A vector field of the displaced body 1 with the rotation point C is shown in FIG. 8. The translation velocity is expressed as v_x=5 and v_y=1 and the rotation ω=1.

It can be discerned in FIG. 9 that the individual linear acceleration sensors 2 are connected to an evaluator 8 for calculating the vector {right arrow over (w)} and the coordinates of the rotation point C. The evaluator has a data storage 9 and a microprocessor 10, which is connected to the data storage 9 via a buss system, which is not shown in any further detail in the drawing. The data matrix A⁻¹ is filed in the data storage 9. For calculation of the individual components of the vector {right arrow over (w)}=(A_x, A_y, A_z, {dot over (ω)}_x, {dot over (ω)}_y, {dot over (ω)}_z, ω_x², ω_y², ω_z², ω_x, ω_y, ω_x, ω_z, ω_y, ω_z), the microprocessor 10 in each case calculates the scalar product from the individual line vectors of the data matrix A⁻¹ and the acceleration measurement signal {right arrow over (a)}=(a₁, a₂, a₃, a₄, . . . a_n) measured with the linear acceleration sensors 2. The vector {right arrow over (w)}=(A_x, A_y, A_z, {dot over (ω)}_x, {dot over (ω)}_y, {dot over (ω)}_z, ω_x², ω_y², ω_z², ω_x, ω_y, ω_x, ω_z, ω_y, ω_z), and the rotation point C are allocated to a measurement signal output 11 in the form of a suitable output signal.

In the method for determining the relative position, velocity, acceleration, and/or the rotation center of a body 1 displaceable in a three dimensional space, at least twelve linear acceleration sensors 2 are provided and in each case arranged on a position stationarily fixed with respect to the body 1. At least one acceleration measurement signal is captured by means of the linear acceleration sensors 2. A position, velocity, acceleration, and/or rotation center signal for the body 1 is generated from the acceleration measurement signal and data describing the position and orientation of the linear acceleration sensors 2 in the bodyfixed coordinate system.

Claims

1. A method for determining the relative position, velocity, and/or acceleration of a body displaceable in a three dimensional space, wherein a plurality of n linear acceleration sensors, which in each case have a sensitive measurement axis, is provided, wherein said plurality n is at least twelve, wherein the individual linear acceleration sensors are in each case arranged on a position Pi which is stationarily fixed with respect to the body and which can be described by a stationary position vector,

2. The method as in claim 1, characterized in that after the arrangement of the linear acceleration signals on the positions Pi, at least five measurements are taken in which the body is accelerated linearly and/or about at least one known rotation center in various known directions with respect to an earth-fixed coordinate system, in that with each measurement an acceleration measurement signal

3. The method as in claim 1, characterized in that the relative position of a rotation center about which the body rotates in the space is determined from the velocity signal.

4. A device for determining the relative position, velocity, and/or acceleration of a body displaceable in a three dimensional space, wherein the device for the capture of at least one acceleration measurement signal

5. The device as in claim 4, characterized in that the evaluator has a data storage in which at least one data signal for a line vector of a data matrix is filed, which is the inverse of a twelve column matrix comprising the column vectors {right arrow over (s)}1, {right arrow over (s)}2, {right arrow over (s)}3, {right arrow over (s)}4, {right arrow over (s)}5, {right arrow over (s)}6, {right arrow over (s)}7, {right arrow over (s)}8, {right arrow over (s)}9, {right arrow over (s)}10, {right arrow over (s)}11 and {right arrow over (s)}12.

6. The device as in claim 5, characterized in that the evaluator has means for generating at least one scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, and in that the at least one data signal is selected so that the at least one scalar product signal corresponds to a vectorial component of the translatory acceleration of the body.

7. The device as in claim 5, characterized in that the evaluator has means for generating at least one scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, and in that the at least one data signal is selected so that the scalar product signal corresponds to a vectorial component of the angulatory acceleration of the body.

8. The device as in claim 7, characterized in that the evaluator has means, should the need arise, for the two-fold integration of the at least one scalar product signal.

9. The device as in claim 7, characterized in that the evaluator has means for generating at least one additional scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, in that the at least one data signal is selected so that the additional scalar product signal corresponds to the square of a vectorial component of the angular velocity of the body, and in that the evaluator has means for generating a square root signal from the additional scalar product signal, and in that the comparison mechanism is configured to compare the square root signal with the signal generated by integration of the at least one scalar product signal, and in that the comparison mechanism has an output for an error status signal dependent on the result of the comparison.

10. The device as in claim 7, characterized in that the evaluator has means for generating at least one additional scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, in that the at least one data signal is selected so that the additional scalar product signal corresponds to the square of a vectorial component of the angular velocity of the body, and in that the evaluator has means for generating a square root signal from the additional scalar product signal, and in that the evaluator has means for the integration of the scalar product signal and for the formation of the square from the result of said integration, in that said means for comparing the signal thus received with the additional scalar product signal is connected to the comparison mechanism, and in that the comparison mechanism has an output for an error status signal dependent on the result of the comparison.

11. The device as in claim 7, characterized in that the evaluator has means for generating at least one additional scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, in that the at least one data signal is selected so that the additional scalar product signal corresponds to the square of a vectorial component of the angular velocity of the body, and in that the evaluator has means for generating a square root signal from the additional scalar product signal, and in that the evaluator has a differentiation mechanism for the differentiation of the square root signal, in that the differentiation mechanism for comparing the differentiated signal with the scalar product signal is connected to the comparison mechanism, and in that the comparison mechanism has an output for an error status signal dependent on the result of the comparison.

12. The device as in claim 5, characterized in that the evaluator has means for generating at least one scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, in that the at least one data signal is selected so that the scalar product signal corresponds to the square of a vectorial component of the angular velocity of the body, and in that the evaluator has means for generating a square root signal from the scalar product signal.

13. The device as in claim 5, characterized in that the evaluator has means for generating at least one scalar product signal corresponding to the scalar product from the at least one data signal and the acceleration measurement signal {right arrow over (a)}, in that the at least one data signal is selected so that the scalar product signal corresponds to the product of two different vectorial components of the angular velocity of the body, in that the evaluator has means for integration of the scalar product signals detected in each case for the respective vectorial components and for the generation of at least one product signal from the results of these integrations, in that the evaluator has a comparison mechanism for comparing the magnitude of this product signal with the scalar product signal, and in that the comparison mechanism has an output for an error status signal dependent on the result of the comparison.

14. The device as in claim 4, characterized in that the evaluator is configured so that in the event of a rotary motion of the body about a rotation center it generates a signal for the relative position of a rotation center from the velocity signal.

15. The device as in claim 4, characterized in that the body is a vehicle.