CN114770598B - Underwater robot attitude estimation method and device, electronic equipment and storage medium - Google Patents
Underwater robot attitude estimation method and device, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN114770598B CN114770598B CN202210366748.8A CN202210366748A CN114770598B CN 114770598 B CN114770598 B CN 114770598B CN 202210366748 A CN202210366748 A CN 202210366748A CN 114770598 B CN114770598 B CN 114770598B
- Authority
- CN
- China
- Prior art keywords
- angle
- position information
- coordinate system
- underwater robot
- transformation matrix
- 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
- 238000000034 method Methods 0.000 title claims abstract description 29
- 239000011159 matrix material Substances 0.000 claims abstract description 107
- 230000009466 transformation Effects 0.000 claims abstract description 80
- 230000001131 transforming effect Effects 0.000 claims abstract description 7
- 238000009434 installation Methods 0.000 claims description 21
- 230000006870 function Effects 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 13
- 229940050561 matrix product Drugs 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000004590 computer program Methods 0.000 claims description 3
- 238000004891 communication Methods 0.000 description 11
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000003032 molecular docking Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides an underwater robot attitude estimation method, an underwater robot attitude estimation device, electronic equipment and a storage medium, wherein the method comprises the following steps: acquiring first position information of an acoustic beacon for a reference coordinate system, wherein a plurality of underwater acoustic positioning systems comprising the acoustic beacon and a transducer are arranged in the reference coordinate system; measuring second position information of the acoustic beacon relative to the transducer through the transducer, and further obtaining third position information of the acoustic beacon relative to the underwater robot, wherein the transducer is arranged on the underwater robot; acquiring a first transformation matrix for transforming a reference coordinate system into a robot body coordinate system according to the first position information and the third position information; and calculating the attitude information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system. According to the invention, the acoustic beacons are arranged in different reference coordinate systems, so that the attitude information of the underwater robot relative to the different reference coordinate systems can be accurately measured.
Description
Technical Field
The invention belongs to the technical field of underwater metering, and particularly relates to an underwater robot attitude estimation method, an underwater robot attitude estimation device, electronic equipment and a storage medium.
Background
When the underwater robot flies or works underwater, the state information of the underwater robot needs to be uploaded continuously, and the state information comprises position information and attitude information, so that an operator can respond in time. And when the automatic control system operates, the underwater robot also needs to continuously send the state information of the underwater robot to the control system as state feedback so as to realize an accurate automatic control function.
In order to acquire the attitude information of the underwater robot, it is generally necessary to install a three-axis high-precision gyroscopic sensor, so-called inertial navigation system, on the robot. The inertial navigation system based on the three-axis gyroscope has the advantages of high precision, good stability and the like, and the defects are that the inertial navigation system is high in price, and long-time operation can be influenced by factors such as drift. In addition, the attitude information generally calculated by the inertial navigation system is relative to an earth coordinate system, but in some special occasions, such as underwater automatic docking, object tracking and relative position maintenance, the attitude angle information of the earth coordinate system cannot be directly used for automatically operating the underwater robot to complete corresponding operation tasks.
Disclosure of Invention
The invention provides an underwater robot posture estimation method, an underwater robot posture estimation device, electronic equipment and a storage medium, and aims to solve the problem that the existing underwater robot posture information acquired by an inertial navigation system based on a three-axis gyroscope is easily affected by drift.
Based on the above object, the embodiment of the invention provides an underwater robot attitude estimation method, which includes: a plurality of underwater acoustic positioning systems comprising acoustic beacons and transducers are arranged in a reference coordinate system, and first position information of the acoustic beacons in the reference coordinate system is acquired; the transducer is arranged on the underwater robot, second position information of the acoustic beacon relative to the transducer is measured through the transducer, and third position information of the acoustic beacon relative to the underwater robot is obtained according to the second position information; acquiring a first transformation matrix for transforming the reference coordinate system into the robot body coordinate system according to the first position information and the third position information; and calculating the attitude information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system.
Optionally, the acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information includes: acquiring an installation angle and an installation position of the transducer on the underwater robot; and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, the installation angle of the transducer on the underwater robot and the installation position.
Optionally, the acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information and the installation angle and the installation position of the transducer on the underwater robot includes: determining a second transformation matrix from the transducer coordinate system to the underwater robot coordinate system according to the installation angle and the installation position; and summing the matrix product of the second transformation matrix and the second position information with the first position deviation of the origin of the transducer coordinate system in the robot coordinate system, and acquiring the third position information of the acoustic beacon relative to the underwater robot.
Optionally, the obtaining a first transformation matrix of the transformation from the reference coordinate system to the robot body coordinate system according to the first position information and the third position information includes: acquiring a correlation formula among the first position information, the third position information and the first transformation matrix, wherein the correlation formula is that a matrix product of the first transformation matrix and the third position information is equal to a difference value of a second position deviation of the first position information and an origin of a robot coordinate system in the reference coordinate system; and acquiring position coordinates of any three acoustic beacons, and applying the position coordinates of the three acoustic beacons to the correlation formula calculation to acquire the first transformation matrix.
Optionally, the calculating, according to the first transformation matrix and the euler angle relation matrix of the underwater robot relative to the reference coordinate system, the pose information of the underwater robot relative to the reference coordinate system at the current moment includes: and calculating trigonometric function values of all angles in the gesture information according to the one-to-one correspondence between each point in the Euler angle relation matrix and each point in the first transformation matrix, and respectively calculating all angles in the gesture information according to the trigonometric function values, wherein the gesture information comprises a course angle, a pitch angle and a roll angle.
Optionally, the calculating the trigonometric function value of each angle in the gesture information according to the one-to-one correspondence between each point in the euler angle relation matrix and each point in the first transformation matrix, and calculating each angle in the gesture information according to the trigonometric function value respectively includes: determining a sine value of the pitch angle according to the value of a first row and a third column in the first transformation matrix and the Euler angle relation matrix, and calculating the possible pitch angle according to the sine value of the pitch angle; acquiring sine values and cosine values of the course angle and sine values and cosine values of the roll angle according to values of points in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix; and respectively calculating the course angle and the roll angle according to the sine value and the cosine value of the course angle and the sine value and the cosine value of the roll angle.
Optionally, the obtaining the sine value and the cosine value of the heading angle and the sine value and the cosine value of the roll angle according to the values of each point in the first transformation matrix, the possible pitch angle and the euler angle relation matrix includes: determining a candidate sine value and a candidate cosine value of the course angle according to values of a first column and a second column of a first row in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix; determining a candidate sine value and a candidate cosine value of the roll angle according to values of a second row, a third column and a third row in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix; and determining the sine value and the cosine value of the course angle and the sine value and the cosine value of the roll angle according to the values of other points in the first transformation matrix, the candidate sine value and the candidate cosine value of the course angle and the candidate sine value and the candidate cosine value of the roll angle.
Based on the same inventive concept, the embodiment of the invention also provides an underwater robot attitude estimation device, which comprises: a position acquisition unit for acquiring first position information of an acoustic beacon for a reference coordinate system in which a plurality of underwater acoustic positioning systems including the acoustic beacon and a transducer are arranged; the position conversion unit is used for measuring second position information of the acoustic beacon relative to the transducer through the transducer and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, wherein the transducer is arranged on the underwater robot; a transformation matrix acquisition unit, configured to acquire a first transformation matrix transformed from the reference coordinate system to the robot body coordinate system according to the first position information and the third position information; and the gesture calculation unit is used for calculating the gesture information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system.
Based on the same inventive concept, the embodiment of the invention also provides an electronic device, which comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor executes the program to realize the method.
Based on the same inventive concept, the embodiment of the invention also provides a computer storage medium, wherein at least one executable instruction is stored in the storage medium, and the executable instruction causes a processor to execute the method.
The beneficial effects of the invention are as follows: from the above, it can be seen that the method, the device, the electronic device and the storage medium for estimating the pose of the underwater robot provided by the embodiment of the invention comprise the following steps: a plurality of underwater acoustic positioning systems comprising acoustic beacons and transducers are arranged in a reference coordinate system, and first position information of the acoustic beacons in the reference coordinate system is acquired; the transducer is arranged on the underwater robot, second position information of the acoustic beacon relative to the transducer is measured through the transducer, and third position information of the acoustic beacon relative to the underwater robot is obtained according to the second position information; acquiring a first transformation matrix for transforming the reference coordinate system into the robot body coordinate system according to the first position information and the third position information; according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system, calculating the attitude information of the underwater robot relative to the reference coordinate system at the current moment, and accurately measuring the attitude information of the underwater robot relative to different reference coordinate systems by installing acoustic beacons in different reference coordinate systems.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, and it is obvious that the drawings in the following description are only embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic flow chart of an underwater robot attitude estimation method in an embodiment of the invention;
FIG. 2 is a schematic view of an underwater acoustic positioning system in accordance with an embodiment of the present invention;
fig. 3 is a schematic structural view of an underwater robot pose estimation device according to an embodiment of the present invention;
fig. 4 is a schematic diagram of an electronic device according to an embodiment of the invention.
Detailed Description
For the purposes of promoting an understanding of the principles and advantages of the disclosure, reference will now be made to the embodiments illustrated in the drawings and specific language will be used to describe the same.
It should be noted that unless otherwise defined, technical or scientific terms used in the embodiments of the present invention should be given the ordinary meaning as understood by one of ordinary skill in the art to which the present disclosure pertains. The terms "first," "second," and the like, as used in embodiments of the present invention, do not denote any order, quantity, or importance, but rather are used to distinguish one element from another. The word "comprising" or "comprises", and the like, means that elements or items preceding the word are included in the element or item listed after the word and equivalents thereof, but does not exclude other elements or items. The terms "connected" or "connected," and the like, are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "upper", "lower", "left", "right", etc. are used merely to indicate relative positional relationships, which may also be changed when the absolute position of the object to be described is changed.
The embodiment of the invention provides an underwater robot attitude estimation method. As shown in fig. 1, the underwater robot attitude estimation method includes:
step S11: first position information of an acoustic beacon for a reference coordinate system is acquired, wherein a plurality of underwater acoustic positioning systems comprising the acoustic beacon and a transducer are arranged in the reference coordinate system.
In the embodiment of the present invention, before step S11, an appropriate reference coordinate system is selected according to the requirement. For example, the reference coordinate system may be selected from the earth coordinate system, an underwater platform, or on a surface vessel, etc. For convenience of description, the earth coordinate system is selected as the reference coordinate system in the embodiment of the present invention.
In step S11, arranging a plurality of acoustic beacons required by the underwater acoustic positioning system in a corresponding reference coordinate system, and requiring that the position information (xyz axis information) of the acoustic beacons in the reference coordinate system can be directly measured at any time to obtain the first position information; or the acoustic beacon may be fixed to a referenceIn the coordinate system, the first position information of the acoustic beacon is measured in advance. Of these, the underwater acoustic positioning system is preferably an Ultra-short baseline positioning system (Ultra-Short Baseline acoustic positioning system, USBL). For example, as shown in FIG. 2, the underwater acoustic positioning system is provided with transducers on the underwater robot, 3 acoustic beacons are provided at different positions below the water level, (X usbl ，Y usbl ，Z usbl ) Namely the coordinate system O corresponding to the transducer trans -X trans Y trans Z trans 。For the attitude information of the transducer relative to the underwater robot, < >>The attitude information of the underwater robot relative to a reference coordinate system is provided, wherein Roll is a Roll angle, pitch is a Pitch angle, and Heading is a Heading angle.
Step S12: and measuring second position information of the acoustic beacon relative to the transducer through the transducer, and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, wherein the transducer is arranged on the underwater robot.
In the embodiment of the invention, the transducer required by the underwater acoustic positioning system is arranged on the underwater robot, and a corresponding coordinate system O is defined on the transducer trans -X trans Y trans Z trans . The transducer can continuously measure the second position information of each acoustic beacon relative to the transducerWhere i represents the i-th acoustic beacon.
And then acquiring the installation angle and the installation position of the transducer on the underwater robot. Defining a robot coordinate system O on an underwater robot ROV -X ROV Y ROV Z ROV And then according to the second position information and the transducer under water machineThe installation angle and the installation position on the person acquire third position information of the acoustic beacon relative to the underwater robot. Optionally, determining a second transformation matrix of the transducer coordinate system to the underwater robot coordinate system according to the installation angle and the installation position; summing the matrix product of the second transformation matrix and the second position information with a first position deviation of a transducer coordinate system origin in the robot coordinate system, and acquiring the third position information of the acoustic beacon relative to the underwater robot:
wherein,said third position information representing the acoustic beacon relative to the underwater robot->A second transformation matrix from the transducer coordinate system to the robot coordinate system and the installation deviation angle of the transducer on the underwater robot>Related to the following. />Representing the position of the origin of the transducer coordinate system within the robot coordinate system, i.e. the first positional deviation of the transducer mounting.
Step S13: and acquiring a first transformation matrix for transforming the reference coordinate system into the robot body coordinate system according to the first position information and the third position information.
In the embodiment of the invention, the third position information of the acoustic beacon relative to the robot coordinate system is converted into the first position information of the acoustic beacon relative to the reference coordinate system through coordinate transformation, and the earth coordinate system is selected as the reference coordinate system, so that the corresponding calculation relationship is as follows:
wherein,representing a second positional deviation of the origin of the transducer coordinate system in the reference coordinate system, i.e. the position coordinates of the origin of the transducer coordinate system in the reference coordinate system +.>A first transformation matrix representing a transformation of the reference coordinate system to the robot body coordinate system.
Optionally, obtaining a correlation formula among the first position information, the third position information and the first transformation matrix, wherein the correlation formula is that a matrix product of the first transformation matrix and the third position information is equal to a difference value of a second position deviation of the first position information and an origin of a robot coordinate system in the reference coordinate system; and acquiring position coordinates of any three acoustic beacons, and applying the position coordinates of the three acoustic beacons to the correlation formula calculation to acquire the first transformation matrix. Specifically, according to the foregoing calculation relation of the acoustic beacon relative to the first position information of the reference coordinate system and the acoustic beacon relative to the third position information in the robot coordinate system through coordinate transformation, the following mutual relation among the first position information, the third position information and the first transformation matrix is obtained through deduction:
wherein,
the position coordinates of three acoustic beacons are selected at will, and the following calculation formula can be obtained by applying the position coordinates to the interrelationship formula:
calculating according to the calculation formula to obtain a first transformation matrixValues for points in each row and each column.
Step S14: and calculating the attitude information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system.
In the embodiment of the invention, each point in the Euler angle relation matrix of the underwater robot relative to the reference coordinate system and each point in the first transformation matrix are in one-to-one correspondence:
wherein θ is a pitch angle of the underwater robot, ψ is a heading angle of the underwater robot, and φ is a roll angle of the underwater robot.
In step S14, optionally, trigonometric function values of each angle in the gesture information are calculated according to a one-to-one correspondence between each point in the euler angle relation matrix and each point in the first transformation matrix, and each angle in the gesture information is calculated according to the trigonometric function values, where the gesture information includes a heading angle, a pitch angle, and a roll angle.
Specifically, firstly, determining a sine value of the pitch angle according to the value of the first row and the third column in the first transformation matrix and the Euler angle relation matrix, and calculating the possible pitch angle according to the sine value of the pitch angle. According to the value t of the third column of the first row in the first transformation matrix 13 Determining a sine value sin theta of a pitch angle by using the Euler angle relation matrix, and further determining the sine value sin of the pitch angleThe calculated pitch angle θ may be θ=arcsin (-t) 13 ) Or θ=pi-arcsin (-t) 13 )。
Then, acquiring sine values and cosine values of the course angle and sine values and cosine values of the roll angle according to values of points in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix; and respectively calculating the course angle and the roll angle according to the sine value and the cosine value of the course angle and the sine value and the cosine value of the roll angle.
In the embodiment of the invention, the candidate sine value and the candidate cosine value of the course angle are determined according to the values of the first column and the second column of the first row in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix. I.e. according to t in the first transformation matrix 11 、t 12 And pitch angle θ, cos ψ and sin ψ are calculated. And determining a candidate sine value and a candidate cosine value of the roll angle according to the values of the third columns of the second row and the third column of the third row in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix. I.e. according to t in the first transformation matrix 23 、t 33 And pitch angle θ, cos phi and sin phi are calculated. And determining the sine value and the cosine value of the course angle and the sine value and the cosine value of the roll angle according to the values of other points in the first transformation matrix, the candidate sine value and the candidate cosine value of the course angle and the candidate sine value and the candidate cosine value of the roll angle. The two different pitch angle theta calculation results and the corresponding cos phi, sin phi, cos phi and sin phi can be respectively calculated to be corresponding t 21 、t 22 、t 31 And t 32 And verifying the calculation result and t in the first transformation matrix 21 、t 22 、t 31 And t 32 Whether or not to agree with t in the first transformation matrix 21 、t 22 、t 31 And t 32 And (3) corresponding cos phi, sin phi, cos phi and sin phi of the consistent calculation results, and further respectively calculating the corresponding heading angle phi and roll angle phi of the underwater robot according to the cos phi, the sin phi, the cos phi and the sin phi.
So far, the calculated pitch angle theta, heading angle phi and roll angle phi jointly form the attitude information of the underwater robot relative to a reference coordinate system.
In the embodiment of the invention, the attitude angle estimation of the underwater robot based on the acoustic positioning system is performed by applying different coordinate system transformation relations, and the problem of time drift does not exist because a fixed earth coordinate system is used as a reference coordinate system, so that the method can be used for independently carrying out attitude measurement on the underwater robot and also can be used for calibrating a traditional inertial navigation attitude measurement system. In addition, by changing the reference coordinate system, for example, selecting to define the reference coordinate system on a ship or an underwater working platform, the attitude angle information of the underwater robot relative to the reference coordinate system can be conveniently obtained, and further, certain work tasks needing cooperative operation can be conveniently completed.
According to the underwater robot attitude estimation method, first position information of an acoustic beacon on a reference coordinate system is obtained, wherein a plurality of underwater acoustic positioning systems comprising the acoustic beacon and a transducer are arranged in the reference coordinate system; measuring second position information of the acoustic beacon relative to the transducer through the transducer, and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, wherein the transducer is arranged on the underwater robot; acquiring a first transformation matrix for transforming the reference coordinate system into the robot body coordinate system according to the first position information and the third position information; according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system, calculating the attitude information of the underwater robot relative to the reference coordinate system at the current moment, and accurately measuring the attitude information of the underwater robot relative to different reference coordinate systems by installing acoustic beacons in different reference coordinate systems.
The foregoing describes certain embodiments of the present invention. In some cases, the acts or steps recited in the embodiments of the present invention may be performed in a different order than in the embodiments and still achieve desirable results. In addition, the processes depicted in the accompanying figures do not necessarily require the particular order shown, or sequential order, to achieve desirable results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.
Based on the same conception, the embodiment of the invention also provides an underwater robot attitude estimation device. An automatic driving device or an auxiliary driving device applied to the vehicle. As shown in fig. 3, the underwater robot pose estimation apparatus includes: a position acquisition unit, a position conversion unit, a transformation matrix acquisition unit, and a posture calculation unit. Wherein,
a position acquisition unit for acquiring first position information of an acoustic beacon for a reference coordinate system in which a plurality of underwater acoustic positioning systems including the acoustic beacon and a transducer are arranged;
the position conversion unit is used for measuring second position information of the acoustic beacon relative to the transducer through the transducer and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, wherein the transducer is arranged on the underwater robot;
a transformation matrix acquisition unit, configured to acquire a first transformation matrix transformed from the reference coordinate system to the robot body coordinate system according to the first position information and the third position information;
and the gesture calculation unit is used for calculating the gesture information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system.
For convenience of description, the above devices are described as being functionally divided into various modules, respectively. Of course, the functions of each module may be implemented in the same piece or pieces of software and/or hardware when implementing the embodiments of the present invention.
The device of the above embodiment is applied to the corresponding method of the foregoing embodiment, and has the beneficial effects of the corresponding method embodiment, which are not described herein.
Based on the same inventive concept, the embodiment of the present invention further provides an electronic device, which includes a memory, a processor, and a computer program stored on the memory and executable on the processor, where the processor executes the program to implement the method according to any one of the embodiments above.
Embodiments of the present invention provide a non-transitory computer storage medium storing at least one executable instruction for performing a method as described in any of the embodiments above.
Fig. 4 shows a more specific hardware architecture of an electronic device according to this embodiment, where the device may include: a processor 401, a memory 402, an input/output interface 403, a communication interface 404, and a bus 405. Wherein the processor 401, the memory 402, the input/output interface 403 and the communication interface 404 are in communication connection with each other inside the device via a bus 405.
The processor 401 may be implemented by a general purpose CPU (Central Processing Unit ), a microprocessor, an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), or one or more integrated circuits, etc. for executing relevant programs to implement the technical solutions provided by the method embodiments of the present invention.
The Memory 402 may be implemented in the form of ROM (Read Only Memory), RAM (Random AccessMemory ), static storage device, dynamic storage device, or the like. Memory 402 may store an operating system and other application programs, and when implementing the solutions provided by the method embodiments of the present invention by software or firmware, the relevant program code is stored in memory 402 and invoked for execution by processor 401.
The input/output interface 403 is used to connect with an input/output module to realize information input and output. The input/output module may be configured as a component in a device (not shown) or may be external to the device to provide corresponding functionality. Wherein the input devices may include a keyboard, mouse, touch screen, microphone, various types of sensors, etc., and the output devices may include a display, speaker, vibrator, indicator lights, etc.
The communication interface 404 is used to connect a communication module (not shown in the figure) to enable communication interaction between the present device and other devices. The communication module may implement communication through a wired manner (such as USB, network cable, etc.), or may implement communication through a wireless manner (such as mobile network, WIFI, bluetooth, etc.).
Bus 405 includes a path to transfer information between components of the device (e.g., processor 401, memory 402, input/output interface 403, and communication interface 404).
It should be noted that, although the above device only shows the processor 401, the memory 402, the input/output interface 403, the communication interface 404, and the bus 405, in the implementation, the device may further include other components necessary for realizing normal operation. Furthermore, it will be understood by those skilled in the art that the above-described apparatus may include only the components necessary for implementing the embodiments of the present invention, and not all the components shown in the drawings.
Those of ordinary skill in the art will appreciate that: the discussion of any of the embodiments above is merely exemplary and is not intended to suggest that the scope of the application is limited to these examples; the technical features of the above embodiments or in different embodiments may also be combined under the idea of the present application, the steps may be implemented in any order, and there are many other variations of the different aspects of the present application as described above, which are not provided in details for the sake of brevity.
This application is intended to cover all such alternatives, modifications, and variations that fall within the broad scope of the embodiments of the invention. Therefore, any omissions, modifications, equivalents, improvements and others that are within the spirit and principles of the embodiments are intended to be included within the scope of the present application.
Claims (8)
1. An underwater robot pose estimation method, comprising:
acquiring first position information of an acoustic beacon for a reference coordinate system, wherein a plurality of underwater acoustic positioning systems comprising the acoustic beacon and a transducer are arranged in the reference coordinate system;
measuring second position information of the acoustic beacon relative to the transducer through the transducer, and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, wherein the transducer is arranged on the underwater robot;
acquiring a first transformation matrix for transforming the reference coordinate system into the robot body coordinate system according to the first position information and the third position information, wherein a correlation formula among the first position information, the third position information and the first transformation matrix is acquired, and the correlation formula is that a matrix product of the first transformation matrix and the third position information is equal to a difference value of a second position deviation of the first position information and an origin of the robot coordinate system in the reference coordinate system; acquiring position coordinates of any three acoustic beacons, and applying the position coordinates of the three acoustic beacons to the correlation formula to calculate to obtain values of points in each row and each column in the first transformation matrix;
calculating the attitude information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and the Euler angle relation matrix of the underwater robot relative to the reference coordinate system, wherein the trigonometric function value of each angle in the attitude information is calculated according to the one-to-one correspondence between each point in the Euler angle relation matrix and each point in the first transformation matrix, and each angle in the attitude information is calculated according to the trigonometric function value, and the attitude information comprises a course angle, a pitch angle and a roll angle.
2. The method of claim 1, wherein the obtaining third location information of the acoustic beacon relative to the underwater robot based on the second location information comprises:
acquiring an installation angle and an installation position of the transducer on the underwater robot;
and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, the installation angle of the transducer on the underwater robot and the installation position.
3. The method of claim 2, wherein the obtaining third position information of the acoustic beacon relative to the underwater robot based on the second position information and the mounting angle and the mounting position of the transducer on the underwater robot comprises:
determining a second transformation matrix from the transducer coordinate system to the underwater robot coordinate system according to the installation angle and the installation position;
and summing the matrix product of the second transformation matrix and the second position information with the first position deviation of the origin of the transducer coordinate system in the robot coordinate system, and acquiring the third position information of the acoustic beacon relative to the underwater robot.
4. The method of claim 1, wherein the calculating the trigonometric function value of each angle in the pose information according to the one-to-one correspondence between each point in the euler angle relationship matrix and each point in the first transformation matrix, and calculating each angle in the pose information according to the trigonometric function value, respectively, includes:
determining a sine value of the pitch angle according to the value of a first row and a third column in the first transformation matrix and the Euler angle relation matrix, and calculating the possible pitch angle according to the sine value of the pitch angle;
acquiring sine values and cosine values of the course angle and sine values and cosine values of the roll angle according to values of points in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix;
and respectively calculating the course angle and the roll angle according to the sine value and the cosine value of the course angle and the sine value and the cosine value of the roll angle.
5. The method of claim 4, wherein the obtaining the sine and cosine values of the heading angle and the sine and cosine values of the roll angle from the values of the points in the first transformation matrix, the pitch angle and the euler angle relation matrix, comprises:
determining a candidate sine value and a candidate cosine value of the course angle according to values of a first column and a second column of a first row in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix;
determining a candidate sine value and a candidate cosine value of the roll angle according to values of a second row, a third column and a third row in the first transformation matrix, the possible pitch angle and the Euler angle relation matrix;
and determining the sine value and the cosine value of the course angle and the sine value and the cosine value of the roll angle according to the values of other points in the first transformation matrix, the candidate sine value and the candidate cosine value of the course angle and the candidate sine value and the candidate cosine value of the roll angle.
6. An underwater robot pose estimation device, the device comprising:
a position acquisition unit for acquiring first position information of an acoustic beacon for a reference coordinate system in which a plurality of underwater acoustic positioning systems including the acoustic beacon and a transducer are arranged;
the position conversion unit is used for measuring second position information of the acoustic beacon relative to the transducer through the transducer and acquiring third position information of the acoustic beacon relative to the underwater robot according to the second position information, wherein the transducer is arranged on the underwater robot;
a transformation matrix obtaining unit, configured to obtain a first transformation matrix for transforming the reference coordinate system into a robot body coordinate system according to the first position information and the third position information, where an interrelationship between the first position information, the third position information, and the first transformation matrix is obtained, and the interrelationship is that a matrix product of the first transformation matrix and the third position information is equal to a difference value of a second position deviation of an origin of the first position information and the robot coordinate system in the reference coordinate system; acquiring position coordinates of any three acoustic beacons, and applying the position coordinates of the three acoustic beacons to the correlation formula to calculate to obtain values of points in each row and each column in the first transformation matrix;
and the gesture calculation unit is used for calculating gesture information of the underwater robot relative to the reference coordinate system at the current moment according to the first transformation matrix and an Euler angle relation matrix of the underwater robot relative to the reference coordinate system, wherein trigonometric function values of all angles in the gesture information are calculated according to one-to-one correspondence between each point in the Euler angle relation matrix and each point in the first transformation matrix, and all angles in the gesture information are calculated according to the trigonometric function values, and the gesture information comprises a course angle, a pitch angle and a roll angle.
7. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the method of any one of claims 1-5 when the program is executed by the processor.
8. A computer storage medium having stored therein at least one executable instruction for causing a processor to perform the method of any one of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210366748.8A CN114770598B (en) | 2022-04-08 | 2022-04-08 | Underwater robot attitude estimation method and device, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210366748.8A CN114770598B (en) | 2022-04-08 | 2022-04-08 | Underwater robot attitude estimation method and device, electronic equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114770598A CN114770598A (en) | 2022-07-22 |
| CN114770598B true CN114770598B (en) | 2024-01-26 |
Family
ID=82427093
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210366748.8A Active CN114770598B (en) | 2022-04-08 | 2022-04-08 | Underwater robot attitude estimation method and device, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114770598B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH06131032A (en) * | 1992-04-14 | 1994-05-13 | Hitachi Ltd | Robot device and teaching method for robot device |
| CN104197927A (en) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | Real-time navigation system and real-time navigation method for underwater structure detection robot |
| CN107883951A (en) * | 2017-10-19 | 2018-04-06 | 福建海图智能科技有限公司 | A kind of computational methods and terminal of underwater robot 3 d pose |
| CN107995885A (en) * | 2016-11-18 | 2018-05-04 | 深圳配天智能技术研究院有限公司 | A kind of coordinate system scaling method, system and device |
| CN108801250A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Real-time attitude acquisition methods based on underwater robot and device |
| CN209764112U (en) * | 2019-05-27 | 2019-12-10 | 河海大学 | Small-sized cableless underwater robot combined navigation device |
| CN112857398A (en) * | 2021-01-12 | 2021-05-28 | 中国人民解放军海军工程大学 | Rapid initial alignment method and device for ships in mooring state |
-
2022
- 2022-04-08 CN CN202210366748.8A patent/CN114770598B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH06131032A (en) * | 1992-04-14 | 1994-05-13 | Hitachi Ltd | Robot device and teaching method for robot device |
| CN104197927A (en) * | 2014-08-20 | 2014-12-10 | 江苏科技大学 | Real-time navigation system and real-time navigation method for underwater structure detection robot |
| CN107995885A (en) * | 2016-11-18 | 2018-05-04 | 深圳配天智能技术研究院有限公司 | A kind of coordinate system scaling method, system and device |
| CN107883951A (en) * | 2017-10-19 | 2018-04-06 | 福建海图智能科技有限公司 | A kind of computational methods and terminal of underwater robot 3 d pose |
| CN108801250A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Real-time attitude acquisition methods based on underwater robot and device |
| CN209764112U (en) * | 2019-05-27 | 2019-12-10 | 河海大学 | Small-sized cableless underwater robot combined navigation device |
| CN112857398A (en) * | 2021-01-12 | 2021-05-28 | 中国人民解放军海军工程大学 | Rapid initial alignment method and device for ships in mooring state |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114770598A (en) | 2022-07-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sheng et al. | MEMS-based low-cost strap-down AHRS research | |
| US7197836B2 (en) | Reference coordinate calculating method, reference coordinate calculating program, recording medium thereof, reference plate and form measuring machine | |
| CN113804191B (en) | Mobile device and method for calculating gesture of mobile device | |
| CN106767673B (en) | A kind of direction measurement method of satellite high-precision optical sensitive load | |
| CN102629193A (en) | Software framework and development platform for multi-sensor systems | |
| CN103737433B (en) | Large-size components combined type accuracy measurement method | |
| KR20070043009A (en) | Method and apparatus for calibrating the rotational relationship between two motion sensors of a sensor system | |
| CN106405581B (en) | Pointing accuracy appraisal procedure is coupled between polymorphic type load caused by satellite structure deforms | |
| EP2972683B1 (en) | Dynamically calibrating magnetic sensors | |
| CN113211445A (en) | Robot parameter calibration method, device, equipment and storage medium | |
| CN114770598B (en) | Underwater robot attitude estimation method and device, electronic equipment and storage medium | |
| CN101674358A (en) | Three-dimensional space locating mobile communication device | |
| CN201491271U (en) | Three-dimensional space positioning mobile communication terminal | |
| CN110954080A (en) | Magnetic compass calibration method for eliminating carrier magnetic interference | |
| CN111862141B (en) | AGV device motion precision evaluation method and device, medium and electronic equipment | |
| CN108956003B (en) | Method and device for calibrating six-dimensional sensor posture in real time and terminal equipment | |
| CN110842917B (en) | Method for calibrating mechanical parameters of series-parallel connection machinery, electronic device and storage medium | |
| CN114659709A (en) | Three-dimensional mass center measuring method for large winged spacecraft | |
| Milovzorov et al. | Mathematical models of inclination parameter converters based on double-axis accelerometers | |
| CN112633043B (en) | Lane line determining method and device, electronic equipment and storage medium | |
| KR101391764B1 (en) | The method of harmonization between inertial navigation system and total station | |
| EP3585568B1 (en) | Method and apparatus for selecting initial point for industrial robot commissioning | |
| KR20170092356A (en) | System for calibrating azimuth of 3-axis magnetic sensor | |
| CN110879066A (en) | Attitude calculation algorithm and device and vehicle-mounted inertial navigation system | |
| CN106092100B (en) | The equivalent ship at measuring and controlling equipment of surveying vessel inertial navigation TT&C ship swaying data is shaken into equivalent method |
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 |