Deep-sea robot visual perception and disturbance compensation control method and system for seafloor grabbing operation

Abstract

The application provides a deep-sea robot visual perception and disturbance compensation control method and system for seabed grabbing operation, and relates to the technical field of deep-sea engineering equipment control. The method comprises the following steps: acquiring the base state, the mechanical arm joint state, the target pose, the obstacle information and the sea surface wave information at the current time; based on the mechanical arm joint space sampling, candidate grabbing targets near the reachable boundary are generated, and trajectory optimization is carried out with the optimization objectives of obstacle avoidance and target approaching, and after the end Cartesian path is generated, it is converted into a joint reference trajectory; a servo preview model consistent with the actual mechanical arm dynamic characteristics is used to generate a preview joint state, and the mechanical arm preview disturbance is estimated accordingly; the sea surface wave information is used to estimate the wave disturbance; the two types of disturbances are introduced into the nonlinear model predictive controller as known time-varying disturbances, the propeller control quantity is solved to control the base, and the joint reference trajectory is output for the mechanical arm to execute.

Description

Technical Field

[0001] This application relates to the field of deep-sea engineering equipment control technology, and in particular to a visual perception and disturbance compensation control method and system for deep-sea robots for seabed grasping operations. Background Technology

[0002] Deep-sea hydrothermal environments contain abundant polymetallic nodules, cobalt-rich crusts, polymetallic sulfides, and rare earth resources, urgently requiring autonomous and intelligent deep-sea scientific research operations. Deep-sea exploration and sampling robots with autonomous mobility need to maintain stable posture under the combined effects of waves, currents, buoyancy recovery forces, and thruster constraints during close-range, controllable, and visualized intelligent exploration and refined geological sampling of the seabed. This ensures that the robotic arm mounted on the base can perform operations such as approaching, obstacle avoidance, and grasping under visual perception.

[0003] Because the robotic arm is rigidly connected to the robot base, the joint acceleration, link speed, hydrodynamic damping, and target grasping process of the robotic arm will generate reaction forces and torques on the base through the base-robotic arm coupled dynamics. When the target is near the reachable boundary of the robotic arm's end effector, and there are obstacles such as structural components, reefs, and cables around the target, it is difficult to guarantee the reachability and smoothness of the end effector trajectory by Cartesian linear interpolation and feedback compensation alone.

[0004] In related methods, robotic arm trajectory planning and base disturbance rejection control are usually performed independently. The above methods are effective when the robotic arm's motion amplitude is small or the target is far from the reachable boundary. However, under conditions where the robotic arm's motion amplitude is large, obstacles are dense, wave disturbances are obvious, or the thruster input is limited, the end effector trajectory is prone to deviate from the reachable space, joint motion is prone to abrupt changes, base attitude and angular velocity are prone to oscillation, and thruster energy consumption is high.

[0005] Therefore, how to enable deep-sea exploration and sampling robots to achieve smooth obstacle avoidance movement of the robotic arm's end effector within the reachable space while maintaining stable posture of the base during seabed grasping operations is an urgent technical problem to be solved. Summary of the Invention

[0006] The purpose of this application is to provide a visual perception and disturbance compensation control method and system for deep-sea robots for seabed grasping operations. The aim is to enable the end effector trajectory of the deep-sea exploration and sampling robot to achieve smooth obstacle avoidance movement within the reachable space during seabed grasping operations, while the base can maintain posture stability under the combined action of the robot's reaction disturbance and wave disturbance.

[0007] To solve the above-mentioned technical problems, this application is implemented as follows: A first aspect of this application discloses a visual perception and disturbance compensation control method for deep-sea robots used in seabed grasping operations, the method comprising: Acquire the current base status, robotic arm joint status, target pose, obstacle information, and sea wave information; The current end-effector position is determined based on the joint state of the robotic arm, and at least one candidate grasping target is generated near the reachable boundary of the robotic arm end-effector based on the joint space sampling of the robotic arm, using the current end-effector position as a reference. Starting from the current end position and ending at the candidate capture target, a Cartesian path is generated at the end position through trajectory optimization based on the target pose and obstacle information; the trajectory optimization aims at least on obstacle avoidance and approaching the target pose. The end-effector Cartesian path is converted into a joint reference trajectory for the robotic arm; Using a servo preview model that matches the dynamic characteristics of the actual robotic arm, a preview joint state is generated based on the joint reference trajectory, and the robotic arm preview disturbance generated by the robotic arm motion on the base is estimated based on the preview joint state. The wave disturbances experienced by the base in the future prediction time domain are estimated based on the sea surface wave information. Using the base state as the initial state, the robot arm preview disturbance and the wave disturbance are input into the nonlinear model predictive controller of the base as known time-varying disturbances in the prediction time domain to solve the thruster control quantity; The thruster control quantity is output to control the pose of the base, and the joint reference trajectory is output for the robotic arm to execute.

[0008] Optionally, the target pose, the obstacle information, and the sea wave information are obtained through synchronous binocular visual perception, including: Images of the robotic arm's workspace and the sea surface are captured by a synchronously triggered binocular camera. Distortion correction and epipolar correction are performed on the left and right images, and a three-dimensional point cloud is generated through pixel matching and triangulation. The three-dimensional point cloud is filtered and its coordinates transformed to obtain the target pose and obstacle information; The three-dimensional point cloud is interpolated onto a horizontal plane using a regular grid to extract the vertical height component, thus obtaining a local spatiotemporal wave field, which serves as the sea surface wave information.

[0009] Optionally, the current end effector position is determined based on the joint state of the robotic arm, and using the current end effector position as a reference, at least one candidate grasping target located near the reachable boundary of the robotic arm end effector is generated based on the joint space sampling of the robotic arm, including: The current end effector position is calculated using forward kinematics based on the joint state of the robotic arm. Multiple joint configurations are randomly sampled within the joint limit range of the robotic arm, and the end position corresponding to each joint configuration is calculated using forward kinematics. Calculate the distance between each end position and the current end position, and filter out end positions whose distance is between a preset lower threshold and a preset upper threshold; Obstacle collision detection is performed on the selected end positions, and end positions that enter the safety boundary of the obstacle are eliminated. The remaining end positions are used as candidate grab targets.

[0010] Optionally, taking the current end position as the starting point and the candidate grasping target as the ending point, a Cartesian path is generated at the end point through trajectory optimization based on the target pose and the obstacle information, including: For each candidate grabbing target, an initial path is constructed from the current end position to the candidate grabbing target, and the initial path is discretized to obtain a discrete path point sequence; Construct a comprehensive cost function, which includes smoothness cost, obstacle avoidance cost, trajectory energy cost, reachability boundary cost, and target error cost that makes the end of the path approach the target pose; The discrete path point sequence is optimized with the goal of minimizing the comprehensive cost function to obtain the terminal Cartesian path.

[0011] Optionally, the discrete path point sequence is optimized with minimizing the comprehensive cost function as the optimization objective to obtain the terminal Cartesian path, including: Using the discrete path point sequence corresponding to each candidate grabbing target as the path point sequence of the current iteration, and performing an iterative update process according to the following steps, the candidate terminal Cartesian path corresponding to the candidate grabbing target is obtained: Add multiple random smooth noises to the current iteration's path point sequence to generate multiple trial path point sequences; Calculate the comprehensive cost of each trial path point sequence, and construct weights based on the comprehensive cost of each trial path point sequence; The noise increment vectors of each trial path point sequence relative to the current iteration path point sequence are summed according to the weights to obtain the superimposed noise increment; The superimposed noise increment is superimposed on the path point sequence of the current iteration to obtain the updated path point sequence; Discrete Laplace smoothing is applied to the updated path point sequence to reduce the second-order difference energy of the path, and the smoothed path point sequence is used as the current path point sequence for the next iteration. Repeat the above process until the preset iteration termination condition is met, and take the path point sequence at the time of termination as the candidate end Cartesian path corresponding to the candidate grabbing target. From all candidate terminal Cartesian paths corresponding to the candidate crawling targets, select the one with the lowest overall cost as the terminal Cartesian path.

[0012] Optionally, converting the end-effector Cartesian path into a joint reference trajectory for the robotic arm includes: The terminal Cartesian path is smoothed over time, and a maximum step size constraint is applied to the displacement between adjacent path points to obtain a path point sequence with step size constraint. Using the joint state of the robotic arm as the initial joint solution of the first path point, damped least squares inverse kinematics solution is sequentially performed on each path point in the path point sequence after step length constraint. When the solution residual of the path point is greater than the preset threshold, backtracking search is performed along the line connecting the path point and the previous path point until the residual meets the requirements or the maximum number of backtracking is reached, and the effective joint angle of each path point is obtained. The effective joint angles at each path point are recalculated using forward kinematics to obtain the joint reference trajectory consistent with the definition of forward kinematics.

[0013] Optionally, the servo preview model is a second-order servo model, and the natural frequency and damping ratio of the second-order servo model are calibrated based on the dynamic characteristics of the actual robotic arm; generating a preview joint state based on the joint reference trajectory includes: Obtain the joint reference position, joint reference velocity, and joint reference acceleration of the joint reference trajectory at each sampling time; The joint reference position, joint reference velocity, and joint reference acceleration at each sampling time are input into the second-order servo model to calculate the joint acceleration. An acceleration limit is applied to the joint acceleration to obtain a preview of the joint acceleration. Discretely integrate the preview joint acceleration to obtain the joint velocity, and apply a velocity limit to the joint velocity to obtain the preview joint velocity; Discrete integrals are performed on the preview joint velocity to obtain the preview joint position; The preview joint acceleration, the preview joint velocity, and the preview joint position are used as the preview joint state.

[0014] Optionally, estimating the robot arm preview disturbance on the base caused by the robot arm movement based on the preview joint state includes: Based on the preview joint position, preview joint velocity, and preview joint acceleration, and combined with the inertial coupling term in the base-manipulator coupling dynamics model, the inertial reaction force and torque generated by each link of the manipulator on the base are calculated. Based on the preview joint speed and the preview joint position, and combined with the hydrodynamic damping coefficients of each link of the robotic arm, the hydrodynamic damping reaction force and torque generated by each link of the robotic arm on the base are calculated. The inertial reaction force and torque are superimposed with the hydrodynamic damping reaction force and torque to obtain the preview disturbance of the robotic arm.

[0015] Optionally, estimating the wave disturbances experienced by the base in the future prediction time domain based on the sea surface wave information includes: The sea surface wave information is input into the physical information neural predictor. The physical information neural predictor uses the partial differential equation of fluid dynamics as a constraint to extrapolate the local wave field in the prediction time domain, and calculates the fluid velocity and fluid acceleration based on the local wave field. The base is discretized into multiple equivalent force elements. For each force element, the relative velocity is determined based on the velocity of the force element and the velocity of the fluid. Based on the relative velocity and the fluid acceleration, the Morison equation is applied to calculate the resistance and inertial force on the force element. The resistance and inertial forces experienced by all force elements are transformed into the base coordinate system, and the resistance and inertial forces experienced by all force elements in the base coordinate system are summed to obtain the wave disturbance.

[0016] A second aspect of this application discloses a visual perception and disturbance compensation control system for deep-sea robots used in seabed grasping operations, the system comprising: The status acquisition module is used to acquire the current status of the base, the joint status of the robotic arm, the target pose, obstacle information, and sea wave information. The candidate target generation module is used to determine the current end position based on the joint state of the robotic arm, and with the current end position as a reference, generate at least one candidate grasping target located near the reachable boundary of the robotic arm end based on the joint space sampling of the robotic arm. The trajectory optimization module is used to generate an end Cartesian path by taking the current end position as the starting point and the candidate capture target as the ending point, based on the target pose and the obstacle information; the trajectory optimization has at least obstacle avoidance and approaching the target pose as optimization objectives; A joint reference generation module is used to convert the end effector Cartesian path into a joint reference trajectory for the robotic arm; The robotic arm disturbance estimation module is used to generate a preview joint state based on the joint reference trajectory using a servo preview model consistent with the dynamic characteristics of the actual robotic arm, and to estimate the robotic arm preview disturbance generated by the robotic arm motion on the base based on the preview joint state. The wave disturbance estimation module is used to estimate the wave disturbance that the base will experience in the future prediction time domain based on the sea surface wave information. The predictive control module is used to take the base state as the initial state, input the robot arm preview disturbance and the wave disturbance as known time-varying disturbances in the prediction time domain into the nonlinear model predictive controller of the base, and solve the thruster control quantity. The output control module is used to output the thruster control quantity to control the position and orientation of the base, and to output the joint reference trajectory for the robotic arm to execute.

[0017] A third aspect of this application discloses an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations described in the first aspect of this application.

[0018] A fourth aspect of this application discloses a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations described in the first aspect of this application.

[0019] A fifth aspect of this application discloses a computer program product, including a computer program that, when executed by a processor, implements the steps of the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations described in the first aspect of this application.

[0020] The embodiments of this application have the following advantages: In this embodiment, the current end effector position is determined based on the joint state of the robotic arm, and with this position as a reference, candidate grasping targets located near the reachable boundary are generated based on joint space sampling. This enables the generation of reasonable planning targets for difficult working conditions where the target is located near the workspace boundary.

[0021] Furthermore, the trajectory is optimized by taking the current end-effector position as the starting point and the candidate grasping target as the ending point, with obstacle avoidance and target approach pose as optimization objectives. This ensures that the end-effector Cartesian path remains smooth while avoiding obstacles, reducing the risk of abrupt joint motion changes and trajectory spikes. Simultaneously, the end-effector Cartesian path is converted into a joint reference trajectory to ensure that the planned path can be actually executed by the robotic arm.

[0022] A servo preview model consistent with the dynamic characteristics of the actual robotic arm is used to generate preview joint states, and the preview disturbance of the robotic arm is estimated based on the preview joint states. This ensures that the preview disturbance is consistent with the disturbance generated by the actual execution, avoids disturbance feedforward distortion caused by the inconsistency between the ideal planned trajectory and the actual execution trajectory, and improves the accuracy of base feedforward compensation.

[0023] In addition, wave disturbances are estimated based on sea surface wave information, and preview disturbances of the robotic arm are estimated based on the preview joint status. Both types of disturbances are introduced into the nonlinear model predictive controller of the base as known time-varying disturbances in the prediction time domain. This enables the controller to perform feedforward compensation in advance when the robotic arm disturbance and wave disturbance act together, suppressing the base attitude angle oscillation, angular velocity oscillation and control input oscillation, and reducing the thruster energy consumption.

[0024] Thus, by incorporating the output of the robotic arm joint reference trajectory and the output of the thruster control quantity into the same control framework, the base can maintain a stable hovering posture under the combined influence of the robotic arm's reaction disturbance and environmental disturbance throughout the entire process of the robotic arm performing the grasping operation, thereby improving the success rate and safety of seabed grasping operations. Attached Figure Description

[0025] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the description of the embodiments of this application will be briefly introduced below. Obviously, the 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.

[0026] Figure 1 This is a flowchart illustrating the steps of a deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations provided in this application embodiment; Figure 2 This is an overall architecture diagram of a deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations provided in an embodiment of this application; Figure 3 This is a look-ahead compensation structure diagram of a fusion of wave disturbance and robotic arm disturbance provided in an embodiment of this application; Figure 4 This is a schematic diagram of the three-dimensional trajectory of a robotic arm end effector in the base coordinate system provided in an embodiment of this application; Figure 5 This is a schematic diagram of TCP coordinate system error provided in an embodiment of this application; Figure 6 This is a schematic diagram illustrating the changes of TCP coordinate components over time, provided in an embodiment of this application. Figure 7 This is a schematic diagram of a joint grasping state machine provided in an embodiment of this application; Figure 8 This is a schematic diagram of the motion trajectory of an ROV in three-dimensional space provided in an embodiment of this application; Figure 9 This is a schematic diagram comparing the ROV position with a reference value provided in an embodiment of this application; Figure 10This is a schematic diagram illustrating the variation of Euler angles in a ROV, provided in an embodiment of this application. Figure 11 This is a schematic diagram of the velocity response of an ROV in a body coordinate system provided in an embodiment of this application; Figure 12 This is a schematic diagram of the trajectory of an end effector in the world coordinate system provided in an embodiment of this application; Figure 13 This is a schematic diagram of a q1 tracking result provided in an embodiment of this application; Figure 14 This is a schematic diagram of a q2 tracking result provided in an embodiment of this application; Figure 15 This is a schematic diagram of a q3 tracking result provided in an embodiment of this application; Figure 16 This is a schematic diagram of a q4 tracking result provided in an embodiment of this application; Figure 17 This is a schematic diagram of a q5 tracking result provided in an embodiment of this application; Figure 18 This is a schematic diagram of the structure of a deep-sea robot visual perception and disturbance compensation control system for seabed grasping operations provided in an embodiment of this application; Figure 19 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0027] To make the above-mentioned objectives, features, and advantages of this application more apparent and understandable, the technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0028] In related technologies, deep-sea exploration and sampling robots typically handle robotic arm trajectory planning and base disturbance rejection control as two independent subsystems when performing seabed grasping operations. This architecture has the following shortcomings when dealing with complex operating conditions: (1) The end-effector trajectory planning of the robotic arm lacks reachability constraints. Most related methods use fixed Cartesian interpolation or single-target inverse kinematics to generate the end-effector trajectory, without fully considering the reachability constraints when the target is near the reachable boundary of the end-effector (Tool CenterPoint, TCP). When the grasped target is near the boundary of the workspace, traditional inverse kinematics solutions are prone to problems such as large residuals, abrupt joint solutions, or the actual TCP failing to track the reference trajectory.

[0029] (2) The handling of obstacles is not sufficient. The relevant schemes only perform waypoint obstacle avoidance or rely on human experience to detour, and lack a comprehensive trajectory optimization mechanism that considers smoothness, energy consumption, obstacle avoidance cost, reachability boundary cost and target tracking error at the same time.

[0030] (3) Inconsistent disturbance estimation between the robotic arm and the base. Related methods typically use an ideal joint reference trajectory directly to calculate the reaction disturbance of the robotic arm's motion on the base. However, due to servo lag, velocity / acceleration limiting, and joint physical constraints, the actual execution trajectory of a real robotic arm deviates from the ideal reference trajectory. Directly using the ideal reference to estimate the disturbance leads to inconsistencies between the preview disturbance and the actual disturbance, reducing the effectiveness of feedforward compensation.

[0031] (4) The base controller lacks the ability to predict disturbances of the robotic arm. The relevant base controllers usually only focus on environmental disturbances such as waves and ocean currents, and do not include the reaction force / torque generated by the future movement of the robotic arm in the prediction model. When the robotic arm performs large-amplitude movements, the robotic arm disturbance often becomes the main external disturbance of the base, and the controller can only rely on feedback for hysteresis compensation, which is prone to oscillation of base attitude and angular velocity.

[0032] To overcome the limitations of related technologies, this application provides a visual perception and disturbance compensation control method for deep-sea robots in seabed grasping operations. The technical concept involves integrating robotic arm trajectory planning and base disturbance rejection control into the same collaborative control framework. On the robotic arm side, candidate grasping targets near the TCP reachable boundary are generated through random sampling in joint space. A trajectory optimization method is used, with obstacle avoidance and target approach poses as optimization objectives, to generate an end-effector Cartesian path. This Cartesian path is then converted into a joint reference trajectory. A servo model consistent with the actual dynamic characteristics of the robotic arm is used to generate a preview joint state, and the reaction disturbance of the robotic arm on the base is estimated based on the preview state. On the base side, wave disturbances are estimated by combining the visually reconstructed local spatiotemporal wave field with a physical information neural predictor. The robotic arm preview disturbance is treated as a known time-varying disturbance and introduced into a nonlinear model predictive controller (NMPC) to simultaneously consider wave disturbances and robotic arm preview disturbances in the prediction time domain. Rolling optimization is used to solve the thruster control quantity, achieving advance feedforward compensation for both types of disturbances.

[0033] The technical solution of this application will be described in detail below with reference to the accompanying drawings, through specific embodiments and application scenarios.

[0034] Reference Figure 1 As shown, Figure 1 This is a flowchart illustrating the steps of a deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations, as provided in an embodiment of this application. Figure 1As shown, the method may include steps S110 to S180: Step S110: Obtain the current base status, robotic arm joint status, target pose, obstacle information, and sea wave information.

[0035] In this step, the base state includes the six-degree-of-freedom pose (position coordinates and attitude angles) of the deep-sea exploration and sampling robot base in the inertial coordinate system and its corresponding velocity vector, which can be measured by sensors such as fiber optic inertial navigation, depth gauge, altimeter, and DVL (Doppler Velocity Log). The robotic arm joint state includes the position, velocity, and optional actuator state of each joint; the joint velocity can be directly measured by sensors or estimated by joint position differential. The target pose is the three-dimensional position of the target to be grasped in the base volume coordinate system, the obstacle information is the geometric description of obstacles in the workspace, and the sea wave information is the local spatiotemporal wave field of the robot's location. The above information can be obtained through synchronous binocular vision perception or by fusing information acquired separately by each sensor.

[0036] Step S120: Determine the current end position based on the joint state of the robotic arm, and using the current end position as a reference, generate at least one candidate grasping target located near the reachable boundary of the robotic arm end based on the joint space sampling of the robotic arm.

[0037] In this step, the current end effector position refers to the position of the robotic arm's end effector at the current moment, calculated from the robotic arm's joint states using forward kinematics. Since the grasping target may be located near the boundary of the reachable space of the robotic arm's end effector, performing inverse kinematics solutions in this region can easily lead to no solution or excessively large residuals. Therefore, this step randomly samples multiple joint configurations within the robotic arm's joint constraints, maps them to Cartesian space using forward kinematics, and selects end effector positions located near the reachable boundary that meet obstacle avoidance requirements as candidate grasping targets. By generating multiple candidate targets, diverse planning endpoints are provided for subsequent trajectory optimization, increasing the probability of finding a feasible path under boundary conditions.

[0038] Step S130: Starting from the current end position and ending at the candidate capture target, generate an end Cartesian path through trajectory optimization based on the target pose and obstacle information; the trajectory optimization aims at least on obstacle avoidance and approaching the target pose.

[0039] In this step, the end effector Cartesian path refers to a series of discrete path points traversed by the robotic arm's end effector as it moves from its current position to the candidate grasping target in the base body coordinate system. Specifically, for each candidate grasping target, an initial path from the starting point to the ending point is first constructed. Then, with at least obstacle avoidance and approaching the target pose as optimization objectives, the initial path is optimized so that the final generated end effector Cartesian path satisfies both the task requirements (approaching the target) and the safety requirements (obstacle avoidance).

[0040] Step S140: Convert the end-effector Cartesian path into a joint reference trajectory for the robotic arm.

[0041] In this step, considering that the driving element of the robotic arm is a joint motor, the end effector Cartesian path needs to be converted into a reference trajectory in joint space before it can be executed by the robotic arm. By performing inverse kinematics calculations on each path point, the position information of the path point is mapped to the corresponding joint angle, thereby obtaining the joint reference trajectory that the robotic arm can execute. The joint reference trajectory is the sequence of positions that each joint of the robotic arm should reach at each sampling time.

[0042] Step S150: Using a servo preview model that matches the dynamic characteristics of the actual robotic arm, generate a preview joint state based on the joint reference trajectory, and estimate the robotic arm preview disturbance generated by the robotic arm movement on the base based on the preview joint state.

[0043] Specifically, when the actual robotic arm tracks the joint reference trajectory, due to the dynamic response characteristics of the servo controller, there are deviations between the actual joint position, velocity, and acceleration and the reference values. In order to accurately estimate the reaction disturbance of the robotic arm's motion on the base, this step uses a servo preview model consistent with the dynamic characteristics of the actual robotic arm to generate a preview joint state based on the joint reference trajectory.

[0044] The preview joint states reflect the expected actual motion state of the robotic arm when executing the reference trajectory, rather than the ideal reference motion state. Subsequently, based on the preview joint states and combined with the base-robotic arm coupled dynamics model, the reaction disturbance forces and torques generated by each link of the robotic arm on the base are estimated as the preview disturbances of the robotic arm.

[0045] Step S160: Estimate the wave disturbance that the base will experience in the future prediction time domain based on the sea surface wave information.

[0046] Wave disturbance is one of the main environmental disturbances affecting the hovering accuracy of the base. This step uses the acquired sea surface wave information to estimate the wave disturbance (including wave disturbance force and moment) experienced by the base over a future period. Specifically, the wave disturbance estimation can be based on hydrodynamic theory, considering the relative motion between various parts of the base and wave water particles, thereby calculating the inertial force and damping force generated by the waves on the base to obtain the wave disturbance.

[0047] Step S170: Using the base state as the initial state, input the robot arm preview disturbance and the wave disturbance as known time-varying disturbances in the prediction time domain into the nonlinear model predictive controller of the base, and solve for the thruster control quantity.

[0048] In this step, the nonlinear model predictive controller is a model-based optimal control method used to solve a finite-time optimal control problem in the prediction time domain at each sampling time, with the current base state as the initial value, so that the base state approaches the target pose.

[0049] Specifically, the robot arm preview disturbance estimated in step S150 and the wave disturbance estimated in step S160 are simultaneously incorporated as known time-varying disturbances in the prediction time domain into the nonlinear model predictive controller (NMPC). At each sampling time k, the NMPC is based on the current base state. As the initial state, a discrete prediction model with time-varying perturbations is used in the prediction time domain:

[0050] in, The base state is predicted at the (i+1)th time in the future at sampling time k; The base state is predicted at the i-th future time step at sampling time k; This corresponds to the thruster control quantity; and These represent the wave disturbance and the robotic arm preview disturbance at the i-th time point, respectively. It is a discrete-form base dynamics model, including rigid body dynamics, hydrodynamic damping, and propeller mapping.

[0051] The objective function of the nonlinear model predictive controller (NMPC) This includes penalties for pose error, velocity error, control input amplitude, and control input rate of change. For example, the objective function... It can be represented as:

[0052] in, This refers to pose tracking error; For speed tracking error; , , , These are the weight matrices for pose error, velocity error, control input amplitude, and control input rate of change, respectively. For thruster control quantity, This represents the change in thruster control parameters between adjacent time points. Its penalty term is used to suppress frequent thruster movements, thereby reducing energy consumption and mechanical wear.

[0053] The constraints of NMPC include thruster input saturation constraints, input rate of change constraints, and base safety attitude boundary constraints. The optimal thruster control sequence is obtained through rolling optimization, and the first element of the optimal control sequence is used as the thruster control output.

[0054] In this way, when optimizing the thruster control, NMPC not only considers the current state error but also anticipates the impact of two types of future disturbances, thus achieving feedforward compensation rather than passive feedback adjustment. Compared to related technologies where the base controller only focuses on environmental disturbances and does not incorporate the reaction force generated by the future movements of the robotic arm into the prediction model, this embodiment introduces both the robotic arm preview disturbance and wave disturbance into the NMPC prediction model simultaneously. This allows the controller to compensate in advance when the robotic arm disturbance and wave disturbance act together, effectively suppressing base attitude angle oscillations and thruster energy consumption oscillations.

[0055] Step S180: Output the thruster control quantity to control the pose of the base, and output the joint reference trajectory for the robotic arm to execute.

[0056] In this step, the thruster control quantity obtained in step S170 is sent to the thruster system to drive the thruster to generate corresponding thrust and torque, thereby controlling the base to maintain the target pose under the combined action of wave disturbance and robotic arm reaction disturbance. Simultaneously, the joint reference trajectory generated in step S140 is sent to the robotic arm joint servo controller to drive each joint of the robotic arm to move along a predetermined trajectory, performing tasks such as approaching and grasping. After one sampling cycle ends, the process returns to step S110 to enter the control loop of the next sampling cycle, achieving continuous coordinated control in the rolling time domain.

[0057] The technical solution of this embodiment integrates robotic arm trajectory planning and base disturbance rejection control into the same collaborative framework. On the robotic arm side, an reachable, obstacle-avoiding, and smooth end-effector path and an executable joint reference trajectory are generated. On the base side, robotic arm preview disturbances and wave disturbances are simultaneously introduced into the nonlinear model predictive controller, achieving feedforward compensation for both types of disturbances. This embodiment can ensure the reachability and smoothness of the end-effector trajectory under complex seabed conditions with large robotic arm movements, dense obstacles, and significant wave disturbances, while suppressing base attitude oscillations and thruster energy consumption oscillations, thus improving the stability and success rate of seabed grasping operations.

[0058] In an optional embodiment, the target pose, the obstacle information, and the sea wave information are obtained through synchronous binocular visual perception, specifically including steps A1 to A4: Step A1: Acquire left and right images of the robotic arm's workspace and the sea surface using a synchronously triggered binocular camera.

[0059] In this step, the binocular camera consists of a left camera and a right camera. Both are triggered by hardware synchronization signals to expose simultaneously, ensuring strict temporal alignment of the left and right images. The acquisition area covers the robotic arm's workspace (including the area where the target object is located and the surrounding obstacle environment) and the sea surface area (for wave sensing). The camera is mounted on the robot's base, and its extrinsic parameters (rotation matrix and translation vector relative to the base's volume coordinate system) are pre-determined through calibration.

[0060] Step A2: Perform distortion correction and epipolar correction on the left and right images, and generate a three-dimensional point cloud through pixel matching and triangulation.

[0061] In this step, the camera lens exhibits optical distortion (such as radial and tangential distortion), causing pixel positions in the image to shift. Distortion correction involves transforming the left and right images using pre-calibrated camera intrinsic parameters (including focal length, principal point coordinates, and distortion coefficients) to correct the distorted image to an image under the ideal pinhole model.

[0062] Epipolar correction is used to transform the epipolar lines (i.e., the projection lines on the two image planes of the lines connecting the spatial point and the optical centers of the two cameras) of the left and right images to the horizontal direction, so that the matching points of the same spatial point in the left and right images are located on the same horizontal scan line, thereby improving matching efficiency and accuracy.

[0063] Pixel matching involves finding corresponding points between the left and right images after epipolar correction, specifically determining which pixel in the right image corresponds to a given pixel in the left image. Pixel matching can be performed using window-based normalized cross-correlation matching or feature-point-based sparse matching. After obtaining the matching point pairs, the 3D coordinates of the spatial points are calculated using triangulation principles. Specifically, based on the pixel coordinate deviation (parallax) of the matching point in the left and right images, combined with the camera baseline length and focal length, the depth information of the point is calculated using similar triangle relationships. Performing the above operation on all matching points generates a 3D point cloud describing the workspace and wavefront shape.

[0064] Step A3: Filter and transform the three-dimensional point cloud to obtain the target pose and obstacle information.

[0065] In this step, the 3D point cloud may contain outliers and noise points caused by matching errors, occlusion, or sensor noise. Filtering is used to remove these invalid points. Filtering methods can include statistical filtering (removing outliers outside the statistical distribution) and radius filtering (removing isolated points with insufficient neighborhood points).

[0066] Coordinate transformation is the process of converting a point cloud from the camera coordinate system to the base volume coordinate system or another unified reference coordinate system to align it with the coordinate system used for robot arm control. The transformation matrix is ​​determined by the calibration parameters of the camera's mounting position and attitude, and includes rotation matrices and translation vectors.

[0067] After filtering and coordinate transformation, the point cloud clusters corresponding to the target object are segmented from the point cloud, and their geometric centers are extracted as the target pose; at the same time, the point cloud clusters corresponding to the obstacles are retained as obstacle information.

[0068] Step A4: Perform regular grid interpolation on the horizontal plane to extract the vertical height component of the three-dimensional point cloud and obtain the local spatiotemporal wave field as the sea surface wave information.

[0069] In this step, sea wave information is reflected as the spatiotemporal distribution of sea surface height. The 3D point cloud is projected onto a horizontal plane, discretized into a regular grid, and interpolated for each grid cell based on the height value of the point cloud falling within it to obtain the sea surface height at that location.

[0070] For example, let time The binocular reconstructed point cloud is The local height field after meshing is The local height field can then be expressed as:

[0071] in, This indicates that the point cloud is divided into spatial grids and interpolated based on the mean or median height of the point cloud within each grid. This indicates that the vertical height value of each grid location is extracted. Thus, That is, time. The spatial wave field distribution. Arranging the wave fields at multiple consecutive moments in chronological order yields the local spatiotemporal wave field, which is used as sea surface wave information for subsequent wave disturbance estimation.

[0072] The technical solution adopted in this embodiment, through synchronous binocular visual perception, can simultaneously output three types of information using only one visual sensor system: target pose for endpoint constraints in robotic arm trajectory planning, obstacle point cloud for obstacle avoidance constraints, and local spatiotemporal wave field for wave disturbance estimation. This simplifies system configuration, reduces the complexity of sensor calibration and synchronization, and ensures the consistency of the target, obstacles, and wave field in space and time.

[0073] In an optional embodiment, step S120 above, "determining the current end-effector position based on the joint state of the robotic arm, and generating at least one candidate grasping target located near the reachable boundary of the robotic arm end-effector based on the joint space sampling of the robotic arm using the current end-effector position as a reference," includes steps S120-1 to S120-4: Step S120-1: Calculate the current end effector position using forward kinematics based on the joint state of the robotic arm.

[0074] In this step, forward kinematics is the process of determining the position and orientation of the end effector in Cartesian space given the angles of each joint of the robotic arm. The joint states of the robotic arm can be represented as follows: ,in Let be the angle of the i-th joint (where i ranges from 1 to n, and n is the number of degrees of freedom of the robotic arm). The position of the end effector in the base body coordinate system is denoted as . Then the forward kinematic mapping can be expressed as:

[0075] in, The equation represents the positive kinematic function from joint space to Cartesian space, determined by kinematic parameters such as the link length and joint type of the robotic arm. The current end effector position can be obtained by substituting the current joint state of the robotic arm into the equation.

[0076] Step S120-2: Randomly sample multiple joint configurations within the joint limit range of the robotic arm, and calculate the end position corresponding to each joint configuration through forward kinematics.

[0077] Specifically, the joints of the robotic arm are limited by their physical structure, resulting in a limited range of angle values. ,in This represents the lower limit of the joint angle. This represents the upper limit of the joint angle. Within the joint's limiting range, uniformly distributed random sampling is used to generate a large number of candidate joint configurations. For each sampled joint configuration, the positive kinematics function is used. Calculate the corresponding end position These end positions are distributed across various areas within the workspace accessible to the robotic arm.

[0078] Step S120-3: Calculate the distance between each end position and the current end position, and filter out the end positions whose distance is between a preset lower threshold and a preset upper threshold.

[0079] In this step, in order to obtain candidate targets located near the reachable boundary, it is necessary to calculate the position of each sampling endpoint. With current end position The distance between them. ,in It represents the Euclidean norm (the straight-line distance in three-dimensional space).

[0080] Preset lower threshold and preset upper threshold They jointly defined the range of the interval near the reachable boundary, and preset a lower threshold. This is used to avoid candidate targets being too close to the current endpoint, ensuring that candidate targets can effectively test planning capabilities near the boundary; a preset upper threshold is used. This is used to ensure that the candidate target remains within the reachable workspace of the robotic arm, preventing the target from becoming completely unreachable.

[0081] Retain satisfaction The endpoint position is determined, and points that are too close or too far away are eliminated. This range can be set comprehensively based on the arm length of the robotic arm, the requirements of the task, and the current position of the endpoint in the reachable space.

[0082] Step S120-4: Perform obstacle collision detection on the selected end positions, remove end positions that have entered the obstacle safety boundary, and use the remaining end positions as candidate grab targets.

[0083] Specifically, even if the end position is within reach, it cannot be considered a valid grab target if it collides with an obstacle. Collision detection is performed on each retained end position after filtering. Obstacles can be modeled as geometric shapes (such as spheres, ellipsoids, or bounding boxes). Taking a sphere model as an example, let the obstacle set be O, and the j-th obstacle can be represented as... ,in The center of the obstacle. Let the geometric radius be the obstacle. The safety expansion radius (i.e., the safe distance extended outward from the actual geometry of the obstacle, used to ensure that there is sufficient obstacle avoidance margin between the end effector of the robotic arm and the obstacle).

[0084] For the end position The signed distance from the j-th obstacle is determined. Meaning:

[0085] when When this occurs, it indicates that the end position has entered the safety boundary of the obstacle, meaning a collision has occurred or the margin is insufficient; in this case, the candidate point should be eliminated. Only when... Only when the end position maintains a safe distance from all obstacles is the end position retained.

[0086] After the above screening and collision detection, the remaining end positions simultaneously meet the following conditions: (1) they are located near the reachable boundary (meeting the preset distance range); (2) they maintain a safe distance from obstacles. These end positions are used as candidate grab targets for subsequent trajectory optimization.

[0087] By employing the technical solution of this embodiment, through random sampling in the joint space of the robotic arm and followed by distance filtering and collision detection, multiple effective candidate grasping targets can be generated for difficult working conditions where the grasping target is located near the boundary of the workspace. The multi-candidate point sampling and filtering mechanism increases the probability of finding a feasible path under boundary conditions, providing diverse planning endpoints for subsequent trajectory optimization, thereby reducing the risk of planning failure due to unreachable targets.

[0088] In an optional embodiment, step S130 above, "taking the current end position as the starting point and the candidate grasping target as the ending point, and generating an end Cartesian path through trajectory optimization based on the target pose and the obstacle information," may include steps S130-1 to S130-3: Step S130-1: For each candidate grabbing target, construct an initial path from the current end position to the candidate grabbing target, and discretize the initial path to obtain a discrete path point sequence.

[0089] In this step, an initial path is constructed for each candidate grasping target as the starting point for optimization. For each candidate grasping target, a segmented initial path is constructed according to the stages of the grasping operation. Specifically, the grasping operation typically includes multiple stages: starting from the current end position, passing through the pre-grab approach point to reach the grasping contact point, and then lifting and withdrawing to the recovery point.

[0090] For example, let the current end position be The pre-fetching point is The capture contact point (i.e., the candidate capture target) is The rise point is and recycling points are The initial path is a polyline path that connects these key points sequentially. The initial path can be represented as " ".

[0091] To ensure the continuity of velocity and acceleration at the path endpoints and avoid abrupt changes in motion, each initial path segment is calibrated using a fifth-order time function to obtain a continuous path. The fifth-order time function... Represented as: , For normalized time variables, the fifth-order time function satisfies... , Furthermore, the first and second derivatives at the endpoints are both zero, ensuring that the position, velocity, and acceleration of each path segment are continuous at the connection points.

[0092] The continuous path, calibrated by the time function five times, is discretized in time to obtain a sequence of discrete path points consisting of N discrete path points. ,in These are the first to Nth path points in the discrete path point sequence.

[0093] Step S130-2: Construct a comprehensive cost function, which includes smoothness cost, trajectory energy cost, obstacle avoidance cost, reachability boundary cost, and target error cost that makes the end of the path approach the target pose.

[0094] In this step, to evaluate the quality of the path and guide the optimization direction, a comprehensive cost function containing multiple evaluation indicators is constructed. The comprehensive cost function for the discrete path point sequence P is as follows. It can be represented as:

[0095] in, The smoothness cost measures the smoothness of the path and is represented by the sum of squares of the second-order differences (i.e., accelerations) of the path points. This is used to suppress abrupt changes and oscillations in the path, making the motion smoother. The smaller the smoothness cost, the smoother the path.

[0096] Trajectory energy cost measures the sum of squares of displacements between adjacent path points, reflecting the energy consumption required to execute the path; the smaller the trajectory energy cost, the shorter the total path length and the higher the motion efficiency.

[0097] To assess the obstacle avoidance cost, the proximity of the path to the obstacle is measured; this can be based on the signed distance to the obstacle defined in step S120-4. When a path point enters the obstacle safety boundary ( When the obstacle avoidance cost is low, the path will be significantly penalized, forcing it to move away from the obstacle; the lower the obstacle avoidance cost, the safer the path.

[0098] The reachability boundary cost measures how close a path point is to the reachable space boundary of the robotic arm. A penalty is imposed when a path point approaches a joint limit or a singular region, guiding the path as far away from the reachability boundary as possible to avoid difficulties in inverse kinematics and joint mutations.

[0099] The target error cost measures the deviation between the path endpoint and the target pose; the target error cost ensures that the optimized path can eventually approach the target pose and meet the requirements of the grasping task.

[0100] These are weighting coefficients for smoothness cost, trajectory energy cost, obstacle avoidance cost, reachability boundary cost, and target error cost, respectively. They are used to adjust the relative importance of each cost in the total cost and can be adjusted according to the needs of specific tasks.

[0101] Step S130-3: Optimize the discrete path point sequence with the goal of minimizing the comprehensive cost function to obtain the terminal Cartesian path.

[0102] In this step, the discrete path point sequence in step S130-1 is used as the initial solution, and the comprehensive cost function in step S130-2 is used as the optimization objective. The position of the path points is continuously adjusted through an iterative optimization algorithm to gradually reduce the comprehensive cost. This optimization process simultaneously considers smoothness, energy efficiency, safety (obstacle avoidance and reachability boundary) and task accuracy (approaching the goal), and finally outputs a Cartesian path at the end that meets the requirements.

[0103] Furthermore, step S130-3 above may specifically include steps S130-3-1 to S130-3-2: Step S130-3-1: Using the discrete path point sequence corresponding to each candidate grabbing target as the path point sequence of the current iteration, and performing an iterative update process according to steps B1 to B6, to obtain the candidate terminal Cartesian path corresponding to the candidate grabbing target: Step B1: Add multiple random smooth noises to the current iteration's path point sequence to generate multiple trial path point sequences.

[0104] In this step, while maintaining the overall shape of the path, random noise (i.e., random perturbation) of different magnitudes is added to the path point sequence of the current iteration to generate multiple trial path point sequences. Each trial path point sequence represents a possible direction for improvement of the current path.

[0105] Step B2: Calculate the comprehensive cost of each trial path point sequence and construct weights based on the comprehensive cost of each trial path point sequence.

[0106] In this step, the comprehensive cost of each trial path point sequence is calculated according to the comprehensive cost function in step S130-2. The lower the comprehensive cost of the trial path point sequence, the greater its corresponding weight. In this way, the better trial path point sequences play a greater role in subsequent updates.

[0107] Step B3: Sum the noise increment vectors of each trial path point sequence relative to the current iteration path point sequence according to the weights to obtain the superimposed noise increment.

[0108] In this step, the difference between each trial path point sequence and the current path (i.e., the noise increment vector) is calculated. Then, all noise increment vectors are weighted and summed according to the weights obtained in step B2 to obtain a merged noise increment direction. This direction integrates the improved information from all trial path point sequences.

[0109] Step B4: Add the superimposed noise increment to the current iteration of the path point sequence to obtain the updated path point sequence.

[0110] In this step, the superimposed noise increment obtained in step B3 is added to the pathpoint sequence of the previous iteration to obtain the updated pathpoint sequence. Since the direction of the weighted summation tends to reduce the overall cost, the updated pathpoint sequence is statistically better than the unupdated one.

[0111] Step B5: Apply discrete Laplace smoothing to the updated path point sequence to reduce the second-order difference energy of the path, and use the smoothed path point sequence as the current path point sequence for the next iteration.

[0112] In this step, considering the potential local non-smoothness of the path after noise updates, a discrete Laplace smoothing operation is applied to reduce the second-order difference energy of non-fixed path points. Specifically, each path point is adjusted towards the center of its adjacent path points to remove high-frequency fluctuations in the path, making it smoother. The smoothed path point sequence serves as the current path point sequence for the next iteration.

[0113] Step B6: Repeat the above process until the preset iteration termination condition is met, and take the path point sequence at the time of termination as the candidate end Cartesian path corresponding to the candidate grabbing target.

[0114] Repeat steps B1 to B5 for the iterative update process. The iteration termination condition can be reaching the preset maximum number of iterations, or the change in the comprehensive cost for several consecutive rounds being less than a preset threshold (indicating that the path has converged to the vicinity of the optimal solution). The path point sequence at the time of termination is the optimal path corresponding to the candidate objective.

[0115] Step S130-3-2: Select the one with the lowest overall cost from all candidate terminal Cartesian paths corresponding to all candidate crawling targets as the terminal Cartesian path.

[0116] Since multiple candidate targets are generated in step S120, each candidate target, after optimization in steps S130-1 to S130-3, yields a candidate Cartesian path, each with its own comprehensive cost. This step compares the comprehensive costs of all candidate paths and selects the path with the lowest comprehensive cost as the final output Cartesian path. This multi-candidate target competition further improves the quality of the planning results.

[0117] The technical solution of this embodiment achieves multi-objective optimization of the terminal Cartesian path by simultaneously considering smoothness, energy, obstacle avoidance, reachability boundary, and target error through a comprehensive cost function. Through an iterative optimization strategy combining random perturbation exploration and discrete smoothing, a smooth, safe path that meets task requirements can be generated in complex environments with dense obstacles and limited reachability boundaries. Furthermore, by generating multiple candidate objectives, optimizing them separately, and selecting the best one, the planner's ability to find a globally optimal solution under boundary conditions is improved. Compared to related technologies that use fixed Cartesian interpolation or single-objective inverse kinematics solutions, this embodiment significantly improves path quality, obstacle avoidance capability, and boundary adaptability.

[0118] In an optional embodiment, step S140 above, "converting the end effector Cartesian path into a joint reference trajectory of the robotic arm," may include steps S140-1 to S140-3: Step S140-1: Perform time smoothing on the terminal Cartesian path and apply maximum step size constraint to the displacement between adjacent path points to obtain the path point sequence after step size constraint.

[0119] In this step, the optimized terminal Cartesian path may contain local non-smoothness or abrupt changes at stage transition points. These abrupt changes may lead to instability in the inverse kinematics solution or joint velocity exceeding limits. Therefore, the terminal Cartesian path is first smoothed in time. Specifically, a sliding window smoothing kernel can be used to perform a weighted average on each path point to obtain a smoothed path point sequence.

[0120] After time smoothing, a maximum step size constraint is applied to the displacement between adjacent path points. Specifically, in the smoothed path point sequence, the target path point to be constrained... (i.e., the expected position of the k-th path point after time smoothing), and its adjacent previous path point (i.e., the reachable reference position of the (k-1)th path point), then the displacement between adjacent path points is .like (in If the maximum step size is preset, then the displacement will be scaled to the allowable range:

[0121] The path points after the maximum step size constraint are: Step size constraints prevent excessive changes in the position of the end effector between adjacent time steps, thereby reducing the risk of joint velocity exceeding limits, singularities, and unreachability.

[0122] Step S140-2: Using the joint state of the robotic arm as the initial joint solution of the first path point, perform damped least squares inverse kinematics solution on each path point in the path point sequence after step length constraint, and when the solution residual of the path point is greater than the preset threshold, backtrack search is performed along the line connecting the path point and the previous path point until the residual meets the requirements or the maximum number of backtracks is reached, so as to obtain the effective joint angle of each path point.

[0123] In this step, inverse kinematics (IK) is the process of solving for the angles of each joint of the robotic arm, given the desired position of the end effector in Cartesian space. Damped least squares (DLS) is used for this solution, which effectively suppresses abrupt changes in joint velocity when the Jacobian matrix is ​​close to singular.

[0124] The damped least-squares inverse kinematics solution is sequentially performed on each path point in the path point sequence after step-size constraints. Specifically, the main task is end-effector position tracking, with an error of [missing information]. ,in The desired position of the end effector is the path point after time smoothing and step size constraints in step S140-1. Indicates the current joint angle of the robotic arm The corresponding actual position of the end effector. The position Jacobian matrix is: Then the pseudo-inverse of the position Jacobian matrix of damped least squares is used. It can be represented as:

[0125] in, This is a damping factor used to suppress the amplification effect of small singular values ​​when the Jacobian matrix approaches a singularity. Let I be the position Jacobian matrix, and I be the identity matrix. Joint increments are obtained through... Calculations show that each iteration updates... This continues until the error converges or the maximum number of iterations is reached.

[0126] For the first path point, consider the current state of the robotic arm joints. As the initial solution, the solution of the previous path point is used as the initial solution for subsequent path points. After each iteration update, single-step amplitude limiting and joint angle limiting are applied to ensure that the solution results are within the joint physical constraints.

[0127] After the solution is completed, check the solution residuals for the path points. Is it less than a preset threshold, where Let k be the expected end position of the path point. Current joint angle The corresponding actual end position. If the residual is less than the threshold, the path point is successfully solved. If the residual is greater than the threshold, then along... and The search is performed backwards along the direction of the connecting line. Let the backward step size be... The target point after rollback for:

[0128] Try one by one The first candidate point that satisfies the residual requirement is selected as the valid solution for that path point; if none of the candidate points satisfy the requirement, the candidate point with the smallest residual is selected. Through backtracking, the system proactively retreats to the reachable region when a path point is unreachable, avoiding trajectory interruption.

[0129] Step S140-3: Recalculate the end position of the effective joint angles of each path point using forward kinematics to obtain the joint reference trajectory consistent with the definition of forward kinematics.

[0130] In this step, to ensure that the final joint reference trajectory used for control is consistent with the definition of positive kinematics, positive kinematic mapping is reapplied to all effective joint angles, remapping the joint angles back to Cartesian space:

[0131] in, The end position corresponding to the joint reference trajectory, i.e., the joint reference for each path point. With end position Strictly meet This ensures that the reference trajectory, servo preview trajectory, and actual execution trajectory use the same end effector definition, eliminating positional ambiguities that might be introduced by numerical approximations. Finally, the joint angle sequence... This is the joint reference trajectory of the robotic arm, used for subsequent servo control and disturbance preview.

[0132] The technical solution adopted in this embodiment reduces abrupt changes in the path that could lead to instability in inverse kinematics solutions by applying time smoothing and step size constraints to the Cartesian path. Simultaneously, the maximum step size constraint limits the movement amplitude between adjacent time steps, preventing joint velocities from exceeding limits. Furthermore, damped least squares inverse kinematics solution combined with a backtracking search mechanism actively retreats to the reachable region when a path point is unreachable, ensuring trajectory continuity. Finally, forward kinematics backtracking ensures that the joint reference trajectory is strictly consistent with the forward kinematics definition, providing a unified reference benchmark for subsequent servo preview and disturbance estimation, avoiding control errors caused by inconsistent reference trajectory definitions.

[0133] In an optional embodiment, the servo preview model is a second-order servo model, and the natural frequency and damping ratio of the second-order servo model are calibrated based on the dynamic characteristics of the actual robotic arm. The step S150 above, "generating a preview joint state based on the joint reference trajectory," may include steps S150-1 to S150-5: Step S150-1: Obtain the joint reference position, joint reference velocity, and joint reference acceleration of the joint reference trajectory at each sampling time.

[0134] The joint reference trajectory includes a sequence of joint positions at each sampling time. To obtain the joint reference velocity and acceleration, the position sequence needs to be numerically differentiated. Specifically, the joint reference velocity can be calculated from the position difference between adjacent time steps, and the joint reference acceleration can be calculated from the velocity difference. These reference values ​​represent the ideal position, velocity, and acceleration that each joint of the robotic arm should reach at each time step, and serve as the input commands for the second-order servo model.

[0135] Step S150-2: Input the joint reference position, joint reference velocity, and joint reference acceleration at each sampling time into the second-order servo model, calculate the joint acceleration, and apply acceleration limiting to the joint acceleration to obtain the preview joint acceleration.

[0136] In this step, a second-order servo model is used to simulate the dynamic response characteristics of an actual robotic arm joint servo controller. This model takes the joint reference position, reference velocity, and reference acceleration as inputs and outputs the joint acceleration. The mathematical expression for the second-order servo model is:

[0137] in, These are the joint reference position, reference velocity, and reference acceleration at the current moment, respectively. These are the preview joint position and preview joint velocity at the previous sampling time, respectively; The natural frequency of the second-order servo model determines the system's response speed; The damping ratio determines the system's overshoot and oscillation characteristics; This is the calculated joint acceleration at the current moment.

[0138] According to this formula, joint acceleration consists of three terms, namely, the position error feedback term. Speed ​​error feedback item and reference acceleration feedforward term The natural frequency and damping ratio were obtained through experimental calibration based on the dynamic characteristics of the actual robotic arm servo driver, ensuring that the response characteristics of the servo preview model are consistent with the tracking characteristics of the actual robotic arm.

[0139] After calculating the joint acceleration, considering that the actual robotic arm is subject to physical limitations and the joint acceleration cannot be infinitely large, an acceleration limit is applied to the joint acceleration based on the maximum allowable acceleration of the robotic arm joint to obtain a preview of the joint acceleration.

[0140] Step S150-3: Perform discrete integration on the preview joint acceleration to obtain the joint velocity, and apply a velocity limit to the joint velocity to obtain the preview joint velocity.

[0141] In this step, a discrete-time integration method (such as Euler's integral) can be used to integrate the preview joint acceleration over time to obtain the joint velocity. Similarly, the joint velocity of the actual robotic arm is also subject to physical limitations. Therefore, based on the maximum permissible speed of the robotic arm joints, a limit is applied to the integrated joint velocity to obtain the preview joint velocity.

[0142] Step S150-4: Perform discrete integration on the preview joint velocity to obtain the preview joint position.

[0143] In this step, discrete integration can be used to integrate the preview joint velocity over time to obtain the joint position at the current moment. The joint position obtained at this time is the joint position after the dynamic response of the servo model and physical constraints (acceleration limiting, velocity limiting).

[0144] Step S150-5: Use the preview joint acceleration, the preview joint velocity, and the preview joint position as the preview joint state.

[0145] In this step, the preview joint state reflects the expected actual motion state of the robotic arm when executing the joint reference trajectory, taking into account servo hysteresis and physical constraints, rather than the ideal reference motion state. The preview joint state is used for robotic arm preview disturbance estimation in subsequent steps. Since the consistency between the preview state and the actual execution state is better than that of the ideal reference state, the robotic arm disturbance estimated based on the preview state is closer to the real disturbance, thereby improving the accuracy of the base feedforward compensation.

[0146] The technical solution of this embodiment generates preview joint states by employing a second-order servo model consistent with the actual dynamic characteristics of the robotic arm. This allows the preview states to reflect the actual physical constraints such as servo hysteresis, velocity limiting, and acceleration limiting when the robotic arm executes the joint reference trajectory. Through a complete process of acceleration limiting, discrete integration, velocity limiting, and re-integration, the final output preview joint states match the actual dynamic characteristics of the robotic arm, avoiding the feedforward distortion problem caused by directly estimating disturbances using ideal reference trajectories. This provides a reliable motion state input for accurate estimation of subsequent robotic arm preview disturbances.

[0147] In an optional embodiment, step S150 above, "estimating the robot arm preview disturbance generated by the robot arm movement on the base based on the preview joint state," may include steps S150-6 to S150-8: Step S150-6: Based on the preview joint position, preview joint velocity, and preview joint acceleration, and combined with the inertial coupling term in the base-manipulator coupling dynamics model, calculate the inertial reaction force and torque generated by each link of the manipulator on the base.

[0148] Specifically, the robotic arm is rigidly connected to the robot base. The inertial forces generated by the links of the robotic arm during movement are transmitted to the base through the connection points, creating a reaction disturbance on the base. The magnitude of this disturbance is closely related to the mass, inertial tensor, acceleration, and velocity of each joint of the robotic arm.

[0149] The complete form of the base-robotic arm coupled dynamics model can be represented as a block matrix form. By moving the robotic arm-related terms to the right side of the base dynamics equations, the equivalent perturbation of the robotic arm to the base can be expressed. It can be represented as:

[0150] in, The base-manipulator coupling inertia matrix reflects the inertial coupling effect of the manipulator joint acceleration on the base. This is the matrix for coupling the Coriolis force and the centrifugal force; at this time, To preview joint acceleration, To preview joint speed, It refers to generalized force terms such as gravity and buoyancy.

[0151] In this step, the preview joint acceleration and preview joint velocity from the preview joint state are substituted into the inertial coupling term in the coupled dynamics model to calculate the inertial reaction force and torque generated by the accelerated motion of each link of the robotic arm on the base. The greater the joint acceleration (i.e., the more violent the robotic arm's movement), the more significant the inertial reaction disturbance.

[0152] Step S150-7: Based on the preview joint speed and the preview joint position, and combined with the hydrodynamic damping coefficients of each link of the robotic arm, calculate the hydrodynamic damping reaction force and torque generated by each link of the robotic arm on the base.

[0153] In this step, when the links of the robotic arm move underwater, they are subject to hydrodynamic damping from the seawater. This damping force is related to the movement speed of the link and is transmitted to the base through the link, causing a disturbance to the base. For the i-th link, the local velocity of the link (i.e., the movement speed of the link in the base body coordinate system) is calculated based on its preview joint position. Combined with the hydrodynamic damping coefficient matrix of the link, the hydrodynamic damping force on the link is calculated.

[0154] Because the positions and attitudes of each link relative to the base are different, the lever arm (i.e., the vector from the point of application to the center of mass of the base) of the hydrodynamic damping force is different. Therefore, it is necessary to transform the hydrodynamic damping force on each link to the base body coordinate system and calculate its torque on the base. The hydrodynamic damping forces and torques of all links are summed to obtain the hydrodynamic damping reaction force and torque generated by each link on the base. ).

[0155] Step S150-8: Superimpose the inertial reaction force and torque with the hydrodynamic damping reaction force and torque to obtain the preview disturbance of the robotic arm.

[0156] The inertial reaction force and torque calculated in step S150-6 are superimposed with the hydrodynamic damping reaction force and torque calculated in step S150-7 to obtain the robot arm preview disturbance (i.e. This preview disturbance reflects the expected reaction disturbances on the base caused by the acceleration and hydrodynamic damping of the links of the robotic arm throughout the execution of the joint reference trajectory.

[0157] The technical solution adopted in this embodiment estimates the reaction disturbance of the robotic arm on the base by utilizing the preview joint state output by the servo preview model. Since the preview joint state has already considered servo hysteresis and physical constraints, the disturbance estimated based on the preview state is closer to the actual disturbance generated by the robotic arm during actual execution, avoiding feedforward compensation distortion caused by the inconsistency between the ideal planned trajectory and the actual execution trajectory. At the same time, by separately considering the inertial reaction term and the hydrodynamic damping term, the disturbance estimation model is made more comprehensive, covering the main sources of disturbance during the robotic arm's movement.

[0158] In an optional embodiment, step S160 above, "estimating the wave disturbance experienced by the base in the future prediction time domain based on the sea surface wave information," may include steps S160-1 to S160-3: Step S160-1: Input the sea surface wave information into the physical information neural predictor. The physical information neural predictor uses the partial differential equation of fluid dynamics as a constraint to extrapolate the local wave field in the prediction time domain, and calculates the fluid velocity and fluid acceleration based on the local wave field.

[0159] Among them, the Physics-Informed Neural Network (PINN) is a prediction model that integrates physical laws and data-driven approaches. It incorporates physical constraints during the training of the neural network, so that the prediction results not only fit the observed data, but also satisfy known physical laws.

[0160] The historical observations ζ(x,y,t) of the local spatiotemporal wavefield obtained in step S110 are input into the physical information neural predictor. This physical information neural predictor models the spatiotemporal evolution of the wavefield using partial differential equations of hydrodynamics (such as the Navier-Stokes equations for incompressible fluids) as physical constraints.

[0161] For example, for the current moment historical wave field observations The physical information neural predictor uses partial differential equations of fluid dynamics to extrapolate the local wave field from the current moment to the next moment:

[0162] in, For a moment Wave field prediction measurements, This is the midpoint; The potential velocity field (the hidden state quantity simultaneously estimated by the predictor); It is a dimensionless parameter (reflecting the relative importance of convection and diffusion in the flow); The spatial gradient of the wave field; For the Laplace operator of the wave field, For time step.

[0163] After obtaining the local wave field in the prediction time domain, the fluid velocity and fluid acceleration are further calculated based on the spatial gradient of the local wave field, providing input for the subsequent calculation of wave disturbance force and torque on the deep-sea robot using the Morison equation.

[0164] Step S160-2: Discretize the base into multiple equivalent force elements. For each force element, determine the relative velocity based on the velocity of the force element and the velocity of the fluid. Based on the relative velocity and the fluid acceleration, apply the Morison equation to calculate the resistance and inertial force experienced by the force element.

[0165] Specifically, the shape of an underwater robot base is usually a complex geometry, making it difficult to directly calculate wave forces. This step discretizes the base along its length (or main dimensional direction) into multiple equivalent force elements (such as cylindrical or cuboid segments), each with specific geometric parameters (diameter, length, water-facing area, etc.) and spatial location.

[0166] For the i-th force element, its velocity can be calculated by combining the overall motion state of the base (including translational velocity and rotational velocity) with the position vector of the force element relative to the center of mass of the base. Based on the fluid velocity and fluid acceleration obtained in step S160-1, and the velocity of the force element itself, the relative velocity and fluid acceleration between the force element and the fluid can be determined.

[0167] The Morison equation is a standard method in marine engineering for calculating wave forces on small structures (characteristic dimensions much smaller than wavelength). It applies the Morison equation to each force element to calculate the drag and inertial forces it experiences.

[0168] Step S160-3: Convert the resistance and inertial forces of all force elements to the base coordinate system, and sum up the resistance and inertial forces of all force elements in the base coordinate system to obtain the wave disturbance.

[0169] Since the force elements are distributed at different locations on the base, their force directions and lever arms are different. Therefore, it is necessary to uniformly transform the forces and moments acting on all force elements into the base body coordinate system (i.e., the coordinate system fixed to the center of mass of the base) before they can be accumulated. For the i-th force element, the resultant force acting on it is the sum of the drag force and the inertial force. Combining this with the position vector of the force element in the base body coordinate system, we can further calculate its disturbance force and moment about the center of mass of the base. Finally, by accumulating the disturbance force and disturbance moment of all force elements, we obtain the total wave disturbance experienced by the base.

[0170] The technical solution of this embodiment extrapolates the future local wave field using a physical information neural predictor and calculates the wave disturbance force and torque on the base using the Morison equation, thus achieving advance estimation of wave disturbances in the prediction time domain. This enables the controller to anticipate wave disturbances, thereby adjusting the thruster output in advance to compensate for upcoming disturbances and effectively suppressing base oscillations caused by waves.

[0171] like Figure 2 and Figure 3 As shown, Figure 2 This is an overall architecture diagram of a deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations provided in an embodiment of this application. Figure 3 This is a look-ahead compensation structure diagram of a fusion wave disturbance and robotic arm disturbance preview provided in an embodiment of this application.

[0172] Specifically, Figure 2 The complete data flow and module division of this method are shown at the system level. The perception layer acquires the base state, the manipulator joint state, the target pose, obstacle information, and sea wave information. The planning layer performs candidate target sampling, trajectory optimization, and joint reference generation. The control layer estimates the manipulator disturbance through servo preview and estimates the wave disturbance through the wave disturbance estimation module. Both types of disturbances are introduced into the nonlinear model predictive controller (NMPC) to solve the thruster control quantity to control the base pose. Figure 3 Further focusing on the disturbance feedforward compensation mechanism of the control layer, it is shown that wave disturbances and robotic arm preview disturbances are estimated separately before entering the base and introduced into the NMPC prediction model as known time-varying disturbances. This enables the controller to anticipate future disturbances, thereby achieving advance feedforward compensation rather than passive feedback adjustment.

[0173] The following is combined Figure 2 and Figure 3 The complete process of this method is described below, which includes the following steps 1 to 10: Step 1: Obtain visual and task inputs, namely, the current base state, robotic arm joint state, target pose, obstacle information, and sea wave information.

[0174] Step 2: Perform reachable boundary target sampling. Determine the current end effector position based on the robot arm joint state, and using this position as a reference, generate at least one candidate grasping target located near the reachable boundary of the robot arm end effector based on robot arm joint space sampling.

[0175] Step 3: Perform trajectory optimization. For each candidate grabbing target, construct an initial path from the current end position to the candidate grabbing target, and discretize the initial path to obtain a discrete path point sequence; construct a comprehensive cost function, which includes smoothness cost, obstacle avoidance cost, trajectory energy cost, reachability boundary cost, and target error cost that makes the end of the path approach the target pose; optimize the discrete path point sequence with minimizing the comprehensive cost function as the optimization objective to obtain the end Cartesian path.

[0176] Step 4: Perform reachability projection and smoothing. Perform time smoothing on the terminal Cartesian path and apply maximum step size constraints to the displacements between adjacent path points to obtain the path point sequence after step size constraints.

[0177] Step 5: Perform damped least squares inverse kinematics solution. Using the joint state of the robotic arm as the initial joint solution for the first path point, perform damped least squares inverse kinematics solution sequentially for each path point in the path point sequence after step size constraint. When the solution residual of a path point is greater than a preset threshold, backtrack along the line connecting the path point to the previous path point until the residual meets the requirements or the maximum number of backtracks is reached, to obtain the effective joint angle of each path point. Then, recalculate the end position of each path point using forward kinematics to obtain a joint reference trajectory consistent with the definition of forward kinematics.

[0178] Step 6: Calculate the robot arm servo preview. Obtain the joint reference position, velocity, and acceleration at each sampling time of the joint reference trajectory; input the joint reference position, velocity, and acceleration at each sampling time into the second-order servo model, calculate the joint acceleration, and apply an acceleration limit to the joint acceleration to obtain the preview joint acceleration; perform discrete integration on the preview joint acceleration to obtain the joint velocity, and apply a velocity limit to the joint velocity to obtain the preview joint velocity; perform discrete integration on the preview joint velocity to obtain the preview joint position; finally, use the preview joint acceleration, the preview joint velocity, and the preview joint position as the preview joint state.

[0179] Step 7: Perform equivalent disturbance estimation for the robotic arm. Based on the preview joint position, preview joint velocity, and preview joint acceleration, and combined with the inertial coupling term in the base-robotic arm coupled dynamics model, calculate the inertial reaction force and torque generated by each link of the robotic arm on the base; based on the preview joint velocity and the preview joint position, and combined with the hydrodynamic damping coefficient of each link of the robotic arm, calculate the hydrodynamic damping reaction force and torque generated by each link of the robotic arm on the base; superimpose the inertial reaction force and torque with the hydrodynamic damping reaction force and torque to obtain the preview disturbance of the robotic arm.

[0180] Step 8: Obtain wave disturbance estimation. Input sea surface wave information into a physical information neural predictor. The predictor, constrained by partial differential equations of hydrodynamics, extrapolates to obtain the local wave field in the prediction time domain and calculates the fluid velocity and acceleration based on the local wave field. Discretize the base into multiple equivalent force elements. For each force element, determine the relative velocity based on its velocity and the fluid velocity. Then, apply the Morison equation to calculate the drag and inertial force experienced by the force element based on the relative velocity and fluid acceleration. Transform the drag and inertial forces experienced by all force elements to the base body coordinate system, and sum the drag and inertial forces experienced by all force elements in the base body coordinate system to obtain the wave disturbance.

[0181] Step 9: Using the base state as the initial state, input the robot arm preview disturbance and wave disturbance as known time-varying disturbances in the prediction time domain into the nonlinear model predictive controller of the base, and solve for the thruster control quantity. This step corresponds to... Figure 3 The core mechanism of look-forward compensation, as shown, involves feeding both types of disturbances forward to the nonlinear model predictive controller before they enter the base.

[0182] Step 10: Output thruster control values ​​to control the pose of the base and output joint reference trajectories for the robotic arm to execute.

[0183] Thus, this method integrates robotic arm trajectory planning and base disturbance rejection control into the same collaborative control framework, enabling the base to maintain a stable hovering posture under the combined influence of robotic arm reaction disturbance and wave disturbance throughout the entire process of the robotic arm performing the grasping operation, thereby improving the success rate and safety of seabed grasping operations.

[0184] To verify the effectiveness of the technical solution of this application, multiple sets of comparative experiments were conducted. The following is a combination of... Figures 4 to 17 The performance of the solution in this application is described in detail.

[0185] Figures 4 to 7 The diagram illustrates the results of the robotic arm's end-effector trajectory planning and joint grasping state machine. Specifically, Figure 4 This application provides a schematic diagram of the three-dimensional trajectory of a robotic arm end effector in a base coordinate system, specifically including the original Cartesian requested trajectory, the reachable reference trajectory, and the joint reference positive kinematic trajectory. The figure shows the forward kinematic trajectory, the preview end effector trajectory (preview TCP), and the actual TCP trajectory. As can be seen from the figure, after reachability projection and forward kinematics back substitution, the reachable reference trajectory is basically consistent with the joint reference forward kinematic trajectory, indicating that the planned path can be converted into a joint reference that the robot arm can execute. The preview TCP trajectory and the actual TCP trajectory are generally consistent, indicating that the preview method based on the second-order servo model can reflect the actual robot arm execution process well.

[0186] Figure 5 This is a schematic diagram of TCP coordinate system error provided in an embodiment of this application, wherein... Figure 5 In The error between the actual trajectory and the preview trajectory. The error between the original requested trajectory and the reachable reference. The error is between the reachable reference and the forward kinematics back-substitution result. It can be seen that the overall error between the actual trajectory and the preview trajectory is relatively small, while the error between the original requested trajectory and the reachable reference mainly occurs in the early stage of the task, indicating that the original path is projected into the reachable space of the robotic arm after being corrected by reachability constraints.

[0187] Figure 6 This is a schematic diagram illustrating the change of TCP coordinate components over time, provided in an embodiment of this application. Figure 6 The three coordinate components of the original request trajectory (x, y, and z), the three components of the TCP reference coordinates after reachable projection (x, y, and z), the three components of the TCP coordinates after actual execution (x, y, and z), and the three components of the TCP trajectory after joint reference back-substitution for forward kinematics (x, y, and z) further illustrate that the reference trajectory, actual trajectory, and forward kinematics back-substitution trajectory remain consistent under the same TCP definition.

[0188] Figure 7 This is a schematic diagram of a joint grasping state machine provided in an embodiment of this application. It includes stages such as stable hovering, pre-grab approach, grasping contact, gripper closure, and lifting and withdrawal, which reflects the temporal coordination relationship between the robotic arm trajectory planning and the ROV (Remotely Operated Vehicle) base stabilization control.

[0189] Furthermore, the NMPC with robot arm disturbance preview in this application was compared with the NMPC without robot arm disturbance preview and the sliding mode control (SMC) baseline method. Figures 8 to 11 The diagram illustrates the base's disturbance-resistant hovering and three-dimensional motion response results. Among them, Figure 8This is a schematic diagram of the motion trajectory of an ROV in three-dimensional space, provided by an embodiment of this application. It compares the motion response of NMPC with robotic arm disturbance preview (NMPC + robotic arm disturbance preview), NMPC without robotic arm disturbance preview (NMPC without robotic arm disturbance preview), and the SMC baseline control method relative to a hovering reference point. As can be seen from the figure, the NMPC control method enables the ROV to gradually converge from its initial deviation state to near the target hovering position, and the control trajectory with robotic arm disturbance preview is closer to the reference point.

[0190] Figure 9 This is a schematic diagram comparing the position of an ROV with a reference value provided in an embodiment of this application. Specifically, it includes the x, y, and z position responses of the ROV, indicating that each position component can converge to the vicinity of the expected value in a short time and remain stable during subsequent robotic arm movements.

[0191] Figure 10 This is a schematic diagram illustrating the variation of Euler angles of an ROV provided in an embodiment of this application, specifically including the ROV roll angle ( ), pitch angle ( ) and yaw angle change ( This situation indicates that the attitude angle is still limited to a small range under the influence of external disturbances.

[0192] Figure 11 This is a schematic diagram of the velocity response of an ROV in a volume coordinate system provided in an embodiment of this application; wherein, Figure 11 In the figure (a), the linear velocity in the ROV body coordinate system is given, and u, v, and w are the three linear velocity components in the ROV body coordinate system (i.e., u, v, and w are the linear velocities in the x, y, and z directions, respectively). Figure 11 In Figure (b), the angular velocity in the ROV body coordinate system is represented, where p, q, and r are the three angular velocity components in the ROV body coordinate system (i.e., p, q, and r are the angular velocities in the x, y, and z directions, respectively). It can be seen that the controller can suppress velocity oscillations caused by disturbances, maintaining the system in a controllable hovering state. This figure verifies the effect of the proposed Disturbance Preview (NMPC) method on improving the stability of the ROV base during underwater operations.

[0193] Figures 12 to 17 The diagram illustrates the trajectory of the robotic arm's end effector and the results of joint tracking. Specifically, Figure 12 This is a schematic diagram of the trajectory of an end effector in the world coordinate system provided in an embodiment of this application, including the predicted TCP trajectory and the actual TCP trajectory. It can be seen that the two are basically overlapping, indicating that the reference trajectory of the robotic arm generated by reachable projection, DLS inverse kinematics and servo preview can be well executed by the actual joint movement.

[0194] Figure 13 This is a schematic diagram of a q1 tracking result provided in an embodiment of this application. Figure 14 This is a schematic diagram of a q2 tracking result provided in an embodiment of this application. Figure 15 This is a schematic diagram of a q3 tracking result provided in an embodiment of this application. Figure 16 This is a schematic diagram of a q4 tracking result provided in an embodiment of this application. Figure 17 This is a schematic diagram of the q5 tracking result provided in an embodiment of this application; where q1 to q4 are the main motion joints of the robotic arm, and q5 is the gripper joint. As can be seen from the responses of each joint, the actual joint trajectory can follow the reference joint trajectory well, and the overall joint movement is continuous and smooth, without obvious abrupt changes or unexecutable jumps. This result shows that the proposed robotic arm end-effector trajectory planning method can not only generate a TCP path that meets the task requirements in Cartesian space, but also further convert it into an executable control reference in joint space, providing reliable input for subsequent robotic arm disturbance preview and ROV base compensation control.

[0195] This application also provides a visual perception and disturbance compensation control system for deep-sea robots used in seabed grasping operations, referring to... Figure 18 As shown, Figure 18 This is a schematic diagram of a visual perception and disturbance compensation control system for a deep-sea robot used in seabed grasping operations, provided in an embodiment of this application. The system includes: The status acquisition module 1810 is used to acquire the current status of the base, the joint status of the robotic arm, the target pose, obstacle information, and sea wave information. The candidate target generation module 1820 is used to determine the current end position based on the joint state of the robotic arm, and with the current end position as a reference, generate at least one candidate grasping target located near the reachable boundary of the end of the robotic arm based on the joint space sampling of the robotic arm. The trajectory optimization module 1830 is used to generate an end Cartesian path by taking the current end position as the starting point and the candidate grasping target as the ending point, based on the target pose and the obstacle information; the trajectory optimization has at least obstacle avoidance and approaching the target pose as optimization objectives; The joint reference generation module 1840 is used to convert the end effector Cartesian path into a joint reference trajectory of the robotic arm. The robotic arm disturbance estimation module 1850 is used to generate a preview joint state based on the joint reference trajectory using a servo preview model consistent with the dynamic characteristics of the actual robotic arm, and to estimate the robotic arm preview disturbance generated by the robotic arm motion on the base based on the preview joint state. The wave disturbance estimation module 1860 is used to estimate the wave disturbance that the base will experience in the future prediction time domain based on the sea surface wave information. Predictive control module 1870 is used to take the base state as the initial state, input the robot arm preview disturbance and the wave disturbance as known time-varying disturbances in the prediction time domain into the nonlinear model predictive controller of the base, and solve the thruster control quantity. The output control module 1880 is used to output the thruster control quantity to control the position and orientation of the base, and to output the joint reference trajectory for the robotic arm to execute.

[0196] It is understood that the deep-sea robot visual perception and disturbance compensation control system for seabed grasping operations in the embodiments of this application can realize the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations in the above embodiments. The deep-sea robot visual perception and disturbance compensation control system for seabed grasping operations has the same advantages as the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations compared with the prior art, and will not be repeated here.

[0197] This application also provides an electronic device, see embodiments thereof. Figure 19 , Figure 19 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. For example... Figure 19 As shown, the electronic device 1900 includes a memory 1910 and a processor 1920. The memory 1910 and the processor 1920 are connected via a bus for communication. The memory 1910 stores a computer program, which can run on the processor 1920 to implement the steps of the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations described in the embodiments of this application.

[0198] This application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations described in this application.

[0199] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations described in this application.

[0200] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0201] This application describes embodiments of methods and apparatus according to flowchart illustrations and / or block diagrams. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing terminal device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal device, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0202] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing terminal device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0203] These computer program instructions can also be loaded onto a computer or other programmable data processing terminal equipment, causing a series of operational steps to be performed on the computer or other programmable terminal equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable terminal equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0204] Although preferred embodiments of the present application have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the embodiments of the present application.

[0205] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or terminal device that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or terminal device. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or terminal device that includes said element.

[0206] The foregoing has provided a detailed description of a visual perception and disturbance compensation control method and system for deep-sea robots engaged in seabed grasping operations. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the method and its core ideas. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this application. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for visual perception and disturbance compensation control of a deep-sea robot for seabed grasping operations, characterized in that, include: Acquire the current base status, robotic arm joint status, target pose, obstacle information, and sea wave information; The current end-effector position is determined based on the joint state of the robotic arm, and at least one candidate grasping target is generated near the reachable boundary of the robotic arm end-effector based on the joint space sampling of the robotic arm, using the current end-effector position as a reference. Starting from the current end position and ending at the candidate capture target, a Cartesian path is generated at the end position through trajectory optimization based on the target pose and obstacle information; the trajectory optimization aims at least on obstacle avoidance and approaching the target pose. The end-effector Cartesian path is converted into a joint reference trajectory for the robotic arm; Using a servo preview model that matches the dynamic characteristics of the actual robotic arm, a preview joint state is generated based on the joint reference trajectory, and the robotic arm preview disturbance generated by the robotic arm motion on the base is estimated based on the preview joint state. Estimate the wave disturbance experienced by the base in the future prediction time domain based on the sea surface wave information, including: inputting the sea surface wave information into a physical information neural predictor, which extrapolates the local wave field in the prediction time domain using partial differential equations of fluid dynamics as constraints, and calculates the fluid velocity and fluid acceleration based on the local wave field; discretizing the base into multiple equivalent force elements, determining the relative velocity of each force element based on its velocity and the fluid velocity, and calculating the drag and inertial force experienced by the force element using the Morison equation based on the relative velocity and fluid acceleration; transforming the drag and inertial force experienced by all force elements to the base body coordinate system, and summing the drag and inertial force experienced by all force elements in the base body coordinate system to obtain the wave disturbance; Using the base state as the initial state, the robot arm preview disturbance and the wave disturbance are input into the nonlinear model predictive controller of the base as known time-varying disturbances in the prediction time domain to solve the thruster control quantity; The thruster control quantity is output to control the pose of the base, and the joint reference trajectory is output for the robotic arm to execute.

2. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 1, characterized in that, The target pose, the obstacle information, and the sea wave information are obtained through synchronous binocular visual perception, including: Images of the robotic arm's workspace and the sea surface are captured by a synchronously triggered binocular camera. Distortion correction and epipolar correction are performed on the left and right images, and a three-dimensional point cloud is generated through pixel matching and triangulation. The three-dimensional point cloud is filtered and its coordinates transformed to obtain the target pose and obstacle information; The three-dimensional point cloud is interpolated onto a horizontal plane using a regular grid to extract the vertical height component, thus obtaining a local spatiotemporal wave field, which serves as the sea surface wave information.

3. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 1, characterized in that, The current end effector position is determined based on the joint state of the robotic arm, and using the current end effector position as a reference, at least one candidate grasping target located near the reachable boundary of the robotic arm end effector is generated based on the joint space sampling of the robotic arm, including: The current end effector position is calculated using forward kinematics based on the joint state of the robotic arm. Multiple joint configurations are randomly sampled within the joint limit range of the robotic arm, and the end position corresponding to each joint configuration is calculated using forward kinematics. Calculate the distance between each end position and the current end position, and filter out end positions whose distance is between a preset lower threshold and a preset upper threshold; Obstacle collision detection is performed on the selected end positions, and end positions that enter the safety boundary of the obstacle are eliminated. The remaining end positions are used as candidate grab targets.

4. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 1, characterized in that, Starting from the current end position and ending at the candidate grasping target, a Cartesian path is generated at the end position through trajectory optimization based on the target pose and obstacle information, including: For each candidate grabbing target, an initial path is constructed from the current end position to the candidate grabbing target, and the initial path is discretized to obtain a discrete path point sequence; Construct a comprehensive cost function, which includes smoothness cost, obstacle avoidance cost, trajectory energy cost, reachability boundary cost, and target error cost that makes the end of the path approach the target pose; The discrete path point sequence is optimized with the goal of minimizing the comprehensive cost function to obtain the terminal Cartesian path.

5. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 4, characterized in that, With minimizing the comprehensive cost function as the optimization objective, the discrete path point sequence is optimized to obtain the terminal Cartesian path, including: Using the discrete path point sequence corresponding to each candidate grabbing target as the path point sequence of the current iteration, and performing an iterative update process according to the following steps, the candidate terminal Cartesian path corresponding to the candidate grabbing target is obtained: Add multiple random smooth noises to the current iteration's path point sequence to generate multiple trial path point sequences; Calculate the comprehensive cost of each trial path point sequence, and construct weights based on the comprehensive cost of each trial path point sequence; The noise increment vectors of each trial path point sequence relative to the current iteration path point sequence are summed according to the weights to obtain the superimposed noise increment; The superimposed noise increment is superimposed on the path point sequence of the current iteration to obtain the updated path point sequence; Discrete Laplace smoothing is applied to the updated path point sequence to reduce the second-order difference energy of the path, and the smoothed path point sequence is used as the current path point sequence for the next iteration. Repeat the above process until the preset iteration termination condition is met, and take the path point sequence at the time of termination as the candidate end Cartesian path corresponding to the candidate grabbing target. From all candidate terminal Cartesian paths corresponding to the candidate crawling targets, select the one with the lowest overall cost as the terminal Cartesian path.

6. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 1, characterized in that, Converting the end effector Cartesian path into a joint reference trajectory for the robotic arm includes: The terminal Cartesian path is smoothed over time, and a maximum step size constraint is applied to the displacement between adjacent path points to obtain a path point sequence with step size constraint. Using the joint state of the robotic arm as the initial joint solution of the first path point, damped least squares inverse kinematics solution is sequentially performed on each path point in the path point sequence after step length constraint. When the solution residual of the path point is greater than the preset threshold, backtracking search is performed along the line connecting the path point and the previous path point until the residual meets the requirements or the maximum number of backtracking is reached, and the effective joint angle of each path point is obtained. The effective joint angles at each path point are recalculated using forward kinematics to obtain the joint reference trajectory consistent with the definition of forward kinematics.

7. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 1, characterized in that, The servo preview model is a second-order servo model, and the natural frequency and damping ratio of the second-order servo model are calibrated based on the dynamic characteristics of the actual robotic arm. Generate a preview of the joint state based on the joint reference trajectory, including: Obtain the joint reference position, joint reference velocity, and joint reference acceleration of the joint reference trajectory at each sampling time; The joint reference position, joint reference velocity, and joint reference acceleration at each sampling time are input into the second-order servo model to calculate the joint acceleration. An acceleration limit is applied to the joint acceleration to obtain a preview of the joint acceleration. Discretely integrate the preview joint acceleration to obtain the joint velocity, and apply a velocity limit to the joint velocity to obtain the preview joint velocity; Discrete integrals are performed on the preview joint velocity to obtain the preview joint position; The preview joint acceleration, the preview joint velocity, and the preview joint position are used as the preview joint state.

8. The deep-sea robot visual perception and disturbance compensation control method for seabed grasping operations according to claim 7, characterized in that, Based on the preview joint state, the robot arm movement is estimated to cause preview disturbance to the base, including: Based on the preview joint position, preview joint velocity, and preview joint acceleration, and combined with the inertial coupling term in the base-manipulator coupling dynamics model, the inertial reaction force and torque generated by each link of the manipulator on the base are calculated. Based on the preview joint speed and the preview joint position, and combined with the hydrodynamic damping coefficients of each link of the robotic arm, the hydrodynamic damping reaction force and torque generated by each link of the robotic arm on the base are calculated. The inertial reaction force and torque are superimposed with the hydrodynamic damping reaction force and torque to obtain the preview disturbance of the robotic arm.

9. A visual perception and disturbance compensation control system for deep-sea robots designed for seabed grasping operations, characterized in that, include: The status acquisition module is used to acquire the current status of the base, the joint status of the robotic arm, the target pose, obstacle information, and sea wave information. The candidate target generation module is used to determine the current end position based on the joint state of the robotic arm, and with the current end position as a reference, generate at least one candidate grasping target located near the reachable boundary of the robotic arm end based on the joint space sampling of the robotic arm. The trajectory optimization module is used to generate an end Cartesian path by taking the current end position as the starting point and the candidate capture target as the ending point, based on the target pose and the obstacle information; the trajectory optimization has at least obstacle avoidance and approaching the target pose as optimization objectives; A joint reference generation module is used to convert the end effector Cartesian path into a joint reference trajectory for the robotic arm; The robotic arm disturbance estimation module is used to generate a preview joint state based on the joint reference trajectory using a servo preview model consistent with the dynamic characteristics of the actual robotic arm, and to estimate the robotic arm preview disturbance generated by the robotic arm motion on the base based on the preview joint state. The wave disturbance estimation module is used to estimate the wave disturbance experienced by the base in the future prediction time domain based on the sea surface wave information. This includes: inputting the sea surface wave information into a physical information neural predictor; the physical information neural predictor extrapolates the local wave field in the prediction time domain using partial differential equations of fluid dynamics as constraints, and calculates the fluid velocity and fluid acceleration based on the local wave field; discretizing the base into multiple equivalent force elements; determining the relative velocity of each force element based on its velocity and the fluid velocity; and calculating the drag and inertial force experienced by the force element using the Morison equation based on the relative velocity and fluid acceleration; transforming the drag and inertial forces experienced by all force elements to the base body coordinate system, and summing the drag and inertial forces experienced by all force elements in the base body coordinate system to obtain the wave disturbance. The predictive control module is used to take the base state as the initial state, input the robot arm preview disturbance and the wave disturbance as known time-varying disturbances in the prediction time domain into the nonlinear model predictive controller of the base, and solve the thruster control quantity. The output control module is used to output the thruster control quantity to control the position and orientation of the base, and to output the joint reference trajectory for the robotic arm to execute.

Images