Control method and device for vector propulsion device of underwater robot, storage medium, electronic equipment and computer program product

Abstract

The invention discloses a control method and device for a vector propulsion device of an underwater robot, a storage medium, electronic equipment and a computer program product. The control method comprises the following steps: determining a first predicted extension length of a first push rod motor and a second predicted extension length of a second push rod motor according to a first target deflection angle and a second target deflection angle of the propeller and a first preset model; determining a first target position of the first push rod motor according to the first predicted extension length, and determining a second target position of the second push rod motor according to the second predicted extension length; determining a first control instruction according to the first target position, and determining a second control instruction according to the second target position; and a first control instruction is sent to the first push rod motor, and a second control instruction is sent to the second push rod motor, so that the first push rod motor executes the first control instruction, the second push rod motor executes the second control instruction, and the propeller moves to the first target deflection angle and the second target deflection angle.

Description

Technical Field

[0001] This application relates to the field of underwater robot technology, and more specifically, to a control method, device, storage medium, electronic device, and computer program product for a vector propulsion device of an underwater robot. Background Technology

[0002] Currently, the commonly used vector thruster structure for underwater robots, consisting of pushrod motors and universal joints, involves the thruster's tail end fixed to a universal joint, which drives the tail thruster to achieve spatial vector motion. Two pushrod motors are fixed on opposite axes of the universal joint plane, with their connection points on the outer and inner rings of the universal joint. By pushing the outer and inner rings of the universal joint, the thruster's spatial angular deflection is achieved.

[0003] However, the inventors of this application have found that although this structural form is simple to control, its implementation is relatively complex and dispersed. The push rod requires more structural space, and structural interference is likely to occur during the movement of the push rod.

[0004] The content of the background section is merely technology known to the public and does not necessarily represent existing technology in the field. Summary of the Invention

[0005] According to one aspect of this application, a control method for a vector propulsion device for an underwater robot is provided. The vector propulsion device includes: a first push rod motor, a second push rod motor, and a thruster. The first push rod motor is rotatably connected to the tail of the thruster via a first fulcrum, and the second push rod motor is rotatably connected to the tail of the thruster via a second fulcrum. The first fulcrum and the second fulcrum satisfy preset fulcrum conditions. The control method includes: determining a first predicted extension length of the first push rod motor and a second predicted extension length of the second push rod motor based on a first target deflection angle, a second target deflection angle, and a first preset model; determining a first target position of the first push rod motor based on the first predicted extension length, and determining a second target position of the second push rod motor based on the second predicted extension length; determining a first control command based on the first target position, and determining a second control command based on the second target position; sending the first control command to the first push rod motor and sending the second control command to the second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

[0006] According to some embodiments of this application, before determining the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model, the control method further includes a first preset model step. The first preset model step includes: determining a first coordinate set corresponding to the first deflection angle set based on the collected first deflection angle set; constructing a first design matrix and a first observation vector set based on the first coordinate set and the collected first push rod length set corresponding to the first deflection angle set; determining a first coefficient matrix set based on the first design matrix and the first observation vector set; and determining the predicted push rod length set based on the first coefficient matrix set and the first coordinate set, thereby determining the first preset model.

[0007] According to some embodiments of this application, after determining the predicted push rod length set based on the first coefficient matrix set and the first coordinate set to determine the first preset model, the step of determining the first preset model further includes: determining the first evaluation parameters of the first preset model to optimize the first preset model.

[0008] According to some embodiments of this application, determining a first target position of a first push rod motor based on a first predicted extension length includes: determining a first set of control parameters for the first push rod motor; and determining the first target position based on the first predicted extension length and the first set of control parameters. Determining a second target position of a second push rod motor based on a second predicted extension length includes: determining a second set of control parameters for the second push rod motor; and determining the second target position based on the second predicted extension length and the second set of control parameters.

[0009] According to some embodiments of this application, after sending a first control command to a first push rod motor and a second control command to a second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, the control method further includes: determining a first current push rod length after the first push rod motor executes the first control command, and determining a second current push rod length after the second push rod motor executes the first control command; determining a first current deflection angle and a second current deflection angle of the thruster based on the first current push rod length, the second current push rod length, and a second preset model, until the thruster moves to the first target deflection angle and the second target deflection angle.

[0010] According to some embodiments of this application, before determining the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model, the control method further includes a step of determining a second preset model. The step of determining the second preset model includes: determining a second coordinate set corresponding to the collected second deflection angle set; constructing a second design matrix and a second observation vector set based on the second coordinate set and the collected second push rod length set corresponding to the second deflection angle set; determining a second coefficient matrix set based on the second design matrix and the second observation vector set; determining a predicted coordinate set based on the second coefficient matrix set and the second push rod length set; and determining a predicted deflection angle set based on the predicted coordinate set, thereby determining the second preset model.

[0011] According to some embodiments of this application, after determining the set of predicted deflection angles based on the set of predicted targets to determine the second preset model, the step of determining the second preset model further includes: determining the second evaluation parameters of the second preset model to optimize the second preset model.

[0012] According to some embodiments of this application, before determining the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the thruster, the control method further includes a zero-position calibration step. The zero-position calibration step includes: sending an axis control command to ensure that the axis of the thruster meets preset axis conditions; sending a first zero-position calibration command to determine the position of the second zero-position deflection angle; and sending a second zero-position calibration command to determine the first zero-position deflection angle.

[0013] According to one aspect of this application, this application provides a control device for a vector propulsion device for an underwater robot, characterized in that the vector propulsion device includes: a first push rod motor, a second push rod motor, and a thruster, the first push rod motor being rotatably connected to the tail of the thruster via a first fulcrum, the second push rod motor being rotatably connected to the tail of the thruster via a second fulcrum, the first fulcrum and the second fulcrum satisfying preset fulcrum conditions; the control device includes: a control module. The control module determines the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the thruster. The control module determines the first target position of the first push rod motor based on the first predicted extension length, and the second target position of the second push rod motor based on the second predicted extension length. The control module determines a first control command based on the first target position and a second control command based on the second target position. The control module sends the first control command to the first push rod motor and the second control command to the second push rod motor, causing the first push rod motor to execute the first control command and the second push rod motor to execute the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

[0014] According to another aspect of this application, this application also provides a non-volatile computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is capable of implementing the control method for a vector propulsion device for an underwater robot as described above.

[0015] According to another aspect of this application, this application also provides an electronic device, including: one or more processors; and a storage device for storing one or more programs, which, when executed by the one or more processors, enable the one or more processors to implement the control method for a vector propulsion device for an underwater robot as described above.

[0016] According to another aspect of this application, this application also provides a computer program product, comprising: a computer program stored on a computer-readable storage medium; the computer program includes program instructions that, when executed by a computer, cause the computer to perform the control method for a vector propulsion device for an underwater robot as described above.

[0017] The technical solution of this application can achieve high-precision angle control of the vector propulsion device. Since the range of motion of the push rod is easy to control and predict, interference between the first push rod motor and the second push rod motor and the surrounding component structure can be avoided during the movement. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 A schematic diagram of the vector propulsion device is shown. Figure 2 Another schematic diagram of the vector propulsion device is shown; Figure 3 A schematic diagram of the structure at the tail of the thruster is shown; Figure 4 A schematic diagram of the first plane is shown; Figure 5 A schematic diagram of the second plane is shown; Figure 6 This diagram illustrates the structure of the control device according to an embodiment of this application. Figure 7 A flowchart illustrating the control method 1000 according to an embodiment of this application is shown; Figure 8 A flowchart illustrating the control method 2000 according to an embodiment of this application is shown; Figure 9 A flowchart illustrating step S210 of an embodiment of this application is shown; Figure 10 A flowchart illustrating step S230 of an embodiment of this application is shown; Figure 11 A flowchart illustrating the control method 3000 according to an embodiment of this application is shown; Figure 12 This illustrates another flowchart of the control method 3000 according to an embodiment of this application; Figure 13 A flowchart illustrating step S300 of an embodiment of this application is shown; Figure 14 A flowchart illustrating the control method 4000 according to an embodiment of this application is shown; Figure 15 A flowchart illustrating step S410 of an embodiment of this application is shown.

[0020] Explanation of reference numerals in the attached figures: 50. Vector propulsion device; 51. First push rod motor; 52. Second push rod motor; 53. Thruster; 54. Support; 55. First fulcrum; 56. Second fulcrum; 57. Annular universal joint; 60. Control device; 61. Control module; α, First deflection angle; β, Second deflection angle. Detailed Implementation

[0021] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the embodiments set forth herein; rather, they are provided so that this application will be thorough and complete, and will fully convey the concept of the exemplary embodiments to those skilled in the art. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted.

[0022] The described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. Numerous specific details are provided in the following description to give a full understanding of embodiments of this disclosure. However, those skilled in the art will recognize that the technical solutions of this disclosure can be practiced without one or more of these specific details, or other methods, components, materials, devices, etc. In these cases, well-known structures, methods, devices, implementations, materials, or operations will not be shown or described in detail.

[0023] Furthermore, the terms “comprising” and “having”, and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such process, method, product, or apparatus.

[0024] The terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish different objects, not to describe a specific order.

[0025] The technical solutions of this application will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some, not all, of the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0026] According to one aspect of this application, this application provides a control method 1000 for a vector propulsion device for an underwater robot.

[0027] According to the example embodiment, see Figure 1 and Figure 2 The vector propulsion device 50 may include a first push rod motor 51, a second push rod motor 52, and a thruster 53.

[0028] The first end of the first push rod motor 51 is rotatably connected to the tail of the pusher 53 via a first fulcrum 55, and the first end of the second push rod motor 52 is rotatably connected to the tail of the pusher 53 via a second fulcrum 56. The first fulcrum 55 and the second fulcrum 56 satisfy preset fulcrum conditions. For example, universal joints can be used to achieve rotatable connection at the positions of both the first fulcrum 55 and the second fulcrum 56.

[0029] The vector propulsion device 50 may also include a bracket 54, the first end and the second end of the bracket 54 being annular, and the first end and the second end of the bracket 54 being coaxially arranged.

[0030] See Figure 3 The first end of the bracket 54 is rotatably connected to the tail of the thruster 53. For example, the first end of the bracket 54 can be rotatably connected to the tail of the thruster 53 via an annular universal joint 57.

[0031] According to the example embodiment, based on the principle that three non-collinear points determine a plane, by fixing one point (e.g., the center point of the annular universal joint 57, i.e., the center point of the tail of the thruster 53) and controlling the movement of the other two points (i.e., the first fulcrum 55 and the second fulcrum 56), all possible postures (i.e., two rotational degrees of freedom) of the plane around the fixed point can be changed.

[0032] The preset fulcrum conditions can be that the first fulcrum 55 and the second fulcrum 56 are on the same straight line but not on the same straight line as the center point of the tail of the pusher 53. The straight line containing the first fulcrum 55 and the second fulcrum 56 is parallel to the horizontal axis and is equidistant from the vertical axis. The distance between the first fulcrum 55 and the second fulcrum 56 should be minimized while ensuring that the structures of the first pusher motor 51 and the second pusher motor 52 do not interfere with each other.

[0033] The non-interference structure of the push rod motor ensures that no accidental or harmful physical contact or collision will occur between its own mechanical components or with adjacent parts or equipment housings, whether the push rod motor is fully extended, fully retracted, or in any intermediate position during the entire movement process.

[0034] The second end of the first push rod motor 51 can be rotatably connected to the sub-bracket 54 extending from the second end of the bracket 54 via a universal joint, and the second end of the second push rod motor 52 can be rotatably connected to the sub-bracket 54 extending from the second end of the bracket 54 via a universal joint.

[0035] When the first push rod motor 51 and the second push rod motor 52 extend or retract, the thruster 53 is driven by a vector deflection angle.

[0036] See Figure 6The control device 60 may include a control module 61, the control module 61 ( Figure 1 and Figure 2 (Not shown) can be connected to the first push rod motor 51 and the second push rod motor 52 via a communication connection (e.g., CAN communication). The first push rod motor 51 and the second push rod motor 52 can be equipped with absolute position sensors. The absolute position sensors are equipped with rechargeable batteries to ensure that the position of the absolute position sensors is not lost when the device is powered off.

[0037] See Figure 7 The control method 1000 may include steps S110-S140.

[0038] In step S110, the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor are determined based on the first target deflection angle, the second target deflection angle of the pusher and the first preset model.

[0039] According to the example embodiment, see Figure 4 Within a first plane parallel to the first end of the support, an XOZ coordinate system is established with the center point of the support as the origin. The Z direction is the vertical direction perpendicular to the horizontal axis of the first end of the support, and the X direction is the horizontal direction perpendicular to the horizontal axis of the first end of the support. The first deflection angle α can be the angle between the straight line connecting the tail of the thruster to the origin and the X direction. The range of the first deflection angle α can be 0~360°. The first target deflection angle can be the target value of the first deflection angle α.

[0040] The second deflection angle β can be the angle between the axis of the thruster's tail and the horizontal axis of the first end of the support. For example, see Figure 5 In the second plane perpendicular to the first end of the support, a YOZ coordinate system is established, with the Y direction being the horizontal direction of the horizontal axis of the first end of the support, and the second deflection angle β being the angle between the axis of the tail of the thruster and the horizontal axis of the first end of the support.

[0041] The range of the second deflection angle β can be determined based on the maximum deflection position of the axis at the tail of the thruster. For example, the range of the second deflection angle β can be 0~20°, 0~35°, and 0~45°. The maximum value of the second deflection angle β can be determined based on factors such as the maximum extension length of the first push rod motor, the maximum extension length of the second push rod motor, and the absence of interference between the first and second push rod motors. The second target deflection angle can be the target value of the second deflection angle β.

[0042] The first extension length can be the stroke length of the first push rod motor. The first predicted extension length can be a predicted value of the first extension length. The second extension length can be the stroke length of the second push rod motor. The second predicted extension length can be a predicted value of the second extension length.

[0043] The first preset model can be the inverse kinematics solution model of the vector propulsion device. The first preset model can be a kinematic model constructed and trained based on the actual first deflection angle α, second deflection angle β, first extension length, and second extension length.

[0044] For example, in step S110, the control module 61 determines the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the pusher.

[0045] In step S120, the first target position of the first push rod motor is determined based on the first predicted extension length, and the second target position of the second push rod motor is determined based on the second predicted extension length.

[0046] According to the example embodiment, the first target position can be the stroke change of the first push rod motor. The second target position can be the stroke change of the second push rod motor. The stroke change can be an extension change or a retraction change.

[0047] For example, in step S120, the control module 61 determines the first target position of the first push rod motor based on the first predicted extension length, and determines the second target position of the second push rod motor based on the second predicted extension length.

[0048] Control module 61 can determine a first set of control parameters for the first push rod motor, and control module 61 can determine a first target position based on the first predicted extension length and the first set of control parameters. Control module 61 can determine a first set of control parameters for the second push rod motor, and control module 61 can determine a second target position based on the second predicted extension length and the second set of control parameters.

[0049] In step S130, a first control command is determined based on the first target position, and a second control command is determined based on the second target position.

[0050] According to the example embodiment, the first control command can be a control command for the motor inside the first push rod motor to rotate so that the push rod of the first push rod motor extends or retracts. The control command for the first control command can include control parameters such as the number of pulses, the amount of stroke change, and the pitch.

[0051] The second control command can be a control command that rotates the motor inside the second push rod motor to extend or retract the push rod of the second push rod motor. The control command for the second control command can include control parameters such as the number of pulses, the amount of stroke change, and the pitch.

[0052] For example, in step S130, control module 61 determines a first control command based on a first target position and a second control command based on a second target position. Control module 61 can generate a first control command corresponding to the command format for controlling the first push rod motor based on the first target position. Control module 61 can generate a second control command corresponding to the command format for controlling the second push rod motor based on the second target position.

[0053] In step S140, a first control command is sent to the first push rod motor and a second control command is sent to the second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

[0054] For example, in step S140, the control module 61 sends a first control command to the first push rod motor and a second control command to the second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

[0055] The first push rod motor can execute a first control command to extend or retract its push rod. The second push rod motor can execute a second control command to extend or retract its push rod.

[0056] When the first push rod motor and the second push rod motor extend or retract, the thruster is driven by a vector deflection angle, so that the thruster moves to the first target deflection angle and the second target deflection angle.

[0057] For example, see Figure 4 The direction of rotation of the thruster is described using a first plane perpendicular to the axis of the first end of the support.

[0058] 1. The first push rod motor retracts to its shortest position, the second push rod motor extends, and the center point of the thruster moves from the center of the circle in the positive x-direction and into the first quadrant. During this process, the second push rod motor continues to extend, and the first push rod motor also extends, causing the thruster to move from the first quadrant to the positive z-direction.

[0059] 2. The thruster starts running in the positive direction of z. The first push rod motor extends and the second push rod motor retracts. The thruster starts running in the second quadrant until it reaches the negative direction of x.

[0060] 3. The thruster starts moving from the negative x-direction. The first push rod motor extends, then the second push rod motor extends, and the thruster begins to move towards the third quadrant. During this process, the first push rod motor retracts, then the second push rod motor retracts, until it reaches the negative z-direction.

[0061] 4. The thruster starts running from the negative direction of z. The first push rod motor retracts and the second push rod motor extends. The thruster starts running towards the fourth quadrant and continues until it reaches the positive direction of x.

[0062] Through the above embodiments, the technical solution of this application can determine the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor by using the first target deflection angle, the second target deflection angle and the first preset model of the pusher.

[0063] The technical solution of this application can determine the first target position of the first push rod motor by the first predicted extension length, and determine the second target position of the second push rod motor by the second predicted extension length.

[0064] The technical solution of this application can determine a first control command based on a first target position and a second control command based on a second target position.

[0065] The technical solution of this application can send a first control command to a first push rod motor and a second control command to a second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

[0066] The technical solution of this application can achieve high-precision angle control of the vector propulsion device. Since the range of motion of the push rod is easy to control and predict, interference between the first push rod motor and the second push rod motor and the surrounding component structure can be avoided during the movement.

[0067] According to another aspect of this application, this application also provides a control method 2000 for a vector propulsion device for an underwater robot, see [link to relevant documentation]. Figure 8 The control method 2000 may include steps S210-S250. Steps S220-S250 are the same as steps S110-S140, and therefore will not be described again.

[0068] Step S210 is the step of determining the first preset model. See also Figure 9 Step S210 may include steps S211-S214.

[0069] In step S211, the first coordinate set corresponding to the first deflection angle set is determined based on the collected first deflection angle set.

[0070] According to the example embodiment, the first deflection angle set can be a set composed of the actual values ​​of the first deflection angle α and the actual values ​​of the second deflection angle β of multiple sets of sampling points (e.g., 100, 300, 500, etc.). The number of sampling points can affect the accuracy of the first preset model, and the accuracy can be improved by increasing the number of sampling points.

[0071] The first coordinate set can be a set of XY coordinate points formed by converting each set of first deflection angles α and second deflection angles β in the first deflection angle set.

[0072] For example, in step S211, the control module 61 determines the first coordinate set corresponding to the first deflection angle set based on the collected first deflection angle set.

[0073] Control module 61 can determine the i-th coordinate point of the first coordinate set according to the following formula. : ; ; ; in, Let be the first deflection angle of the i-th sampling point in the first deflection angle set. For the first set of deflection angles and The corresponding second deflection angle, The first set of coordinates and and The corresponding x-coordinate, The first set of coordinates and and The corresponding y-coordinate.

[0074] In step S212, a first design matrix and a first observation vector set are constructed based on the first coordinate set and the first push rod length set corresponding to the first deflection angle set.

[0075] According to the example embodiment, the first push rod length set can be a set consisting of the actual values ​​of the first push rod length and the actual values ​​of the second push rod length corresponding to multiple sets of sampling points and the first deflection angle set. The first design matrix can be the design matrix in the inverse kinematics solution model. The first observation vector set can be the observation vectors (i.e., the set of actual values) in the inverse kinematics calculation model.

[0076] For example, in step S212, the control module 61 constructs a first design matrix and a first observation vector set based on the first coordinate set and the first push rod length set corresponding to the first deflection angle set.

[0077] Control module 61 can construct the first design matrix according to the following formula: ; in, For the first design matrix, The dimension is N rows * M columns, where N is the number of sampling points and M is the total number of terms in the polynomial fitting. Taking fourth-order polynomial fitting as an example, M is 15. for The i-th coordinate point The corresponding row.

[0078] The i-th coordinate point The i-th row of the first design matrix of the fourth-order polynomial It can be represented as:

[0079] Control module 61 can construct the first set of observation vectors according to the following formula: ; ; in, The observation vector of the first push rod motor in the first set of observation vectors; The actual value of the first push rod length at the i-th sampling point, and... and correspond; The observation vector of the second push rod motor is in the first set of observation vectors. The actual value of the second push rod length at the i-th sampling point, and... and correspond.

[0080] In step S213, the first set of coefficient matrices is determined based on the first design matrix and the first set of observation vectors.

[0081] According to the example embodiment, the first set of coefficient matrices can be the coefficient matrices in the inverse kinematics solution model.

[0082] For example, in step S213, the control module 61 determines the first coefficient matrix set based on the first design matrix and the first observation vector set.

[0083] Control module 61 can construct the first set of coefficient matrices according to the following formula: ; ; in, This is the coefficient matrix of the first push rod motor in the first coefficient matrix set; ~ All are coefficient values; This is the coefficient matrix of the second push rod motor in the first coefficient matrix set; ~ All are coefficient values.

[0084] Control module 61 uses the least squares method to solve for the first set of coefficient matrices. For example, control module 61 can solve for the first set of coefficient matrices according to the following formula: ; ; Control module 61 determines the least squares method. ~ , ~ The value.

[0085] In step S214, the predicted push rod length set is determined based on the first coefficient matrix set and the first coordinate set, so as to determine the first preset model.

[0086] According to the example embodiment, the predicted push rod length set can be a set consisting of the predicted values ​​of the first extension length and the predicted values ​​of the second extension length.

[0087] For example, in step S214, the control module 61 determines the predicted push rod length set based on the first coefficient matrix set and the first coordinate set, so as to determine the first preset model.

[0088] Control module 61 can determine the predicted value of the length of the i-th first push rod in the predicted push rod length set according to the following formula. :

[0089] Control module 61 can determine the predicted value of the i-th second push rod length in the predicted push rod length set according to the following formula. :

[0090] The control module 61 can calculate the predicted value of the first push rod length and the predicted value of the second push rod length at each sampling point, forming a set of predicted push rod lengths, thereby determining the first preset model.

[0091] Through the above embodiments, the technical solution of this application can determine the first coordinate set corresponding to the first deflection angle set by collecting the first deflection angle set. The technical solution of this application can construct a first design matrix and a first observation vector set by using the first coordinate set and the collected first push rod length set corresponding to the first deflection angle set. The technical solution of this application can determine a first coefficient matrix set by using the first design matrix and the first observation vector set. The technical solution of this application can determine the predicted push rod length set by using the first coefficient matrix set and the first coordinate set, thereby determining a first preset model.

[0092] The technical solution of this application can perform inverse kinematics calculations on a vector propulsion device by constructing a first preset model.

[0093] Optionally, see Figure 9 Step S210 may also include step S215.

[0094] In step S215, the first evaluation parameters of the first preset model are determined to optimize the first preset model.

[0095] According to the example embodiment, the first evaluation parameter can be an accuracy parameter for evaluating the first preset model. For example, in step S215, the control module 61 determines the first evaluation parameter of the first preset model to optimize the first preset model.

[0096] The control module 61 can evaluate the accuracy of the first preset model based on the first set of push rod lengths (actual values) and the corresponding set of predicted push rod lengths (predicted values). If the accuracy does not meet the requirements, the first preset model can be optimized.

[0097] For example, the control module 61 can increase the order of the polynomial, for example, from a fourth-order polynomial to a fifth-order polynomial, and the total number of terms M can be increased from 15 to 21. The control module 61 can also improve the accuracy of the first preset model by increasing the sampling density (for example, reducing the sampling step size from 2° to 1°).

[0098] Optionally, see Figure 10 Step S230 may include steps S231-S232. Step S230 may also include steps S233-S234. Steps S231-S232 may be executed synchronously with steps S233-S234.

[0099] In step S231, the first set of control parameters for the first push rod motor is determined.

[0100] According to the example embodiment, the control parameter set can be a set of control parameters controlling the actuator motor end. For example, the control parameter set may include parameters such as pitch, number of pulses per revolution at the load end, and zero-point actuator length. The first control parameter set can be a set of control parameters for the motion of the corresponding first actuator motor. The first control parameter set can be determined based on the hardware parameters of the first actuator motor.

[0101] For example, in step S231, the control module 61 determines the first set of control parameters for the first push rod motor.

[0102] In step S232, the first target position is determined based on the first predicted extension length and the first set of control parameters.

[0103] For example, in step S232, the control module 61 determines the first target position based on the first extension length and the first set of control parameters.

[0104] Control module 61 can determine the first target position according to the following formula: ; in, The first target location, The pitch of the first push rod motor is... The number of pulses per revolution at the load end of the first push rod motor. For the first predicted extension length, This is the zero-point push rod length of the first push rod motor.

[0105] In step S233, the second set of control parameters for the second push rod motor is determined.

[0106] The second set of control parameters can be the set of control parameters for the motion of the corresponding second push rod motor. The second set of control parameters can be determined based on the hardware parameters of the second push rod motor.

[0107] For example, in step S233, the control module 61 determines the second set of control parameters for the second push rod motor.

[0108] In step S234, the second target position is determined based on the second predicted extension length and the second set of control parameters.

[0109] For example, in step S234, the control module 61 determines the second target position based on the second extension length and the second set of control parameters.

[0110] Control module 61 can determine the second target position according to the following formula: ; in, The second target location, The pitch of the second push rod motor. The number of pulses per revolution at the load end of the second push rod motor. For the second predicted extension length, This is the zero-point push rod length of the second push rod motor.

[0111] Through the above embodiments, the technical solution of this application can determine the first target position using a first predicted extension length and a first set of control parameters. The technical solution of this application can determine the second target position using a second predicted extension length and a second set of control parameters.

[0112] According to another aspect of this application, this application also provides a control method 3000 for a vector propulsion device for an underwater robot, see [link to relevant documentation]. Figure 11 The control method 3000 may include steps S310-S360. Steps S310-S340 are the same as steps S110-S140, so they will not be described again.

[0113] In step S350, the first current push rod length after the first push rod motor executes the first control command is determined, and the second current push rod length after the second push rod motor executes the command is determined.

[0114] According to the example embodiment, the first current push rod length can be the current actual value of the first extension length. The second current push rod length can be the current actual value of the second extension length. The first push rod motor can collect the stroke change during the movement, and the second push rod motor can collect the stroke change during the movement.

[0115] For example, in step S350, the control module 61 determines the first current push rod length after the first push rod motor executes the first control command, and determines the second current push rod length after the second push rod motor executes the command.

[0116] The control module 61 can determine the first current push rod length by the stroke change during the movement of the first push rod motor, and the control module 61 can determine the second current push rod length by the stroke change during the movement of the second push rod motor.

[0117] In step S360, the first current deflection angle and the second current deflection angle of the thruster are determined based on the first current push rod length, the second current push rod length, and the second preset model, until the thruster moves to the first target deflection angle and the second target deflection angle.

[0118] According to the example embodiment, the second preset model can be a forward kinematics solution model for a vector propulsion device. The second preset model can be a kinematic model constructed and trained based on the actual first deflection angle α, second deflection angle β, first extension length, and second extension length.

[0119] For example, in step S360, the control module 61 determines the first current deflection angle and the second current deflection angle of the thruster based on the first current push rod length, the second current push rod length, and the second preset model, so as to monitor and provide feedback control of the thruster until the thruster moves to the first target deflection angle and the second target deflection angle.

[0120] Through the above embodiments, the technical solution of this application can determine the first current push rod length and the second current push rod length. The technical solution of this application can determine the first current deflection angle and the second current deflection angle of the thruster using the first current push rod length, the second current push rod length, and the second preset model, until the thruster moves to the first target deflection angle and the second target deflection angle.

[0121] The technical solution of this application can monitor and provide feedback during the movement of the thruster, thereby improving control accuracy.

[0122] Optionally, see Figure 12 Before step S310, the control method 3000 may also include step S300.

[0123] Step S300 is the step of determining the second preset model. See also Figure 13 Step S300 may include steps S301 to S305.

[0124] In step S301, the second coordinate set corresponding to the second deflection angle set is determined based on the collected second deflection angle set.

[0125] According to the example embodiment, the second deflection angle set can be a set composed of the actual values ​​of the first deflection angle α and the actual values ​​of the second deflection angle β of multiple sets of sampling points (e.g., 100, 300, 500, etc.). The number of sampling points can affect the accuracy of the second preset model, and the accuracy can be improved by increasing the number of sampling points.

[0126] The second coordinate set can be a set of XY coordinate points formed by converting each set of first deflection angles α and β in the first deflection angle set. The second deflection angle set can be the same as the first deflection angle set, in which case the second coordinate set can be the same as the first coordinate set.

[0127] For example, in step S301, the control module 61 determines the second coordinate set corresponding to the second deflection angle set based on the collected second deflection angle set.

[0128] Control module 61 can determine the j-th coordinate point of the second coordinate set according to the following formula. : ; ; ; in, The first deflection angle is the sampling point of the j-th group in the second deflection angle set. For the first set of deflection angles and The corresponding second deflection angle, The first set of coordinates and and The corresponding x-coordinate, The first set of coordinates and and The corresponding y-coordinate.

[0129] In step S302, a second design matrix and a second observation vector set are constructed based on the second coordinate set and the second push rod length set corresponding to the collected second deflection angle set.

[0130] According to the example embodiment, the second push rod length set can be a set consisting of the actual values ​​of the first push rod length and the actual values ​​of the second push rod length corresponding to multiple sets of sampling points and the second deflection angle set. The second design matrix can be the design matrix in the forward kinematics solution model. The second observation vector set can be the observation vectors (i.e., the set of actual values) in the forward kinematics calculation model.

[0131] For example, in step S302, the control module 61 constructs a second design matrix and a second observation vector set based on the second coordinate set and the second push rod length set corresponding to the collected second deflection angle set.

[0132] Control module 61 can construct the second design matrix according to the following formula: ; in, For the second design matrix, The dimension is N rows * M columns, where N is the number of sampling points and M is the total number of terms in the polynomial fitting. Taking fourth-order polynomial fitting as an example, M is 15. for The j-th coordinate point The corresponding row.

[0133] The j-th coordinate point The j-th row of the second design matrix of the fourth-order polynomial It can be represented as:

[0134] in, The actual value of the j-th first pusher length in the second pusher length set, and... and correspond; The actual value of the j-th second pusher length in the set of second pusher lengths, and... and correspond.

[0135] Control module 61 can construct the second set of observation vectors according to the following formula: ; ; in, The observation vector of the first push rod motor in the second set of observation vectors; This is the observation vector of the second push rod motor in the second set of observation vectors.

[0136] In step S303, the second coefficient matrix set is determined based on the second design matrix and the second observation vector set.

[0137] According to the example embodiment, the second set of coefficient matrices can be the coefficient matrices in the forward kinematics solution model.

[0138] For example, in step S303, the control module 61 determines the second coefficient matrix set based on the second design matrix and the second observation vector set.

[0139] Control module 61 can construct a second set of coefficient matrices according to the following formula: ; ; in, The coefficient matrix for the x-coordinate in the second set of coefficient matrices; ~ All are coefficient values; This is the coefficient matrix for the y-coordinate in the second set of coefficient matrices; ~ All are coefficient values.

[0140] ; ; Control module 61 determines the least squares method. ~ , ~ The value.

[0141] In step S304, the predicted coordinate set is determined based on the second coefficient matrix set and the second push rod length set.

[0142] According to the example embodiment, the predicted coordinate set can be a set consisting of the predicted values ​​of the x-coordinate and the predicted values ​​of the y-coordinate.

[0143] For example, in step S304, the control module 61 determines the predicted coordinate set based on the second coefficient matrix set and the second push rod length set.

[0144] Control module 61 can determine the predicted value of the j-th x-coordinate in the predicted push rod coordinate set according to the following formula. :

[0145] Control module 61 can determine the predicted value of the j-th y-coordinate in the predicted push rod coordinate set according to the following formula. :

[0146] The control module 61 can calculate the predicted x-coordinate and the predicted y-coordinate of each sampling point to form a set of predicted push rod coordinates.

[0147] In step S305, the predicted deflection angle set is determined based on the predicted coordinate set, so as to determine the second preset model.

[0148] According to the example embodiment, the predicted deflection angle set can be a set composed of the predicted values ​​of the first deflection angle α and the predicted values ​​of the second deflection angle β.

[0149] For example, in step S305, the control module 61 determines the predicted deflection angle set based on the predicted coordinate set, thereby determining the second preset model. The control module 61 can convert the predicted coordinate set into the predicted deflection angle set.

[0150] Control module 61 can determine the set of predicted deflection angles according to the following formula: ; ; ; in, For the predicted value of the j-th first deflection angle, in In this case, ; This is the predicted value for the j-th second deflection angle; This represents the arctangent function.

[0151] The control module 61 can calculate the predicted value of the first deflection angle α and the predicted value of the second deflection angle α for each sampling point, forming a set of predicted deflection angles, thereby determining the second preset model.

[0152] Through the above embodiments, the technical solution of this application can determine the second coordinate set corresponding to the second deflection angle set by collecting the second deflection angle set. The technical solution of this application can construct a second design matrix and a second observation vector set using the second coordinate set and the collected second push rod length set corresponding to the second deflection angle set. The technical solution of this application can determine a second coefficient matrix set using the second design matrix and the second observation vector set. The technical solution of this application can determine a predicted coordinate set using the second coefficient matrix set and the second push rod length set. The technical solution of this application can determine the predicted deflection angle set using the predicted coordinate set to determine a second preset model.

[0153] The technical solution of this application can perform forward kinematics calculations on the vector propulsion device by constructing a second preset model.

[0154] Optionally, see Figure 13 Step S300 may also include step S306.

[0155] In step S306, the second evaluation parameters of the second preset model are determined to optimize the second preset model.

[0156] According to the example embodiment, the second evaluation parameter can be an accuracy parameter for evaluating the second preset model. For example, in step S306, the control module 61 determines the second evaluation parameter of the second preset model to optimize the second preset model.

[0157] The control module 61 can evaluate the accuracy of the second preset model based on the second set of deflection angles (actual values) and the corresponding set of predicted deflection angles (predicted values). If the accuracy does not meet the requirements, the second preset model can be optimized.

[0158] For example, the control module 61 can increase the order of the polynomial, for example, from a fourth-order polynomial to a fifth-order polynomial, and the total number of terms M can be increased from 15 to 21. The control module 61 can also improve the accuracy of the second preset model by increasing the sampling density (for example, reducing the sampling step size from 2° to 1°).

[0159] According to another aspect of this application, this application also provides a control method 4000 for a vector propulsion device for an underwater robot. Referring to the figure, the control method 4000 may include steps S410-S450. Steps S420-S450 are the same as steps S110-S140, and therefore will not be described again.

[0160] Step S410 is the zero-point calibration step, see [link / reference] Figure 15 Step S410 may include: steps S411-S413.

[0161] In step S411, an axis control command is sent so that the axis of the thruster meets the preset axis conditions.

[0162] According to an example embodiment, the axis control command can be a control command for controlling the axis of the thruster. The preset axis condition can be that the axis is aligned with the horizontal axis of the underwater robot body. For example, a vector propulsion device can be installed at the stern of the underwater robot, with the horizontal axis of the underwater robot body aligned with the axis of the first end of the support.

[0163] In step S411, the control module 61 sends an axis control command to ensure that the axis of the thruster meets the preset axis conditions.

[0164] The control module 61 sends axis control commands to the first and second push rod motors to set them to speed control mode, thereby slowly controlling the axis of the thrusters to remain aligned with the horizontal axis of the underwater robot body. At this time, the second deflection angle β is 0°.

[0165] In step S412, a first zero-position calibration command is sent to determine the position of the second zero-position deflection angle.

[0166] According to the example embodiment, the first zero-position calibration command can be a control command for zero-position calibration of the second deflection angle β. The second zero-position deflection angle position can be the absolute position of the first push rod motor and the absolute position of the second push rod motor when the second deflection angle β is 0°.

[0167] For example, in step S412, the control module 61 sends a first zero-position calibration command to determine the second zero-position deflection angle.

[0168] The control module 61 sends a first zero-position calibration command, taking the current absolute position of the first push rod motor and the current absolute position of the second push rod motor as the second zero-position deflection angle position. At this time, the zero-position calibration of the second deflection angle β is completed.

[0169] In step S413, a second zero-position calibration command is sent to determine the position of the first zero-position deflection angle.

[0170] According to the example embodiment, the second zero-position calibration command can be a control command for zero-position calibration of the first deflection angle α. The first zero-position deflection angle position can be the absolute position of the first push rod motor and the absolute position of the second push rod motor when the second deflection angle β is 0°.

[0171] For example, in step S413, the control module 61 sends a second zero-position calibration command to determine the position of the first zero-position deflection angle.

[0172] The control module 61 sends a second zero-position calibration command, setting the control mode of the first and second push rod motors to position control mode. The position control of the second zero-position calibration command can be such that the first deflection angle α is 0°, and the second deflection angle β is not 0°. The first and second push rod motors drive the actuators to a position where the first deflection angle is 0°, for example, the positions of points A, B, C, and D in the figure. The control module 61 determines whether the current zero position of the first deflection angle α is consistent with the zero position of the user-preset first deflection angle α.

[0173] If the current zero position of the first deflection angle α is consistent with the zero position of the user-preset first deflection angle α, the current absolute position of the first push rod motor and the current absolute position of the second push rod motor are taken as the first zero position deflection angle position. At this time, the zero position calibration of the first deflection angle α is completed.

[0174] If the current zero point of the first deflection angle α is inconsistent with the user-preset zero point of the first deflection angle α, the control module 61 sends an adjustment value for the first deflection angle α until the zero point of the first deflection angle α is consistent with the user-preset zero point of the first deflection angle (for example, adjusting from point A to point B). The current first deflection angle α is taken as the zero point of the first deflection angle α, and the current absolute position of the first push rod motor and the current absolute position of the second push rod motor are taken as the first zero deflection angle position. At this time, the zero point calibration of the first deflection angle is completed.

[0175] Through the above embodiments, the technical solution of this application can use axis control commands to ensure that the axis of the thruster meets preset axis conditions. The technical solution of this application can use a first zero-position calibration command to determine the second zero-position deflection angle position. The technical solution of this application can use a second zero-position calibration command to determine the first zero-position deflection angle position.

[0176] The technical solution of this application can reduce the control deviation of the thruster by performing zero-position calibration on the thruster.

[0177] According to another aspect of this application, this application also provides a non-volatile computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is capable of implementing the control method for a vector propulsion device for an underwater robot as described above.

[0178] According to another aspect of this application, this application also provides an electronic device, including: one or more processors; and a storage device for storing one or more programs, which, when executed by the one or more processors, enable the one or more processors to implement the control method for a vector propulsion device for an underwater robot as described above.

[0179] According to another aspect of this application, this application also provides a computer program product, comprising: a computer program stored on a computer-readable storage medium; the computer program includes program instructions that, when executed by a computer, cause the computer to perform the control method for a vector propulsion device for an underwater robot as described above.

[0180] Finally, it should be noted that the above description is merely a preferred embodiment of this application and is not intended to limit this application. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions of the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A control method for a vector propulsion device for an underwater robot, characterized in that, The vector propulsion device includes: a first push rod motor, a second push rod motor, and a propeller. The first push rod motor is rotatably connected to the tail of the propeller through a first fulcrum, and the second push rod motor is rotatably connected to the tail of the propeller through a second fulcrum. The first fulcrum and the second fulcrum satisfy preset fulcrum conditions. The control method includes: Based on the first target deflection angle, the second target deflection angle, and the first preset model of the thruster, the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor are determined. The first target position of the first push rod motor is determined based on the first predicted extension length, and the second target position of the second push rod motor is determined based on the second predicted extension length. A first control command is determined based on the first target location, and a second control command is determined based on the second target location; The first control command is sent to the first push rod motor, and the second control command is sent to the second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

2. The control method according to claim 1, characterized in that, Before determining the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the pusher, the control method further includes: The step of determining the first preset model includes: Based on the collected first set of deflection angles, determine the first set of coordinates corresponding to the first set of deflection angles; Based on the first coordinate set and the first push rod length set collected corresponding to the first deflection angle set, a first design matrix and a first observation vector set are constructed. Based on the first design matrix and the first set of observation vectors, determine the first set of coefficient matrices; Based on the first set of coefficient matrices and the first set of coordinates, a set of predicted push rod lengths is determined to determine the first preset model.

3. The control method according to claim 2, characterized in that, After determining the predicted push rod length set based on the first coefficient matrix set and the first coordinate set to determine the first preset model, the step of determining the first preset model further includes: Determine the first evaluation parameters of the first preset model to optimize the first preset model.

4. The control method according to claim 1, characterized in that, Determining the first target position of the first push rod motor based on the first predicted extension length includes: Determine the first set of control parameters for the first push rod motor; The first target position is determined based on the first predicted extension length and the first set of control parameters; The step of determining the second target position of the second push rod motor based on the second predicted extension length includes: Determine the second set of control parameters for the second push rod motor; The second target position is determined based on the second predicted extension length and the second set of control parameters.

5. The control method according to claim 1, characterized in that, After sending the first control command to the first actuator motor and the second control command to the second actuator motor, so that the first actuator motor executes the first control command and the second actuator motor executes the second control command, the control method further includes: Determine the first current push rod length after the first push rod motor executes the first control command, and determine the second current push rod length after the second push rod motor executes the command. Based on the first current push rod length, the second current push rod length, and the second preset model, determine the first current deflection angle and the second current deflection angle of the thruster until the thruster moves to the first target deflection angle and the second target deflection angle.

6. The control method according to claim 5, characterized in that, Before determining the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the pusher, the control method further includes: The step of determining the second preset model includes: Based on the collected set of second deflection angles, determine the second set of coordinates corresponding to the set of second deflection angles; Based on the second coordinate set and the second push rod length set collected corresponding to the second deflection angle set, a second design matrix and a second observation vector set are constructed. Based on the second design matrix and the second set of observation vectors, determine the second set of coefficient matrices; The predicted coordinate set is determined based on the second set of coefficient matrices and the second set of push rod lengths; Based on the predicted coordinate set, a predicted deflection angle set is determined to determine the second preset model.

7. The control method according to claim 6, characterized in that, After determining the set of predicted deflection angles based on the set of predicted targets to determine the second preset model, the step of determining the second preset model further includes: Determine the second evaluation parameters of the second preset model to optimize the second preset model.

8. The control method according to claim 1, characterized in that, Before determining the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the pusher, the control method further includes: Zero-point calibration steps include: Send axis control commands to ensure that the axis of the thruster meets preset axis conditions; Send the first zero-position calibration command to determine the position of the second zero-position deflection angle; Send a second zero-position calibration command to determine the first zero-position deflection angle.

9. A control device for a vector propulsion system of an underwater robot, characterized in that, The vector propulsion device includes: The system comprises a first push rod motor, a second push rod motor, and a propeller. The first push rod motor is rotatably connected to the tail of the propeller via a first fulcrum, and the second push rod motor is rotatably connected to the tail of the propeller via a second fulcrum. The first fulcrum and the second fulcrum satisfy preset fulcrum conditions. The control device includes: The control module determines the first predicted extension length of the first push rod motor and the second predicted extension length of the second push rod motor based on the first target deflection angle, the second target deflection angle, and the first preset model of the pusher. The control module determines the first target position of the first push rod motor based on the first predicted extension length, and determines the second target position of the second push rod motor based on the second predicted extension length; The control module determines a first control command based on the first target position and a second control command based on the second target position; The control module sends the first control command to the first push rod motor and the second control command to the second push rod motor, so that the first push rod motor executes the first control command and the second push rod motor executes the second control command, thereby causing the thruster to move to the first target deflection angle and the second target deflection angle.

10. A non-volatile computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the control method for a vector propulsion device for an underwater robot as described in any one of claims 1-8.

11. An electronic device, characterized in that, include: One or more processors; Storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the control method for a vector propulsion device for an underwater robot as described in any one of claims 1-8.

12. A computer program product, characterized in that, The method includes a computer program stored on a computer-readable storage medium, the computer program including program instructions that, when executed by a computer, cause the computer to perform the control method for a vector propulsion device for an underwater robot as described in any one of claims 1-8.

Images