CN107883951A - A kind of computational methods and terminal of underwater robot 3 d pose - Google Patents
A kind of computational methods and terminal of underwater robot 3 d pose Download PDFInfo
- Publication number
- CN107883951A CN107883951A CN201710980157.9A CN201710980157A CN107883951A CN 107883951 A CN107883951 A CN 107883951A CN 201710980157 A CN201710980157 A CN 201710980157A CN 107883951 A CN107883951 A CN 107883951A
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- acceleration
- information
- informations
- mrow
- 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.)
- Pending
Links
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The present invention relates to underwater robot technical field, more particularly to a kind of computational methods and terminal of underwater robot 3 d pose.Its method includes：S1：Establish the threedimensional model of underwater robot, and the three-dimensional space model in the sub-aqua sport space where underwater robot；S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of underwater robot；S3：According to the three axis angular rate information and three shaft angle acceleration informations of underwater robot, by complementary filter algorithm, three axle rotary angle informations of underwater robot are calculated；S4：According to the positional information of underwater robot and three axle rotary angle informations, position and attitude information of the renewal threedimensional model in three-dimensional space model；S5：Every the default very first time interval repeat step S2 to S4.The problem of it is bad that the present invention solves underwater photoenvironment, and object of reference is failed to understand, the video camera directly carried using underwater robot carries out position judgment and more difficult interactive operation.
Description
Technical field
The present invention relates to underwater robot technical field, more particularly to a kind of computational methods of underwater robot 3 d pose
And terminal.
Background technology
Underwater robot, it carry after the equipment such as upper sonar, laser ranging, detecting instrument can for Underwater Engineering,
Bridge dam, environmental protection, oil, chemical industry, nuclear power, ocean scientific investigation and military aspect provide survey data service, are led in social life
Domain, under water underwater photograph technical of shooting, coral island diving, shipwreck archaeology and TV station etc. offer image service.
At present to the control of underwater robot, mainly using manual control with automatically controlling two classes.Held high for equipment
For expensive and complex underwater environment underwater robot, robot control is carried out by the way of interactive operation, is its behaviour
The major way of work.However, due to can not directly be observed after water under underwater robot, camera, sound that can only be by its carrying
, laser, gyroscope etc. equipment carry out the perception of surrounding environment and oneself state.Because underwater photoenvironment is bad, and join
Fail to understand according to thing, directly carry out position judgment using the video camera carried and interactive operation is more difficult.
The content of the invention
The technical problems to be solved by the invention are：The invention provides a kind of calculating side of underwater robot 3 d pose
Method and terminal, solve the video camera directly carried using underwater robot and carry out position judgment and interactive operation more difficulty
Problem.
In order to solve the above-mentioned technical problem, the invention provides a kind of computational methods of underwater robot 3 d pose, bag
Include following steps：
S1：Establish the threedimensional model of underwater robot, and the three-dimensional space in the sub-aqua sport space where underwater robot
Between model；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, pass through complementary filter
Algorithm, three axle rotary angle informations of the underwater robot are calculated；
S4：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated described three
Position and attitude information in dimension space model；
S5：Every the default very first time interval repeat step S2 to S4.
Present invention also offers a kind of computing terminal of underwater robot 3 d pose, including memory, processor and deposit
Storage realizes following step on a memory and the computer program that can run on a processor, during the computing device described program
Suddenly：
S1：Establish the threedimensional model of underwater robot, and the three-dimensional space in the sub-aqua sport space where underwater robot
Between model；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, pass through complementary filter
Algorithm, three axle rotary angle informations of the underwater robot are calculated；
S4：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated described three
Position and attitude information in dimension space model；
S5：Every the default very first time interval repeat step S2 to S4.
Beneficial effects of the present invention are：
The present invention establishes the threedimensional model of underwater robot and its three-dimensional space model of sub-aqua sport, by obtaining water
The positional information of lower robot, and the axle rotary angle information of underwater robot three is calculated, so as to realize the three-dimensional of robot under water
According to the positional information and 3 d pose of very first time interval renewal underwater robot in spatial model, when between the very first time
When sufficiently small, you can obtain sub-aqua sport track and real-time attitude information, it is bad to solve underwater photoenvironment, and object of reference
It is not clear, the problem of the video camera progress position judgment directly carried using underwater robot and more difficult interactive operation.
Brief description of the drawings
Fig. 1 is to be illustrated according to a kind of computational methods key step of underwater robot 3 d pose of the embodiment of the present invention
Figure；
Fig. 2 is the flow chart according to a kind of computational methods of underwater robot 3 d pose of the embodiment of the present invention one；
Fig. 3 is the structural representation according to a kind of computing terminal of underwater robot 3 d pose of the embodiment of the present invention；
Label declaration：
1st, memory；2nd, processor.
Embodiment
To describe the technology contents of the present invention, the objects and the effects in detail, below in conjunction with embodiment and coordinate attached
Figure is explained in detail.
The design of most critical of the present invention is：According to the positional information of underwater robot and the underwater robot being calculated
Three axle rotary angle informations, underwater robot threedimensional model is updated in the position of three-dimensional space model and appearance according to very first time interval
State information.
Fig. 1 to Fig. 2 is refer to, the invention provides a kind of computational methods of underwater robot 3 d pose, including it is following
Step：
S1：Establish the threedimensional model of underwater robot, and the three-dimensional space in the sub-aqua sport space where underwater robot
Between model；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, pass through complementary filter
Algorithm, three axle rotary angle informations of the underwater robot are calculated；
S4：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated described three
Position and attitude information in dimension space model；
S5：Every the default very first time interval repeat step S2 to S4.
It was found from foregoing description, the present invention establishes the threedimensional model of underwater robot and its three dimensions of sub-aqua sport
Model, by obtaining the positional information of underwater robot, and the axle rotary angle information of underwater robot three is calculated, so as to realize in water
The positional information and 3 d pose of underwater robot are updated in the three-dimensional space model of lower robot according to very first time interval, when
When the very first time interval is sufficiently small, you can obtain sub-aqua sport track and real-time attitude information, solve Underwater Optical and shine ring
Border is bad, and object of reference is failed to understand, the video camera directly carried using underwater robot carries out position judgment and interactive operation more
The problem of difficult.
Further, the S2 is specially：
Three axis angular rate information of underwater robot are obtained by gyroscope；
Three shaft angle acceleration informations of underwater robot are obtained by three axis accelerometer.
It was found from foregoing description, three axis angular rate information of effective quick obtaining underwater robot are capable of by the above method
And three shaft angle acceleration information.
Further, also include between the S3 and S4：
The cable of underwater robot is obtained relative to the first distance of default first buoy；Obtain the electricity of underwater robot
Cable is relative to the second distance for presetting the second buoy；
The cable of underwater robot is obtained relative to the 3rd distance of operator；
According to the described first distance, second distance and the 3rd distance, by three-point fix principle, the underwater is obtained
Relative to the relative position information of operator；
According to the relative position information and the GPS information of operator present position, the position of institute's underwater robot is obtained
Information.
It was found from foregoing description, the position that underwater robot can be rapidly and accurately acquired by the above method is believed
Breath, is solved the problems, such as to underwater robot location difficulty.
Further, the S3 is specially：
The three axis angular rates information includes the multiple instantaneous angular acceleration values in three directions, corresponding according to each direction
Angle instantaneous acceleration value, the acceleration average value in each direction is calculated；
The three axis angular rates information includes the multiple angular speed in three directions, according to angle speed corresponding to each direction
Degree, the angular speed function in each direction is calculated；
It is respectively that the acceleration average value in each direction is public by default complementary filter with corresponding angular speed function
Formula, is calculated three axle rotary angle informations, and the three axles rotary angle information includes the anglec of rotation corresponding to three directions；
The complementary filter formula is specially：
Wherein θ is the anglec of rotation, and acc is acceleration average value, and g is acceleration of gravity, θ0For predetermined angle constant, A, G divide
Not Wei acceleration weights and angular speed weights, and A+G=1, w are angular speed function, and t is the very first time interval.
It was found from foregoing description, because three axis accelerometer itself sensitivity is higher, external disturbance especially vibration interference
Larger noise can be brought to acceleration measuring value with external magnetic field, the data directly acquired with accelerometer calculate angle
Spending error can be very big；And the measurement data of gyroscope is three axis angular rates, it is integrated to obtain angle in the short time, but also can band
Carry out integral error.Therefore correct angle can be obtained according to the complementary characteristic of two sensorses.It is achieved that complementary filter is calculated
The thinking of method is exactly that the measured value of gyroscope in the short time is more accurate, and angular speed weights are big, and the fluctuation of acceleration measuring value is big, adds
Speed weights are small, and the default complementary filter formula in complementary filter algorithm calculates the three axle anglecs of rotation；With acceleration in for a long time
Based on the average of degree, acceleration weights are big, and angular speed weights are small, calculate attitude angle and calculate three axles according to default complementary filter formula
The anglec of rotation.
Further, " the acceleration average value in each direction is passed through with corresponding angular speed function respectively default
Complementary filter formula, three axle rotary angle informations are calculated " be specially：
According to the complementary filter formula, quaternary number equation, Euler's transformation formula or matrixing formula corresponding to structure；
According to the acceleration average value in each direction and corresponding angular speed function, pass through the quaternary number equation, Europe
Draw transformation for mula or matrixing formula that three axle rotary angle informations are calculated.
It was found from foregoing description, the three axle anglecs of rotation can be calculated simultaneously by the above method, improve data-handling efficiency.
It refer to Fig. 3, a kind of computing terminal of underwater robot 3 d pose provided by the invention, including memory 1, place
Manage device 2 and be stored in the computer program that can be run on memory 1 and on processor 2, it is characterised in that the processor 2
Following steps are realized when performing described program：
S1：Establish the threedimensional model of underwater robot, and the three-dimensional space in the sub-aqua sport space where underwater robot
Between model；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, pass through complementary filter
Algorithm, three axle rotary angle informations of the underwater robot are calculated；
S4：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated described three
Position and attitude information in dimension space model；
S5：Every the default very first time interval repeat step S2 to S4.
Further, a kind of computing terminal of described underwater robot 3 d pose, the S2 are specially：
Three axis angular rate information of underwater robot are obtained by gyroscope；
Three shaft angle acceleration informations of underwater robot are obtained by three axis accelerometer.
Further, the computing terminal of described a kind of underwater robot 3 d pose, also includes between the S3 and S4：
The cable of underwater robot is obtained relative to the first distance of default first buoy；Obtain the electricity of underwater robot
Cable is relative to the second distance for presetting the second buoy；
The cable of underwater robot is obtained relative to the 3rd distance of operator；
According to the described first distance, second distance and the 3rd distance, by three-point fix principle, the underwater is obtained
Relative to the relative position information of operator；
According to the relative position information and the GPS information of operator present position, the position of institute's underwater robot is obtained
Information.
Further, a kind of computing terminal of described underwater robot 3 d pose, the S3 are specially：
The three axis angular rates information includes the multiple instantaneous angular acceleration values in three directions, corresponding according to each direction
Angle instantaneous acceleration value, the acceleration average value in each direction is calculated；
The three axis angular rates information includes the multiple angular speed in three directions, according to angle speed corresponding to each direction
Degree, the angular speed function in each direction is calculated；
It is respectively that the acceleration average value in each direction is public by default complementary filter with corresponding angular speed function
Formula, is calculated three axle rotary angle informations, and the three axles rotary angle information includes the anglec of rotation corresponding to three directions；
The complementary filter formula is specially：
Wherein θ is the anglec of rotation, and acc is acceleration average value, and g is acceleration of gravity, θ0For predetermined angle constant, A, G divide
Not Wei acceleration weights and angular speed weights, and A+G=1, w are angular speed function, and t is the very first time interval.
Further, the computing terminal of described a kind of underwater robot 3 d pose, " adding each direction respectively
With corresponding angular speed function, by default complementary filter formula, three axle rotary angle informations are calculated in speed average " tool
Body is：
According to the complementary filter formula, quaternary number equation, Euler's transformation formula or matrixing formula corresponding to structure；
According to the acceleration average value in each direction and corresponding angular speed function, pass through the quaternary number equation, Europe
Draw transformation for mula or matrixing formula that three axle rotary angle informations are calculated.
Fig. 2 is refer to, embodiments of the invention one are：
The invention provides a kind of computational methods of underwater robot 3 d pose, comprise the following steps：
S1：Establish the threedimensional model of underwater robot, and the three-dimensional space in the sub-aqua sport space where underwater robot
Between model；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
The underwater robot has a three degree of freedom, three above-mentioned axis angular rate information refer to for underwater x, y,
The direction angular velocity of rotations of z tri-；The acceleration magnitude for the direction of x, y, z three that three shaft angle acceleration informations refer to；
The S2 is specially：
Three axis angular rate information of underwater robot are obtained by gyroscope；
Three shaft angle acceleration informations of underwater robot are obtained by three axis accelerometer.
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, pass through complementary filter
Algorithm, three axle rotary angle informations of the underwater robot are calculated；
The S3 is specially：
The three axis angular rates information includes the multiple instantaneous angular acceleration values in three directions, corresponding according to each direction
Angle instantaneous acceleration value, the acceleration average value in each direction is calculated；
The three axis angular rates information includes the multiple angular speed in three directions, according to angle speed corresponding to each direction
Degree, the angular speed function in each direction is calculated；
It is respectively that the acceleration average value in each direction is public by default complementary filter with corresponding angular speed function
Formula, is calculated three axle rotary angle informations, and the three axles rotary angle information includes the anglec of rotation corresponding to three directions；Three axle
What rotary angle information referred to navigates by water coordinate system B (oxyz) relative to reference frame i.e. inertial coodinate system E for underwater robot
(OXYZ) in the anglec of rotation in three directions；
The complementary filter formula is specially：
Wherein θ is the anglec of rotation, and acc is acceleration average value, and g is acceleration of gravity, θ0For predetermined angle constant, A, G divide
Not Wei acceleration weights and angular speed weights, and A+G=1, w are angular speed function, and t is the very first time interval.
S4：Calculate the positional information of underwater robot；
The S4 is specially：
The cable of underwater robot is obtained relative to the first distance of default first buoy；Obtain the electricity of underwater robot
Cable is relative to the second distance for presetting the second buoy；
The cable of underwater robot is obtained relative to the 3rd distance of operator；
According to the described first distance, second distance and the 3rd distance, by three-point fix principle, the underwater is obtained
Relative to the relative position information of operator；
According to the relative position information and the GPS information of operator present position, the position of institute's underwater robot is obtained
Information.
S5：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated described three
Position and attitude information in dimension space model；
S6：Step S2 to S5 is repeated every 0.1s.
Fig. 3 is refer to, embodiments of the invention two are：
A kind of computing terminal of underwater robot 3 d pose, including memory, processor and storage are on a memory simultaneously
The computer program that can be run on a processor, following steps are realized during the computing device described program：
S1：Establish the threedimensional model of underwater robot, and the three-dimensional space in the sub-aqua sport space where underwater robot
Between model；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
The underwater robot has a three degree of freedom, three above-mentioned axis angular rate information refer to for underwater x, y,
The direction angular velocity of rotations of z tri-；The acceleration magnitude for the direction of x, y, z three that three shaft angle acceleration informations refer to；
The S2 is specially：
Three axis angular rate information of underwater robot are obtained by gyroscope；
Three shaft angle acceleration informations of underwater robot are obtained by three axis accelerometer.
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, pass through complementary filter
Algorithm, three axle rotary angle informations of the underwater robot are calculated；
The S3 is specially：
The three axis angular rates information includes the multiple instantaneous angular acceleration values in three directions, corresponding according to each direction
Angle instantaneous acceleration value, the acceleration average value in each direction is calculated；
The three axis angular rates information includes the multiple angular speed in three directions, according to angle speed corresponding to each direction
Degree, the angular speed function in each direction is calculated；
It is respectively that the acceleration average value in each direction is public by default complementary filter with corresponding angular speed function
Formula, is calculated three axle rotary angle informations, and the three axles rotary angle information includes the anglec of rotation corresponding to three directions；Three axle
What rotary angle information referred to navigates by water coordinate system B (oxyz) relative to reference frame i.e. inertial coodinate system E for underwater robot
(OXYZ) in the anglec of rotation in three directions；
The complementary filter formula is specially：
Wherein θ is the anglec of rotation, and acc is acceleration average value, and g is acceleration of gravity, θ0For predetermined angle constant, A, G divide
Not Wei acceleration weights and angular speed weights, and A+G=1, w are angular speed function, and t is the very first time interval.
S4：Calculate the positional information of underwater robot；
The S4 is specially：
The cable of underwater robot is obtained relative to the first distance of default first buoy；Obtain the electricity of underwater robot
Cable is relative to the second distance for presetting the second buoy；
The cable of underwater robot is obtained relative to the 3rd distance of operator；
According to the described first distance, second distance and the 3rd distance, by three-point fix principle, the underwater is obtained
Relative to the relative position information of operator；
According to the relative position information and the GPS information of operator present position, the position of institute's underwater robot is obtained
Information.
S5：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated described three
Position and attitude information in dimension space model；
S6：Step S2 to S5 is repeated every 0.1s.
In summary, the present invention establishes the threedimensional model of underwater robot and its three-dimensional space model of sub-aqua sport,
By obtaining the positional information of underwater robot, and the axle rotary angle information of underwater robot three is calculated, so as to realize machine under water
According to the positional information and 3 d pose of very first time interval renewal underwater robot in the three-dimensional space model of device people, when described
The very first time interval it is sufficiently small when, you can obtain sub-aqua sport track and real-time attitude information, solve underwater photoenvironment not
It is good, and object of reference is failed to understand, the video camera directly carried using underwater robot carries out position judgment and interactive operation is more difficult
The problem of；The present invention can rapidly and accurately acquire the position letter of underwater robot by the above method and terminal simultaneously
Breath, solve the problems, such as to underwater robot location difficulty, and the present invention improves three axles by complementary filter algorithm and rotated
The counting accuracy at angle.
Embodiments of the invention are the foregoing is only, are not intended to limit the scope of the invention, it is every to utilize this hair
The equivalents that bright specification and accompanying drawing content are made, or other related technical areas are directly or indirectly used in, similarly
It is included within the scope of the present invention.
Claims (10)
1. a kind of computational methods of underwater robot 3 d pose, it is characterised in that comprise the following steps：
S1：Establish the threedimensional model of underwater robot, and the three dimensions mould in the sub-aqua sport space where underwater robot
Type；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, by complementary filter algorithm,
Three axle rotary angle informations of the underwater robot are calculated；
S4：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated in the three-dimensional space
Between position in model and attitude information；
S5：Every the default very first time interval repeat step S2 to S4.
A kind of 2. computational methods of underwater robot 3 d pose according to claim 1, it is characterised in that the S2 tools
Body is：
Three axis angular rate information of underwater robot are obtained by gyroscope；
Three shaft angle acceleration informations of underwater robot are obtained by three axis accelerometer.
A kind of 3. computational methods of underwater robot 3 d pose according to claim 1, it is characterised in that the S3 and
Also include between S4：
The cable of underwater robot is obtained relative to the first distance of default first buoy；Obtain the cable of underwater robot
Relative to the second distance of default second buoy；
The cable of underwater robot is obtained relative to the 3rd distance of operator；
According to the described first distance, second distance and the 3rd distance, by three-point fix principle, it is relative to obtain the underwater
In the relative position information of operator；
According to the relative position information and the GPS information of operator present position, the positional information of institute's underwater robot is obtained.
A kind of 4. computational methods of underwater robot 3 d pose according to claim 1, it is characterised in that the S3 tools
Body is：
The three axis angular rates information includes the multiple instantaneous angular acceleration values in three directions, according to angle corresponding to each direction
Instantaneous acceleration value, the acceleration average value in each direction is calculated；
The three axis angular rates information includes the multiple angular speed in three directions, according to angular speed corresponding to each direction, meter
Calculation obtains the angular speed function in each direction；
The acceleration average value in each direction is passed through into default complementary filter formula, meter with corresponding angular speed function respectively
Calculation obtains three axle rotary angle informations, and the three axles rotary angle information includes the anglec of rotation corresponding to three directions；
The complementary filter formula is specially：
<mrow>
<mi>&theta;</mi>
<mo>=</mo>
<mi>A</mi>
<mo>*</mo>
<mi>a</mi>
<mi>c</mi>
<mi> </mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>a</mi>
<mi>c</mi>
<mi>c</mi>
</mrow>
<mi>g</mi>
</mfrac>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>G</mi>
<mo>*</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<munder>
<mo>&Integral;</mo>
<mi>t</mi>
</munder>
<mi>w</mi>
<mi>d</mi>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
Wherein θ is the anglec of rotation, and acc is acceleration average value, and g is acceleration of gravity, θ0For predetermined angle constant, A, G are respectively to add
Speed weights and angular speed weights, and A+G=1, w are angular speed function, t is the very first time interval.
5. the computational methods of a kind of underwater robot 3 d pose according to claim 1, it is characterised in that " respectively will
With corresponding angular speed function, by default complementary filter formula, three axles are calculated in the acceleration average value in each direction
Rotary angle information " is specially：
According to the complementary filter formula, quaternary number equation, Euler's transformation formula or matrixing formula corresponding to structure；
According to the acceleration average value in each direction and corresponding angular speed function, pass through the quaternary number equation, Ou Labian
Change formula or three axle rotary angle informations are calculated in matrixing formula.
6. a kind of computing terminal of underwater robot 3 d pose, including memory, processor and storage are on a memory and can
The computer program run on a processor, it is characterised in that realize following steps during the computing device described program：
S1：Establish the threedimensional model of underwater robot, and the three dimensions mould in the sub-aqua sport space where underwater robot
Type；
S2：Obtain the three axis angular rate information and three shaft angle acceleration informations of the underwater robot；
S3：According to the three axis angular rate information and three shaft angle acceleration informations of the underwater robot, by complementary filter algorithm,
Three axle rotary angle informations of the underwater robot are calculated；
S4：According to the positional information of underwater robot and three axle rotary angle informations, the threedimensional model is updated in the three-dimensional space
Between position in model and attitude information；
S5：Every the default very first time interval repeat step S2 to S4.
A kind of 7. computing terminal of underwater robot 3 d pose according to claim 6, it is characterised in that the S2 tools
Body is：
Three axis angular rate information of underwater robot are obtained by gyroscope；
Three shaft angle acceleration informations of underwater robot are obtained by three axis accelerometer.
A kind of 8. computing terminal of underwater robot 3 d pose according to claim 6, it is characterised in that the S3 and
Also include between S4：
The cable of underwater robot is obtained relative to the first distance of default first buoy；Obtain the cable of underwater robot
Relative to the second distance of default second buoy；
The cable of underwater robot is obtained relative to the 3rd distance of operator；
According to the described first distance, second distance and the 3rd distance, by three-point fix principle, it is relative to obtain the underwater
In the relative position information of operator；
According to the relative position information and the GPS information of operator present position, the positional information of institute's underwater robot is obtained.
A kind of 9. computing terminal of underwater robot 3 d pose according to claim 6, it is characterised in that the S3 tools
Body is：
The three axis angular rates information includes the multiple instantaneous angular acceleration values in three directions, according to angle corresponding to each direction
Instantaneous acceleration value, the acceleration average value in each direction is calculated；
The three axis angular rates information includes the multiple angular speed in three directions, according to angular speed corresponding to each direction, meter
Calculation obtains the angular speed function in each direction；
The acceleration average value in each direction is passed through into default complementary filter formula, meter with corresponding angular speed function respectively
Calculation obtains three axle rotary angle informations, and the three axles rotary angle information includes the anglec of rotation corresponding to three directions；
The complementary filter formula is specially：
<mrow>
<mi>&theta;</mi>
<mo>=</mo>
<mi>A</mi>
<mo>*</mo>
<mi>a</mi>
<mi>c</mi>
<mi> </mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>a</mi>
<mi>c</mi>
<mi>c</mi>
</mrow>
<mi>g</mi>
</mfrac>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>G</mi>
<mo>*</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<munder>
<mo>&Integral;</mo>
<mi>t</mi>
</munder>
<mi>w</mi>
<mi>d</mi>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
Wherein θ is the anglec of rotation, and acc is acceleration average value, and g is acceleration of gravity, θ0For predetermined angle constant, A, G are respectively to add
Speed weights and angular speed weights, and A+G=1, w are angular speed function, t is the very first time interval.
10. the computing terminal of a kind of underwater robot 3 d pose according to claim 6, it is characterised in that " respectively will
With corresponding angular speed function, by default complementary filter formula, three axles are calculated in the acceleration average value in each direction
Rotary angle information " is specially：
According to the complementary filter formula, quaternary number equation, Euler's transformation formula or matrixing formula corresponding to structure；
According to the acceleration average value in each direction and corresponding angular speed function, pass through the quaternary number equation, Ou Labian
Change formula or three axle rotary angle informations are calculated in matrixing formula.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710980157.9A CN107883951A (en) | 2017-10-19 | 2017-10-19 | A kind of computational methods and terminal of underwater robot 3 d pose |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710980157.9A CN107883951A (en) | 2017-10-19 | 2017-10-19 | A kind of computational methods and terminal of underwater robot 3 d pose |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107883951A true CN107883951A (en) | 2018-04-06 |
Family
ID=61781964
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710980157.9A Pending CN107883951A (en) | 2017-10-19 | 2017-10-19 | A kind of computational methods and terminal of underwater robot 3 d pose |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107883951A (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108801260A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Data processing method based on underwater robot and device |
| CN108803673A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Directional controlling method and device |
| CN109507658A (en) * | 2018-11-21 | 2019-03-22 | 浙江大学 | A kind of comprehensive tracing positioning device of underwater robot coastal waters bed motion |
| CN110515465A (en) * | 2019-08-29 | 2019-11-29 | 张洋 | Control method and device based on brain wave and gesture recognition |
| CN111968407A (en) * | 2020-08-26 | 2020-11-20 | 国网山西省电力公司大同供电公司 | Wireless communication method for underwater robot |
| CN114770598B (en) * | 2022-04-08 | 2024-01-26 | 上海中车艾森迪海洋装备有限公司 | Underwater robot attitude estimation method and device, electronic equipment and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110077903A1 (en) * | 2009-09-30 | 2011-03-31 | Samsung Electronics Co., Ltd. | Apparatus and method sensing motion |
| CN102121828A (en) * | 2010-12-21 | 2011-07-13 | 浙江大学 | Method for estimating body posture angle of humanoid robot in real time |
| CN104197927A (en) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | Real-time navigation system and real-time navigation method for underwater structure detection robot |
| CN105072580A (en) * | 2015-08-25 | 2015-11-18 | 深圳先进技术研究院 | WIFI (Wireless Fidelity) fingerprint map automatic acquisition system and method based on sweeping robot |
| CN105865448A (en) * | 2016-03-18 | 2016-08-17 | 常州大学 | Indoor positioning method based on IMU |
| CN106482734A (en) * | 2016-09-28 | 2017-03-08 | 湖南优象科技有限公司 | A kind of filtering method for IMU Fusion |
-
2017
- 2017-10-19 CN CN201710980157.9A patent/CN107883951A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110077903A1 (en) * | 2009-09-30 | 2011-03-31 | Samsung Electronics Co., Ltd. | Apparatus and method sensing motion |
| CN102121828A (en) * | 2010-12-21 | 2011-07-13 | 浙江大学 | Method for estimating body posture angle of humanoid robot in real time |
| CN104197927A (en) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | Real-time navigation system and real-time navigation method for underwater structure detection robot |
| CN105072580A (en) * | 2015-08-25 | 2015-11-18 | 深圳先进技术研究院 | WIFI (Wireless Fidelity) fingerprint map automatic acquisition system and method based on sweeping robot |
| CN105865448A (en) * | 2016-03-18 | 2016-08-17 | 常州大学 | Indoor positioning method based on IMU |
| CN106482734A (en) * | 2016-09-28 | 2017-03-08 | 湖南优象科技有限公司 | A kind of filtering method for IMU Fusion |
Non-Patent Citations (1)
| Title |
|---|
| 李卫国 等: "《创意之星 模块化机器人设计与竞赛 第2版》", 30 September 2016, 北京航空航天大学出版社 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108801260A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Data processing method based on underwater robot and device |
| CN108803673A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Directional controlling method and device |
| CN109507658A (en) * | 2018-11-21 | 2019-03-22 | 浙江大学 | A kind of comprehensive tracing positioning device of underwater robot coastal waters bed motion |
| CN110515465A (en) * | 2019-08-29 | 2019-11-29 | 张洋 | Control method and device based on brain wave and gesture recognition |
| CN111968407A (en) * | 2020-08-26 | 2020-11-20 | 国网山西省电力公司大同供电公司 | Wireless communication method for underwater robot |
| CN114770598B (en) * | 2022-04-08 | 2024-01-26 | 上海中车艾森迪海洋装备有限公司 | Underwater robot attitude estimation method and device, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107883951A (en) | A kind of computational methods and terminal of underwater robot 3 d pose | |
| CN104567931B (en) | A kind of heading effect error cancelling method of indoor inertial navigation positioning | |
| CN106979780B (en) | A kind of unmanned vehicle real-time attitude measurement method | |
| CN106323334B (en) | A kind of magnetometer calibration method based on particle group optimizing | |
| CN109141433A (en) | A kind of robot indoor locating system and localization method | |
| CN106885566B (en) | Wearable motion sensor and magnetic field interference resisting method thereof | |
| CN103940442B (en) | A kind of localization method and device using acceleration convergence algorithm | |
| CN104459828B (en) | Based on the non-aligned bearing calibration of earth magnetism vector system around method of principal axes | |
| CN105091744A (en) | Pose detection apparatus and method based on visual sensor and laser range finder | |
| CN105841698B (en) | A kind of AUV rudder angle precision real time measuring systems without zeroing | |
| CN109752003A (en) | A kind of robot vision inertia dotted line characteristic positioning method and device | |
| CN103487011B (en) | A kind of attitude angle detection method of data glove | |
| CN208751577U (en) | A kind of robot indoor locating system | |
| CN101424534A (en) | Inertia/gravity combined navigation semi-physical object simulating device | |
| CN108939488B (en) | Sailing boat auxiliary training path planning method based on augmented reality | |
| CN104848818B (en) | Stewart platform stances measurement apparatus and measuring method | |
| CN106643717A (en) | Method and device for performance detection of nine-axis sensor | |
| CN109739088A (en) | A kind of unmanned boat finite time convergence control state observer and its design method | |
| CN113189875A (en) | Unmanned aerial vehicle robust landing method on mobile platform based on circular features | |
| CN106979779A (en) | A kind of unmanned vehicle real-time attitude measuring method | |
| Krishnamurthy et al. | A self-aligning underwater navigation system based on fusion of multiple sensors including DVL and IMU | |
| CN103253357B (en) | Control method for ship posture display device | |
| Huang et al. | Study on INS/DR integration navigation system using EKF/RK4 algorithm for underwater gliders | |
| CN105427371B (en) | The method that the elemental areas such as Drawing Object are shown is kept in a kind of three-dimensional perspective projection scene | |
| CN104913776B (en) | Unmanned plane localization method and device |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180406 |