JP5160963B2 - Posture / movement trajectory detection device, detection method, program, and recording medium - Google Patents

Description

  The present invention targets objects that can freely change posture and move in a three-dimensional space such as a cup and a toothbrush, and receives sensor data from sensor nodes attached to these objects to determine the posture and movement trajectory of the object. The present invention relates to a posture / movement trajectory detection device to be detected, a detection method, a program, and a recording medium.

Conventionally, in order to detect the movement trajectory of an object, positioning by a beacon system using GPS (Global Positioning System), ultrasonic waves, infrared rays, and weak radio waves, and image analysis by computer vision have been used.
In addition to the above-mentioned image analysis by computer vision, detection of the orientation of an object uses detection of the direction of magnetic north by an orientation sensor that measures geomagnetism and detection of the vertical direction by measuring gravitational acceleration by an acceleration sensor. (For example, refer nonpatent literature 1). This detection method is based on the principle that the three-dimensional posture of an object can be determined if the directions of two non-parallel vectors such as magnetic north and vertical can be observed.

Takayuki Hoshi, Hiroyuki Shinoda, "Cloth-like device capable of measuring shape in real time", 8th Society of Instrument and Control Engineers System Integration Division Lecture (SI2007), p. 977-978, 2007

However, GPS positioning has a problem that it cannot be applied to indoor objects that cannot receive GPS radio waves.
In addition, the detection by the beacon system and the computer vision has a problem that it is necessary to install a beacon transmitter or camera in the space so as to surround the object to be observed, and it is difficult to install it in a daily living environment. .

  On the other hand, azimuth sensors and acceleration sensors have been miniaturized, and these sensors can be attached to small objects such as cups and toothbrushes in the living environment. Furthermore, it is conceivable to embed a sensor in an object at the time of production or shipment, such as a barcode or an RF-ID tag. If these sensors can be used to detect the movement trajectory in addition to the posture of the object, the problem of installation in positioning using the GPS, beacon system, and computer vision described above can be avoided.

In particular, the use of an acceleration sensor is considered suitable for detecting the movement trajectory of an object because it can detect an external force related to the movement of the object. However, since the acceleration that can be measured by the acceleration sensor is a resultant force of the gravitational acceleration acting in the vertical direction on the earth and the external force related to the movement of the object, it is necessary to separate the gravitational acceleration and the external force.
For an object with a fixed installation direction (for example, an object that is installed horizontally with respect to the ground surface and moves while maintaining its posture), the measured gravitational acceleration can be considered fixed, so the acceleration The external force related to the movement can be obtained by subtracting the predetermined gravitational acceleration from the measured value of this, however, this assumption does not hold for objects that can be moved freely such as cups and toothbrushes. It is difficult to obtain the external force excluding gravitational acceleration immediately.

  The present invention has been made in order to solve the above-described problem, and a posture / movement trajectory detection device capable of detecting the posture and movement trajectory of an object using sensor data from a sensor node attached to the object, An object is to provide a detection method, a program, and a recording medium.

The present invention provides an attitude detection / movement trajectory detection apparatus for detecting the attitude and movement trajectory of an object to which a sensor node is attached, and an activity detection for detecting an activity section in which the object is moving based on sensor data received from the sensor node. Means, and when the activity section is detected, a stationary section search means for searching for a stationary section before and after the activity section, and the posture of the object from the sensor data of the stationary section before and after the activity section. Stationary posture estimation means for estimating the movement posture estimation means for estimating the posture of the object at each time of the activity section based on the posture of the object in the stationary section before and after the activity section, and the activity section The motion posture estimation means estimates for each time A gravitational acceleration estimating means for estimating the gravitational acceleration in the posture of the object, and a motion for estimating the acceleration of the object caused by an external force from the sensor data and the gravitational acceleration estimated by the gravitational acceleration estimating means at each time of the activity section An acceleration estimation unit; and a movement trajectory estimation unit that obtains a movement speed and a movement amount of the object in the activity section from the acceleration of the object estimated by the movement acceleration estimation unit, and the movement posture estimation unit performs the interpolation. Further, the jerk minimum principle is independently applied to the three-axis posture parameters estimated by the stationary posture estimation means, and the three-axis posture parameters of the object at each time in the activity section are calculated. Is.

Further , in one configuration example of the posture / movement trajectory detection device of the present invention, the sensor data includes a three-axis acceleration observation value and a three-axis azimuth observation value.

In addition, the posture / movement trajectory detection method of the present invention includes an activity detection procedure for detecting an activity section in which an object is moving based on sensor data received from a sensor node, and the activity detection procedure when the activity section is detected. A stationary section search procedure for searching for a stationary section before and after the section, a stationary posture estimation procedure for estimating the posture of an object for each stationary section from sensor data of the stationary section before and after the activity section, and before and after the activity section Based on the posture of the object in the stationary section, the motion posture estimation procedure for estimating the posture of the object at each time of the activity section by interpolation, and the posture of the object estimated by the motion posture estimation procedure for each time of the activity section Gravity acceleration estimation procedure to estimate gravity acceleration and A motion acceleration estimation procedure for estimating the acceleration of an object caused by an external force from the sensor data and the gravitational acceleration estimated by the gravitational acceleration estimation procedure for each time of the activity section, and an acceleration of the object estimated by the motion acceleration estimation procedure A movement trajectory estimation procedure for obtaining a movement speed and a movement amount of the object in the activity section, and the motion posture estimation procedure is a three-axis posture parameter estimated by the stationary posture estimation procedure when performing the interpolation. In this case, the jerk minimum principle is independently applied to each of the three, and the parameters of the three-axis posture of the object at each time in the activity section are calculated .

In addition, the posture / movement trajectory detection program of the present invention includes an activity detection procedure for detecting an activity section in which an object is moving based on sensor data received from a sensor node, and this activity section when the activity section is detected. A stationary section search procedure for searching for a stationary section before and after the section, a stationary posture estimation procedure for estimating the posture of an object for each stationary section from sensor data of the stationary section before and after the activity section, and before and after the activity section Based on the posture of the object in the stationary section, the motion posture estimation procedure for estimating the posture of the object at each time of the activity section by interpolation, and the posture of the object estimated by the motion posture estimation procedure for each time of the activity section Gravity acceleration estimation to estimate gravity acceleration The motion acceleration estimation procedure for estimating the acceleration of an object generated by an external force from the sensor data and the gravitational acceleration estimated by the gravitational acceleration estimation procedure for each time of the activity interval, and the motion acceleration estimation procedure A movement trajectory estimation procedure for determining the movement speed and movement amount of the object in the activity section from the acceleration of the object is executed by the computer, and the motion posture estimation procedure is estimated by the stationary posture estimation procedure when performing the interpolation. The jerk minimum principle is applied to each of the three-axis posture parameters independently, and the three-axis posture parameters of the object at each time in the activity section are calculated .
The recording medium of the present invention records an attitude / movement trajectory detection program.

  According to the present invention, the activity section in which the object is moving is detected, the posture of the object is estimated for each stationary section from the sensor data of the stationary section before and after the activity section, and based on the estimated posture of the object By estimating the posture of the object in the activity section by interpolation, the posture of the object in motion can be obtained. Furthermore, in the present invention, the gravitational acceleration in the posture of the object estimated for the activity section is estimated, the acceleration of the object caused by the external force is estimated from the sensor data measured for the activity section and the gravitational acceleration, and the activity is determined from the acceleration of the object. Since the movement speed and movement amount of the object in the section are calculated, the gravitational acceleration observed during the movement of the object and the external force related to the movement of the object can be separated, and the movement trajectory (movement of the object) Speed and amount of movement). In the present invention, since a method such as a beacon system or computer vision is not used, it can be easily applied to a daily living environment.

  Further, in the present invention, when the motion posture estimation means performs interpolation, the jerk minimum principle is independently applied to the three-axis posture parameters estimated by the stationary posture estimation means, and the object 3 at each time of the activity section is applied. Since the axis posture parameters are calculated, it is possible to interpolate in consideration of the movement of the human hand involved in the movement of the object, and to accurately detect the posture and movement trajectory during movement of the object from the sensor data. Is possible.

[Principle of the Invention]
In detecting the posture and movement trajectory of an object that can freely change and move in a three-dimensional space, the present invention obtains the posture of the object in a stationary state before and after the movement, Estimate the posture of the moving object by interpolating the posture of the object between the stationary state, calculate the gravitational acceleration that should be measured in the estimated posture of the object, and calculate from the actually measured acceleration The acceleration due to the external force is estimated by reducing the gravitational acceleration, the moving speed and the moving amount of the object are calculated by integrating the estimated acceleration with respect to time, and the moving trajectory of the object is estimated.

  The posture of the object in a stationary state before and after the movement can be obtained by a conventional technique. In the interpolation of the posture of an object, it is possible to use interpolation by a linear expression mathematically. However, since the movement of an object in daily life environment involves human actions, By using an interpolation formula based on the jerk minimum model, which is known as a method for approximating the above, it is possible to appropriately estimate the posture and movement trajectory of the object.

[Embodiment]
Hereinafter, embodiments of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram showing a configuration of a posture / movement trajectory detection apparatus according to an embodiment of the present invention.
The posture / movement trajectory detection device 1 of the present embodiment includes a server 10, an activity detection unit 11, a stationary section search unit 12, a stationary posture estimation unit 13, a motion posture estimation unit 14, and a gravitational acceleration estimation unit 15. And a motion acceleration estimation unit 16 and a movement trajectory estimation unit 17.

  In the environment assumed by the present embodiment, general-purpose sensor nodes 2 are attached to various objects in the room. FIG. 2 is a block diagram showing a configuration of the sensor node 2 according to the embodiment of the present invention. The sensor node 2 includes a plurality of types of sensors 20, a communication module 21 that transmits data to other sensor nodes 2 and the server 10, a memory 22 that stores a program, and a sensor 20 and a communication according to a program stored in the memory 22. And a processor 23 for controlling the module 21.

  The processor 23 of the sensor node 2 always collects and processes the measurement values of the sensor 20 that is operating. Further, the processor 23 sends the collected data to other sensor nodes 2 and the server 10 through the communication module 21 as necessary, and entrusts the processing. The processor 23 transmits data at regular time intervals, for example.

  Since the sensor node 2 is attached to an object that can freely change its posture and move in a three-dimensional space, it is preferable to use wireless communication for communication between the sensor nodes 2 and between the sensor node 2 and the server 10. . As the wireless communication, for example, a wireless LAN (Local Area Network) can be used.

  In detecting the posture and movement trajectory of an object, information collected from an orientation sensor that detects geomagnetism and an acceleration sensor that detects acceleration among a plurality of types of sensors 20 mounted on the sensor node 2 is used. The azimuth sensor is composed of sensors arranged in two or three directions crossing each other, and can measure the magnitude of geomagnetism as a two-dimensional or three-dimensional vector value. Similarly, the acceleration sensor is also composed of sensors arranged in two or three directions, and the magnitude of acceleration can be measured as a two-dimensional or three-dimensional vector value. The frequency of measurement is arbitrary, but currently commercially available sensors can continuously measure at intervals of about 5 ms.

  The posture of the object when it is stationary can be determined by using two direction vectors. For example, if only the direction of magnetic north detected by the direction sensor is given as the first direction vector, the posture of the object cannot be determined by itself, and there remains a degree of freedom for rotation around the vector pointing to magnetic north. . Therefore, if another vector that is not parallel to the vector pointing to magnetic north is given as a vector of gravity acceleration that means the vertical direction on the earth, the posture of the object can be determined.

  Further, when there is a restriction on the posture of the object, it is not always necessary to use a three-dimensional vector. For example, in the case of a sensor mounted on a door or the like, it is not necessary to consider a change in posture in the vertical direction, and it is sufficient if a two-dimensional vector can be detected. Currently available sensors include three-dimensional azimuth sensors and acceleration sensors that are packaged. Therefore, it is easy to implement using a set of magnetic north vectors and gravity acceleration vectors. However, when an external force is acting on the object, it is necessary to devise in detecting the direction of gravitational acceleration, and this embodiment solves this point.

  FIG. 3 is a flowchart showing the operation of the posture / movement trajectory detection apparatus 1 of the present embodiment. As described above, the server 10 receives data from the sensor node 2 (step S1 in FIG. 3). In the present embodiment, data transmitted from the sensor node 2 is an acceleration observation value that is a triaxial acceleration vector and an azimuth observation value that is a triaxial geomagnetic vector.

  Next, it is necessary to determine whether or not the object is stationary prior to determining the posture of the object when stationary. When the object is in a stationary state, the measured values of direction and acceleration do not change, so the time interval when the sensor data does not change is set as the stationary interval, while the change occurs continuously in the sensor data. The time interval is the activity interval. The activity detection unit 11 detects a time interval in which changes occur continuously in the sensor data received from the sensor node 2 as an activity interval, and sets the other time intervals as a stationary interval (step S2 in FIG. 3).

  Since it is not necessary to continue to actively calculate the state change of an object that is not moving, the detection of the posture and movement trajectory of the object is performed for the activity section. When the activity section is detected, the posture of the object in the stationary section before and after the activity section is obtained, and the posture and movement trajectory of the object in the activity section are estimated using the posture.

  In estimating the movement trajectory of an object, first, the estimation of the movement trajectory when there is no change in the posture of the object will be considered. When an external force is applied to the object, the object moves. The acceleration according to the strength of the external force at this time is measured by the acceleration sensor of the sensor node 2 attached to this object. Here, the measurement value by the acceleration sensor is obtained by superimposing the gravitational acceleration at the point where the object exists in addition to the acceleration by the external force.

  If there is no change in the posture of the object, the observed gravitational acceleration does not change before and after the movement, so the acceleration value measured when the object is stationary is the gravitational acceleration at the stationary point, and this gravitational acceleration is being moved. By subtracting from the individual measured acceleration values, the acceleration due to the external force that contributes to the motion can be calculated. The speed of the object can be obtained by integrating the acceleration due to the external force with respect to time, and the amount of movement of the object can be obtained by further integrating the speed of the object.

  When there is a change in the posture of the object, the direction of the gravitational acceleration as viewed from the object changes according to the posture, and thus the value of the gravitational acceleration measured at rest cannot be simply subtracted from the measured acceleration value. Therefore, by estimating the posture of the object at each time point, the magnitude of the gravitational acceleration that should be observed by the acceleration sensor in that posture is estimated, and the estimated gravitational acceleration is subtracted from the acceleration measurement value, thereby contributing to motion. Acceleration due to external force can be calculated.

When the activity detection unit 11 detects an activity section, the still section search unit 12 searches for a still section before and after this activity section (step S3 in FIG. 3). Here, it is assumed that the still section immediately before the activity section is ST ₁ and the still section immediately after the activity section is ST ₂ .

Static attitude estimation section 13 estimates the resting posture of the object in the still picture sequence ST _1, each stationary section ST ₁ from the measured sensor data in ST _2, ST ₂ (FIG. 3 step S4). Here, the parameters of the posture of the object are determined as follows. First, a spatial coordinate system is defined in which the north direction at the point where the object is stationary is X, the west direction is Y, and the vertical upward direction is Z. As the relationship between the three-dimensional xyz coordinate system in the sensor node 2 attached to this object and the above-described XYZ space coordinate system, the x-axis, y-axis, and z-axis coincide with the X-axis, Y-axis, and Z-axis, respectively. Is the reference posture of the sensor node 2 (object). From this reference posture, first, the rotation angle ξ is rotated about the z axis, then the rotation angle φ is rotated about the y axis, and the posture of the object when the rotation angle θ is rotated about the x axis ( (θ, φ, ξ).

In third sensor node 2 having an azimuth sensor of the acceleration sensor and three-axis axis, each moment acceleration vector _{a (t) = (a x} (t), a y (t), a z (t)), geomagnetism The vector h (t) = (h _x (t), h _y (t), h _z (t)) can be observed. _Assume that the acceleration vector when the object is stationary in the reference posture is a ₀ = (a _x0 , a _y0 , a _z0 ), and the geomagnetic vector is h ₀ = (h _x0 , h _y0 , h _z0 ). The acceleration in the stationary posture corresponds to the gravitational acceleration vector at the stationary point.

And the object is stationary at a certain time t, from the sensor node 2 observations _{a (t) = (a x} (t), a y (t), a z (t)) and h (t) = (h _x (T), h _y (t), h _z (t)) are obtained. Since the gravitational acceleration vector and the geomagnetic vector are substantially constant around the observation point, the observation values a ₀ and h ₀ in the reference posture coincide with the observation values a (t) and h (t) at time t, respectively. When the object is rotated from the reference posture, the posture parameters (θ (t), φ (t), ξ (t)) of the object at time t can be obtained.

Assuming that the activity section in which the object is moving is t ₁ ≦ t ≦ t ₂ , the stationary posture estimation unit 13 determines the stationary section ST ₁ immediately before the activity section (more precisely, the stationary state at the tip time t ₁ of the activity section). From the observed values a (t ₁ ), h (t ₁ ) at this time and the observed values a ₀ , h ₀ at the reference posture, the object posture parameters (θ (t ₁ ), φ (t ₁ ), ξ (t ₁ )), and the observed values a (t ₂ ), h (t ₂ ) for the stationary section ST ₂ immediately after the activity section (more precisely, the stationary state at the end time t ₂ of the activity section). ) And the observed values a ₀ and h ₀ at the reference posture, the object posture parameters (θ (t ₂ ), φ (t ₂ ), ξ (t ₂ )) are obtained (step S 4 in FIG. 3).

The motion posture estimation unit 14 estimates the posture of the object at each time in the activity section by performing interpolation from the posture of the object in the stationary section ST ₁ and the posture of the object in the stationary section ST ₂ (step S5 in FIG. 3). . In this embodiment, linear interpolation is sufficient for this interpolation. However, in this embodiment, attention is paid to the fact that human actions are involved in the movement of objects in daily life space. Interpolate based on a jerk minimum model known as approximation of hand movement.

  The jerk minimum model is obtained as a result of observing the movement of a person's hand, and the person instinctively moves his hand along a trajectory that minimizes jerk (the degree of jump, ie, time derivative of acceleration). It shows that. Intuitively, the movement of a human hand draws a trajectory in which the acceleration changes smoothly, and the amount of movement at that time is expressed as a quintic equation with respect to time. The motion posture estimation unit 14 performs interpolation using the jerk minimum model for each of the posture parameters (θ, φ, ξ).

Let x (t) be the position of the object at time t. Assuming that x (t) can be differentiated n times with respect to t, x (t) differentiated n times is represented as x ⁽ⁿ⁾ (t). Thus, the velocity of the object at time t can be expressed as x ⁽¹⁾ (t), the acceleration can be expressed as x ⁽²⁾ (t), and the recoil degree can be expressed as x ⁽³⁾ (t). When the recoil degree is minimum, x ⁽⁵⁾ (t) = 0 holds for the second derivative.

Similarly to the above, the activity section is set to t ₁ ≦ t ≦ t ₂ . In the stationary state at the front end time t ₁ of the activity section and the stationary state at the end time t ₂ , both the velocity and the acceleration are 0. Therefore, the position of the object at time t ₁ is x ₁ = x (t ₁ ), and time t ₂ X (t) can be defined as a solution of the following equation: x ₂ = x (t ₂ ).

x ⁽⁵⁾ (t) = 0 (1)
x (t ₁ ) = x ₁ , x ⁽¹⁾ (t ₁ ) = 0, x ⁽²⁾ (t ₁ ) = 0 (2)
x (t ₂ ) = x ₂ , x ⁽¹⁾ (t ₂ ) = 0, x ⁽²⁾ (t ₂ ) = 0 (3)

The solution of this equation is the following quintic equation for t.
x (t) = x ₁ + (x ₂ −x ₁ ) (10 (t / (t ₂ −t ₁ )) ³
−15 (t / (t ₂ −t ₁ )) ⁴ +6 (t / (t ₂ −t ₁ )) ⁵ ) (4)

Exercise posture estimation unit 14, an interpolation based on the above jerk minimum principle posture parameter theta, phi, by applying independently to xi], posture parameter θ (t ₁₎ at time t _1, phi (t _{1), ξ} (t ₁₎ and, the attitude parameters at time _{t 2 θ (t 2),} φ (t 2), ξ ( since t ₂₎ and, of the activity interval time _{t (t 1 ≦ t ≦ t} 2) The posture parameters θ ^ (t), φ ^ (t), and ξ ^ (t) can be obtained (step S5 in FIG. 3).

Next, the gravitational acceleration estimation unit 15 estimates the gravitational acceleration in the posture of the object estimated by the motion posture estimation unit 14 for each time of the activity section (step S6 in FIG. 3). The gravitational acceleration estimation unit 15 uses the posture parameters θ ^ (t), φ ^ (t), and ξ ^ (t) at the time t (t ₁ ≦ t ≦ t ₂ ) of the activity section, and the gravitational acceleration in the stationary posture By rotating the vector a ₀ , the gravitational acceleration g ^ (t) to be observed with the posture of the object at time t can be obtained.

The motion acceleration estimation unit 16 estimates the acceleration of the object caused by the external force contributing to the motion at each time in the activity section (step S7 in FIG. 3). The motion acceleration estimation unit 16 detects the acceleration vector a (t) = (a _x (t), a _y (a) (the sensor node 2 attached to the object observed at the time t (t ₁ ≦ t ≦ t ₂ ) of the activity section. By subtracting the gravitational acceleration g ^ (t) estimated by the gravitational acceleration estimator 15 from t), a _z (t)), the acceleration a ^ (t) of the object related to the instantaneous motion can be obtained. .
a ^ (t) = a (t) -g ^ (t) (5)

  Finally, the movement trajectory estimation unit 17 integrates the object acceleration a ^ (t) obtained by the motion acceleration estimation unit 16 with respect to time, so that the object movement speed v (t) and the movement amount x ( t) is calculated (step S8 in FIG. 3). The movement trajectory estimation unit 17 can obtain the movement speed v (t) by integrating the movement acceleration a ^ (t) obtained by the movement acceleration estimation unit 16 with respect to the time t, and further calculates the movement speed v (t). The amount of movement x (t) can be obtained by integrating.

  As described above, in the present embodiment, the posture and movement trajectory of an object during movement can be obtained.

  Note that the posture / movement trajectory detection apparatus of the present embodiment can be realized by a computer having a CPU, a storage device, and an external interface, and a program for controlling these hardware resources. In such a computer, the attitude / movement locus detection program for realizing the attitude / movement locus detection method of the present invention is recorded in a recording medium such as a flexible disk, a CD-ROM, a DVD-ROM, or a memory card. Provided in. The CPU writes the program read from the recording medium into the storage device, and executes processing as described in this embodiment according to the program.

  The present invention can be applied to a technique for detecting the posture and movement trajectory of an object in motion using sensor data from a sensor node attached to the object.

It is a block diagram which shows the structure of the attitude | position and movement trace detection apparatus which concerns on embodiment of this invention. It is a block diagram which shows the structure of the sensor node which concerns on embodiment of this invention. It is a flowchart which shows operation | movement of the attitude | position and movement trace detection apparatus which concerns on embodiment of this invention.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Posture / movement trajectory detection device, 2 ... Sensor node, 10 ... Server, 11 ... Activity detection unit, 12 ... Static section search unit, 13 ... Static posture estimation unit, 14 ... Motion posture estimation unit, 15 ... Gravity acceleration estimation 16, motion acceleration estimation unit, 17 movement trajectory estimation unit, 20 sensor, 21 communication module, 22 memory, 23 processor.

Claims (6)

In an attitude / movement trajectory detection device for detecting the attitude and movement trajectory of an object to which a sensor node is attached,
Activity detecting means for detecting an activity section in which an object is moving based on sensor data received from a sensor node;
When the activity section is detected, a stationary section search means for searching for a stationary section before and after the activity section,
Static posture estimation means for estimating the posture of an object for each of the static intervals from sensor data of the static interval before and after the activity interval;
Based on the posture of the object in the stationary section before and after the activity section, the motion posture estimation means for estimating the posture of the object at each time of the activity section by interpolation,
Gravity acceleration estimating means for estimating the gravitational acceleration in the posture of the object estimated by the motion posture estimating means for each time of the activity section;
Motion acceleration estimation means for estimating the acceleration of an object caused by an external force from the sensor data and the gravitational acceleration estimated by the gravitational acceleration estimation means for each time of the activity section;
A movement trajectory estimating means for obtaining a moving speed and a moving amount of the object in the activity section from the acceleration of the object estimated by the motion acceleration estimating means ,
The motion posture estimation means applies the jerk minimum principle independently to the three-axis posture parameters estimated by the stationary posture estimation means when performing the interpolation, and the three axes of the object at each time in the activity section A posture / movement trajectory detection apparatus characterized by calculating a posture parameter of a robot. The posture / movement trajectory detecting device according to claim 1,
The posture / movement trajectory detecting apparatus, wherein the sensor data includes a three-axis acceleration observation value and a three-axis azimuth observation value. In an attitude / movement trajectory detection method for detecting the attitude and movement trajectory of an object to which a sensor node is attached,
An activity detection procedure for detecting an activity section in which an object is moving based on sensor data received from a sensor node;
When detecting the activity section, a stationary section search procedure for searching for a stationary section before and after this activity section,
A stationary posture estimation procedure for estimating the posture of an object for each stationary section from sensor data of a stationary section before and after the activity section;
Based on the posture of the object in the stationary section before and after the activity section, a motion posture estimation procedure for estimating the posture of the object at each time of the activity section by interpolation,
A gravitational acceleration estimation procedure for estimating the gravitational acceleration in the posture of the object estimated in the motion posture estimation procedure for each time of the activity section;
A motion acceleration estimation procedure for estimating an acceleration of an object caused by an external force from the sensor data and the gravitational acceleration estimated by the gravitational acceleration estimation procedure for each time of the activity section;
A movement trajectory estimation procedure for obtaining the movement speed and movement amount of the object in the activity section from the acceleration of the object estimated by the motion acceleration estimation procedure ,
In the motion posture estimation procedure, when performing the interpolation, the jerk minimum principle is independently applied to the three axis posture parameters estimated in the stationary posture estimation procedure, and the three axes of the object at each time of the activity section are applied. A posture / movement trajectory detection method characterized by calculating a parameter of a posture. In the posture / movement trajectory detection method according to claim 3 ,
The sensor data includes a three-axis acceleration observation value and a three-axis azimuth observation value. In an attitude / movement trajectory detection program for operating a computer as an attitude / movement trajectory detection device for detecting the attitude and movement trajectory of an object to which a sensor node is attached,
An activity detection procedure for detecting an activity section in which an object is moving based on sensor data received from a sensor node;
When detecting the activity section, a stationary section search procedure for searching for a stationary section before and after this activity section,
A stationary posture estimation procedure for estimating the posture of an object for each stationary section from sensor data of a stationary section before and after the activity section;
Based on the posture of the object in the stationary section before and after the activity section, a motion posture estimation procedure for estimating the posture of the object at each time of the activity section by interpolation,
A gravitational acceleration estimation procedure for estimating the gravitational acceleration in the posture of the object estimated in the motion posture estimation procedure for each time of the activity section;
A motion acceleration estimation procedure for estimating an acceleration of an object caused by an external force from the sensor data and the gravitational acceleration estimated by the gravitational acceleration estimation procedure for each time of the activity section;
Causing the computer to execute a movement trajectory estimation procedure for obtaining the movement speed and movement amount of the object in the activity section from the acceleration of the object estimated in this motion acceleration estimation procedure ;
In the motion posture estimation procedure, when performing the interpolation, the jerk minimum principle is independently applied to the three axis posture parameters estimated in the stationary posture estimation procedure, and the three axes of the object at each time of the activity section are applied. A posture / movement trajectory detection program characterized by calculating parameters of the posture of the robot . 6. A recording medium on which the posture / movement locus detection program according to claim 5 is recorded.

Images