CN108627153B - Rigid body motion tracking system based on inertial sensor and working method thereof - Google Patents
Rigid body motion tracking system based on inertial sensor and working method thereof Download PDFInfo
- Publication number
- CN108627153B CN108627153B CN201810448583.2A CN201810448583A CN108627153B CN 108627153 B CN108627153 B CN 108627153B CN 201810448583 A CN201810448583 A CN 201810448583A CN 108627153 B CN108627153 B CN 108627153B
- Authority
- CN
- China
- Prior art keywords
- inertial sensor
- rigid body
- data
- motion
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G08—SIGNALLING
- G08C—TRANSMISSION SYSTEMS FOR MEASURED VALUES, CONTROL OR SIMILAR SIGNALS
- G08C17/00—Arrangements for transmitting signals characterised by the use of a wireless electrical link
- G08C17/02—Arrangements for transmitting signals characterised by the use of a wireless electrical link using a radio link
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- User Interface Of Digital Computer (AREA)
Abstract
The invention discloses a rigid body motion tracking system based on an inertial sensor and a working method thereof. The inertial sensor node comprises an inertial sensor, a calculation module and a wireless communication module. The data acquisition processing terminal comprises a wireless communication module and a computer system. The user arranges the inertial sensor nodes on the rigid body, and the nodes acquire sensor data in real time and transmit the sensor data to the data acquisition and processing terminal in a wireless manner. When the movement is finished, the computer system executes a movement tracking and movement analysis algorithm, preprocesses data, obtains the acceleration of the inertial sensor node, calculates the speed and the movement track of the inertial sensor node, restores the movement tracks of other points on the rigid body based on the posture of the inertial sensor node and the relative position relation of the other points and the sensor node, and finally can present the movement track of the selected point of the rigid body and analyze the translation and the rotation generated in the movement process.
Description
Technical Field
The invention relates to sensing identification, motion tracking and motion analysis of an inertial sensor, in particular to a rigid motion tracking system based on the inertial sensor and a working method thereof.
Background
The motion tracking refers to tracking the motion state of a target object, and mainly comprises measuring, tracking and recording the motion track of the target object in a three-dimensional space. Current motion tracking solutions are mainly based on computer graphics and image processing techniques, positioning techniques such as ultrasound, radar, laser, and inertial sensors. The most popular of these solutions are generally based on computer graphics and image processing techniques. The method comprises the steps of recording the motion state of a moving object (usually, a mark node is arranged) in an image mode through a plurality of cameras arranged in a three-dimensional space, and processing the image data by a computer system to finally obtain the space coordinates (X, Y and Z) of the moving object at different moments. Computer vision based solutions can achieve very high accuracy, but require large-scale deployment, are easily disturbed by visual factors, and have very large deviations in accuracy when moving objects are occluded.
With the development of inertial sensors and wearable computing technologies, motion tracking technologies based on wearable inertial sensors and smart terminals are also receiving much attention. By means of the wearable inertial sensor nodes, a user can acquire inertial sensor data in real time, and the inertial sensor data are processed by a motion tracking and motion analysis algorithm on the terminal equipment, so that the motion process of a target rigid body can be tracked and analyzed. At present, some motion tracking work based on an inertial sensor mainly focuses on restoring a motion attitude or a motion track of a target object, analysis of a conversion process between different states in a motion process is lacked, and how to take motion tracking and motion analysis into consideration is a topic worthy of being researched.
Disclosure of Invention
In order to solve the technical problems, the invention provides a rigid body motion tracking system based on an inertial sensor and a working method thereof, wherein the system and the method only need one inertial sensor node and a data acquisition and processing terminal, are convenient to deploy and simple to operate, can relatively and accurately capture the motion process of a rigid body attached with the inertial sensor node, acquire the motion acceleration and speed information of the rigid body at each moment, reduce the motion tracks of a plurality of selected points on the rigid body, establish a mathematical model for the motion process of the rigid body, and reliably analyze the translation and rotation generated in the motion process.
In order to achieve the purpose, the technical scheme of the invention is as follows:
the invention relates to a rigid body motion tracking system based on an inertial sensor, which comprises:
an inertial sensor node: various inertial sensors are arranged in the device, have certain calculation, storage and communication capabilities, and can be conveniently attached to the surface of a rigid body.
Data acquisition processing terminal: and the computer system with calculation, storage and communication capabilities receives various sensor data transmitted from the inertial sensor nodes, stores and processes the sensor data, finally generates the trajectory of the rigid motion and the analysis result of translation and rotation generated in the rigid motion process, and displays the analysis result to a user.
The rigid body is an object which has basically unchanged shape and size and unchanged relative position of each point in the rigid body or negligibly changed degree in the motion process and after stress action. The rigid body motion refers to the rotation and translation of a rigid body in a three-dimensional space, and can be understood as the mapping from one set of three-dimensional coordinates to another set of three-dimensional coordinates in the three-dimensional space. In addition, the motion tracking refers to tracking the motion state of the target object, and mainly comprises measuring, tracking and recording the motion track of the target object in a three-dimensional space.
Inertial sensor node fixed mounting is in rigid body surface, and it includes inside:
an inertial sensor module: including accelerometers, gyroscopes and magnetometers, for measuring corresponding inertial sensor data in real time.
A calculation module: the method is used for processing the original data of the inertial sensor, acquiring the attitude information of nodes of the inertial sensor in real time through a built-in signal filtering algorithm, estimating the components of the gravity acceleration on three axes of a reference coordinate system of the inertial sensor, and subtracting the original acceleration data to acquire the linear acceleration.
A wireless communication module: the data acquisition and processing terminal is used for information interaction with the terminal equipment, mainly used for receiving instructions of the data acquisition and processing terminal and periodically sending sensor data to the terminal equipment.
Further, the data acquisition processing terminal mainly comprises:
a wireless communication module: the method is used for information interaction with the inertial sensor nodes.
A computer system: the system is a computer system with calculation, storage and communication capabilities, receives inertial sensor data as input through a wireless communication module, executes a motion tracking and motion analysis algorithm, acquires acceleration and speed information of a rigid body at each moment, calculates a track of rigid body motion, and analyzes the conditions of translation and rotation of the rigid body in the motion process.
Based on the tracking system, the invention also introduces a working method, which comprises the following steps:
1) the user places a single inertial sensor node on the rigid body in preparation for triggering rigid body motion.
2) And the user establishes wireless connection between the inertial sensor node and the data acquisition and processing terminal.
3) A user starts data acquisition at the data acquisition and processing terminal, the inertial sensor node starts to acquire various sensor data in real time and periodically sends the sensor data to the data acquisition and processing terminal, and rigid body motion starts.
4) And (5) ending the rigid body motion, and ending data acquisition at the data acquisition and processing terminal by the user.
5) The data acquisition processing terminal executes a motion tracking and motion analysis algorithm through a computer system, obtains the acceleration and speed information of the rigid body at each moment, generates the track of the rigid body motion and the analysis result of translation and rotation generated in the rigid body motion process, and displays the analysis result to a user.
Further, the motion tracking and motion analysis algorithm in the step 5) comprises the following steps:
and 5.1, preprocessing data, namely performing smoothing operation on the original sensor data, then calculating the amplitude of the three-axis linear acceleration data and the amplitude of the gyroscope data at each moment, and determining the starting time and the ending time of the movement by using a threshold judgment method. Specifically, at the moment of the start of the movement, the amplitudes of the two types of sensor data exceed a threshold value respectively, and at the moment of the end of the movement, the amplitudes of the two types of sensor data return to be below the corresponding threshold values respectively. And selecting a reasonable threshold value through a large amount of experimental data, so that the data corresponding to the motion process can be accurately extracted. And then converting the extracted linear acceleration data into a reference coordinate system, wherein the conversion of the reference coordinate system is to use a gravity acceleration vector
And magnetometer data vectors
Constructing a geographical coordinate system, in particular firstly because
Vector pointing to the north, so that on the horizontal plane, it points to the geographical east
Can be solved by:
and due to
Not strictly horizontal, so the vector pointing to the geodetic north on the horizontal plane is obtained as follows
Then
The three orthogonal vectors can form a geographic coordinate system, and after the three vectors are respectively normalized, a direction cosine matrix DCM can be constructed, so that the conversion from the self coordinate system of the inertial sensor node to the geographic coordinate system can be realized:
(Xg,Yg,Zg)’＝DCM×(X,Y,Z)’
wherein (X)g,Yg,Zg) The (X, Y, Z) is raw data based on the coordinate system of the inertial sensor node.
According to the requirements of specific application scenes, the geographic coordinate system can be transformed once again by using the same method to obtain a reference coordinate system corresponding to the application scenes, and finally linear acceleration data relative to a fixed reference coordinate system in the motion process can be obtained.
And 5.2, executing motion tracking, and updating the motion state of the inertial sensor node at the current moment based on the motion state of the inertial sensor node at the previous moment and the inertial sensor data at the current moment, so as to obtain the relative position and posture information of the inertial sensor node in the three-dimensional space at each moment.
And 5.3, executing track reduction, and estimating the positions of the selected points on the rigid body according to the relative position and the posture of the inertial sensor node in the space at the current moment and the relative position relation between the selected points on the rigid body and the inertial sensor node, so as to reduce the motion tracks of a plurality of points on the rigid body.
And 5.4, executing motion analysis, selecting a motion track of a certain plane in a rigid body, analyzing the motion process of a selected time slice length, establishing a mathematical model, equivalently decomposing continuous motion into a combination of translation and rotation, solving the rotation condition of the motion by using a quaternion method, and finally outputting the translation and rotation conditions generated in the motion process, wherein the translation is represented by a vector of a three-dimensional space, and the rotation can be represented by a quaternion (comprising a rotating shaft and a rotating angle).
Further, the motion state of the inertial sensor node in step 5.2 mainly includes the velocity, the relative position and the posture of the inertial sensor node in the space, and the specific motion state updating step includes:
step 5.2.1 relative position information update: the motion tracking algorithm takes the position of the inertial sensor node starting to move as the origin of a reference coordinate system corresponding to an application scene, firstly integrates the linear acceleration once in a time domain to obtain the speed information of each moment, and then compensates the speed by using error correction means such as zero speed correction. And then, continuously performing integration on the compensated speed on a time domain once, solving the displacement generated at adjacent moments, and updating the relative position (namely the three-dimensional coordinate) of the inertial sensor node in the space at each sampling moment.
Step 5.2.2 attitude information update: in the initial stage of movement, a matrix containing attitude information of nodes of the inertial sensor is constructed by instantaneous data of the magnetometer and the gravitational acceleration and is used as an initial attitude of the nodes of the inertial sensor, a rotation matrix from a local coordinate system of the nodes in the initial stage to a geographic coordinate system is recorded, in the subsequent stage, the initial attitude is updated by using one-time integration of data of the gyroscope in a time domain, and the attitude measured by the magnetometer and the gravitational acceleration is calibrated at some proper time points, so that the attitude information of the nodes of the inertial sensor is updated at each sampling moment.
The invention provides a rigid body motion tracking system based on an inertial sensor and a working method thereof, which are lighter in weight, easy to deploy and low in cost and are not limited by visual factors compared with a motion tracking system based on computer vision. Compared with other motion tracking systems based on the inertial sensor of the same type, the system can restore the motion tracks of a plurality of points on the rigid body and give a reliable motion analysis result.
The beneficial effects of the invention are as follows:
1. a new method for analyzing the motion process of a rigid body in a three-dimensional space is provided: besides analyzing the motion trail, the analysis of translation and rotation conditions in the motion process is introduced. The motion of the rigid body in the three-dimensional space is decomposed into the combination of translation and rotation by establishing a geometric model, so that the result of motion analysis is more intuitive.
2. The method can realize the reduction of the motion tracks of a plurality of selected points on the rigid body: the method comprises the steps of constructing a fixed reference coordinate system corresponding to an application scene, and fully utilizing real-time attitude information of the inertial sensor and the relative position relation between a selected point on the rigid body and nodes of the inertial sensor to restore the motion tracks of a plurality of selected points on the rigid body.
3. Easy deployment: a large number of cameras and cables do not need to be deployed around a target in advance, and a user only needs to arrange the inertial sensor nodes on a rigid body to be tracked.
4. The cost is relatively low: the rigid motion process can be tracked and analyzed by only one common data acquisition and processing terminal and the inertial sensor node with relatively low cost without purchasing high-cost cameras, switches, cables and other equipment.
Drawings
FIG. 1 is a diagram of an inertial sensor-based rigid body motion tracking system architecture.
Fig. 2 is a flow chart of a motion tracking and motion analysis algorithm of an inertial sensor-based rigid body motion tracking system.
Fig. 3 is a schematic diagram of a reference coordinate system related to a rigid body motion tracking system working method based on an inertial sensor.
FIG. 4 is a diagram of the relative position of a selected point on a rigid body and an inertial sensor node.
FIG. 5 is a schematic representation of rigid body motion.
FIG. 6 is a schematic diagram of modeling analysis of rigid body motion process.
Detailed Description
In order to facilitate understanding for those skilled in the art, the present invention will be further described with reference to the accompanying drawings.
Fig. 1 is an architecture diagram of a rigid body motion tracking system based on inertial sensors, which mainly comprises two parts, namely an inertial sensor node attached to a rigid body and a data acquisition and processing terminal, wherein the inertial sensor node and the data acquisition and processing terminal are mainly used for data interaction through bluetooth wireless communication.
The inertial sensor node is fixedly arranged on the surface of the rigid body and can be selectively adhered to the surface of the rigid body, so that the relative position relation between the inertial sensor module and the rigid body is fixed. The inertial sensor node comprises an inertial sensor, a calculation module and a communication module. The inertial sensor comprises an accelerometer, a gyroscope and a magnetometer and is used for measuring corresponding inertial sensor data in real time; the calculation module is used for processing the data of the original sensor, acquiring the attitude information of the nodes of the inertial sensor in real time through a built-in signal filtering algorithm, estimating the components of the gravity acceleration on three axes of a reference coordinate system of the inertial sensor, and subtracting the original acceleration data to acquire the linear acceleration; the wireless communication module is used for information interaction with the terminal equipment, mainly receiving instructions of the data acquisition and processing terminal and periodically sending sensor data to the terminal equipment.
The data acquisition processing terminal mainly comprises a wireless communication module and a computer system. The wireless communication module is used for carrying out information interaction with the inertial sensor node; the computer system is a computer system with calculation, storage and communication capabilities, receives inertial sensor data as input through a wireless communication module, executes a motion tracking and motion analysis algorithm, calculates the track of rigid motion, and analyzes the conditions of translation and rotation of the rigid in the motion process.
Fig. 2 is a flow chart of a motion tracking and motion analysis algorithm, which mainly includes data preprocessing, motion tracking, trajectory reduction and motion analysis.
The data preprocessing part mainly scans the data of the linear acceleration and the gyroscope sensor from front to back according to the time sequence and accurately finds out the moments of the start and the end of the movement. The sensor data is first smoothed and a simple mean smoothing may be used to filter out some of the high frequency noise. And then respectively calculating the amplitudes AccM and Gyro M of the three-axis linear acceleration and the gyroscope data, namely respectively solving the 2-norm of the two three-dimensional vectors. Here, through analysis of a large amount of previous experimental data, we have a threshold thresA and thresG respectively for the amplitude of two types of triaxial sensor data, and we count the ratio of the sensor data in the window to be larger than and smaller than the corresponding threshold by setting a fixed-length sliding window. When the ratio of the amplitudes of the two sensor data smaller than the set threshold is larger than p (the system is set as 80%), the current window motion state is not active, and when the ratio of the amplitude of one sensor data larger than the set threshold is larger than p, the current window motion state is active. We find the window for the first inactive state to transition to the active state and then set the motion start time tsThen, the window of transition from active state to inactive state is searched backwards, and the termination of motion is setTime teSo far we only need to intercept tsAnd teAnd then, converting the intercepted linear acceleration data into a reference coordinate system as the input of the backward part of the algorithm according to the sensor data. The conversion of the reference coordinate system here means using the gravitational acceleration vector
And magnetometer data vectors
Constructing a geographical coordinate system, in particular, firstly due to
Vector pointing to the north, so that on the horizontal plane, it points to the geographical east
Can be solved by:
and due to
Not strictly horizontal, so the vector pointing to the geodetic north on the horizontal plane is obtained as follows
Then
The three orthogonal vectors can form a geographic coordinate system, and the three vectors can be normalized respectively to construct directionsThe cosine matrix DCM can realize the conversion from the coordinate system of the inertial sensor node to the geographic coordinate system:
(Xg,Yg,Zg)’＝DCM×(X,Y,Z)’
wherein (X)g,Yg,Zg) The (X, Y, Z) is raw data based on the coordinate system of the inertial sensor node.
According to the requirements of specific application scenes, the geographic coordinate system can be transformed once again by using the same method to obtain a reference coordinate system corresponding to the application scenes, and finally linear acceleration data relative to a fixed reference coordinate system in the motion process can be obtained.
The motion tracking part is mainly used for updating the motion trail of the nodes of the inertial sensor by utilizing linear acceleration data and gyroscope data which are generated in a preprocessing stage and are based on a reference coordinate system corresponding to an application scene, and meanwhile, generating the attitude information of the nodes of the inertial sensor at each sampling moment. Specifically, in a track updating part, an algorithm takes a position where an inertial sensor node starts to move as an origin of a reference coordinate system corresponding to an application scene, linear acceleration is integrated for one time in a time domain to obtain speed information of each moment, errors are accumulated due to integration because noise exists in linear acceleration data, and the speed is corrected by using error correction means such as zero-speed correction. And then, continuously performing integration on the compensated speed on a time domain for one time, solving the displacement generated at the adjacent moment, and updating the relative position (namely the three-dimensional coordinate) of the inertial sensor node in the space. The zero-speed correction is mainly based on the assumption that the speed of the inertial sensor node is zero at the motion termination moment, so that when the linear acceleration is subjected to first integration to obtain the speed at each moment, the speed can be compensated, and the accumulated error can be reduced. And in the attitude updating part, a matrix containing attitude information of nodes of the inertial sensor is constructed by instantaneous data of the magnetometer and the gravity acceleration in the initial stage of motion as the initial attitude of the nodes of the inertial sensor, a rotation matrix from a local coordinate system of the nodes to a geographic coordinate system in the initial stage is recorded, the initial attitude is updated by using the first integral of the data of the gyroscope in the time domain in the subsequent stage, and the attitude measured by the magnetometer and the gravity acceleration is calibrated at some proper time points to eliminate the accumulated error brought by the gyroscope, so that the attitude information of the nodes of the inertial sensor is updated. The appropriate point in time as referred to herein is determined primarily by these factors:
1) and real-time angular velocity information, wherein when the value of the angular velocity is below a certain threshold value, the attitude information measured by the magnetometer and the gravity acceleration at the moment is considered to be relatively reliable and can be used for calibration.
2) The similarity between the attitude change calculated by the gyroscope and the attitude change obtained by the magnetometer and the gravity acceleration is considered to be more reliable and can be used for calibration when the attitude changes measured by the two means are similar.
The track restoring part obtains the relative position relation between the selected point on the rigid body and the inertial sensor node under the fixed reference coordinate system corresponding to the application scene by utilizing the track information and the attitude information of the inertial sensor node generated by the motion tracking part and combining the relative position relation between the selected point on the rigid body and the inertial sensor node under the self reference coordinate system of the node obtained in advance, and then estimates and restores the motion track of the selected point on the rigid body. The relative position relationship between the selected points on the rigid body and the nodes of the inertial sensor can be specifically referred to the description of fig. 4 below.
The motion analysis part takes the track of the selected point on the rigid body output by the track reduction part as input and analyzes by using the established mathematical model of the rigid body motion process. Specifically, we select three points on a plane on the rigid body which are not on the same straight line, the motion trail of the three points is known, and the motion process corresponding to the plane is known, so that the motion process of the rigid body from one moment to another moment can be equivalently simplified into the combination of one translation and one rotation by using the established mathematical model of the motion process of the rigid body. Finally, the analysis result of the movement is givenIs a vector of translation occurring during motion
Rotating shaft for a rotating process
And an angle theta of counterclockwise rotation about the axis of rotationequal. The mathematical model of the rigid body motion process can be seen in the following description of fig. 5 and 6.
FIG. 3 is a schematic diagram of several reference coordinate systems involved in a method of operation of an inertial sensor-based rigid body motion tracking system. The left half part of the figure takes a point N as the origin of a coordinate system, XNYNZNThe coordinate system with coordinate axes is the coordinate system of the inertial sensor node itself, the raw sensor data generated by the inertial sensor node is based on the coordinate system, and the coordinate system with point O as the origin of the coordinate system and XYZ as the coordinate axes is the reference coordinate system corresponding to the application scenario described above, which is only one possible case of the above, and the reference coordinate system corresponding to the specific application scenario may have different definitions. The right half of the figure shows the geographical coordinate system, and we can see that the X-axis points in the geographical east, the Y-axis in the geographical north, and the Z-axis in the opposite direction to gravity. In the actual algorithm process, the process of converting the reference coordinate system is to convert the linear acceleration data from the coordinate system of the inertial sensor node to the geographic coordinate system, and then convert the linear acceleration data from the geographic coordinate system to the reference coordinate system corresponding to the application scene.
Fig. 4 is an illustration of the relative positional relationship of selected points on a rigid body to nodes of an inertial sensor. Point N in the figure represents an inertial sensor node, and O, A, B, C four points which are not in the same plane are selected on a rigid body as an example, and the coordinate system given in the figure is the coordinate system of the inertial sensor node. For simplicity, point N is taken here as the coordinate system origin. In this coordinate system, since the shape and size of the rigid body itself and the deployment position of the inertial sensor are known, the coordinates of the point O, A, B, C, N can be calculated, i.e. the four coordinates shown in the figure(Vector)
Can be measured and calculated. Therefore, the relative position of the selected rigid body point O, A, B, C in the three-dimensional space can be estimated through the relative position of the inertial sensor node N in the three-dimensional space. Specifically, the relative position relationship between the selected point and the inertial sensor node (i.e., four vectors) is determined at each time
) The coordinate system of the node itself is converted into a geographic coordinate system, and then converted into a reference coordinate system corresponding to the application scenario shown in fig. 3. The motion trail of the inertial sensor node N (i.e. the relative position of the inertial sensor node in the three-dimensional space at each moment) solved by the motion tracking part is based on the reference coordinate system corresponding to the application scene, so that four vectors in the reference coordinate system corresponding to the application scene can be utilized
The solution point O, A, B, C has coordinates in the reference coordinate system corresponding to the application scenario, and finally the trajectory of the selected point on the rigid body can be restored.
Fig. 5 is an example of the process of rigid body motion, where a rigid body is represented by four points on the rigid body that are not in the same plane, without loss of generality. We can see that the starting position of the four vertices of the rigid body in the figure is O, A, B, C, and after moving, the positions of the corresponding vertices are transformed to O ', a', B ', C', which is a generalized representation of the states before and after the rigid body movement, and as mentioned above, is equivalent to the mapping of one set of three-dimensional coordinates (O, A, B, C) to another set of three-dimensional coordinates (O ', a', B ', C').
Fig. 6 is a schematic diagram of analysis modeling of a rigid body motion process, where the motion process of the rigid body is relatively complex, but we can perform equivalent transformation on the motion process in the analysis process under the condition of ensuring that the states before and after the motion are unchanged so as to achieve the purpose of simplifying the motion process. We will be likeThe motion process of the rigid body between adjacent time slices is decomposed into translation and rotation, which needs to be described by establishing a geometric model. FIG. 6(a) is the first step in the decomposition of the motion process, where we select a point (O) on the rigid body that points to the vector of the corresponding point (O') after the motion
I.e. the vector of the rigid body making translational motion. After translation, the rigid body position is transformed to O, as shown in FIG. 6(b)1、A1、B1、C1In which O is1Coinciding with O', in a second step we need to put the edge O of the rigid body1A1Rotating about the axis of point O ' to coincide with O ' A ', the axis of rotation
The calculation of (d) is as follows:
and the angle of rotation theta0May be composed of vectors
And vector
And (5) dot product acquisition. After the first rotation, the rigid body position is converted to O, as shown in FIG. 6(c)2、A2、B2、C2While being O2A2Coinciding with the edge O 'a'. At this time, we select and O2、A2Third point B not on the same straight line2In this step, the rigid body needs to be wound around a shaft
(conversion to Unit vector)
) Rotated to the plane O2A2B2Coplanar with plane O ' A ' B ', by angle of rotation theta1The acquisition of the included angle between the two planes can be directly calculated. Specifically, this is done by first solving the normal vectors of the two planes shown in FIG. 6(c)
And
then solving the included angle theta between the two normal vectors to obtain the included angle theta between the two planes1. Finally, graph (d) shows the rigid body position O after the second rotation3、A3、B3、C3I.e. positions O ', a', B ', C'. The rigid motion decomposition process of one translation and two rotations shown in fig. 4 is relatively intuitive, in the actual calculation process, the translation transformation process can be realized by adding and subtracting vectors in the translation process, and the rotation transformation process can be solved by using a quaternion method. In addition, the quaternion method can be used for compounding the two rotations, so that the motion process of the rigid body is simplified into translation and rotation around a fixed shaft. We take quaternions
And
representing two rotations in the above-described process,
is a quaternion representation of the coordinates of the rotation front point,
is a quaternion representation of the point coordinates after the first rotation,
quaternion representation of point coordinates after the second rotation, the specific rotationThe compounding process is as follows:
The present invention has many applications, and the above embodiment is only one preferred embodiment of the present invention, and thus the present invention is not limited to the above embodiment. Without departing from the principles of the invention, one skilled in the art may devise other embodiments that incorporate the teachings of the invention and which are also considered to be within the scope of the invention.
Claims (4)
1. An inertial sensor-based rigid body motion tracking system, the system comprising:
an inertial sensor node: various inertial sensors are built in, the sensors have the calculation, storage and communication capabilities, sensor data are periodically sent to a data acquisition and processing terminal, and nodes of the inertial sensors are attached to the surface of a rigid body;
data acquisition processing terminal: the computer system with calculation, storage and communication capabilities receives various sensor data transmitted from the inertial sensor nodes, stores and processes the sensor data, finally generates the track of rigid motion and the analysis result of translation and rotation generated in the rigid motion process, and displays the analysis result to a user;
the rigid body motion tracking refers to tracking the motion state of a target rigid body, and comprises measuring, tracking and recording the motion track of the target rigid body in a three-dimensional space;
inertial sensor node fixed mounting is in rigid body surface, and it includes inside:
an inertial sensor module: the inertial sensor module can be selectively adhered to the surface of the rigid body, so that the relative position relation between the inertial sensor module and the rigid body is fixed, and the inertial sensor module comprises a three-axis accelerometer, a gyroscope and a magnetometer and is used for measuring corresponding inertial sensor data in real time;
a calculation module: the system is used for processing the original data of the inertial sensor, acquiring the attitude information of nodes of the inertial sensor in real time through a built-in signal filtering algorithm, estimating the components of the gravity acceleration on three axes of a reference coordinate system of the inertial sensor, and subtracting the original acceleration data to acquire the linear acceleration;
the calculation module converts the extracted linear acceleration data into a reference coordinate system, wherein the conversion of the reference coordinate system is realized by utilizing a gravity acceleration vector
And magnetometer data vectors
Constructing a geographical coordinate system;
firstly, due to
Vector pointing to the north, so that on the horizontal plane, it points to the geographical east
Can be used forThe solution is given by:
the vector pointing to the geographical north of the earth on the horizontal plane is obtained according to the following formula
Then
The three orthogonal vectors can form a geographic coordinate system, and after the three vectors are respectively normalized, a direction cosine matrix DCM is constructed to realize the conversion from the self coordinate system of the inertial sensor node to the geographic coordinate system:
(Xg,Yg,Zg)’＝DCM×(X,Y,Z)’
wherein (X)g,Yg,Zg) The data are sensor data under a geographic coordinate system, and (X, Y and Z) are original data based on the self coordinate system of the inertial sensor node;
the sensor wireless communication module: the method is used for information interaction with the terminal equipment.
2. An inertial sensor-based rigid body motion tracking system according to claim 1, wherein said data acquisition and processing terminal comprises:
the processing terminal wireless communication module: the inertial sensor node is used for information interaction with the inertial sensor node;
a computer system: the system is characterized in that the system is a computer with calculation, storage and communication capabilities, receives inertial sensor data as input through a processing terminal wireless communication module, executes a motion tracking and motion analysis algorithm, acquires acceleration and speed information of a rigid body at each moment, calculates a track of the motion of the rigid body, and analyzes the conditions of translation and rotation of the rigid body in the motion process.
3. A rigid body motion tracking system working method based on an inertial sensor is characterized by comprising the following steps:
1) a user arranges a single inertial sensor node on a rigid body to prepare for triggering rigid body motion;
2) a user establishes wireless connection between the inertial sensor node and the data acquisition and processing terminal;
3) a user starts data acquisition at a data acquisition and processing terminal, an inertial sensor node starts to acquire various sensor data in real time and periodically sends the sensor data to the data acquisition and processing terminal, and rigid body motion starts;
4) after the rigid body motion is finished, the user terminates the data acquisition at the data acquisition and processing terminal;
5) the data acquisition processing terminal executes a motion tracking and motion analysis algorithm through a computer system, obtains the acceleration and speed information of the rigid body at each moment, generates the track of the rigid body motion, models and analyzes the conditions of translation and rotation in the rigid body motion process, and finally displays the conditions to a user;
the motion tracking and motion analysis algorithm in the step 5) comprises the following steps:
step 5.1, preprocessing data, namely performing smoothing operation on original sensor data, then calculating the amplitude of triaxial linear acceleration data and gyroscope data at each moment, determining the starting moment and the ending moment of movement by using a threshold judgment method, extracting data between the starting moment and the ending moment of movement, and then converting the reference coordinate system of the data to obtain linear acceleration data relative to a fixed reference coordinate system in the movement process;
converting the extracted linear acceleration data into a reference coordinate system, wherein the conversion of the reference coordinate system refers to the utilization of a gravity acceleration vector
And magnetometer data vectors
Constructing a geographical coordinate system;
firstly, due to
Vector pointing to the north, so that on the horizontal plane, the vector points to the geographic east
Can be solved by:
the vector pointing to the geographical north of the earth on the horizontal plane is obtained according to the following formula
Then
The three orthogonal vectors can form a geographic coordinate system, and after the three vectors are respectively normalized, a direction cosine matrix DCM is constructed to realize the conversion from the self coordinate system of the inertial sensor node to the geographic coordinate system:
(Xg,Yg,Zg)’＝DCM×(X,Y,Z)’
wherein (X)g,Yg,Zg) The data are sensor data under a geographic coordinate system, and (X, Y and Z) are original data based on the self coordinate system of the inertial sensor node;
step 5.2, executing motion tracking, and updating the motion state of the inertial sensor node at the current moment based on the motion state of the inertial sensor node at the previous moment and the inertial sensor data at the current moment, so as to obtain the relative position and posture information of the inertial sensor node in the three-dimensional space at each moment;
step 5.3, executing track reduction, and estimating the positions of the selected points on the rigid body by combining the relative position relation between the selected points on the rigid body and the nodes of the inertial sensor according to the relative position and the posture of the nodes of the inertial sensor in the space at the current moment, so as to reduce the motion tracks of a plurality of points on the rigid body;
step 5.4, executing motion analysis, selecting a motion track of a certain plane in a rigid body, analyzing the motion process of the selected time slice length, establishing a mathematical model, equivalently decomposing continuous motion into a combination of translation and rotation, and finally outputting the conditions of translation and rotation generated in the motion process;
the step 5.1 of converting the reference coordinate system refers to constructing a geographic reference coordinate system based on sensor data measured by the accelerometer and the magnetometer, performing primary conversion on the geographic coordinate system according to an actual application scene to obtain a reference coordinate system corresponding to the application scene, and finally converting the linear acceleration data to the reference coordinate system corresponding to the application scene.
4. The method of claim 3, wherein the rigid body motion tracking system based on inertial sensor is further characterized in that,
in the step 5.2, the motion state of the inertial sensor node mainly includes the speed, the relative position and the posture of the inertial sensor node in the space, and the motion state updating step includes:
step 5.2.1 relative position information update: the motion tracking algorithm takes the position of the inertial sensor node starting to move as the origin of a reference coordinate system corresponding to an application scene, firstly integrates the linear acceleration once in a time domain to obtain the speed information of each moment, and then compensates the speed by using a zero-speed correction error correction means; then, continuously performing primary integration on the compensated speed in a time domain, solving the displacement generated at adjacent moments, and updating the relative position of the inertial sensor node in the space;
step 5.2.2 attitude information update: in the initial stage of movement, a matrix containing attitude information of nodes of the inertial sensor is constructed by instantaneous data of the magnetometer and the gravitational acceleration and is used as an initial attitude of the nodes of the inertial sensor, a rotation matrix from a local coordinate system of the nodes in the initial stage to a geographic coordinate system is recorded, in the subsequent stage, the initial attitude is updated by using the first integral of data of the gyroscope in a time domain, and the attitude measured by the magnetometer and the gravitational acceleration is calibrated, so that the influence of accumulated errors of the gyroscope is reduced as much as possible, and the attitude information of the nodes of the inertial sensor is updated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810448583.2A CN108627153B (en) | 2018-05-11 | 2018-05-11 | Rigid body motion tracking system based on inertial sensor and working method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810448583.2A CN108627153B (en) | 2018-05-11 | 2018-05-11 | Rigid body motion tracking system based on inertial sensor and working method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108627153A CN108627153A (en) | 2018-10-09 |
| CN108627153B true CN108627153B (en) | 2022-06-07 |
Family
ID=63692625
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810448583.2A Active CN108627153B (en) | 2018-05-11 | 2018-05-11 | Rigid body motion tracking system based on inertial sensor and working method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108627153B (en) |
Families Citing this family (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109343546A (en) * | 2018-10-10 | 2019-02-15 | 上海海洋大学 | A kind of submersible six-freedom motion real-time measurement system |
| CN111307174A (en) * | 2020-02-05 | 2020-06-19 | 深圳前海达闼云端智能科技有限公司 | Calibration method of sensor, moving object and storage medium |
| DE102020105759A1 (en) | 2020-03-04 | 2021-09-09 | Ewellix AB | Sensor system for an actuator, actuator and method for moving an actuator part |
| CN111831959A (en) * | 2020-03-05 | 2020-10-27 | 北京嘀嘀无限科技发展有限公司 | Motion data processing method, motion data processing device, terminal and computer-readable storage medium |
| CN111413995B (en) * | 2020-03-24 | 2021-03-05 | 北京科技大学 | Method and system for tracking relative position and synchronously controlling posture between double rigid body characteristic points |
| CN111536966A (en) * | 2020-04-07 | 2020-08-14 | 广州体育学院 | High-precision position detection sensor |
| CN112248940B (en) * | 2020-08-27 | 2022-03-29 | 清华大学 | Sensor arrangement resolving method and system for vehicle body attitude control |
| CN112308110B (en) * | 2020-09-21 | 2023-07-21 | 哈尔滨工业大学（深圳） | Hand motion recognition method and system capable of achieving man-machine interaction |
| CN112489082A (en) * | 2020-12-03 | 2021-03-12 | 海宁奕斯伟集成电路设计有限公司 | Position detection method, position detection device, electronic equipment and readable storage medium |
| CN112595338B (en) * | 2020-12-24 | 2023-04-07 | 中国联合网络通信集团有限公司 | Navigation method and navigation system |
| JP7461913B2 (en) * | 2021-08-03 | 2024-04-04 | 富士フイルムヘルスケア株式会社 | Magnetic resonance imaging apparatus and control method thereof |
| CN113916228A (en) * | 2021-10-09 | 2022-01-11 | 台州学院 | Method for monitoring regional activities of infants |
| CN114139319B (en) * | 2021-12-13 | 2024-04-16 | 吉林大学 | Reconfigurable multifunctional numerical control processing module configuration analysis method |
| CN114562993A (en) * | 2022-02-28 | 2022-05-31 | 联想(北京)有限公司 | Track processing method and device and electronic equipment |
| CN115131874B (en) * | 2022-06-29 | 2023-10-17 | 深圳市神州云海智能科技有限公司 | User behavior recognition prediction method, system and intelligent safety helmet |
| CN115372659A (en) * | 2022-08-05 | 2022-11-22 | 东莞市汇能电子有限公司 | Method for calculating battery motion track and speed through three-axis gyroscope |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101367015A (en) * | 2007-06-18 | 2009-02-18 | 神奇游戏智能公司 | Self contained inertial navigation system for interactive control using movable controller |
| CN101579238A (en) * | 2009-06-15 | 2009-11-18 | 吴健康 | Human motion capture three dimensional playback system and method thereof |
| CN101886927A (en) * | 2010-06-25 | 2010-11-17 | 武汉大学 | Three-dimensional motion tracking system and method based on inertial sensor and geomagnetic sensor |
| EP2419057B1 (en) * | 2009-04-13 | 2014-10-01 | DEKA Products Limited Partnership | System and apparatus for orientation control |
| CN105607760A (en) * | 2015-12-18 | 2016-05-25 | 上海开圣影视文化传媒股份有限公司 | Micro-inertial sensor based track recovery method and system |
| CN107092882A (en) * | 2017-04-19 | 2017-08-25 | 南京大学 | A kind of Activity recognition system and its method of work perceived based on sub- action |
| CN107115114A (en) * | 2017-04-28 | 2017-09-01 | 王春宝 | Human Stamina evaluation method, apparatus and system |
| CN106679649B (en) * | 2016-12-12 | 2020-01-10 | 浙江大学 | Hand motion tracking system and method |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8229163B2 (en) * | 2007-08-22 | 2012-07-24 | American Gnc Corporation | 4D GIS based virtual reality for moving target prediction |
| CN106767770B (en) * | 2016-11-29 | 2020-06-19 | 西安交通大学 | User walking direction detection and tracking method based on portable intelligent device |
| CN107560613B (en) * | 2017-08-15 | 2020-03-31 | 江苏科技大学 | Robot indoor track tracking system and method based on nine-axis inertial sensor |
| CN107421537B (en) * | 2017-09-14 | 2020-07-17 | 桂林电子科技大学 | Object motion attitude sensing method and system based on rigid body grid of inertial sensor |
-
2018
- 2018-05-11 CN CN201810448583.2A patent/CN108627153B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101367015A (en) * | 2007-06-18 | 2009-02-18 | 神奇游戏智能公司 | Self contained inertial navigation system for interactive control using movable controller |
| EP2419057B1 (en) * | 2009-04-13 | 2014-10-01 | DEKA Products Limited Partnership | System and apparatus for orientation control |
| CN101579238A (en) * | 2009-06-15 | 2009-11-18 | 吴健康 | Human motion capture three dimensional playback system and method thereof |
| CN101886927A (en) * | 2010-06-25 | 2010-11-17 | 武汉大学 | Three-dimensional motion tracking system and method based on inertial sensor and geomagnetic sensor |
| CN105607760A (en) * | 2015-12-18 | 2016-05-25 | 上海开圣影视文化传媒股份有限公司 | Micro-inertial sensor based track recovery method and system |
| CN106679649B (en) * | 2016-12-12 | 2020-01-10 | 浙江大学 | Hand motion tracking system and method |
| CN107092882A (en) * | 2017-04-19 | 2017-08-25 | 南京大学 | A kind of Activity recognition system and its method of work perceived based on sub- action |
| CN107115114A (en) * | 2017-04-28 | 2017-09-01 | 王春宝 | Human Stamina evaluation method, apparatus and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108627153A (en) | 2018-10-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108627153B (en) | Rigid body motion tracking system based on inertial sensor and working method thereof | |
| CN109376785B (en) | Navigation method based on iterative extended Kalman filtering fusion inertia and monocular vision | |
| Li et al. | Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera | |
| CN109885080B (en) | Autonomous control system and autonomous control method | |
| Marins et al. | An extended Kalman filter for quaternion-based orientation estimation using MARG sensors | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| Zhou et al. | 2D human gesture tracking and recognition by the fusion of MEMS inertial and vision sensors | |
| CN111462231B (en) | Positioning method based on RGBD sensor and IMU sensor | |
| CN109313417A (en) | Help robot localization | |
| CN111091587B (en) | Low-cost motion capture method based on visual markers | |
| CN108225370B (en) | Data fusion and calculation method of motion attitude sensor | |
| US20210183100A1 (en) | Data processing method and apparatus | |
| CN111609868A (en) | Visual inertial odometer method based on improved optical flow method | |
| CN114608554B (en) | Handheld SLAM equipment and robot instant positioning and mapping method | |
| CN108734762B (en) | Motion trail simulation method and system | |
| CN112985450B (en) | Binocular vision inertial odometer method with synchronous time error estimation | |
| CN108534772B (en) | Attitude angle acquisition method and device | |
| Wenk et al. | Posture from motion | |
| CN111811421B (en) | High-speed real-time deformation monitoring method and system | |
| CN116167919A (en) | Laser point cloud data de-distortion method based on kernel ridge regression | |
| CN111610800B (en) | Loosely-coupled unmanned aerial vehicle control system | |
| Xu et al. | Towards human motion tracking: an open-source platform based on multi-sensory fusion methods | |
| CN114764830A (en) | Object pose estimation method based on quaternion EKF and uncalibrated hand-eye system | |
| Yu et al. | Research on Robot Positioning Technology Based on Multi Sensor | |
| Zhou et al. | Kinematic model aided inertial motion tracking of human upper limb |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |