Underwater relative pose sensing method based on pressure dot matrix measurement system

Abstract

The invention discloses an underwater relative pose sensing method based on a pressure dot matrix measurement system, and the method mainly comprises the steps: installing the pressure dot matrix measurement system on an underwater robot, and enabling one or more compression springs of the pressure dot matrix measurement system to be squeezed by a to-be-detected target structure to generate pressure; the real compression amount of the compression spring is obtained, the relative distance and posture information of the underwater robot relative to the current to-be-detected target structure are calculated, a corresponding underwater relative posture feedback control instruction is output, and autonomous stable control over the posture of the underwater robot relative to the surface of the to-be-detected target structure in the detection operation process is achieved. According to the method, the distance and the attitude of the underwater robot relative to the measured target structure are calculated by using the pressure dot matrix feedback data, and for the robot needing to approach the surface of the underwater structure for observation, the sensing scheme blank of relative positioning data missing under a short-distance condition when laser or visual ranging fails in a muddy water environment is filled up, and the accuracy of the sensing scheme is improved. The important application value is realized.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater robot automation, and in particular relates to an underwater relative posture sensing method based on a pressure dot matrix measurement system. Background Art

[0002] With the popularization and application of underwater robot technology, more and more underwater structure inspection operations have begun to use underwater robots with a higher degree of automation to replace traditional divers to explore, to the greatest extent possible to reduce costs, reduce personnel risks, and improve work efficiency. When performing underwater structure inspection operations, underwater robots need to overcome adverse underwater environmental influences (such as water pressure, low light, turbid water, etc.) and get as close to the surface of the structure as possible to obtain sufficiently clear image data. In order to ensure the stability of image acquisition during the operation, the position (vertical spacing is the most important) and posture of the robot relative to the surface of the structure to be inspected must be maintained stably, which requires the robot to have the ability to autonomously control its own posture state relative to the surface of the object to be measured. The premise for realizing this function is to effectively measure the current distance and posture of the robot relative to the surface of the object to be measured.

[0003] Existing distance measurement methods mainly include underwater laser and ultrasonic distance measurement. Underwater laser distance measurement has high accuracy and fast response under ideal water quality, while acoustic distance measurement can be applied to large-scale distance measurement under various water qualities, but the measurement accuracy is not high and the timeliness is lower than that of laser distance measurement. More importantly, these two types of distance measurement methods cannot be well applied to underwater structure detection tasks. Taking the detection of inland river piers with the most complex environment as an example, inland river basins generally have high flow velocity, shallow riverbed, and high sediment content in the water. In this environment, the laser distance measurement is extremely compressed, and it is easy to cause serious deviations in the measured distance due to the strong reflection of suspended particles, and the practical effect is extremely limited. On the other hand, in turbid scenes, the robot needs to be very close to the surface of the pier to see the surface of the structure clearly. In this case, it is very likely that the minimum range of the ranging sonar will not be reached, and the accuracy fluctuates severely. At the same time, there is no relevant technology that can directly measure the posture of the underwater robot relative to the object being measured. Therefore, the existing technical means cannot well meet the requirements of the robot's posture perception relative to the surface of the object being measured. Summary of the invention

[0004] The purpose of the present invention is to provide an underwater relative posture perception method based on a pressure dot matrix measurement system to address the above shortcomings. This patent mainly uses the pressure dot matrix feedback data to solve the data of the relative distance and posture of the underwater robot, and obtains the actual spatial position and posture of the underwater robot so that the underwater robot can perform subsequent operation tasks. For robots that need to approach the surface of underwater structures for observation, this method fills the gap in the existing sensing scheme of the lack of relative positioning data under close-range conditions, and has important application value.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions:

[0006] An underwater relative posture perception method based on a pressure dot matrix measurement system comprises the following steps:

[0007] Step 1: Install a pressure matrix measurement system on the underwater robot;

[0008] Step 2: Navigate the underwater robot to the vicinity of the surface of the target structure to be detected by remote control or autonomous indexing;

[0009] Step 3: Using the first-person perspective information returned by the onboard camera of the underwater robot, adjust the body posture of the underwater robot so that the pressure dot matrix measurement system faces the surface of the target structure to be detected;

[0010] Step 4: Control the underwater robot to move closer to the target structure to be detected, so as to force one or more compression springs of the pressure matrix measurement system to be squeezed by the target structure to be detected. At this time, the squeezed compression spring simultaneously generates pressure on the pressure sensor of the corresponding pressure matrix measurement system. The real compression amount of the compression spring can be obtained by combining the real-time pressure measurement signal of the pressure sensor with the "force-deformation" model of the compression spring.

[0011] Step 5: Calculate the relative distance and posture information of the underwater robot relative to the current target structure to be detected by the actual compression amount of the compression spring;

[0012] Step six, based on the relative distance and posture information of the underwater robot to the current target structure to be detected, formulate and output corresponding underwater relative posture feedback control instructions to achieve autonomous and stable control of the posture of the underwater robot relative to the surface of the target structure to be detected during the detection operation.

[0013] The present invention further illustrates that the pressure matrix measurement system includes a mounting base plate and a pressure measuring device; at least four pressure measuring devices are arranged on the mounting base plate; the pressure measuring device is composed of a compression spring, a pressure sensor, a spherical universal wheel and an axially movable positioning optical axis; the pressure sensor is fixed to a corresponding position of the mounting base plate; one end of the compression spring is pressed on the pressure sensor, and the other end is connected to the spherical universal wheel; one end of the axially movable positioning optical axis directly passes through the inner center of the compression spring and is connected to the inner side of the spherical universal wheel to guide the compression spring only along the axial direction, and the other end moves with low resistance along the corresponding penetrating shaft hole on the mounting base plate.

[0014] The present invention further describes that the compression spring is first subjected to a quantitative compression test before installation to obtain matching data between different compression amounts and pressures, and to fit a "force-deformation" model of the compression spring.

[0015] The present invention further illustrates that the calculation process of step five is:

[0016] Remember to compress the spring at Q m -x m y m The projection points in the plane are P1, P2, P3, and P4, and their coordinates are (-a, -b, 0), (a, -b, 0), (a, b, 0), and (-a, b, 0), respectively; the contact points between the spherical universal wheels at the ends of the compression springs and the surface of the object are T1, T2, T3, and T4, and their coordinates are (-a, -b, z1), (a, -b, z2), (a, b, z3), and (-a, b, z4), respectively, where z i (i=1,2,3,4) represents the compression amount of the i-th compression spring;

[0017] Assume that the external normal unit vector of the reference end surface P1P2P3P4 on the underwater robot is in the reference coordinate system {O m} is represented by n m =[0,0,1] T ;

[0018] Using any three points of the contact points T1, T2, T3, and T4 to form a plane triangle, we get four plane combinations, namely ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, and ΔT3T4T1. Let their corresponding external normal unit vectors be n m1 、n m2 、n m3 、n m4 ;

[0019] According to the four combinations of ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, and ΔT3T4T1, two sides of each triangle are selected as orthogonal vectors in the plane according to the right-hand rule as follows:

[0020] For ΔT4T1T2, the vector is selected as and

[0021] For ΔT1T2T3, the vector is selected as and

[0022] For ΔT2T3T4, the vector is selected as and

[0023] For ΔT3T4T1, the vector is selected as and

[0024] According to the vector cross product operation and the right-hand rule, the outer normal unit vector corresponding to the triangle plane is:

[0025]

[0026] in, is a vector The Euclidean length of

[0027] Similarly,

[0028]

[0029] in,

[0030] Based on the ZYX rotation rule to describe the rotation relationship, each triangular plane ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, ΔT3T4T1 can be regarded as the reference plane P1P2P3P4 on the underwater robot through ① zero yaw angle, that is, ψ=0; ② pitch angle, that is ③Roll angle, that is After three rotations, we get: This rotation can be represented by the rotation matrix R∈SO(3);

[0031] Take the plane triangle ΔT4T1T2 as an example, assuming that relative to the reference plane P1P2P3P4, there is ψ1=0, The reference coordinate system originally located in the reference plane {O m}rotated to the coordinate system in the triangular plane ΔT4T1T2{O m1}; and the rotation matrix R1 defined by this rotation is:

[0032]

[0033] Among them, R1 satisfies the following formula:

[0034] R1n m1 =e3(1.6)

[0035] Where, e3 = [0,0,1] T For m1} m1 The axis unit vector is in {O m1} expression under the system;

[0036] According to formula (1.6),

[0037]

[0038] (That is: according to formula (1.6), multiply both sides by the inverse matrix of R1 (for the special matrix R1, its inverse matrix is ​​equal to its transposed matrix), and then compare the three components on both sides of the equation to obtain formula (1.7)).

[0039] According to the second formula of formula (1.7),

[0040]

[0041] According to the first and third equations of equation (1.7),

[0042]

[0043] Similarly, the rotation relationship between the triangular planes ΔT1T2T3, ΔT2T3T4, ΔT3T4T1 and the reference plane P1P2P3P4 is represented by the rotation matrices R2, R3, and R4, respectively, and satisfies the following conditions, respectively.

[0044]

[0045] Then the corresponding external normal unit vector n mi , (i=2,3,4) calculate the corresponding attitude angle:

[0046]

[0047] Finally, the average distance between the reference plane P1P2P3P4 and the surface of the target structure currently being detected represented by the four pressure contact points T1T2T3T4 is obtained. and average posture for:

[0048]

[0049] The present invention further illustrates that the above calculation method is not limited to the array form of 4 pressure measurement units. Theoretically, as long as there are 3 groups of non-collinear pressure measurement points T i (i=1,2,...,k, and k≥3), the average distance can be calculated and average posture The more measurement points there are, the less sensitive the calculated average result is to the measurement noise introduced by each measurement unit.

[0050] The measuring system is generally fixed on the underwater robot structure by threaded connection or other connection methods. Generally speaking, when the underwater robot is selected for inspection, the measuring system needs to be installed on the side facing the surface to be inspected.

[0051] Advantages of the present invention:

[0052] 1. The present invention can be used to reliably realize the distance and posture perception of the underwater robot relative to the measured structure under any water environment conditions (including turbid water), which is an effective method for the underwater robot to make up for the failure of conventional acoustic and optical detection in harsh underwater conditions.

[0053] 2. The present invention provides a new underwater relative posture perception method based on pressure feedback.

[0054] 3. Based on the relative posture perception information obtained, the underwater robot can achieve autonomous control to maintain a stable distance and angle relative to the target structure when facing the structure to be inspected, without the operator having to pay attention to and adjust these degrees of freedom in real time. During the operation, the operator only needs to control the robot to move up and down and left and right on the surface of the structure to be inspected, which greatly simplifies the remote control difficulty of the inspection operation, improves the efficiency of searching for surface defects of the structure and the clarity of the collected images. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 It is a schematic structural diagram of an underwater robot equipped with a pressure matrix measurement system in one embodiment of the present invention.

[0056] Figure 2 Schematic diagram of the structure of a pressure matrix measurement system in one embodiment of the present invention.

[0057] Figure 3 It is a front view of a pressure matrix measurement system and its key geometric relationship diagram in one embodiment of the present invention.

[0058] Figure 4 It is an oblique view of a pressure matrix measurement system and its key geometric relationship diagram in one embodiment of the present invention.

[0059] Figure 5 1 is a coordinate rotation transformation relationship diagram of the contact plane and the robot reference plane of the pressure matrix measurement system in one embodiment of the present invention.

[0060] Figure 6 This is a diagram of the architecture of the relative posture stabilization control system based on the relative posture measurement of the underwater pressure lattice in the present invention.

[0061] Figure numerals: 1-underwater robot body, 2-horizontal thruster, 3-vertical thruster, 4-mounting base plate, 5-compression spring, 6-pressure sensor, 7-spherical universal wheel, 8-axially movable positioning optical axis. DETAILED DESCRIPTION

[0062] The present invention is further described below in conjunction with the accompanying drawings.

[0063] Embodiment 1:

[0064] An underwater relative posture perception method based on a pressure dot matrix measurement system is characterized by comprising the following steps:

[0065] Step 1, such as Figure 1 As shown, a pressure matrix measurement system is installed on the underwater robot;

[0066] Step 2: Navigate the underwater robot to the vicinity of the surface of the target structure to be detected by remote control or autonomous indexing;

[0067] Step 3: Using the first-person perspective information returned by the onboard camera of the underwater robot, adjust the body posture of the underwater robot so that the pressure dot matrix measurement system faces the surface of the target structure to be detected;

[0068] Step 4: Control the underwater robot to move closer to the target structure to be detected, so as to force one or more compression springs of the pressure matrix measurement system to be squeezed by the target structure to be detected. At this time, the squeezed compression spring simultaneously generates pressure on the pressure sensor of the corresponding pressure matrix measurement system. The real compression amount of the compression spring can be obtained by combining the real-time pressure measurement signal of the pressure sensor with the "force-deformation" model of the compression spring.

[0069] Step 5: Calculate the relative distance and posture information of the underwater robot relative to the current target structure to be detected by the actual compression amount of the compression spring;

[0070] Step 6: According to the relative distance and posture information of the underwater robot to the current target structure to be detected, the corresponding underwater relative posture feedback control scheme architecture can be designed (such as Figure 6 The underwater robot can also be used to detect the surface of the target structure and output the corresponding underwater relative posture feedback control instructions to achieve autonomous and stable posture control of the underwater robot relative to the surface of the target structure to be detected during the detection operation.

[0071] Taking the example of keeping the underwater robot facing the surface of the structure being measured and keeping the distance constant, the relative posture tracking target set at this time is and ψ d =θ d =φ d =0.

[0072] This embodiment further illustrates that the pressure matrix measurement system includes a mounting base plate 4 and a pressure measurement device; four pressure measurement devices are arranged on the mounting base plate 4, as shown in FIG. Figure 2 , Figure 3 and Figure 4 As shown: Take the center of the mounting base plate 4 as the origin, and mark the lower direction as X m Axis positive direction, right direction is marked as Y m The positive direction of the axis is determined according to the right-hand screw rule, and the direction perpendicular to the mounting base plate 4 is marked as Z.m The positive direction of the axis forms an orthogonal coordinate system Q based on the surface of the mounting base 4. m -x m y m z m , denoted as the reference coordinate system {O m}; According to this coordinate system, pressure measuring devices are installed at the positions (a, b), (a, -b), (-a, b), and (-a, -b) in the four quadrants of the mounting base 4 respectively.

[0073] The pressure measuring device is composed of a compression spring 5, a pressure sensor 6, a spherical universal wheel 7 and an axially movable positioning optical axis 8; the pressure sensor 6 is fixed to the corresponding position of the mounting base plate 4; one end of the compression spring 5 is pressed on the pressure sensor 6, and the other end is connected to the spherical universal wheel 7; one end of the axially movable positioning optical axis 8 directly passes through the inner center of the compression spring 5 and is connected to the inner side of the spherical universal wheel 7 to guide the compression spring 5 only along the axial direction, and the other end moves with low resistance along the corresponding penetrating shaft hole on the mounting base plate 4.

[0074] This embodiment further illustrates that the compression spring 5 is subjected to a quantitative compression test before installation to obtain matching data between different compression amounts and pressures, and to fit the "force-deformation" model of the compression spring 5, thereby providing basic knowledge for the subsequent relative posture solution algorithm in real time.

[0075] Further explanation of this embodiment, the calculation process of the above step 5 is specifically as follows:

[0076] like Figure 2 , Figure 3 , Figure 4 , Figure 5 As shown in the figure, take the arrangement of four pressure measuring devices on the mounting base as an example, note that the compression springs at four locations are at Q m -x m y m The projection points in the plane are P1, P2, P3, and P4, and their coordinates are (-a, -b, 0), (a, -b, 0), (a, b, 0), and (-a, b, 0), respectively; the contact points between the spherical universal wheels at the ends of the compression springs and the surface of the object are T1, T2, T3, and T4, and their coordinates are (-a, -b, z1), (a, -b, z2), (a, b, z3), and (-a, b, z4), respectively, where z i (i=1,2,3,4) represents the compression amount of the i-th compression spring; through the following calculation, the distance and posture of the virtual plane composed of the four points T1, T2, T3, and T4 (which is also the structural surface contacted by the underwater robot) relative to the end face P1P2P3P4 of the underwater robot can be obtained, and finally the robot can realize the self-perception of the relative posture information of the current position relative to the surface of the structure to be detected in front.

[0077] Assume that the unit vector of the external normal (pointing to the target structure to be detected) of the reference end surface P1P2P3P4 on the underwater robot in the reference coordinate system {Om} is expressed as n m =[0,0,1] T ;

[0078] Using any three points of the contact points T1, T2, T3, and T4 to form a plane triangle, we get four plane combinations, namely ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, and ΔT3T4T1. Let their corresponding external normal unit vectors be n m1 、n m2 、n m3 、n m4 ;

[0079] According to the four combinations of ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, and ΔT3T4T1, two sides of each triangle are selected as orthogonal vectors in the plane according to the right-hand rule as follows:

[0080] For ΔT4T1T2, the vector is selected as and

[0081] For ΔT1T2T3, the vector is selected as and

[0082] For ΔT2T3T4, the vector is selected as and

[0083] For ΔT3T4T1, the vector is selected as and

[0084] According to the vector cross product operation and the right-hand rule, the outer normal unit vector corresponding to the triangle plane is:

[0085]

[0086] in, is a vector The Euclidean length of

[0087] Similarly,

[0088]

[0089] in,

[0090] Based on the ZYX rotation rule to describe the rotation relationship, each triangular plane ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, ΔT3T4T1 can be regarded as the reference plane P1P2P3P4 on the underwater robot through ① zero yaw angle, that is, ψ=0; ② pitch angle, that is ③Roll angle, that is After three rotations, we get: This rotation can be represented by the rotation matrix R∈SO(3);

[0091] Take the plane triangle ΔT4T1T2 as an example, assuming that relative to the reference plane P1P2P3P4, there is ψ1=0, The reference coordinate system originally located in the reference plane {O m}rotated to the coordinate system in the triangular plane ΔT4T1T2{O m1}(like Figure 5 ); and the rotation matrix R1 defined by this rotation is:

[0092]

[0093] Among them, R1 satisfies the following formula:

[0094] R1n m1 =e3(1.6)

[0095] Where, e3 = [0,0,1] T For m1} m1 The axis unit vector is in {O m1} expression under the system;

[0096] According to formula (1.6),

[0097]

[0098] According to the second formula of formula (1.7),

[0099]

[0100] According to the first and third equations of equation (1.7),

[0101]

[0102] Similarly, the rotation relationship between the triangular planes ΔT1T2T3, ΔT2T3T4, ΔT3T4T1 and the reference plane P1P2P3P4 is represented by the rotation matrices R2, R3, and R4, respectively, and satisfies the following conditions, respectively.

[0103]

[0104] Then the corresponding external normal unit vector n mi , (i=2,3,4) calculate the corresponding attitude angle:

[0105]

[0106] Finally, the average distance between the reference plane P1P2P3P4 and the surface of the target structure currently being detected represented by the four pressure contact points T1T2T3T4 is obtained. and average posture for:

[0107]

[0108] It is worth noting that the present invention is not limited to the array form of four pressure measurement units in the above embodiment. In theory, as long as there are three groups of non-collinear pressure measurement points T i (i=1,2,...,k, and k≥3), the average distance can be calculated and average posture The more measurement points there are, the less sensitive the calculated average result is to the measurement noise introduced by each measurement unit.

[0109] Obviously, the above embodiments are only examples for clearly explaining the present invention, and are not intended to limit the implementation of the present invention. For ordinary technicians in the relevant technical field, other different forms of changes or modifications can be made based on the above description; it is not necessary and impossible to list all the implementation methods here; and the obvious changes or modifications derived from them are still within the protection scope of the present invention.

Claims

1. An underwater relative posture perception method based on a pressure dot matrix measurement system, characterized in that The following steps are involved: Step 1: Install a pressure matrix measurement system on the underwater robot; Step 2: Navigate the underwater robot to the vicinity of the surface of the target structure to be detected by remote control or autonomous indexing; Step 3: Using the first-person perspective information returned by the onboard camera of the underwater robot, adjust the body posture of the underwater robot so that the pressure dot matrix measurement system faces the surface of the target structure to be detected; Step 4: Control the underwater robot to move closer to the target structure to be detected, so as to force one or more compression springs of the pressure matrix measurement system to be squeezed by the target structure to be detected. At this time, the squeezed compression spring simultaneously generates pressure on the pressure sensor of the corresponding pressure matrix measurement system. The real compression amount of the compression spring can be obtained by combining the real-time pressure measurement signal of the pressure sensor with the "force-deformation" model of the compression spring. Step 5: Calculate the relative distance and posture information of the underwater robot relative to the current target structure to be detected by the actual compression amount of the compression spring; Step six, based on the relative distance and posture information of the underwater robot to the current target structure to be detected, formulate and output corresponding underwater relative posture feedback control instructions to achieve autonomous and stable control of the posture of the underwater robot relative to the surface of the target structure to be detected during the detection operation.

2. The underwater relative posture perception method based on the pressure dot matrix measurement system according to claim 1 is characterized in that: The pressure matrix measurement system comprises a mounting base plate (4) and a pressure measuring device; at least four pressure measuring devices are arranged on the mounting base plate (4); the pressure measuring device is composed of a compression spring (5), a pressure sensor (6), a spherical universal wheel (7) and an axially movable positioning optical axis (8); the pressure sensor (6) is fixed to a corresponding position of the mounting base plate (4); one end of the compression spring (5) is pressed on the pressure sensor (6), and the other end is connected to the spherical universal wheel (7); one end of the axially movable positioning optical axis (8) directly passes through the inner center of the compression spring (5) and is connected to the inner side of the spherical universal wheel (7) to guide the compression spring (5) only in the axial direction, and the other end moves with low resistance along the corresponding through-axis hole on the mounting base plate (4).

3. The underwater relative posture perception method based on the pressure dot matrix measurement system according to claim 2 is characterized in that: The compression spring (5) is first subjected to a quantitative compression test before installation to obtain matching data between different compression amounts and pressures, and to fit a "force-deformation" model of the compression spring (5).

4. The underwater relative posture perception method based on the pressure dot matrix measurement system according to claim 3 is characterized by: The calculation process of step 5 is specifically as follows: Take the arrangement of four pressure measuring devices on the mounting base plate (4) as an example, that is, the center of the mounting base plate (4) is taken as the origin, and the downward direction is marked as X. m Axis positive direction, right direction is marked as Y m The positive direction of the axis is determined according to the right-hand screw rule. The direction perpendicular to the mounting base (4) is marked as Z. m The positive direction of the axis forms an orthogonal coordinate system Q based on the surface of the mounting base plate (4). m -x m y m z m , denoted as the reference coordinate system {O m }; According to the coordinate system, pressure measuring devices are installed at the positions (a, b), (a, -b), (-a, b), and (-a, -b) in the four quadrants of the installation base plate (4); Remember to compress the spring at Q m -x m y m The projection points in the plane are P1, P2, P3, and P4, and their coordinates are (-a, -b, 0), (a, -b, 0), (a, b, 0), and (-a, b, 0), respectively; the contact points between the spherical universal wheels at the ends of the compression springs and the surface of the object are T1, T2, T3, and T4, and their coordinates are (-a, -b, z1), (a, -b, z2), (a, b, z3), and (-a, b, z4), respectively, where z i (i=1,2,3,4) represents the compression amount of the i-th compression spring; Assume that the external normal unit vector of the reference end surface P1P2P3P4 on the underwater robot is in the reference coordinate system {O m } is represented by n m =[0,0,1] T ; Using any three points of the contact points T1, T2, T3, and T4 to form a plane triangle, we get four plane combinations, namely ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, and ΔT3T4T1. Let their corresponding external normal unit vectors be n m1 、n m2 、n m3 、n m4 ; According to the four combinations of ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, and ΔT3T4T1, two sides of each triangle are selected as orthogonal vectors in the plane according to the right-hand rule as follows: For ΔT4T1T2, the vector is selected as and For ΔT1T2T3, the vector is selected as and For ΔT2T3T4, the vector is selected as and For ΔT3T4T1, the vector is selected as and According to the vector cross product operation and the right-hand rule, the outer normal unit vector corresponding to the triangle plane is: in, is a vector The Euclidean length of Similarly, in, Based on the ZYX rotation rule to describe the rotation relationship, each triangular plane ΔT4T1T2, ΔT1T2T3, ΔT2T3T4, ΔT3T4T1 can be regarded as the reference plane P1P2P3P4 on the underwater robot through ① zero yaw angle, that is, ψ=0; ② pitch angle, that is ③Roll angle, that is After three rotations, we get: This rotation can be represented by the rotation matrix R∈SO(3); Take the plane triangle ΔT4T1T2 as an example, assuming that relative to the reference plane P1P2P3P4, there is ψ1=0, The reference coordinate system originally located in the reference plane {O m }rotated to the coordinate system in the triangular plane ΔT4T1T2{O m1 }; and the rotation matrix R1 defined by this rotation is: Among them, R1 satisfies the following formula: R1n m1 =e3 (1.6) Where, e3 = [0,0,1] T For {O m1 } m1 The axis unit vector is in {O m1 } expression under the system; According to formula (1.6), According to the second formula of formula (1.7), According to the first and third equations of equation (1.7), Similarly, the rotation relationship between the triangular planes ΔT1T2T3, ΔT2T3T4, ΔT3T4T1 and the reference plane P1P2P3P4 is represented by the rotation matrices R2, R3, and R4, respectively, and satisfies the following conditions, respectively. Then the corresponding external normal unit vector n mi , (i=2,3,4) calculate the corresponding attitude angle: Finally, the average distance between the reference plane P1P2P3P4 and the surface of the current target structure represented by the four pressure contact points T1T2T3T4 is obtained. and average posture for:

Images