A method and device for positioning a subsea formation space robot

Abstract

The present invention relates to a method and device for positioning a subsea formation space robot. The positioning method includes the following steps: determining the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point; determining the transformation matrix between the moving coordinate systems of two adjacent key points; and determining the position coordinates of the last key point in the fixed coordinate system based on the projection and the transformation matrix, as the position coordinates of the subsea formation space robot. According to the data information of the connection base station and the key points arranged on the sensing and positioning cable of the subsea formation space robot, the present invention realizes the accurate positioning of the subsea formation space robot based on the coordinate system space transformation and matrix operation method.

Description

Technical Field

[0001] The present invention relates to the technical field of subsea formation space robots, and particularly to a positioning method and device for a subsea formation space robot. Background Art

[0002] The deep sea contains rich strategic resources, and the reserves of these resources can be used by human society for a long time. When carrying out the exploration and development of these resources, many researchers have deployed various subsea formation space robot devices, such as various drilling robots, into the deep sea formation, so as to realize the exploration and operation in the deep sea formation. When these subsea formation space robots operate in the deep sea formation, their positioning technology often plays a crucial role in whether the subsea formation space robot can work smoothly. Summary of the Invention

[0003] In view of this, the present invention provides a positioning method and device for a subsea formation space robot, so as to realize the accurate positioning of a subsea formation space robot operating in the deep sea formation.

[0004] To achieve the above object, the present invention provides the following solutions:

[0005] A positioning method for a subsea formation space robot, the positioning method includes the following steps:

[0006] According to the data information of each key point, determine the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point of the two key points; the key points are points arranged in ascending order of the distance from the base station on the sensing positioning cable connecting the base station and the subsea formation space robot, the position of the first key point is the same as the position of the base station, and the position of the last key point is the same as the position of the subsea space robot, and the data information includes acceleration information and magnetic field intensity information;

[0007] According to the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point of the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point, determine the transformation matrix between the moving coordinate systems of two adjacent key points;

[0008] According to the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point of the two key points and the transformation matrix between the moving coordinate systems of two adjacent key points, determine the position coordinates of the last key point in the fixed coordinate system, and use it as the position coordinates of the subsea formation space robot.

[0009] Optionally, according to the data information of each key point, the projections of the arc segment of the sensing and positioning cable between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points are as follows:

[0010]

[0011] Among them, dx i , dy i and dz i are respectively the projections of the arc segment of the sensing and positioning cable between the i-th and the (i + 1)-th key points on the x-axis, y-axis, and z-axis of the moving coordinate system of the i-th key point; Δφ i represents the rotation angle of the x-axis, Δφ i = φ i+1 - φ i , φ i and φ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the x-axis; Δψ i represents the rotation angle of the z-axis, Δψ i = ψ i+1 - ψ i , ψ i and ψ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the z-axis, k i represents the curvature corresponding to the arc-shaped sensing and positioning cable between the i-th and the (i + 1)-th key points;

[0012]

[0013]

[0014] Among them, m xi and m yi respectively represent the magnetic field intensities in the x-axis direction and y-axis direction in the magnetic field intensity information of the i-th key point, a xi , a yi and a zi respectively represent the accelerations in the x-axis direction, y-axis direction, and z-axis direction in the acceleration information of the i-th key point.

[0015] Optionally, according to the projections of the arc segment of the sensing and positioning cable between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point, the transformation matrix between the moving coordinate systems of two adjacent key points is determined as follows:

[0016]

[0017] Among them, dxi , dy i and dz i are the projections of the sensing and positioning cable arc segment between the i-th and the (i + 1)-th key points on the x-axis, y-axis, and z-axis of the moving coordinate system at the i-th key point, respectively;

[0018] [R i represents the intermediate rotation matrix;

[0019] represents the first spatial rotation matrix, represents the second spatial transformation matrix;

[0020]

[0021]

[0022] Δφ i represents the x-axis rotation angle, Δφ i = φ i+1 - φ i , φ i and φ i+1 represent the spatial rotation Euler angles of the moving coordinate systems of the i-th and the (i + 1)-th key points on the x-axis, respectively; Δψ i represents the z-axis rotation angle, Δψ i = ψ i+1 - ψ i , ψ i and ψ i+1 represent the spatial rotation Euler angles of the moving coordinate systems of the i-th and the (i + 1)-th key points on the z-axis, respectively.

[0023] Optionally, according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the respective coordinate axes of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of the two adjacent key points, determine the position coordinates of the last key point in the fixed coordinate system, and take it as the position coordinates of the subsea formation space robot: [F] = [T] -1 [M I-1

[0024] where, [F] represents the position coordinates of the last key point in the fixed coordinate system, T represents the total transformation matrix obtained by multiplying the transformation matrices between the fixed coordinate system and the moving coordinate system of the 1st key point, and between the moving coordinate systems of all adjacent two key points, and M I-1 represents the coordinate vector composed of the projections of the sensing and positioning cable arc segment between the I-th key point and the (I - 1)-th key point on the x-axis, y-axis, and z-axis of the moving coordinate system of the (I - 1)-th key point, and the I-th key point is the last key point.​

[0025] An undersea formation space robot positioning device, the positioning device comprising: a data acquisition module and a model calculation module;

[0026] The data acquisition module includes: a sensing and positioning cable with two ends respectively connected to a base station and an undersea formation space robot, a fixed IMU sensor fixed on the base station, and a plurality of mobile IMU sensors arranged in ascending order of distance from the base station on the sensor positioning cable, and the mobile IMU sensor farthest from the base station is located at one end of the sensing and positioning cable connected to the undersea formation space robot;

[0027] The fixed IMU sensor and each of the mobile IMU sensors are connected to the model calculation module; the model calculation module is configured to determine the position coordinates of the last key point as the position coordinates of the undersea formation space robot by using a positioning algorithm according to the data information of each key point obtained by the fixed IMU sensor and each of the mobile IMU sensors; wherein, the position where the key point is located corresponds to the position where the fixed IMU sensor or the mobile IMU sensor is located, and the data information includes acceleration information and magnetic field strength information.

[0028] Optionally, the model calculation module specifically includes:

[0029] A projection information acquisition sub-module, configured to determine the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point;

[0030] A transformation matrix determination sub-module, configured to determine the transformation matrix between the moving coordinate systems of two adjacent key points according to the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point;

[0031] A position coordinate determination sub-module, configured to determine the position coordinates of the last key point in the fixed coordinate system as the position coordinates of the undersea formation space robot according to the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of two adjacent key points.

[0032] Optionally, the projection information acquisition sub-module includes a projection information acquisition unit;

[0033] The projection information acquisition unit is used to determine the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point, specifically including: determining the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point as follows:

[0034]

[0035] where dx i , dy i and dz i are respectively the projections of the sensing and positioning cable arc segment between the i-th and the (i + 1)-th key points on the x-axis, y-axis and z-axis of the moving coordinate system of the i-th key point; Δφ i represents the x-axis rotation angle, Δφ i = φ i+1 - φ i , φ i and φ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th and the (i + 1)-th key points on the x-axis; Δψ i represents the z-axis rotation angle, Δψ i = ψ i+1 - ψ i , ψ i and ψ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th and the (i + 1)-th key points on the z-axis, and k i represents the curvature corresponding to the arc-shaped sensing and positioning cable between the i-th and the (i + 1)-th key points;

[0036]

[0037]

[0038] where m xi and m yi respectively represent the magnetic field intensities in the x-axis direction and y-axis direction in the magnetic field intensity information of the i-th key point, and a xi , a yi and a zi respectively represent the accelerations in the x-axis direction, y-axis direction and z-axis direction in the acceleration information of the i-th key point.

[0039] Optionally, the transformation matrix determination sub-module includes a transformation matrix determination unit;

[0040] The conversion matrix determination unit is configured to determine the conversion matrix between the moving coordinate systems of two adjacent key points as follows, based on the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of the key points:

[0041]

[0042] where dx i , dy i and dz i are respectively the projections of the sensing and positioning cable arc segment between the i-th and the (i + 1)-th key points on the x-axis, y-axis, and z-axis of the moving coordinate system of the i-th key point;

[0043] [R i represents an intermediate rotation matrix;

[0044] represents a first spatial rotation matrix, represents a second spatial conversion matrix;

[0045]

[0046]

[0047] Δφ i represents the rotation angle about the x-axis, Δφ i = φ i+1 - φ i , φ i and φ i+1 respectively represent the spatial rotation Euler angles about the x-axis of the moving coordinate systems of the i-th and the (i + 1)-th key points; Δψ i represents the rotation angle about the z-axis, Δψ i = ψ i+1 - ψ i , ψ i and ψ i+1 respectively represent the spatial rotation Euler angles about the z-axis of the moving coordinate systems of the i-th and the (i + 1)-th key points.

[0048] Optionally, the positioning device further includes: a visualization module;

[0049] The visualization module is configured to visualize the position coordinates of the subsea formation space robot and the movement trajectory of the subsea formation space robot.

[0050] A model calculation module.

[0051] According to the specific embodiments provided by the present invention, the following technical effects are disclosed by the present invention:

[0052] The present invention discloses a positioning method and device for a subsea formation space robot. The positioning method includes the following steps: according to the data information of each key point, determining the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points; determining the transformation matrix between the moving coordinate systems of two adjacent key points according to the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point; determining the position coordinates of the last key point in the fixed coordinate system according to the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of two adjacent key points, and taking the position coordinates as the position coordinates of the subsea formation space robot. The present invention realizes the accurate positioning of the subsea formation space robot based on the data information of the sensing positioning cable connecting the base station and the key points arranged on the subsea formation space robot, by means of coordinate system space transformation and matrix operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required to be used in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0054] Figure 1 It is a flowchart of a positioning method for a subsea formation space robot provided in Embodiment 1 of the present invention;

[0055] Figure 2 It is a schematic diagram of the established space coordinate system provided in Embodiment 1 of the present invention;

[0056] Figure 3 It is a schematic structural diagram of a positioning device for a subsea formation space robot provided in Embodiment 2 of the present invention;

[0057] Figure 4 It is an installation schematic diagram of a positioning device for a subsea formation space robot provided in Embodiment 2 of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0058] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0059] The object of the present invention is to provide a method and device for positioning a subsea formation space robot to achieve accurate positioning of a robot for deep-sea formation operations.

[0060] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0061] Embodiment 1

[0062] As Figure 1 shown, Embodiment 1 of the present invention provides a method for positioning a subsea formation space robot, and the positioning method includes the following steps:

[0063] Step 101: Determine the projections of the sensing positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point; the key points are points arranged in ascending order of the distance from the base station on the sensing positioning cable connecting the base station and the subsea formation space robot, the position of the first key point is the same as the position of the base station, and the position of the last key point is the same as the position of the subsea space robot, and the data information includes acceleration information and magnetic field intensity information.

[0064] Establishing the space coordinate system of the subsea formation space robot specifically includes: establishing a fixed coordinate system and establishing the moving coordinate systems of each key point. The fixed coordinate system, the moving coordinate systems of the key points, and the moving coordinate systems of each key point can all be transformed through a transformation matrix.

[0065] The defined fixed coordinate system is: [F], and the position coordinates of the subsea formation space robot obtained by positioning are also represented by this coordinate system. The defined moving coordinate systems of each key point are: [M0] and [M i, the subscript of i starts from 1, [M0] represents the carrier coordinate system for fixing the IMU sensor, [M1] represents the carrier coordinate system of the first moving IMU sensor on the sensing and positioning cable at the current moment, [M2] represents the carrier coordinate system of the second moving IMU sensor on the sensing and positioning cable at the current moment, and so on. The first moving IMU sensor on the sensing and positioning cable at the current moment refers to the moving IMU sensor closest to the base station, the second moving IMU sensor on the sensing and positioning cable at the current moment refers to the moving IMU sensor second closest to the base station, and so on.

[0066] The fixed coordinate system [F], the moving coordinate system [M i are oriented as Figure 2 shown, where x F O F y F (z0O0y0) represents the formation plane.

[0067] During the movement of the subsea formation space robot according to the present invention, the data information of each key point specifically includes: the acceleration information in the x, y, and z axes in the moving coordinate system of each key point, the angular velocity information in the x, y, and z axes in the moving coordinate system of each key point, the magnetic field intensity information in the x, y, and z axes in the moving coordinate system of each key point, and the distance information between each key point.

[0068] The data information of the key point is collected by the moving IMU sensor and based on the moving coordinate system of the key point. a xi 、a yi 、a zi respectively represent the acceleration information in the three axes collected by the i-th moving IMU sensor on the sensing and positioning cable, m xi 、m yi 、m zi respectively represent the magnetic intensity information in the three axes collected by the i-th moving IMU sensor on the sensing and positioning cable.

[0069] According to the data information of the key point, the spatial rotation attitude information of each moving coordinate system [M i can be obtained. The spatial rotation attitude information of each moving coordinate system [M i shown is calculated based on the direction of [M0]. Define ψ, θ, and φ as the spatial rotation Euler angles around the z-axis, y-axis, and x-axis respectively. The spatial rotation attitude information of the moving coordinate system [M i can be calculated and solved by the following formula.

[0070]

[0071]

[0072]

[0073] Calculate O i to O i+1 of the arc length ds of the arc segment, and calculate the projection values dx i and [M i+1 according to the spatial relationship between [M i on each axis of [M i , dy i , dz i , specifically including:

[0074] The spatial relationship conversion order between the said [M i and [M i+1 is as follows. Order one: Rotate the coordinate system [M i around the x i axis by Δφ i , and the spatial rotation matrix is [Rx i , Δφ i to obtain the coordinate system [M' i ; Order two: Rotate the coordinate system [M' i around the z' i axis by Δψ i , and the spatial rotation matrix is [Rz i , Δψ i to obtain the coordinate system [M i+1 .

[0075] The Δφ i in the said order one = φ i+1 - φ i , and the spatial transformation matrix from [M i to [M' i in the said order one is as follows:

[0076]

[0077] The Δψ i in the said order two = ψ i+1 - ψ i , and the spatial transformation matrix diagram from [M' i to [M i+1 in the said order two is as follows:

[0078]

[0079] The arc length ds of the arc segment from O i to O i+1 , when i = 0, ds = Δl, when i > 0, ds = Δs. And it satisfies the following relationship: Δψ i = ds × k i , ki Denote the curvature corresponding to the arc-shaped sensing and positioning cable between the i-th and the (i + 1)-th key points.

[0080] The projection values dx i of the arc length ds on each axis in [M i , dy i , dz i are calculated as follows:

[0081]

[0082] where dx i , dy i and dz i are the projections of the arc segment of the sensing and positioning cable between the i-th and the (i + 1)-th key points on the x-axis, y-axis, and z-axis of the moving coordinate system at the i-th key point respectively; Δφ i represents the rotation angle of the x-axis, and Δφ i = φ i+1 - φ i , where φ i and φ i+1 represent the spatial rotation Euler angles of the moving coordinate systems at the i-th and the (i + 1)-th key points on the x-axis respectively; Δψ i represents the rotation angle of the z-axis, and Δψ i = ψ i+1 - ψ i , where ψ i and ψ i+1 represent the spatial rotation Euler angles of the moving coordinate systems at the i-th and the (i + 1)-th key points on the z-axis respectively, and k i denotes the curvature corresponding to the arc-shaped sensing and positioning cable between the i-th and the (i + 1)-th key points.

[0083] Step 102: Determine the transformation matrix between the moving coordinate systems of two adjacent key points according to the projections of the arc segment of the sensing and positioning cable between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point.

[0084] The present invention defines [T i as the transformation matrix from [F] to [M0], and then from [M0] to [M i , as shown in the following formula:

[0085] [M i = [T i [F]

[0086] The transformation matrix [T iThe calculation method is as follows. According to the defined spatial relationship from [F] to [M0], the transformation matrix from [F] to [M0] can be obtained as follows:

[0087]

[0088] From [F] to [M i ](i>0) transformation matrix, [T i ]=[t i-1 ][T i-1 ];

[0089] In order to facilitate description and understanding, the limiting conditions and formula theory are combined, and the formula can be written as [T i+1 ]=[t i ][T i ].

[0090] The [t i ] indicates that from [M i ] coordinate system to [M i+1 ] coordinate system transformation matrix, [M i ] to [M i+1 The transformation matrix of ] is as follows:

[0091] [M i+1 ]=[t i ][M i ];

[0092] Among them, [t i ] is shown in the following formula:

[0093]

[0094] Step 103, based on the projection of the arc segment of the sensor positioning cable between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point of the two key points and the transformation matrix between the moving coordinate systems of the two adjacent key points, determine the position coordinates of the last key point in the fixed coordinate system as the position coordinates of the seabed formation space robot.

[0095] Step 103 specifically includes: the key point O i+1 M i+1 The origin of the coordinate system is [M i ] The coordinates in the coordinate system are {dx i ,dy i ,dz i}, according to the [T i ] can find the key point O i+1 The coordinate values in [F] are as follows:

[0096] [F]=[T i ]-1 [M i

[0097] Specifically, it can be expressed as:

[0098]

[0099] The key point coordinates measured by the last IMU sensor on the sensing and positioning cable are the position coordinates of the subsea formation space robot at this time. According to the position coordinates of the subsea formation space robot at different times, the motion trajectory of the subsea formation space robot can be drawn.

[0100] Embodiment 2

[0101] Embodiment 2 of the present invention provides a positioning device for a subsea formation space robot, as Figure 3 shown. The positioning device includes: a data acquisition module and a model calculation module;

[0102] The data acquisition module includes: a sensing and positioning cable with two ends respectively connected to a base station and a subsea formation space robot, a fixed IMU sensor fixed on the base station, and a plurality of mobile IMU sensors arranged in ascending order of distance from the base station on the sensor positioning cable. The mobile IMU sensor farthest from the base station is located at one end of the sensing and positioning cable connected to the subsea formation space robot.

[0103] The fixed IMU sensor and each mobile IMU sensor are both connected to the model calculation module; the model calculation module is used to determine the position coordinates of the last key point as the position coordinates of the subsea formation space robot by using a positioning algorithm according to the data information of each key point obtained by the fixed IMU sensor and each mobile IMU sensor; wherein, the position where the key point is located corresponds to the position where the fixed IMU sensor or the mobile IMU sensor is located, and the data information includes acceleration information and magnetic field intensity information.

[0104] Specifically, as Figure 4 ​As shown, a plurality of mobile IMU sensors are arranged at equal intervals and in equal directions on the sensing and positioning cable, and the distance between the mobile IMU sensors on the sensing and positioning cable is Δs. The sensing and positioning cable is externally wrapped with a flexible rubber sleeve during manufacturing, which can effectively prevent self-twisting. The fixed IMU sensor refers to the IMU sensor fixed on the base station, and the distance between the fixed IMU sensor mounted on the base station and the nearest mobile IMU sensor on the positioning and sensing cable entering the seabed formation is Δl. The distance dl will be measured by a length measuring device mounted on the base station. The equal intervals and equal directions mean that when the sensing and positioning cable is horizontally placed, the distances between the mobile IMU sensors are equal, and the directions pointed by the x, y, and z axes of each mobile IMU sensor are the same.

[0105] The installation of the sensing and positioning cable refers to being fixed on the base station by a winch, and the end of the sensing and positioning cable is connected to the tail of the space robot located in the seabed formation. The installation of the fixed IMU sensor fixed on the base station is that the fixed IMU sensor is installed at the winch of the base station.

[0106] Exemplarily, the positioning device further includes a lower computer and an upper computer. The lower computer is connected to the data acquisition module through a communication bus and can collect the data information collected by the data acquisition module in real time. The installation of the lower computer refers to the lower computer being fixed at the base station and connected to the data acquisition module.

[0107] The lower computer is connected to the upper computer through a communication bus. The software of the upper computer can control the space robot located in the seabed formation for the user to operate. The installation of the software of the upper computer is mainly installed on the PC side.

[0108] Among them, the model calculation module specifically includes: a projection information acquisition sub-module, which is used to determine the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points according to the data information of each key point; a transformation matrix determination sub-module, which is used to determine the transformation matrix between the moving coordinate systems of two adjacent key points according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point; a position coordinate determination sub-module, which is used to determine the position coordinates of the last key point in the fixed coordinate system according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of two adjacent key points, and use it as the position coordinates of the space robot in the seabed formation.

[0109] The projection information acquisition sub-module includes a projection information acquisition unit; the projection information acquisition unit is used to determine the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point, specifically including: determining the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point as follows:

[0110]

[0111] where dx i , dy i and dz i are respectively the projections of the sensing and positioning cable arc segment between the i-th and the (i + 1)-th key points on the x-axis, y-axis and z-axis of the moving coordinate system of the i-th key point; Δφ i represents the rotation angle of the x-axis, Δφ i = φ i+1 - φ i , φ and φ i+1 are respectively the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the x-axis; Δψ i represents the rotation angle of the z-axis, Δψ i = ψ i+1 - ψ i , ψ i and ψ i+1 are respectively the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the z-axis, and k i represents the curvature corresponding to the arc-shaped sensing and positioning cable between the i-th and the (i + 1)-th key points;

[0112]

[0113]

[0114] where m xi and m yi are respectively the magnetic field intensities in the x-axis direction and y-axis direction in the magnetic field intensity information of the i-th key point, and a xi , a yi and a zi are respectively the accelerations in the x-axis direction, y-axis direction and z-axis direction in the acceleration information of the i-th key point.

[0115] The transformation matrix determination sub-module includes a transformation matrix determination unit.

[0116] The conversion matrix determination unit is configured to determine the conversion matrix between the moving coordinate systems of two adjacent key points according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of the key points as follows:

[0117]

[0118] wherein, dx i , dy i and dz i are respectively the projections of the sensing and positioning cable arc segment between the i-th and the (i + 1)-th key points on the x-axis, y-axis and z-axis of the moving coordinate system of the i-th key point;

[0119] [R i represents an intermediate rotation matrix;

[0120] represents a first spatial rotation matrix, represents a second spatial conversion matrix;

[0121]

[0122]

[0123] Δφ i represents the rotation angle of the x-axis, Δt i = φ i+1 - φ i , φ i and φ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the x-axis; Δψ i represents the rotation angle of the z-axis, Δψ i = ψ i+1 - ψ i , ψ i and ψ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the z-axis.

[0124] The positioning device further includes: a visualization module;

[0125] The visualization module is configured to visualize the position coordinates of the subsea formation space robot and the motion trajectory of the subsea formation space robot. Exemplarily, the position coordinates of the subsea formation space robot and the motion trajectory of the subsea formation space robot can be visualized on a PC.

[0126] Exemplarily, the data acquisition module, the model calculation module, and the visualization module are connected through a data communication interface.

[0127] Embodiment 3

[0128] Embodiment 3 of the present invention provides a model calculation module.

[0129] According to the specific embodiments provided by the present invention, the following technical effects are disclosed:

[0130] (1) The positioning device of the present invention is a multi-sensor array, which has low cost and high precision. Under the same comparison conditions, the positioning accuracy is better than the positioning methods based on inertial navigation, acoustic wave, and magnetic signal.

[0131] (2) The positioning method established by the present invention collects the data information of each key point, establishes a space coordinate system (i.e., a moving coordinate system) during the movement of the subsea formation space robot based on differential geometry, uses the data information of the key points and the established space coordinate system, and determines the position of the subsea formation space robot at different times through a positioning algorithm. The movement trajectory of the subsea formation space robot is reconstructed according to the position of the subsea formation space robot at different times, thereby completing the positioning of the subsea formation space robot.

[0132] In this specification, each embodiment is described in a progressive manner. The key point of each embodiment is to illustrate the differences from other embodiments. The same or similar parts among the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the description of the method part.

[0133] Specific examples are used in this article to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation manner and application scope. In summary, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for positioning a subsea formation space robot, characterized in that, The positioning method includes the following steps: According to the data information of each key point, determine the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points; the key points are points arranged in ascending order of the distance from the base station on the sensing and positioning cable connecting the base station and the subsea formation space robot, the position of the first key point is the same as the position of the base station, and the position of the last key point is the same as the position of the subsea formation space robot, and the data information includes acceleration information and magnetic field intensity information; According to the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point, determine the transformation matrix between the moving coordinate systems of two adjacent key points; According to the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of two adjacent key points, determine the position coordinates of the last key point in the fixed coordinate system as the position coordinates of the subsea formation space robot; According to the data information of each key point, the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points are: Among them, dx i , dy i and dz i are the projections of the arc segment of the sensing and positioning cable between the i-th and (i + 1)-th key points on the x-axis, y-axis, and z-axis of the moving coordinate system at the i-th key point, respectively; Δφ i represents the rotation angle of the x-axis. Δφ i = φ i+1 - φ i , where φ i and φ i+1 represent the spatial rotation Euler angles of the moving coordinate systems of the i-th and (i + 1)-th key points on the x-axis, respectively; Δψ i represents the rotation angle of the z-axis. Δψ i = ψ i+1 - ψ i , where ψ i and ψ i+1 represent the spatial rotation Euler angles of the moving coordinate systems of the i-th and (i + 1)-th key points on the z-axis, respectively. k i represents the curvature corresponding to the arc-shaped sensing and positioning cable between the i-th and (i + 1)-th key points. where m xi and m yi represent the magnetic field intensities in the x-axis direction and y-axis direction respectively in the magnetic field intensity information of the i-th key point, and a xi , a yi and a zi represent the accelerations in the x-axis direction, y-axis direction and z-axis direction respectively in the acceleration information of the i-th key point.

2. The method for positioning a subsea formation space robot according to claim 1, wherein According to the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of each key point, the transformation matrix between the moving coordinate systems of two adjacent key points is: wherein, dx i , dy i and dz i are respectively the projections of the sensing and positioning cable arc segment between the i-th and the (i + 1)-th key points on the x-axis, y-axis and z-axis of the moving coordinate system at the i-th key point; [R i represents the intermediate rotation matrix; represents the first spatial rotation matrix, represents the second spatial transformation matrix; Δφ i represents the rotation angle about the x-axis, Δφ i = φ i+1 - φ i where φ i and φ i+1 respectively represent the Euler angles of spatial rotation about the x-axis of the moving coordinate systems of the i-th key point and the (i + 1)-th key point; Δψ i represents the rotation angle about the z-axis, Δψ i = ψ i+1 - ψ i where ψ i and ψ i+1 respectively represent the Euler angles of spatial rotation about the z-axis of the moving coordinate systems of the i-th key point and the (i + 1)-th key point.

3. The method for positioning a subsea formation space robot according to claim 1, wherein, Determine the position coordinates of the last key point in the fixed coordinate system based on the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of the two adjacent key points, and take it as the position coordinates of the subsea formation space robot: [F] = [T] -1 [M I-1 ​ Where, [F] represents the position coordinates of the last key point in the fixed coordinate system, T represents the total transformation matrix obtained by multiplying the transformation matrices between the fixed coordinate system and the moving coordinate system of the first key point, and the moving coordinate systems of all adjacent two key points, M I-1 represents the coordinate vector composed of the projections of the sensing and positioning cable arc segment between the I-th key point and the (I-1)-th key point on the x-axis, y-axis, and z-axis of the moving coordinate system of the (I-1)-th key point, and the I-th key point is the last key point.

4. An undersea formation space robot positioning device, characterized in that, The positioning device includes: a data acquisition module and a model calculation module; The data acquisition module includes: a sensing and positioning cable with two ends respectively connected to the base station and the subsea formation space robot, a fixed IMU sensor fixed on the base station, and a plurality of moving IMU sensors arranged in ascending order of the distance from the base station on the sensing and positioning cable, and the moving IMU sensor farthest from the base station is located at one end of the sensing and positioning cable connected to the subsea formation space robot; The fixed IMU sensor and each moving IMU sensor are both connected to the model calculation module; the model calculation module is used to determine the position coordinates of the last key point as the position coordinates of the subsea formation space robot by using a positioning algorithm according to the data information of each key point obtained by the fixed IMU sensor and each moving IMU sensor; wherein, the position where the key point is located corresponds to the position where the fixed IMU sensor or the moving IMU sensor is located, and the data information includes acceleration information and magnetic field intensity information; The model calculation module specifically includes: A projection information acquisition sub-module, configured to determine the projections of the sensing and positioning cable arc segment between two adjacent key points on each coordinate axis of the moving coordinate system of the previous key point among the two key points according to the data information of each key point; A transformation matrix determination sub-module, configured to determine a transformation matrix between the moving coordinate systems of two adjacent key points according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of the respective key points; A position coordinate determination sub-module, configured to determine the position coordinates of the last key point in the fixed coordinate system as the position coordinates of the subsea formation space robot according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the transformation matrix between the moving coordinate systems of the two adjacent key points; The projection information acquisition sub-module includes a projection information acquisition unit; The projection information acquisition unit is configured to determine the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points according to the data information of each key point. Specifically, it includes: determining that the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points according to the data information of each key point are: wherein, dx i , dy i and dz i are respectively the projections of the arc segment of the sensing positioning cable between the i-th key point and the (i + 1)-th key point on the x-axis, y-axis, and z-axis of the moving coordinate system at the i-th key point; Δφ i represents the rotation angle of the x-axis, and Δφ i = φ i+1 - φ i , where φ i and φ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the x-axis; Δψ i represents the rotation angle of the z-axis, and Δψ i = ψ i+1 - ψ i , where ψ i and ψ i+1 respectively represent the spatial rotation Euler angles of the moving coordinate systems of the i-th key point and the (i + 1)-th key point on the z-axis, and k i represents the curvature corresponding to the arc-shaped sensing positioning cable between the i-th key point and the (i + 1)-th key point; where m xi and m yi represent the magnetic field intensities in the x-axis direction and y-axis direction in the magnetic field intensity information of the i-th key point respectively, and a xi , a yi and a zi represent the accelerations in the x-axis direction, y-axis direction and z-axis direction in the acceleration information of the i-th key point respectively.

5. The undersea formation space robot positioning device according to claim 4, characterized in that, The transformation matrix determination sub-module includes a transformation matrix determination unit; The transformation matrix determination unit is configured to determine the transformation matrix between the moving coordinate systems of two adjacent key points as: according to the projections of the sensing and positioning cable arc segment between two adjacent key points on the coordinate axes of the moving coordinate system of the previous key point among the two key points and the spatial rotation Euler angles of the moving coordinate systems of the respective key points; wherein, dx i , dy i and dz i are respectively the projections of the sensing and positioning cable arc segment between the i-th key point and the (i + 1)-th key point on the x-axis, y-axis and z-axis of the moving coordinate system at the i-th key point; [R i represents the intermediate rotation matrix; represents the first spatial rotation matrix, represents the second spatial transformation matrix; Δφ i represents the rotation angle about the x-axis, Δφ i = φ i+1 - φ i where φ i and φ i+1 respectively represent the Euler angles of spatial rotation about the x-axis of the moving coordinate systems of the i-th key point and the (i + 1)-th key point; Δψ i represents the rotation angle about the z-axis, Δψ i = ψ i+1 - ψ i where ψ i and ψ i+1 respectively represent the Euler angles of spatial rotation about the z-axis of the moving coordinate systems of the i-th key point and the (i + 1)-th key point.

6. The positioning device for the subsea formation space robot according to claim 4, characterized in that, The positioning device further includes: a visualization module; The visualization module is configured to visualize the position coordinates of the subsea formation space robot and the motion trajectory of the subsea formation space robot.

7. A model calculation module according to any one of claims 4-6.

Images