Multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method and system

Abstract

The present application belongs to the field of intelligent control technology of amphibious robots, and particularly relates to a multi-modal fusion identification and CBF obstacle avoidance amphibious robot control method and system, which comprises the following steps: acquiring multi-view visual images and laser radar point clouds, respectively extracting visual semantic features and point cloud spatial geometric features, and obtaining a fused BEV environment feature map; outputting an obstacle state set based on the fused features through multi-branch detection; constructing a robot kinematics model, constructing a high-order control barrier function safety constraint using the obstacle state; taking a preset reference motion control amount as an optimization target, introducing a relaxation variable, converting the CBF safety constraint and the actuator physical constraint into a quadratic programming problem, and solving the optimal control input in real time. Through the deep fusion of vision and laser radar, the present application uses CBF to map the identification uncertainty to a dynamic safety boundary, and realizes the real-time coordination of the control target and the safety constraint based on quadratic programming.

Description

Technical Field

[0001] This invention belongs to the field of intelligent control technology for amphibious robots, and particularly relates to a control method and system for amphibious robots with multimodal fusion recognition and CBF obstacle avoidance. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] With the development of needs such as waterway inspection, water rescue, wetland operations and unmanned operations in disaster environments, amphibious robots need to operate continuously in unstructured scenarios with multiple coupled environments such as water surface, shallows, wetlands and land. They need to perceive dynamic and static obstacles such as pedestrians, floating objects, boats, shore structures and potholes in real time, and generate safe and smooth control commands under water flow disturbance and terrain changes to achieve stable movement across media and autonomous obstacle avoidance.

[0004] Among existing obstacle recognition and avoidance methods, those based on a single visual sensor are semantically rich but easily affected by light reflection and have difficulty obtaining 3D position; those based on LiDAR have strong spatial perception capabilities but weak semantic recognition, sparse and noisy point clouds over water areas, and difficulty in segmenting at the water-land interface; traditional path planning methods such as artificial potential field method, A* algorithm, and dynamic window method are prone to getting trapped in local optima in dynamic environments, and the planning results are discontinuous and unsmooth; obstacle avoidance methods based on model predictive control balance accuracy and smoothness, but have high computational complexity, and model bias in uncertain environments can easily lead to insufficient safety margin.

[0005] Despite existing research combining deep learning perception with local obstacle avoidance, the following problems still exist: multi-sensor fusion often remains at the target level with simple stitching, failing to fully explore the complementarity between image semantics and point cloud geometry; distance error, confidence level, and velocity uncertainty in target recognition have not been mapped into safety control constraints; obstacle avoidance control relies on empirical safety distances or static thresholds, making it difficult to dynamically adjust according to the obstacle's motion state and the water-land boundary.

[0006] Therefore, there is an urgent need for a control method for amphibious robots based on target recognition and CBF obstacle avoidance using a fusion of vision and lidar, to achieve high-confidence recognition of environmental targets, three-dimensional spatial positioning, dynamic risk assessment and real-time safety control. Summary of the Invention

[0007] To overcome the shortcomings of the prior art, this invention provides a multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method and system. It aims to achieve high-confidence recognition and three-dimensional spatial positioning of obstacles in complex water and land environments through multimodal fusion perception of vision and lidar. It also constructs formal safety constraints by combining control barrier functions, mapping perception uncertainty into dynamic safety boundaries. The optimal control command that satisfies the safety constraints is solved in real time through quadratic programming, thereby realizing safe, smooth and autonomous obstacle avoidance control of amphibious robots under multiple environmental coupling conditions such as water surface, shallow water and land.

[0008] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions: Firstly, a multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method is disclosed, including: Acquire multi-view visual images and lidar point cloud data of amphibious robot, extract visual features and point cloud spatial features respectively, generate visual BEV feature map and lidar BEV feature map, and fuse them to obtain fused BEV environment feature map. Based on the fused BEV environment feature map, obstacle detection and state estimation are performed through a multi-branch parallel detection head, and an obstacle state set is output; combined with the dual-mode motion characteristics of the amphibious robot, a robot kinematic model is constructed. Based on the robot's motion state information and the set of obstacle states, a control barrier function constraint is constructed; using a preset reference motion control quantity as the optimization objective, and combining the control barrier function constraint with the physical limitations of the robot actuator, a quadratic programming problem is constructed to solve for the optimal control input in real time.

[0009] Furthermore, the step of extracting visual features and point cloud spatial features respectively to generate visual BEV feature maps and LiDAR BEV feature maps includes: Multi-view visual images are input into a visual feature extraction network to extract multi-scale visual features, which are then transformed by viewpoint to generate a visual BEV feature map. LiDAR point clouds are input into a LiDAR feature encoding network to extract spatial distribution and geometric structure, which are then compressed and aggregated to generate a LiDAR BEV feature map.

[0010] Furthermore, obtaining the fused BEV environment feature map includes: The visual BEV feature map and the LiDAR BEV feature map are spatially aligned, concatenated along the channel dimension, and then input into a static fusion convolutional layer to obtain preliminary fusion features. The preliminary fusion features are then subjected to global average pooling and linear transformation to generate channel weights. After normalization by the Sigmoid function, the adaptively fused fusion BEV environment feature map is obtained by channel attention weighting.

[0011] Furthermore, the multi-branch parallel detection head includes a semantic classification branch, a position regression branch, and a motion estimation branch.

[0012] Furthermore, the obstacle state set consists of the state vectors of each obstacle, and a single obstacle state vector is represented as:

[0013] in, , The first The coordinates of the center position of each obstacle in the bird's-eye view coordinate system; The first The length and width of each obstacle, For the first The heading angle of each obstacle; The first An obstacle in the BEV coordinate system direction and The velocity component in the direction.

[0014] Furthermore, the step of establishing a safe distance function between the robot and the obstacle based on the robot's motion state information and the obstacle state set includes:

[0015] in, For the robot's real-time position, For the first Real-time center coordinates of each dynamic obstacle. The equivalent radius of the obstacle. The minimum safe distance for both land and water adaptation; Define the security set as:

[0016] Based on the second-order dynamic characteristics of the robot, the second-order time derivative of the safety distance function is calculated, and second-order constraints are constructed based on the theory of higher-order control barrier functions:

[0017] in, , These parameters are used to adjust the convergence speed of safety constraints and the dynamic response characteristics of the system. First derivative of the safety distance function This is the second derivative of the safety distance function.

[0018] Furthermore, the quadratic programming problem also includes introducing slack variables when constructing the quadratic programming problem, transforming the control barrier function constraint into an inequality constraint with slack variables.

[0019] Secondly, a multimodal fusion recognition and CBF obstacle avoidance amphibious robot control system is disclosed, including: A multi-source data acquisition module is used to acquire multi-view visual images and lidar point cloud data of the amphibious robot; The multimodal feature extraction and fusion module is used to extract visual features and point cloud spatial features, generate visual BEV feature maps and LiDAR BEV feature maps, and obtain fused BEV environment feature maps after fusion. The obstacle detection and state estimation module is used to perform obstacle detection and state estimation based on the fused BEV environment feature map through a multi-branch parallel detection head, and output an obstacle state set. The kinematic model building module is used to construct a kinematic model of the robot by combining the dual-mode motion characteristics of the amphibious robot. The CBF safety constraint construction module is used to construct control barrier function constraints based on the robot motion state information and the obstacle state set. The quadratic programming optimization solution module is used to construct a quadratic programming problem by using a preset reference motion control quantity as the optimization objective, combined with the control barrier function constraints and the physical limitations of the robot actuator, and solve for the optimal control input in real time.

[0020] Thirdly, a computer device is disclosed, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to perform the steps of the method described above.

[0021] Fourthly, a computer-readable storage medium is disclosed having a computer program stored thereon, which, when executed by a processor, implements the steps of the method described above.

[0022] The above one or more technical solutions have the following beneficial effects: This invention integrates the semantic recognition capability of visual target detection with the three-dimensional ranging capability of lidar point clouds, enabling stable output of target category, three-dimensional position, contour size and motion state in complex traffic scenarios. This reduces false detection, missed detection and depth estimation errors caused by single sensors under conditions of lighting changes, occlusion, sparse point clouds or long-distance perception.

[0023] This invention transforms the uncertainties in obstacle position, speed, size, and state from the fused perception results into dynamic safety distances, and further constructs CBF constraints, so that obstacle avoidance control no longer relies on fixed empirical thresholds, but can adjust the safety boundary in real time according to vehicle speed, relative obstacle motion, and recognition confidence, thereby improving obstacle avoidance robustness and safety.

[0024] This invention achieves real-time coordination between nominal control input and CBF safety constraints through quadratic programming, minimizing control corrections without compromising the path tracking target. It avoids control abrupt changes, path jitter, or excessive conservatism caused by traditional rule-based obstacle avoidance, thus realizing safe, smooth, and executable amphibious robot control.

[0025] This invention can be deployed in vehicle-mounted or embedded edge computing systems of amphibious robot platforms. It is suitable for work sites with complex terrain, high environmental uncertainty and potential safety risks, such as wetland environments, water patrol areas, complex terrain areas in parks and mining areas, as well as amphibious robot systems in various closed or semi-closed areas, thereby achieving highly reliable target recognition and safe obstacle avoidance control under multiple environmental coupling conditions such as water surface, shallow water and land.

[0026] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0027] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0028] Figure 1 This is a flowchart of the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method according to Embodiment 1 of the present invention; Figure 2 This is a schematic diagram of the fusion of visual BEV feature map and lidar BEV feature map in the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method of Embodiment 1 of the present invention; Figure 3 This is a schematic diagram of obstacle recognition in the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method of Embodiment 1 of the present invention; Figure 4 This is a diagram showing the constraint relationship of the second-order control barrier function in the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method of Embodiment 1 of the present invention; Figure 5 This is a diagram of the quadratic programming optimization control framework in the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method of Embodiment 1 of the present invention. Detailed Implementation

[0029] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0030] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations of the present invention.

[0031] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0032] Example 1 Existing obstacle recognition and avoidance methods for amphibious robots mainly focus on the following approaches: Obstacle recognition based on a single vision sensor: This method uses a camera mounted on an amphibious robot to capture images of the working environment and then uses algorithms such as target detection to identify pedestrians, boats, and various obstacles. While this method is low-cost and rich in semantic information, it is susceptible to factors such as changes in lighting, water surface reflection, and ripple disturbances in mixed aquatic and terrestrial environments. Especially in wetlands, shallow waters, and areas with complex shorelines, it is prone to unstable recognition, false detections, or missed detections. Furthermore, it struggles to accurately obtain the three-dimensional spatial location information of obstacles, limiting its effectiveness in complex environments.

[0033] Obstacle recognition based on LiDAR: This method uses LiDAR to acquire environmental point cloud data, performs 3D modeling of the environment surrounding the amphibious robot, extracts obstacle boundaries, and combines path planning algorithms to achieve obstacle avoidance. While this method has strong spatial structure perception capabilities, its semantic recognition ability for target categories is weak. Furthermore, in aquatic environments, the point cloud data may be sparse or noisy due to factors such as water surface reflection and unstable point cloud echoes. In densely vegetated areas or areas where land and water meet, inaccurate obstacle segmentation and difficulty in determining passable areas can easily occur, thus affecting the reliability of obstacle avoidance decisions.

[0034] Obstacle avoidance methods based on traditional path planning include artificial potential field method, A* algorithm, Dijkstra's algorithm, RRT algorithm, and dynamic window method. These methods can generate obstacle avoidance paths for amphibious robots in known or partially known environments. However, in complex environments alternating between water surfaces, shallows, and land, the presence of uncertainties such as water flow disturbances and wave effects leads to dynamic changes and irregular obstacle distribution. This causes traditional path planning methods to easily fall into local optima or result in discontinuous and unsmooth planning. Furthermore, in the transitional areas between water and land, changes in the motion model and constraints further increase the difficulty of maintaining consistency between path planning and execution, thus affecting obstacle avoidance performance and system stability.

[0035] Obstacle avoidance methods based on model predictive control or optimization control: By establishing a kinematic or dynamic model of the amphibious robot and considering system constraints, rolling optimization is performed on the control input over a future period to achieve path tracking and obstacle avoidance control. This method can balance trajectory tracking accuracy and control smoothness to a certain extent, but its computational complexity is high and it is highly dependent on model accuracy and environmental perception results. In mixed amphibious environments, due to uncertainties such as water flow disturbances, wave effects, and terrain changes, the system model is prone to deviations. When there are errors in the perception results or obstacles suddenly appear, it can easily lead to untimely constraint response and insufficient safety margin, thus affecting the safety and stability of system operation.

[0036] Although existing research attempts to combine deep learning-based perception models with local obstacle avoidance control, the following problems still exist: (1) Multi-sensor fusion mostly remains at the target level and simply stitches together, without making full use of the complementary relationship between image semantic features and point cloud geometric features; (2) The distance error, category confidence and speed estimation uncertainty in the target recognition results are not effectively mapped to safety control constraints; (3) Obstacle avoidance control often relies on empirical safety distance or static threshold, and it is difficult to dynamically adjust according to the obstacle's motion state and the water and land boundary; (4) The control algorithm is not sufficiently coupled with vehicle kinematics and dynamic constraints, which may result in the generated steering angle, acceleration or braking force commands being unexecutable or having poor comfort.

[0037] Therefore, there is an urgent need for a control method for amphibious robots based on target recognition and CBF obstacle avoidance using a fusion of vision and lidar, to achieve high-confidence recognition of environmental targets, three-dimensional spatial positioning, dynamic risk assessment and real-time safety control.

[0038] Based on this, such as Figure 1 As shown, this embodiment discloses a multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method, including: Step 1: Acquire multi-view visual images, LiDAR point clouds, and robot motion status; At least two visual sensors and at least one lidar are deployed on the amphibious robot platform. The visual sensors can be monocular cameras, binocular cameras, or fisheye cameras, and the lidar can be mechanical 3D lidar, semi-solid-state lidar, or solid-state lidar.

[0039] The amphibious robot also includes an inertial measurement unit, a combined navigation module, and water and motion state perception sensors to acquire state information such as the robot's pose, velocity, acceleration, and heading angle in water and land environments.

[0040] At any sampling time Acquire visual image frames LiDAR point cloud Robot status and the control input of the previous cycle ,in:

[0041] in, The spatial position in the global coordinate system. These are roll angle, pitch angle, and yaw angle. These are forward velocity, lateral velocity, and vertical velocity, respectively. They are respectively around angular velocity around the axis angular velocity around the axis Axial angular velocity.

[0042] To achieve unified timestamp synchronization for visual sensors, lidar, inertial measurement units, and amphibious robot control systems, hardware triggering, Precision Time Protocol (PTP) clock synchronization, or software interpolation synchronization methods can be used.

[0043] In mixed land and water environments, the robot may experience wave disturbances and attitude changes while operating on the water surface, placing higher demands on time synchronization accuracy. For multi-source data with inconsistent sampling frequencies, nearest neighbor matching or interpolation methods combined with motion compensation are used to align point cloud data and robot state to the image timestamp, ensuring the consistency of multimodal data in the time domain.

[0044] Step 2: Extract multi-scale semantic features from multi-view visual images and generate visual BEV feature maps through viewpoint transformation; extract spatial geometric features from lidar point clouds and compress them to generate lidar BEV feature maps. Feature extraction and unified representation learning are performed on the multi-view visual image data and LiDAR point cloud data after time synchronization and coordinate calibration in step 1. Specifically, the visual image frames... The input is a Transformer-based visual feature extraction network. This network includes a Patch Embedding module and a four-level hierarchical Transformer encoding stage. First, let the input visual image frame be:

[0045] in, Image height, 3 represents the image width, and 3 represents the RGB three-channel dimension.

[0046] The input image is divided into non-overlapping 4×4 image patches. Each patch is flattened into a one-dimensional vector of length 48. The Patch Embedding module then maps the vectors to a feature space of dimension C to obtain the initial feature map.

[0047] in, This is the number of basic feature channels, typically 96.

[0048] The visual feature extraction network consists of four sequentially connected encoding stages (Stage 1 to Stage 4). Each stage comprises several Swing Transformer Blocks and downsampling modules. It extracts features from different receptive fields through window self-attention and shifted window self-attention mechanisms, progressively reducing spatial resolution and increasing the number of feature channels, ultimately outputting four-level multi-scale visual features. Stage 1: Contains two cascaded Swing Transformer Blocks, extracting shallow semantic and texture features while maintaining the feature size, and outputting the first-scale features:

[0049] Stage 2: First, the input features are downsampled using a 2×2 neighborhood concatenation method via the Patch Merging module, halving the spatial resolution and doubling the number of feature channels; then, they are encoded by two Swin Transformer Blocks to output the second-scale features.

[0050] Stage 3: Similarly, after Patch Merging downsampling, high-level semantic features are extracted through 6 Swin Transformer Blocks, and the third-scale features are output:

[0051] Stage 4: First, downsampling is performed using Patch Merging, then the highest-level global semantic features are extracted using two Swin Transformer Blocks, outputting the fourth-scale features:

[0052] The four-level multi-scale visual features A multi-scale feature pyramid is constructed, in which shallow features retain rich spatial details and edge texture information, while deep features have a larger receptive field and stronger semantic expression ability. Subsequently, the multi-scale features are fused into a uniform-sized image feature map through feature upsampling and channel stitching, and then input into the viewpoint conversion module.

[0053] Furthermore, the Lift-Splat-Shoot (LSS) perspective transformation method is adopted to elevate the multi-view image features from the image plane to three-dimensional space and map them to the Bird's-Eye View (BEV) coordinate system.

[0054] First, let the first... The two-dimensional image features extracted from each camera are represented as follows:

[0055] in, , These represent the height and width of the image feature map, respectively. The number of channels representing image features. =1,2, , , Indicates the number of cameras.

[0056] To recover the three-dimensional spatial information corresponding to the image, a depth prediction network is used to predict the discrete depth probability distribution corresponding to each pixel: , in, Represents the pixel coordinates of the image. Represented as a discrete depth layer index. This indicates the preset total number of discrete depth layers. Represents pixels Located in the The probability of each depth layer.

[0057] Based on the discrete depth probability distribution, the two-dimensional image features are uplifted to three-dimensional space along the depth dimension to obtain three-dimensional voxel features: , in, Represents pixels In the The three-dimensional voxel features corresponding to each depth layer, "This indicates that the depth probability is multiplied element-wise by the image features channel by channel."

[0058] Subsequently, by combining the camera calibration parameters, the 3D voxel features are restored to 3D spatial coordinates in the robot coordinate system. Let the camera intrinsic parameter matrix be... The rotation matrix is The translation vector is Then the corresponding three-dimensional spatial coordinates can be expressed as: , , in, Represents the three-dimensional coordinates in the camera coordinate system. Represents the three-dimensional coordinates in the robot's coordinate system. This represents the camera intrinsic parameter matrix. Represents the rotation matrix. This represents the translation vector.

[0059] After obtaining the three-dimensional spatial features, the features in the three-dimensional space are projected onto the corresponding BEV grids according to the preset BEV grid division rules, and then aggregated along the height direction to obtain the visual BEV feature map. The aggregation process is represented as follows: , in, This represents the grid index in the BEV coordinate system. This indicates that 3D features falling within the same BEV mesh are aggregated along the height direction.

[0060] Finally, a visual BEV feature map is generated:

[0061] in, and These represent the spatial resolution of the visual BEV feature map in the horizontal and vertical directions, respectively. This represents the number of channels in the visual BEV feature map.

[0062] At the same time, the lidar point cloud data Spatial features are extracted by inputting the LiDAR feature encoding network.

[0063] First, let the first... The preprocessed lidar point cloud is represented as follows: , in, , Indicates the first Point cloud points, They represent spatial coordinates, Indicates the intensity of laser reflection. This represents the number of points in a point cloud.

[0064] Next, based on the preset voxel size: , in They are respectively , , The side length of a single voxel in three directions.

[0065] The point cloud is divided into voxels, and the space is divided into a regular three-dimensional voxel mesh. Each voxel is represented as follows: , in, , , They represent voxels in , , Indexes in three directions.

[0066] Subsequently, the non-empty voxels are input into a sparse convolutional coding network to extract the spatial geometric features of the point cloud layer by layer, thereby obtaining the 3D LiDAR feature volume: , in, , , These represent the dimensions of the three-dimensional feature volume in three directions, respectively. This represents the number of three-dimensional feature channels.

[0067] Furthermore, the 3D LiDAR feature volume is compressed and aggregated along the height direction to obtain the 2D bird's-eye view feature, the expression of which is: , in, , Indicates the BEV grid index. Indicates the height direction index. This indicates that three-dimensional features are aggregated along the height direction.

[0068] The final generated LiDAR BEV feature map:

[0069] in, This represents the number of channels in the LiDAR BEV feature map.

[0070] Step 3: Fuse the visual BEV feature map and the LiDAR BEV feature map to obtain the fused BEV environment feature map; like Figure 2 As shown, according to step 2 and First, spatial alignment is performed to ensure that the two types of features are aligned to a uniform resolution. The next pixel is matched to ensure spatial consistency between semantic and geometric information. After alignment, the two modal features are concatenated along the channel dimension to obtain the fused input features.

[0071] in, This indicates a channel-dimensional splicing operation. Subsequently, a static fusion function is used. The splicing features are processed by a convolutional layer with a kernel size of 5×5. Its function is to fuse local spatial information and compress the channel dimension, yielding a preliminary fusion result.

[0072] Furthermore, to enhance the expressive power of the fused features, an adaptive fusion strategy based on channel attention is introduced. First, a global average pooling operation is performed on the static fused features:

[0073] in, Subsequently, channel weights are generated through a linear transformation and then normalized using the Sigmoid function:

[0074] in, Represents a linear transformation matrix. This represents the Sigmoid activation function.

[0075] Finally, the channel weights are applied to the static fusion feature to achieve adaptive weighted fusion, resulting in the final bird's-eye view fusion feature:

[0076] in, For the channel attention weight vector, "" indicates element-wise multiplication by channel. The overall fusion process can be uniformly represented as:

[0077] The fusion result not only preserves the high-level semantic representation of visual information, but also makes full use of the accuracy of LiDAR in spatial geometry, and can dynamically adjust the contribution ratio of the two modes according to different scenarios, thereby significantly improving the system's perception robustness and real-time performance while ensuring computational efficiency.

[0078] Step 4: Perform obstacle detection based on the fused BEV environment feature map to obtain obstacle information; Based on the high-precision fused BEV environment feature map generated in step 3 We construct an obstacle detection branch adapted to mixed water and land environments, enabling accurate classification and full-state parameter estimation of obstacles, providing core perception input for subsequent safe obstacle avoidance.

[0079] First, the BEV feature is fused into the lightweight detection head, which employs a multi-branch parallel structure to balance real-time performance and detection accuracy. This includes: Semantic classification branch: fusing BEV features The input is passed sequentially through a 3×3 convolutional layer, a batch normalization layer, a ReLU activation function, and a 1×1 convolutional layer to obtain the class response map; then, the Softmax activation function is used to calculate the probability of each class, expressed as: , in,

[0080] Indicates the number of obstacle categories.

[0081] Obstacle categories are determined based on category probabilities, including static obstacles (rocks, dams, aquatic vegetation, reefs), dynamic obstacles (ships, floating objects, amphibious vehicles), and passable areas (water surface, land).

[0082] Location regression branch: fusing BEV features As input, the predicted geometric properties of the target are obtained after continuous convolution operations, and are represented as: , in, , Indicates the coordinates of the obstacle's center. Indicates the length of the obstacle, Indicates the width of the obstacle Indicates the heading angle of the obstacle.

[0083] Motion estimation branch: also using fused BEV features As input, the target velocity information is predicted by a convolutional network: , in, , , These represent the obstacles in the BEV coordinate system. direction and The velocity component in the direction.

[0084] Then, non-maximum suppression (NMS) is used to remove redundant detection boxes, and error compensation is performed based on the characteristics of the water and land environment (water surface wave disturbance correction, land terrain undulation correction), finally outputting the obstacle state set:

[0085] Single obstacle state vector:

[0086] in, For obstacle categories, The first The coordinates of the center position of each obstacle in the bird's-eye view (BEV) coordinate system; The first The length and width of each obstacle, For the first The heading angle of each obstacle; The first An obstacle in the BEV coordinate system direction and velocity components in the direction, The number of valid obstacles in the environment; obstacle recognition results are as follows: Figure 3 As shown.

[0087] Step 5: Based on the obstacle information and vehicle motion state, construct a control barrier function to form safety constraints; Combining the dual-mode motion characteristics of the robot's water surface navigation and land movement, a unified rigid body kinematic model is established to provide a theoretical basis for the control algorithm. The model parameters are related to the robot's state vector obtained in step 1. Strict matching.

[0088] Ignoring high-frequency minute perturbations, a two-dimensional planar kinematic model of the robot is established:

[0089] in, Yaw angle For the robot's forward velocity, For the robot's lateral speed, For the heading angular velocity, 、 This is the acceleration / deceleration control quantity.

[0090] Robot's underlying control input vector:

[0091] in, This refers to steering control parameters (rudder angle / track differential). For velocity control (thrust / driving force); the model supports adaptive switching between water and land modes, with water resistance compensation added for water surface mode and ground friction constraints added for land mode.

[0092] Step 6: Construct control barrier function constraints based on the robot motion state information and the obstacle state set; Fusion robot's own state With obstacle status An adaptive safety distance function for both land and water is constructed and transformed into a control barrier function constraint, theoretically ensuring the robot's collision-free safe operation. Figure 4 As shown.

[0093] First, define the robot and the first Safety distance function for each obstacle:

[0094] in, For the robot's real-time position, Let i be the real-time center coordinates of the i-th dynamic obstacle. The equivalent radius of the obstacle. To achieve the minimum safe distance for both water and land adaptation, the water surface mode automatically increases the safety margin, while the land mode adaptively adapts to terrain constraints. The barrier function satisfies... At that time, the robot and the obstacle are in a collision-free safe zone.

[0095] To ensure that the robot always meets the safety distance constraint in a dynamic environment, the safety distance function is converted into a control barrier function constraint.

[0096] Define the security set as:

[0097] When the system state always meets this condition, the robot maintains a collision-free safe relationship with the obstacle.

[0098] Considering that the robot system satisfies second-order dynamic characteristics, the safety function has a relative order of 2 with respect to the control input. Therefore, a high-order control barrier function (HOCBF) is used to construct the constraints.

[0099] First, the first-order time derivative of the security function is calculated:

[0100] Furthermore, its second-order time derivative is calculated:

[0101] in, , These represent the robot's velocity and acceleration, respectively. , These represent the velocity and acceleration of the obstacle, respectively.

[0102] Based on the theory of higher-order control barrier functions, the following second-order constraint conditions are constructed:

[0103] in, , These are adjustable parameters used to control the convergence speed of safety constraints and the dynamic response characteristics of the system.

[0104] Step 7: Using the preset reference motion control quantity as the optimization objective, and combining the control barrier function constraints and the physical limitations of the robot actuator, construct a quadratic programming problem and solve for the optimal control input in real time.

[0105] In this embodiment, to obtain smooth and executable control inputs while satisfying system dynamics and safety constraints, an optimized controller based on quadratic programming (QP) is constructed, such as... Figure 5 As shown, the optimizer solves for the optimal control input in real time during each control cycle. .

[0106] First, based on the reference control input given by the path planning or trajectory tracking module. Construct the following quadratic objective function to approximate the desired control as closely as possible while satisfying safety constraints:

[0107] in, This is the weight matrix, used to adjust control smoothness and tracking performance.

[0108] To enhance feasibility, slack variables can be introduced. And by penalizing it, we obtain the extended objective function:

[0109] in, The penalty coefficient for slack variables.

[0110] Then, for each obstacle By applying the second-order constraints constructed in step 6 and introducing slack variables to ensure the problem is always solvable, we obtain:

[0111] Expanding it, it can be expressed in the standard linear constraint form:

[0112] Wherein, the coefficient matrix With constant term Based on the robot's current state and the first The states of each obstacle are calculated in real time, and their expressions are as follows: , , Where, in the formula, and These represent the robot's position vector and velocity vector, respectively. They represent the first The position vector, velocity vector, and acceleration vector of each obstacle; The control input matrix is ​​determined by the aforementioned robot kinematics model; and They represent the first The safe distance function for each obstacle and its first-order time derivative; and This is a positive real number that controls the gain parameter of the barrier function, used to adjust the convergence speed of the safety constraint.

[0113] Then, considering the physical limitations of the robot's actuators, boundary constraints are applied to the control inputs:

[0114] If necessary, an acceleration rate of change constraint (smoothing constraint) can also be added:

[0115] in To control the upper limit of the input change rate, it is used to limit the abrupt change in control commands and ensure smooth motion.

[0116] In summary, the optimization problem can be uniformly represented as the following standard quadratic programming problem:

[0117] Within each control cycle, the optimal control input is calculated in real time by the QP solver. It then sends the data to the robot's actuators, which include propulsion mechanisms, servo motors, and track motors, to achieve closed-loop control of the robot's motion.

[0118] Example 2 Based on the method described in Embodiment 1, the purpose of this embodiment is to provide a multimodal fusion recognition and CBF obstacle avoidance amphibious robot control system, including: A multi-source data acquisition module is used to acquire multi-view visual images and lidar point cloud data of the amphibious robot; The multimodal feature extraction and fusion module is used to extract visual features and point cloud spatial features, generate visual BEV feature maps and LiDAR BEV feature maps, and obtain fused BEV environment feature maps after fusion. The obstacle detection and state estimation module is used to perform obstacle detection and state estimation based on the fused BEV environment feature map through a multi-branch parallel detection head, and output an obstacle state set. The kinematic model building module is used to construct a kinematic model of the robot by combining the dual-mode motion characteristics of the amphibious robot. The CBF safety constraint construction module is used to construct control barrier function constraints based on the robot motion state information and the obstacle state set. The quadratic programming optimization solution module is used to construct a quadratic programming problem by using a preset reference motion control quantity as the optimization objective, combined with the control barrier function constraints and the physical limitations of the robot actuator, and solve for the optimal control input in real time.

[0119] Example 3 The purpose of this embodiment is to provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the above-described method.

[0120] Example 4 The purpose of this embodiment is to provide a computer-readable storage medium.

[0121] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the above-described method.

[0122] The steps and methods involved in the apparatus of the above embodiments correspond to those in Embodiment 1. For specific implementation details, please refer to the relevant description section of Embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood as including any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and enabling the processor to perform any of the methods in this invention.

[0123] Those skilled in the art will understand that the modules or steps of the present invention described above can be implemented using general-purpose computer devices. Optionally, they can be implemented using computer-executable program code, thereby allowing them to be stored in a storage device for execution by a computer device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. The present invention is not limited to any particular combination of hardware and software.

[0124] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method, characterized in that, include: Acquire multi-view visual images and lidar point cloud data of amphibious robot, extract visual features and point cloud spatial features respectively, generate visual BEV feature map and lidar BEV feature map, and fuse them to obtain fused BEV environment feature map. Based on the fused BEV environment feature map, obstacle detection and state estimation are performed through a multi-branch parallel detection head, and an obstacle state set is output; combined with the dual-mode motion characteristics of the amphibious robot, a robot kinematic model is constructed. Based on the robot's motion state information and the set of obstacle states, a control barrier function constraint is constructed; using a preset reference motion control quantity as the optimization objective, and combining the control barrier function constraint with the physical limitations of the robot actuator, a quadratic programming problem is constructed to solve for the optimal control input in real time.

2. The multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in claim 1, characterized in that, The step of extracting visual features and point cloud spatial features respectively to generate visual BEV feature maps and LiDAR BEV feature maps includes: Multi-view visual images are input into a visual feature extraction network to extract multi-scale visual features, which are then transformed by viewpoint to generate a visual BEV feature map. LiDAR point clouds are input into a LiDAR feature encoding network to extract spatial distribution and geometric structure, which are then compressed and aggregated to generate a LiDAR BEV feature map.

3. The multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in claim 1, characterized in that, The obtained fused BEV environment feature map includes: The visual BEV feature map and the LiDAR BEV feature map are spatially aligned, concatenated along the channel dimension, and then input into a static fusion convolutional layer to obtain preliminary fusion features. The preliminary fusion features are then subjected to global average pooling and linear transformation to generate channel weights. After normalization by the Sigmoid function, the adaptively fused fusion BEV environment feature map is obtained by channel attention weighting.

4. The multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in claim 1, characterized in that, The multi-branch parallel detection head includes a semantic classification branch, a position regression branch, and a motion estimation branch.

5. The multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in claim 1, characterized in that, The obstacle state set consists of the state vectors of each obstacle, and a single obstacle state vector is represented as: in, For obstacle categories, The first The coordinates of the center position of each obstacle in the bird's-eye view coordinate system; The first The length and width of each obstacle, For the first The heading angle of the obstacle; The first An obstacle in the BEV coordinate system direction and The velocity component in the direction.

6. The multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in claim 1, characterized in that, The step of establishing a safe distance function between the robot and the obstacle based on the robot's motion state information and the obstacle state set includes: in, For the robot's real-time position, Let i be the real-time center coordinates of the i-th dynamic obstacle. The equivalent radius of the obstacle. The minimum safe distance for both land and water adaptation; Define the security set as: Based on the second-order dynamic characteristics of the robot, the second-order time derivative of the safety distance function is calculated. Based on the theory of higher-order control barrier functions, the following second-order constraint conditions are constructed: in, , These parameters are used to adjust the convergence rate of safety constraints and the dynamic response characteristics of the system. First derivative of the safety distance function This is the second derivative of the safety distance function.

7. The multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in claim 1, characterized in that, The quadratic programming problem further includes introducing slack variables when constructing the quadratic programming problem, transforming the control barrier function constraint into an inequality constraint with slack.

8. A control system for a multimodal fusion recognition and CBF obstacle avoidance amphibious robot, characterized in that, include: A multi-source data acquisition module is used to acquire multi-view visual images and lidar point cloud data of the amphibious robot; The multimodal feature extraction and fusion module is used to extract visual features and point cloud spatial features, generate visual BEV feature maps and LiDAR BEV feature maps, and obtain fused BEV environment feature maps after fusion. The obstacle detection and state estimation module is used to perform obstacle detection and state estimation based on the fused BEV environment feature map through a multi-branch parallel detection head, and output an obstacle state set. The kinematic model building module is used to construct a kinematic model of the robot by combining the dual-mode motion characteristics of the amphibious robot. The CBF safety constraint construction module is used to construct control barrier function constraints based on the robot motion state information and the obstacle state set. The quadratic programming optimization solution module is used to construct a quadratic programming problem by using a preset reference motion control quantity as the optimization objective, combined with the control barrier function constraints and the physical limitations of the robot actuator, and solve for the optimal control input in real time.

9. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method according to any one of claims 1-7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it performs the steps of the multimodal fusion recognition and CBF obstacle avoidance amphibious robot control method as described in any one of claims 1-7.

Images