Underwater robot global positioning and trajectory tracking method based on hull NURBS curved surface

Abstract

The invention discloses an underwater robot global positioning and trajectory tracking method based on a hull NURBS curved surface. The method comprises the following steps: firstly, constructing a hull NURBS curved surface model, and extracting differential geometric parameters including a curved surface basis vector, a curved surface normal vector, a curved surface first basic form matrix and a Cristomide symbol; then, on the basis of the differential geometry theory, according to the hull NURBS curved surface model, a continuous time nonlinear kinematics model is constructed; and finally, constructing an extended Kalman filter based on the continuous time nonlinear kinematics model. Based on an extended Kalman filter, fusing the multi-modal sensing data to perform global positioning, and obtaining a posteriori estimation state vector at the current moment; and performing curved surface trajectory tracking based on nonlinear model predictive control according to the posteriori estimation state vector at the current moment. The problem that the trajectory tracking precision is low or fails due to the fact that the robot is prone to accumulative drifting in positioning and cannot achieve global positioning in underwater, curved-surface, GPS-free and external-vision-free denial environments is solved.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot positioning and tracking, specifically a global positioning and tracking method for underwater robots based on the NURBS surface of the hull. Background Technology

[0002] Underwater positioning and trajectory tracking are fundamental for autonomous navigation and path planning of underwater cleaning robots. Existing underwater positioning research largely focuses on the fusion of acoustic, visual, and other sensor data. However, these positioning methods have significant limitations when considering the specific working conditions of underwater cleaning robots, such as high environmental noise, turbid water, and close proximity to the hull surface.

[0003] Chinese patent application CN120778115A discloses a method for positioning and tracking the hull surface of an underwater cleaning robot. This method fuses data from a wheel encoder, IMU, and depth gauge using an extended Kalman filter, and corrects the state using either the depth gauge or IMU yaw angle depending on whether the robot is positioned on the side or bottom of the hull. However, this method simplifies the hull surface to two simple geometric shapes: a horizontal plane and a vertical plane. When dealing with areas with complex curvature, such as the bow and stern, this simple planar assumption fails, potentially leading to a severe mismatch between the positioning model and the actual environment. Furthermore, the standard difference driving model based on the planar assumption generates systematic biases that cannot be explained by sensor noise when predicting states in areas with high curvature, causing the extended Kalman filter to diverge, thus reducing positioning accuracy and even leading to tracking failure. Summary of the Invention

[0004] To address the shortcomings of existing technologies, the technical problem this invention aims to solve is to provide a global positioning and trajectory tracking method for underwater robots based on NURBS hull surfaces. This method aims to solve the problem that in underwater, curved, GPS-free, and externally vision-free environments, relying solely on odometry for positioning can easily lead to cumulative drift, making global positioning impossible and resulting in low trajectory tracking control accuracy or failure.

[0005] The present invention solves the aforementioned technical problem by adopting the following technical solution: A method for global localization and trajectory tracking of underwater robots based on NURBS surfaces of ship hulls, characterized by the following steps: Step 1: Construct a NURBS surface model of the hull and extract differential geometric parameters including surface basis vectors, surface normal vectors, the first fundamental form matrix of the surface, and Christofel notation; Step 2: Based on differential geometry theory and the NURBS surface model of the ship hull, construct a continuous-time nonlinear kinematic model. ; (15) In the formula, Represents the rate of change of the state vector. , express Xianghe The rate of change of surface parameters in the direction, Indicates the rate of change of heading angle. and express Xianghe The coefficient function of the direction, This represents the correction function. Indicates the robot's forward speed. Indicates the robot's normal angular velocity; Step 3: Construct an extended Kalman filter based on a continuous-time nonlinear kinematic model; based on the extended Kalman filter, fuse multimodal sensor data for global localization, and obtain... The posterior estimated state vector at time t; First, the continuous-time nonlinear kinematic model is discretized within the sampling period to obtain... Prior estimate of state vector at time step ; (16) In the formula, express The posterior estimated state vector at time t. express Control vector at time, Represents the discrete-time state transition function. This represents a function representing a continuous-time nonlinear kinematic model. Indicates the sampling period; calculate Prior estimate of covariance matrix at time step : (twenty three) In the formula, express The state transition Jacobian matrix at time step 1. express The posterior estimated covariance matrix at time t. express The process noise covariance matrix at time step 1. Indicates the transpose operation; Then, construct the observation model. Calculate the Jacobian matrix of the observation model; (28) In the formula, Represents the theoretical depth observation function. Represents the roll angle observation function. Represents the pitch angle observation function; Finally, calculate Observation residuals at time : (30) In the formula, express The actual observed vector at time t, express Prior estimate of the observation vector at time step; calculate Residual covariance matrix at time step : (31) In the formula, express The Jacobian matrix of the time-observation model, express The observation noise covariance matrix at time step; calculate Kalman gain at time step : (33) calculate Posterior estimated state vector at time 1 : (34) Step 4: According to The posterior estimated state vector at time step is used for surface trajectory tracking based on nonlinear model predictive control. First, construct the constraints for the objective function; the objective function is: (36) In the formula, Represents the stage cost function. , Indicates time step State vector and control vector, Represents the terminal cost function. Indicates the first The state vector at each time step Represents the penalty function. As slack variables, Indicates the number of time steps; The constraints of nonlinear model predictive control include dynamic constraints, physical and safety constraints; The dynamic constraints are: (45) In the formula, Indicates time step The state vector, , , This indicates the slope at the start, midpoint, and end point of the prediction interval. Indicates based on Recalculate the slope at the midpoint of the prediction interval; Physical and safety constraints include control magnitude constraints, control rate of change constraints, and surface boundary safety constraints; Then, considering the constraints, the optimal control sequence is obtained by solving the objective function. Finally, the inverse kinematics are solved based on the predicted forward speed and normal angular velocity included in the first element of the optimal control sequence to obtain the angular velocities of the robot's left and right drive wheels.

[0006] Furthermore, Prior estimate of state vector at time step The state components are represented as follows: (17) (18) (19) In the formula, express Prior estimates of surface parameters at time t. express Prior estimate of heading angle at time [time] express The posterior estimated surface parameters at time t. express The posterior estimated heading angle at time [time]. express The actual heading angle at that moment, , express The robot's forward speed and normal angular velocity at any given moment. Indicates geodesic curvature, , , The first fundamental form matrix of the surface elements, , , , and express Xianghe Towards the surface basis vector.

[0007] Furthermore, The state transition Jacobian matrix at time 10:00 Represented as: (20) In the formula, It is the identity matrix. yes Jacobian matrix of a continuous-time nonlinear kinematic model; Process noise covariance matrix at time step Represented as: (twenty two) (twenty one) In the formula, yes The input Jacobian matrix at time t, yes The input noise covariance matrix at time t.

[0008] Furthermore, the surface boundary safety constraint is as follows: (48) (49) (50) In the formula, , Indicates time step of , Towards surface parameters, , express To the minimum and maximum values ​​of the surface parameters, , express To the minimum and maximum values ​​of the surface parameters, , Indicates time step of , Relaxation variables for surface parameters.

[0009] Compared with the prior art, the beneficial effects of the present invention are: 1. To address the problem of visual / acoustic positioning failure in underwater wall-hugging operations, this invention innovatively proposes a global positioning method that integrates odometry, IMU, depth gauge, and NURBS surface model. By comparing depth gauge measurements with the NURBS surface model, global coordinate observation is provided for robot positioning. Furthermore, by comparing IMU attitude with surface normal vectors, error correction is achieved in the two-dimensional surface parameter space, thus solving the global positioning drift problem without external assistance.

[0010] 2. To address the issue of low accuracy of traditional planar kinematic models on curved surfaces, this invention establishes a high-precision nonlinear kinematic model in the NURBS two-dimensional surface parameter space based on differential geometry theory. This model accurately describes the intrinsic relationship between robot motion and surface curvature and is uniformly applied to EKF state prediction and NMPC trajectory prediction, significantly improving the system's model consistency, positioning accuracy, and control robustness. It eliminates the systematic errors caused by the hull planar assumption and significantly improves the accuracy and robustness of positioning in high curvature regions.

[0011] 3. To address the problem that traditional controllers struggle to handle multiple constraints, this invention employs a nonlinear model predictive control (NMPC) strategy. Within a unified framework, it optimizes tracking accuracy, control magnitude, and smoothness, and can explicitly and proactively handle the physical limits of the actuator and the safe operating boundaries of the NURBS surface, ensuring the safety and optimality of robot motion. Attached Figure Description

[0012] Figure 1 This is a schematic diagram of the global coordinate system; Figure 2 This is a schematic diagram of the robot's coordinate system. Figure 3 A schematic diagram of the NURBS surface and state vector of the ship's hull; Figure 4 A logical block diagram for EKF-based multi-sensor data fusion positioning; Figure 5 This is a schematic diagram of the EKF observation model; Figure 6 This is a schematic diagram of the tracking trajectory. Detailed Implementation

[0013] Specific embodiments are given below with reference to the accompanying drawings. These specific embodiments are only used to describe the technical solution of the present invention in detail, and are not intended to limit the scope of protection of this application.

[0014] like Figures 1-6 As shown, this invention provides a method for global localization and trajectory tracking of an underwater robot based on the NURBS surface of a ship's hull, comprising the following steps: Step 1: Construct a NURBS surface model of the ship's hull and extract differential geometric parameters; A high-precision mathematical model of the hull surface is obtained by using non-uniform rational B-splines (NURBS), resulting in a NURBS surface model of the hull. Any point on the hull surface can be mapped to the global coordinate system through this model, thus transforming the robot localization problem from three-dimensional space to a two-dimensional surface parameter space. The NURBS surface model of the hull is represented as follows: (1) In the formula, For the first Line number Column control points For the first Line number Column control point weights , The number of rows and columns of the control points. for Upward B-order spline basis functions, for Upward B-order spline basis functions; The global coordinate system is a fixed inertial coordinate system used to describe the global position of the ship and the robot, such as... Figure 1 As shown, in this embodiment, the stern endpoint is set as the origin of the global coordinate system, the z-axis of the global coordinate system is perpendicular to the water surface and upwards, and the xy plane is the horizontal plane. The robot coordinate system is as follows: Figure 2 As shown.

[0015] Based on the NURBS surface model of the hull, the differential geometric parameters necessary for constructing the robot's kinematic model are extracted, including the surface basis vectors, surface normal vectors, the first fundamental form matrix of the surface, and Christofel notation.

[0016] Taking the partial derivative of equation (1), we get Xianghe To the surface basis vector and : (2) The cross product of two basis vectors is the surface normal vector. : (3) Based on surface base vectors and Construct the first fundamental form matrix of the surface : (4) in, , , ; Calculate the Christofer symbol based on the first fundamental form matrix of the surface. , , , , and These symbols describe the surface basis vectors as a function of the surface parameters. The rate of change reflects the bending characteristics of the surface; (5) in, , , , , , .

[0017] Step 2: Construct a continuous-time nonlinear kinematic model based on differential geometry theory; Define state vector To fully describe the robot's pose in the surface parameter space, the state vector consists of surface parameters and heading angle, represented as: (6) In the formula, These are surface parameters that describe the robot's position in the surface parameter space; The heading angle of the robot describes the robot's instantaneous direction of motion, specifically the robot's forward direction. (axis) and To the surface basis vector The included angle between them, see Figure 3 .

[0018] Define control vector Control Vector It is a physical quantity directly controlled by the robot's actuator, determined by the robot's forward speed. and normal angular velocity Composition, represented as: (7) This invention, based on differential geometry theory, directly establishes a robot kinematic model in the surface parameter space. The aim is to establish a precise mathematical mapping between the robot's control input and its state changes in the surface parameter space, thereby eliminating the geometric errors generated by traditional planar kinematic models on complex surfaces. However, when a robot moves on a complex surface, it faces two geometric characteristics significantly different from planar motion: the metric tensor effect and the geodesic curvature effect. The metric tensor effect refers to the robot's forward velocity... With the rate of change of surface parameters and The proportional relationship between them is not fixed, but changes dynamically with the local stretching or compression of the surface. The geodesic curvature effect refers to the phenomenon where, even if the robot maintains a straight line, the curvature changes. heading angle It can also be passively deflected due to the curvature of the surface. Therefore, it is necessary to calculate the rate of change of the surface parameters. and Used to solve metric tensor effects; calculate the rate of change of heading angle. It is used to solve the geodesic curvature effect.

[0019] Since the robot's instantaneous direction of motion is its forward direction, and this direction always lies on the tangent plane of the surface, according to the principles of linear algebra, any vector on the tangent plane can be linearly represented by the basis vectors of that plane. Therefore, the unit vector of the robot's forward direction is... It can be represented as a surface basis vector. and The weighted sum. Considering that the surface basis vectors may be non-orthogonal and their lengths may not be 1, this linear representation needs to be normalized and orthogonalized by introducing the parameters of the first fundamental form of the surface. Therefore, the unit vector representing the robot's forward direction is: (8) Robot forward velocity vector It can be described from two perspectives: on the one hand, from the perspective of the surface parameter space, it is the rate of change of the surface parameters. and Synthesis along the basis vector direction: (9) On the other hand, from the perspective of the robot's movement space, it is the forward speed. Vector along the direction of travel: (10) In the formula, , It is a coefficient; Due to the surface basis vector and Since they are linearly independent, their coefficients must be equal. This can be obtained from equations (9) and (10). , By solving the coefficients and The rate of change of surface parameters can be obtained. and With the robot's forward speed Heading angle The precise mapping relationship between them; (11) Rate of change of heading angle It is the superposition of two effects: active steering, that is, the normal angular velocity directly generated by the robot actuator. Passive deflection, which occurs when the robot moves on a curved surface, causing the surface basis vectors to change. It itself rotates in space, in order to maintain the heading angle The definition of the datum must compensate for this basis vector rotation; this effect is called geodesic curvature in differential geometry. The resulting deflection. Therefore, the dynamic equation for the heading angle is: (12) Among them, geodesic curvature term It is the surface geometric parameters and the rate of change of the surface parameters. and Functions: (13) In the formula, The direction angle of the parameter velocity is equal to the heading angle in this embodiment. ; Substituting equation (11) into equation (13), the complex geometric terms are reorganized into a unified expression concerning the state vector. and forward speed function Substituting this into equation (12), we obtain the rate of change of heading angle: (14) Equation (14) shows that the actual change in the robot's heading is equal to the angular velocity measured by the sensor plus the drift correction determined by the terrain.

[0020] Combining equations (11) and (14), we obtain a continuous-time nonlinear kinematic model. This model describes the rate of change of the state vector. With control vector The relationship that changes over time.

[0021] (15) In the formula, and It is derived from equation (13) Xianghe The coefficient function of the direction is used to solve the metric tensor effect; It is the correction function derived in equation (14), which is used to solve the geodesic curvature effect.

[0022] Step 3: Construct an Extended Kalman Filter (EKF) based on a continuous-time nonlinear kinematic model; perform global localization based on the EKF and by fusing multimodal sensor data to obtain the current (…). The posterior estimated state vector at time ( ). Real-time acquisition of robot forward speed normal angular velocity Roll angle Pitch angle and the z-coordinate in the global coordinate system Based on a continuous-time nonlinear kinematics model, an extended Kalman filter is used to fuse measurement data from odometry (to obtain forward velocity), IMU (to obtain normal angular velocity, roll angle, and pitch angle), and depth gauge (to obtain z-coordinate) to estimate the robot's state vector in the surface parameter space in real time. For example... Figure 4 As shown, the EKF algorithm is mainly divided into two stages: state prediction and observation update. At the same time, it establishes geometric constraints by constructing an observation model.

[0023] 3-1) State prediction stage: State prediction is to calculate the current position based on the position at the previous moment and the motion command at the current moment.

[0024] First, in the sampling period The continuous-time nonlinear kinematic model is discretized, and the first-order Euler integral method is used to obtain the discrete-time state prediction equation. (16) In the formula, express The prior estimated state vector at time t. express Prior estimates of surface parameters at time t. express Prior estimate of heading angle at time [time] Represents the discrete-time state transition function. express The posterior estimated state vector at time t. This represents a function representing a continuous-time nonlinear kinematic model. yes Control vector at time, , express The robot's forward velocity and normal angular velocity at any given moment; Prior estimation of state vector The state components are represented as follows: (17) (18) (19) In the formula, express The posterior estimated surface parameters at time t. express The posterior estimated heading angle at time [time]. express The actual heading angle at that moment; Due to issues such as slippage in odometers and drift in gyroscopes, the state vector is estimated prior to the time. Errors are inevitable; to accurately estimate these errors, the prior covariance matrix needs to be calculated simultaneously. First, the sensitivity of the kinematic model to small changes in state is calculated based on its Jacobian matrix, thus obtaining the state transition Jacobian matrix. (20) in, yes The state transition Jacobian matrix at time step 1. It is the identity matrix. yes Jacobian matrix of a continuous-time nonlinear kinematic model; To quantify the impact of input noise on the system, it is necessary to calculate the input Jacobian matrix; (twenty one) In the formula, yes The input Jacobian matrix at time step; Based on the input Jacobian matrix and the preset input noise covariance, calculate the process noise covariance matrix introduced by the uncertainty of the control input; (twenty two) In the formula, yes The process noise covariance matrix at time step 1. yes The input noise covariance matrix at time t, where , It is the noise variance of the robot's forward speed and normal angular velocity; Based on the state transition Jacobian matrix and the process noise covariance matrix, the prior estimate of the covariance matrix is ​​obtained through the linear transfer equation. (twenty three) In the formula, yes The prior estimate of the covariance matrix at time t. yes The posterior estimated covariance matrix at time t.

[0025] 3-2) Constructing an observation model like Figure 5 As shown, using the NURBS surface model of the hull as the absolute reference, and based on depth geometry constraints—that is, when the robot moves on the surface, the global depth is uniquely determined by its position on the surface—the theoretical depth observation function is: (twenty four) In the formula, Indicates the robot's surface parameters Global depth at the location; Then, according to the normal orientation constraint, that is, when the robot is in close contact with the surface, its orientation is determined by the normal vector of the surface at that point. First, the surface parameters are calculated. Unit normal vector at: (25) In the formula, , and Represents the x, y, and z components of the unit normal vector; Based on the geometric relationships in the gravitational field, the roll and pitch angles can be calculated from the components of the unit normal vector. Therefore, the roll angle observation function... and pitch angle observation function Represented as: (26) (27) In the formula, , This represents the estimated values ​​for roll and pitch angles; Finally, the observation model is obtained as follows: (28) To backpropagate the observation error to the state vector, the Jacobian matrix of the observation model needs to be calculated, which reflects the sensitivity of the observation to small changes in the state. (29) In the formula, yes The Jacobian matrix of the time-lapse observation model has a zero third column, indicating that the heading angle cannot be directly observed from depth and IMU attitude. However, it can strongly constrain surface parameters. .

[0026] 3-3) During the observation update phase, the estimated values ​​are corrected, and the difference between the estimated values ​​and the observed values ​​is used to eliminate accumulated errors; First, calculate the observation residual, which is the difference between the actual observed value and the theoretical observed value; (30) In the formula, yes The observation residual at time, yes The actual observed vector at time t, express Prior estimate of the observation vector at time step; Secondly, the residual covariance matrix is ​​calculated, which measures the confidence of the observation residuals and includes prediction uncertainty and observation noise. (31) In the formula, yes The residual covariance matrix at time t. yes The observation noise covariance matrix at time step; To improve robustness, an adaptive covariance adjustment strategy is adopted to address the singularity that may occur in the IMU on the vertical wall, and the observation noise covariance matrix is ​​calculated. (32) In the formula, The measurement noise variance of the depth gauge. and These are the basic noise variances for roll angle and pitch angle, respectively. This is the attitude-related variance scaling factor, used to adjust the system's confidence in the IMU measurement data.

[0027] The Kalman gain is calculated based on the prior estimated covariance matrix, the Jacobian matrix of the observation model, and the residual covariance matrix, which determines the correction strength. (33) In the formula, yes Kalman gain at time step; Using Kalman gain and observation residuals, for The prior estimated state vector at time step 1 is corrected to obtain... The posterior estimated state vector at time t; (34) In the formula, yes The posterior estimated state vector at time t; right The prior estimated covariance matrix at time step (Equation (23)) is updated to obtain the result. Posterior estimated covariance matrix at time 1 : (35) Step 4: Based on the posterior estimated state vector at the current moment, perform surface trajectory tracking using nonlinear model predictive control (NMPC); Surface trajectory tracking based on nonlinear model predictive control (NMPC) transforms the trajectory tracking problem into an open-loop optimal control problem (OCP) that is solved in a rolling manner within the prediction time domain, thereby achieving tracking of the reference trajectory in the NURBS surface parameter space. High-precision tracking. The core idea of ​​NMPC is to find the best control strategy for the future by solving an optimal control problem (OCP) at each control moment. The key lies in mathematically defining the optimal control. For underwater robot trajectory tracking tasks, this means following the reference trajectory as accurately as possible while satisfying all safety and physical constraints. At the same time, maintaining smooth and efficient movement. To achieve this goal, in At time 3, the posterior estimated state vector obtained in the third step As an initial condition, searching for the future The optimal control sequence for each time step.

[0028] 4-1) To quantify the control effect, an objective function is designed, following the logic below: (1) Tracking accuracy: The robot should follow the reference trajectory as closely as possible; (2) Energy efficiency: The control action should not be too large; (3) Smooth motion: Control commands should not change drastically to reduce mechanical wear and jitter; (4) Stability: At the predicted endpoint, the robot should converge to the vicinity of the reference trajectory.

[0029] Based on the above logic, the objective function Defined as the sum of stage cost and terminal cost, expressed as: (36) In the formula, Represents the stage cost function. , Indicates time step State vector and control vector, Represents the terminal cost function. Indicates the first The state vector at each time step Represents the penalty function. These are slack variables; The stage cost function takes the form of a quadratic form, and its expansion is as follows: (37) In the formula, the first term is the state deviation penalty term, the second term is the control quantity amplitude penalty term, and the third term is the control quantity change rate penalty term. Indicates time step The reference state vector, Indicates time step The control vector, This is the weight matrix.

[0030] To handle heading angle For periodic problems, the state deviation calculation incorporates a wraparound treatment, then: (38) In the formula, , Indicates time step of , Towards surface parameters, , Indicates time step of , To the reference surface parameters, Indicates time step The heading angle, Indicates time step Reference heading angle, To map the angle to The interval normalization function is used to ensure that the control system always chooses the direction with the smallest rotation angle to adjust the attitude; The terminal cost function is used to penalize the error in predicting the endpoint to ensure closed-loop stability, and thus: (39) In the formula, Indicates the first The reference state vector at each time step. This represents the terminal weight matrix, which determines the degree of importance attached to tracking errors at the endpoint, strengthens the penalty for terminal errors, and ensures that the system converges to the reference trajectory. The penalty function is used to quantify the degree to which the robot goes out of bounds, and its expression is: (40) In the formula, and The penalty coefficient is... Denotes the square of the L2 norm. This represents the L1 norm.

[0031] 4-2) Construct the prediction model and constraints, including dynamic constraints, physical constraints, and safety constraints; By minimizing the above objective function NMPC can automatically balance tracking accuracy, energy consumption, and smoothness under multiple constraints to calculate the optimal control command.

[0032] To eliminate control errors caused by model mismatch, a continuous-time nonlinear kinematic model is adopted. As a predictive model for the NMPC controller, this model It not only considers the kinematic characteristics of the robot, but also incorporates the geometric information of the NURBS surface, which can accurately describe the influence of surface curvature on the robot's motion.

[0033] To improve prediction accuracy, this invention employs the fourth-order Runge-Kutta (RK4) method in NMPC to perform high-precision discretization of the prediction model. The RK4 method obtains a fourth-order accurate numerical solution by performing four slope samplings and weighted averaging within each integration step, which is significantly better than the first-order accurate Euler method.

[0034] For each time step in the prediction time domain (from arrive The state evolution must satisfy the following RK4 integral formula constraints; First, calculate the slope at the starting point of the prediction interval under the current state vector. : (41) Secondly, calculate the slope at the midpoint of the prediction interval. ,based on Make a prediction: (42) In the formula, To control the cycle; Subsequently, based on Recalculate the slope at the midpoint of the prediction interval : (43) Finally, calculate the slope at the end of the prediction interval. ,based on Make a prediction: (44) Finally, a weighted average of the four slopes is calculated to determine the state vector for the next time step. That is, dynamic constraints; (45) The aforementioned dynamic constraints ensure that the trajectory planned by NMPC is physically feasible and has extremely high dynamic fidelity.

[0035] Physical and safety constraints include actuator physical constraints and surface boundary safety constraints; actuator physical constraints include control variable amplitude constraints and control variable rate of change constraints. The robot's drive motors have defined physical performance limits; exceeding these limits may lead to motor overheating, damage, or control failure.

[0036] Control amplitude constraint: Limits the speed of the motor body mapped to its maximum speed and torque, expressed as: (46) In the formula, and These are the lower and upper bounds of the control vector, respectively. Control variable change rate constraint: In order to protect the mechanical structure from impact and prevent drastic fluctuations in motor current, the rate of change of the control variable must be limited. (47) In the formula, and These represent the maximum allowable decrease and increase in the control vector within the control cycle, respectively.

[0037] Surface boundary safety constraints: This is a key constraint unique to surface operations in this invention. It ensures that the solver strictly adheres to the boundaries under normal conditions, allowing only brief and minute deviations beyond the boundaries in special circumstances, thereby guaranteeing the availability and safety of the control system under extreme conditions. For example... Figure 6 As shown, the robot must always remain within the effective parameter region defined by the NURBS surface to prevent it from veering off the hull edge or entering unmodeled areas. If hard constraints are directly applied, the optimization problem becomes unsolvable when the robot is pushed outside the boundary by strong external forces such as water flow or when the estimated state temporarily exceeds the limit due to positioning noise. Therefore, to improve the robustness of the system, this invention introduces relaxation variables, transforming hard constraints into soft constraints: (48) (49) (50) In the formula, , Indicates time step of , Towards surface parameters, , express To the minimum and maximum values ​​of the surface parameters, , express To the minimum and maximum values ​​of the surface parameters, , Indicates time step of , Relaxing variables for surface parameters; 4-3) Considering the constraints, the objective function is solved using the sequential quadratic programming method (NLP) to obtain the optimal control sequence. ; 4-4) Solve the inverse kinematics to obtain the angular velocities of the robot's left and right drive wheels; According to the rolling time-domain control principle, the system executes only the first element of the optimal control sequence. To drive the underlying motors, the predicted forward speed and normal angular velocity output by the NMPC need to be converted into the linear velocities of the left and right drive wheels, which requires solving the robot's inverse kinematics.

[0038] The underwater robot in this embodiment is a differential drive robot, and its kinematic parameters include the wheelbase between the left and right drive wheels. and drive wheel radius First, based on the principles of rigid body kinematics, the robot predicts its forward speed. and predicted normal angular velocity With the linear speed of the left and right drive wheels , The relationship is: (51) (52) By combining equations (51) and (52), the linear velocities of the left and right drive wheels can be obtained: (53) (54) Based on the assumption of pure rolling of the wheel, the relationship between linear velocity and angular velocity is as follows: Therefore, the angular velocities of the left and right drive wheels are calculated based on their linear velocities. and (Unit: radians / second); (55) (56) This completes the full closed-loop control process from NMPC high-level trajectory planning to low-level motor execution; the system will proceed to the next ( ) Receive the posterior estimated state vector at time ) Then repeat step four to achieve continuous and stable tracking of the reference trajectory.

[0039] Any aspects not covered in this invention are applicable to existing technologies.

Claims

1. A method for global localization and trajectory tracking of an underwater robot based on a ship hull NURBS surface, characterized in that, The method comprises the following steps: First step: constructing a ship hull NURBS surface model, and extracting differential geometry parameters including surface base vectors, surface normal vectors, surface first fundamental form matrix, and Christoffel symbols; Second step: Based on the theory of differential geometry, according to the NURBS surface model of ship hull, the continuous time nonlinear kinematic model is constructed ; (15) wherein denotes the rate of change of the state vector, , denotes the rate of change of the surface parameters, the rate of change of the surface parameters, denotes the rate of change of the heading angle, and denotes the coefficient functions of the heading and the coefficient functions of the heading and denotes the correction function, denotes the robot forward velocity, denotes the robot normal angular velocity; Third step: constructing an extended Kalman filter based on a continuous-time nonlinear kinematics model; Based on the extended Kalman filter, the global positioning is performed by fusing multi-modal sensor data to obtain the posterior estimation state vector at the moment; First, the continuous-time nonlinear kinematic model is discretized over the sampling period to obtain the prior estimate state vector at time instant ; (16) wherein represents represents the a posteriori estimation state vector at time instant represents represents the control vector at time instant represents the state transition function for discrete time represents the continuous time nonlinear kinematic model function represents the sampling period Computing a priori estimate covariance matrix : (23) wherein denotes the state transition Jacobian matrix at time k, denotes the posterior estimation covariance matrix at time k, denotes the process noise covariance matrix at time k, denotes the transpose operation; Then, an observation model is constructed , a Jacobian matrix of the observation model is calculated; (28) wherein represents a theoretical depth observation function, represents a roll angle observation function, represents a pitch angle observation function; Finally, the calculation observation residual at the time instant : (30) In the formula, represents actual observation vector at time t, represents prior estimation observation vector at time t; Computing Residual covariance matrix at time instant : (31) In the formula, denotes the Jacobian matrix of the time instant observation model, denotes the observation noise covariance matrix of the time instant Computing Kalman gain at time instant : (33) Computing the posterior estimate state vector at time instant : (34) Step 4: Based on the the posterior estimation state vector of the time instant, the curved surface trajectory tracking is performed based on the nonlinear model predictive control. Firstly, a target function constraint condition is constructed; The target function is: (36) wherein denotes a stage cost function, , denotes a state vector and a control vector at time step , denotes a terminal cost function, denotes a state vector at time step , denotes a penalty function, is a slack variable, denotes a number of time steps; Constraint conditions of the nonlinear model predictive control include dynamics constraints, physical and safety constraints; The dynamics constraints are: (45) wherein denotes the state vector at time step , , , denotes the slope at the start, middle and end of the prediction interval, denotes the recalculating of the slope at the middle of the prediction interval based on the slope at the start of the prediction interval; The physical and safety constraints include control quantity amplitude constraints, control quantity rate of change constraints, and surface boundary safety constraints; Then, the target function is solved to obtain an optimal control sequence by considering the constraint conditions; Finally, inverse kinematics is solved according to a predicted forward speed and a normal angular velocity included in a first element of the optimal control sequence to obtain left and right drive wheel angular velocities of the robot.

2. The method of claim 1, wherein, the a priori estimate state vector at time each state component of the a priori estimate state vector at time (17) (18) (19) wherein denotes the a priori estimate of the surface parameter at time instant denotes the a priori estimate of the heading angle at time instant denotes the a posteriori estimate of the surface parameter at time instant denotes the a posteriori estimate of the heading angle at time instant denotes the actual heading angle at time instant , denotes the robot forward speed and normal angular velocity at time instant denotes the geodesic curvature , , is an element of the first fundamental form matrix of the surface , , , , and denotes the tangent and the normal surface basis vectors.

3. The method of claim 1, wherein, the state transition Jacobian matrix at time k is represented as: (20) wherein is the identity matrix, is the Jacobian matrix of the continuous-time nonlinear kinematic model at time time-dependent process noise covariance matrix is represented as: (22) (21) wherein is is the input Jacobian matrix at time k, is is the input noise covariance matrix at time k.

4. The method of claim 1, wherein, The surface boundary safety constraints are: (48) (49) (50) wherein , denotes the time step of , to the surface parameters, , denotes the minimum and maximum values of the surface parameters, , denotes the minimum and maximum values of the surface parameters, , denotes the time step of , the relaxation variable to the surface parameters.

Images