Composite robot navigation and path autonomous planning method and system based on job task large model

Abstract

This invention belongs to the field of artificial intelligence technology, specifically relating to a method and system for navigation and autonomous path planning of a composite robot based on a large task model. The method includes the following steps: 1. Constructing a vehicle-arm coordinated coupled kinematic or dynamic integrated model and identifying parameters; 2. Performing task-oriented coordinated control through behavior trees; 3. Performing task understanding and requirement modeling; 4. Collecting environmental data through sensors, performing environmental perception and real-time map construction; 5. Performing global path planning and dynamic execution, selecting the local optimal trajectory; 6. Performing motion control and feedback adjustment. This method improves the motion coordination and real-time performance of the composite robot, effectively avoiding joint position and speed exceeding limits.

Description

Technical Field

[0001] This application belongs to the field of artificial intelligence technology, specifically relating to a method and system for navigation and autonomous path planning of a composite robot based on a large task model. Background Technology

[0002] Hybrid robots integrate the functions of a mobile chassis and a robotic arm, and are widely used in industrial production, service robots, and logistics handling. Their core requirement is to achieve precise navigation and efficient path planning in complex environments. Existing path planning systems often employ open-loop models or incorporate machine learning and reinforcement learning methods. While these can handle multi-degree-of-freedom constraints, they struggle to simultaneously ensure the hybrid robot's motion coordination, path optimization, and environmental adaptability. Their main technical problems include:

[0003] (1) The coordination control accuracy caused by the separation modeling of vehicle-arm motion is insufficient, which makes it difficult to meet the closed-loop operation requirements in dynamic environment, and is prone to problems such as joint position and speed exceeding limits.

[0004] (2) The planning time is long and the real-time performance is poor, making it impossible to respond quickly to environmental changes.

[0005] (3) Path planning often only considers the shortest path, ignoring multi-dimensional requirements such as task priority, energy consumption, obstacle avoidance flexibility, etc.

[0006] (4) It lacks an effective dynamic feedback adjustment mechanism and is not adaptable enough to environmental changes such as the movement or addition of obstacles.

[0007] Therefore, there is an urgent need for a new approach that can solve the challenges of real-time performance, accuracy, and adaptability of traditional technologies through the deep integration of integrated modeling, reactive control, and dynamic programming. Summary of the Invention

[0008] To address the aforementioned technical problems in existing technologies, the purpose of this invention is to solve the issues of insufficient vehicle-arm coordination, poor real-time performance, and weak dynamic adjustment capability in traditional systems. The technical solution is as follows:

[0009] A method for navigation and autonomous path planning of a composite robot based on a large task model includes the following steps:

[0010] Step 1: Construct an integrated kinematic or dynamic model of vehicle-arm coordinated coupling, define the world coordinate system, chassis coordinate system, robot arm base coordinate system and end effector coordinate system, derive the relationship between end effector pose and joint velocity through kinematic derivation, and identify parameters;

[0011] Step 2: Design a reactive motion controller based on the integrated kinematic or dynamic model of vehicle-arm coordinated coupling, and perform task-oriented coordinated control through behavior tree;

[0012] Step 3: Perform task understanding and requirements modeling;

[0013] Step 4: Collect environmental data through sensors to perform environmental perception and real-time map construction;

[0014] Step 5: Integrate A* algorithm or RRT* for initial path planning, search for the optimal reference path from the current pose to the target point, and use dynamic window method or time elastic band algorithm for local trajectory planning. Dynamically adjust the trajectory evaluation function weight according to the navigation mode specified by the large model of the task to select the local optimal trajectory.

[0015] Step 6: Calculate the expected rotational speeds of the left and right drive wheels to eliminate tracking errors between the actual pose and the planned path. Collect feedback information in real time and set automatic triggering conditions to automatically trigger the path replanning and task plan correction process. Adjust parameters dynamically based on the feedback information.

[0016] Furthermore, the vehicle-arm coordinated coupling kinematic or dynamic integrated model, based on the world coordinate system, establishes the conversion relationship between joint velocity and end effector velocity through the relative poses of the moving chassis, the relative poses of the robotic arm base, and the pose of the end effector. Its expression is as follows:

[0017]

[0018] Where {0} represents the world coordinate system, {b} represents the relative pose of the mobile chassis, {a} represents the relative pose of the robotic arm base, and {e} represents the robotic arm end effector. For joint velocity, This refers to the terminal velocity.

[0019] Furthermore, the reactive motion controller employs quadratic programming (QP) to construct an optimization problem, the expression for which the optimization variables are:

[0020]

[0021] in, For the joint speed of the robotic arm, For chassis joint speed;

[0022] The objective functions include: joint velocity cost, relaxation cost, and the objective of maximizing maneuverability, as well as the objective of optimizing the base-end orientation. Their expressions are as follows:

[0023]

[0024]

[0025] The constraints include joint position and velocity limits, and their expressions are as follows:

[0026]

[0027]

[0028] Wherein, the relaxation cost coefficient is the reciprocal of the Euclidean distance error between the end effector and the desired posture, Q is a quadratic term matrix, C is a linear term matrix, J is the Jacobian matrix of the integrated composite robot, and A and B are constraint matrices. , These are the upper and lower limits for joint variables.

[0029] Furthermore, the specific steps of step 3 are as follows:

[0030] Step 3.1: Combine the natural language task instructions, scene semantic description, robot motion constraints and environmental prior information into structured prompt text, and input it into the pre-trained task model;

[0031] Step 3.2: The large-scale task model generates semantic feature vectors through an embedding layer, the expression of which is:

[0032]

[0033] in, Provide prompt text for structured tasks;

[0034] Step 3.3: Decompose the overall task into a sequence of subtasks with sequential execution logic using a classification reasoning network, and extract navigation constraints. The function calculates the constraint class probability, and its expression is:

[0035]

[0036] in, For the first Navigation constraints, For the first The classification probability value of the navigation constraint. To constrain the weight matrix of the classification network, For bias terms;

[0037] Step 3.4: Construct logical dependencies between subtasks through thought chain reasoning, forming a directed acyclic task relationship graph, and determine the execution order of subtasks through topological sorting;

[0038] Step 3.5: Perform feasibility verification on the task sequence based on the environmental raster map information, remove unexecutable branches and adjust the task order. The feasibility verification score calculation expression is as follows:

[0039]

[0040] in, To determine the geometric feasibility of the task, For task completion rate, To estimate the execution time, , , As a weight for feasibility scoring;

[0041] Step 3.6: Introduce feedback data from the execution process to iteratively optimize the task plan. The expression for calculating the optimization loss function value is as follows:

[0042]

[0043] in, For feedback penalty coefficient, , These are the initial generated and the optimized task plan vectors, respectively. To calculate the feedback deviation loss value.

[0044] Furthermore, the specific implementation steps of step 4 are as follows:

[0045] Step 4.1: Collect environmental 3D point cloud data, RGB-D visual images, robot posture acceleration and angular velocity, drive wheel speed signals and contact force signals through LiDAR, depth camera, inertial measurement unit, wheel odometry and end force sensor respectively, and perform preprocessing;

[0046] Step 4.2: Perform multi-source information fusion on the preprocessed data using the extended Kalman filter algorithm;

[0047] Step 4.3: Input the data from each sensor into the global map coordinate system to obtain the robot's real-time accurate pose in the working environment;

[0048] Step 4.4: Based on the fused pose information and environmental perception data, construct an occupancy grid map and a dynamic cost map, and perform dilation processing on the obstacle grid.

[0049] Step 4.5: Calculate the cost map grid passage cost based on the grid occupancy probability and obstacle distance information, and divide different passage areas.

[0050] Furthermore, in step 4.1, the preprocessing includes:

[0051] (1) Radial distortion correction is performed on the RGB-D visual image. The main process is as follows:

[0052] ①Original pixel coordinates with normalized image coordinates The expression for intrinsic parameter mapping is:

[0053]

[0054]

[0055] in, , These are the original coordinates of the pixel. , The horizontal and vertical focal lengths of the camera. , The coordinates of the principal point in the camera image;

[0056] ② Perform radial distortion correction on the image, the expression of which is:

[0057]

[0058]

[0059] In the formula, , and The radial distortion coefficient of the lens. Let be the radial distance from the pixel to the center of the image, and ;

[0060] ③ Map the corrected normalized coordinates back to pixel coordinates to obtain the final distortion-corrected pixel coordinates, the expression of which is:

[0061]

[0062]

[0063] in, These are the pixel coordinates after distortion correction;

[0064] (2) Outlier noise points are removed from the three-dimensional point cloud data using statistical filtering. The expression for identifying outlier points in the point cloud is as follows:

[0065]

[0066] in, For the point cloud neighborhood The spatial coordinate vector of a point, The coordinates are the mean coordinates. To count the number of neighboring points in a point cloud, the mean of the neighboring points is calculated, and point clouds exceeding a set threshold are identified as noise and removed.

[0067] (3) Perform zero-bias error compensation and attitude calculation on the robot's attitude acceleration and angular velocity to generate real-time roll, pitch and yaw attitude data of the robot;

[0068] (4) Perform kinematic integral calculation, time synchronization and cumulative error correction on the drive wheel speed signal to generate robot displacement and velocity data;

[0069] (5) The robot contact state is determined by using the composite contact force to analyze the contact force signal. The expression for the composite contact force is as follows:

[0070]

[0071] in, , and The contact force components of the end effector on the coordinate axis;

[0072] In step 4.2, the expression for calculating the filter gain of the extended Kalman algorithm is as follows:

[0073]

[0074] in, The covariance matrix is ​​estimated for the state. For the observation noise matrix, The observation matrix;

[0075] The filter state update expression of the extended Kalman filter algorithm is as follows:

[0076]

[0077] in, for The robot's optimal state vector updated by observation at each moment;

[0078] In step 4.5, the calculation expression for the cost map grid passage cost is as follows:

[0079]

[0080] in, In a raster map The access value of a grid , For cost weighting coefficients, The probability of grid occupancy. This is a penalty for obstacle distance.

[0081] The access areas include: safe access areas, restricted access areas, and prohibited access areas.

[0082] Furthermore, in step 5, the total cost function expression of the A* algorithm is:

[0083]

[0084] in, The actual cost from the starting point to the current node. The price of inspiration for Euclidean distance This is the semantic constraint penalty coefficient. This is a penalty item for violating semantic constraints;

[0085] The formula for calculating the total path cost of the RRT* algorithm is as follows:

[0086]

[0087] in, The obstacle distance penalty coefficient, The spatial distance from the waypoint to the nearest obstacle;

[0088] The trajectory evaluation function expression of the dynamic window method is as follows:

[0089]

[0090] in, and They are linear velocity and angular velocity, respectively. , and These are the evaluation items: trajectory speed, safe distance, and target orientation. , , Assigning weights to trajectory evaluation;

[0091] The selection of the optimal local trajectory involves selecting the optimal local trajectory that has no collision risk, fits the global reference path, and faces the target point. The formula for determining trajectory collision is as follows:

[0092]

[0093] In the formula, For the first [item] on the robot's motion trajectory item, For the environment The coordinates of each obstacle point This is the safe distance threshold.

[0094] Furthermore, in step 6, the desired rotational speed of the left and right drive wheels is calculated based on the local optimal trajectory and the differential chassis kinematic model, the expression of which is:

[0095]

[0096] in, and These are the rotational speeds of the left and right wheels, respectively. The wheelbase of the robot;

[0097] The tracking error between the actual pose and the planned path is eliminated by a PID controller, and the control law expression of the PID controller is as follows:

[0098]

[0099] in, To control the output, , as well as These are PID control parameters;

[0100] The feedback information includes: environmental dynamic change information, robot pose deviation data, collision detection signals, and operation progress feedback information;

[0101] The automatic triggering conditions are when the path is obstructed by an obstacle, the tracking error exceeds a threshold, or the reward value is lower than a set lower limit. This automatically triggers the path replanning and task plan correction process, and its expression is:

[0102]

[0103] in, This is a global path blocking indicator. This represents the actual overall operational error value. To trigger the error threshold for replanning, To enhance learning, accumulate reward points.

[0104] Furthermore, the aforementioned The cumulative reward value in reinforcement learning is calculated based on the immediate reward function of reinforcement learning, the expression of which is:

[0105]

[0106] in, for Constantly reinforce learning with immediate reward values. As a safety reward for obstacle avoidance, As a reward for path efficiency, This is a collision penalty.

[0107] A composite robot navigation and path autonomous planning system based on a large task model, used to implement the method as described in any one of claims 1 to 9, comprising:

[0108] Modeling module: Used to construct a coordinated kinematic / dynamic integrated model of the composite robot vehicle-arm, and to identify parameters with high precision;

[0109] Control module: Used to design reactive motion controllers based on the integrated kinematic / dynamic model of the composite robot's vehicle-arm coordinated coupling, and to achieve task-oriented coordinated control by combining behavior trees;

[0110] Task processing module: Used to receive task instructions and realize task understanding and requirement modeling;

[0111] Environmental perception module: used to collect environmental data and realize environmental perception and real-time map construction;

[0112] Path planning module: used for global path planning and dynamic execution, and to select the local optimal trajectory;

[0113] Execution feedback module: used for motion control and feedback adjustment.

[0114] Beneficial effects: (1) Improved the motion coordination and real-time performance of the composite robot, effectively avoiding joint position and speed over-limit problems, and ensuring the accuracy of closed-loop operation in dynamic environments. (2) Realized deep parsing of natural language commands, which can accurately extract task requirements and make path planning more in line with actual application scenarios. (3) Balanced the optimality of the initial path with the adaptability to environmental changes, and improved obstacle avoidance flexibility and path execution reliability through real-time feedback adjustment. (4) The integrated system architecture realizes full-process collaboration, effectively reducing the coupling between modules and improving the scalability and maintainability of the system. Attached Figure Description

[0115] Figure 1 This is a flowchart of the composite robot navigation and path autonomous planning method based on a large task model according to the present invention.

[0116] Figure 2 This is a schematic diagram of the integrated modeling and reactive controller structure of the composite robot of the present invention;

[0117] Figure 3 This is a framework diagram of the composite robot navigation and path autonomous planning system based on a large task model according to the present invention. Detailed Implementation

[0118] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0119] like Figure 1 As shown, the navigation and path planning method for composite robots based on a large task model of the present invention mainly includes the following steps:

[0120] S1: Taking a two-wheel differential-driven composite robot as the object, construct an integrated kinematic or dynamic model of vehicle-arm coordinated coupling, define the world coordinate system, chassis coordinate system, robot arm base coordinate system and end effector coordinate system, derive the relationship between end effector pose and joint velocity through kinematic derivation, and perform parameter identification to ensure that the model is consistent with the actual robot motion characteristics.

[0121] The vehicle-arm coordinated coupling kinematic or dynamic integrated model uses the world coordinate system as a reference and establishes the conversion relationship between joint velocities and end effector velocities through the relative poses of the moving chassis, the relative poses of the robotic arm base, and the pose of the end effector. The expression is as follows:

[0122]

[0123] Where {0} represents the world coordinate system, {b} represents the relative pose of the mobile chassis, {a} represents the relative pose of the robotic arm base, and {e} represents the robotic arm end effector. For joint velocity, This refers to the terminal velocity.

[0124] S2: Design a reactive motion controller based on the aforementioned vehicle-arm coordinated coupling kinematic or dynamic integrated model, such as... Figure 2 As shown, the reactive motion controller is constructed as a QP optimization problem, and task-oriented coordinated control is performed through behavior tree. The task flow of "grab-move-return" is designed, including error recovery branches.

[0125] The reactive motion controller employs quadratic programming (QP) to construct an optimization problem, with the optimization variable expression as follows:

[0126]

[0127] in, For the joint speed of the robotic arm, For chassis joint speed;

[0128] The objective functions include: joint velocity cost, relaxation cost, and the objective of maximizing maneuverability, as well as the objective of optimizing the base-end orientation. Their expressions are as follows:

[0129]

[0130]

[0131] The constraints include joint position and velocity limits, and their expressions are as follows:

[0132]

[0133]

[0134] Wherein, the relaxation cost coefficient is the reciprocal of the Euclidean distance error between the end effector and the desired posture, Q is a quadratic term matrix, C is a linear term matrix, J is the Jacobian matrix of the integrated composite robot, and A and B are constraint matrices. , These are the upper and lower limits for joint variables.

[0135] S3: Perform task understanding and requirements modeling, the specific process is as follows:

[0136] S3.1: Combine natural language task instructions, scene semantic descriptions, robot motion constraints, and prior environmental information into structured prompt text, and input it into the pre-trained task model;

[0137] S3.2: The large-scale task model generates semantic feature vectors through an embedding layer, the expression of which is:

[0138]

[0139] in, Provide prompt text for structured tasks;

[0140] S3.3: The overall task is broken down into a sequence of sub-tasks with sequential execution logic using a classification reasoning network, and navigation constraints are extracted, including planning constraints such as obstacle avoidance priority, maximum speed limit, and priority passage area. The function calculates the constraint class probability, and its expression is:

[0141]

[0142] in, For the first Navigation constraints, For the first The classification probability value of the navigation constraint. To constrain the weight matrix of the classification network, As a bias term, the core constraints applicable to navigation planning are determined through probability screening;

[0143] S3.4: The large-scale task model constructs the logical dependencies between subtasks through thought chain reasoning, forming a directed acyclic task relationship graph, and determines the execution order of subtasks through topological sorting.

[0144] S3.5: Perform feasibility verification on the task sequence based on the environmental raster map information, remove unexecutable branches and adjust the task order. The feasibility verification score calculation expression is as follows:

[0145]

[0146] in, To determine the geometric feasibility of the task, For task completion rate, To estimate the execution time, , , As a weight for feasibility scoring;

[0147] S3.6: Introducing execution process feedback data to iteratively optimize the task plan, ensuring that the task sequence and navigation planning strategy align with actual operational and navigation requirements. The expression for calculating the optimization loss function value is as follows:

[0148]

[0149] in, For feedback penalty coefficient, , These are the initial generated and the optimized task plan vectors, respectively. To optimize and correct the task plan by minimizing the planned variation and execution deviation, the feedback deviation loss value is implemented.

[0150] S4: Collect environmental data through sensors to perform environmental perception and real-time map construction. The specific process is as follows:

[0151] S4.1: Collect environmental 3D point cloud data, RGB-D visual images, robot posture acceleration and angular velocity, drive wheel speed signals and contact force signals through LiDAR, depth camera, inertial measurement unit, wheel odometry and end force sensor respectively, and perform preprocessing.

[0152] The preprocessing includes:

[0153] (1) Radial distortion correction is performed on the RGB-D visual image. The main process is as follows:

[0154] ①Original pixel coordinates with normalized image coordinates The expression for parameter mapping is:

[0155]

[0156]

[0157] in, , These are the original coordinates of the pixel. , The horizontal and vertical focal lengths of the camera. , The coordinates of the principal point in the camera image;

[0158] ② Perform radial distortion correction on the image, the expression of which is:

[0159]

[0160]

[0161] In the formula, , and The radial distortion coefficient of the lens. Let be the radial distance from the pixel to the center of the image, and ;

[0162] ③ Map the corrected normalized coordinates back to pixel coordinates to obtain the final distortion-corrected pixel coordinates, the expression of which is:

[0163]

[0164]

[0165] in, These are the pixel coordinates after distortion correction;

[0166] (2) Outlier noise points are removed from the three-dimensional point cloud data using statistical filtering. The expression for identifying outlier points in the point cloud is as follows:

[0167]

[0168] in, For the point cloud neighborhood The spatial coordinate vector of a point, The coordinates are the mean coordinates. To count the number of neighboring points in a point cloud, the mean of the neighboring points is calculated, and point clouds exceeding a set threshold are identified as noise and removed.

[0169] (3) Perform zero-bias error compensation and attitude calculation on the robot's attitude acceleration and angular velocity to generate real-time roll, pitch and yaw attitude data of the robot;

[0170] (4) Perform kinematic integral calculation, time synchronization and cumulative error correction on the drive wheel speed signal to generate robot displacement and velocity data;

[0171] (5) The robot contact state is determined by using the composite contact force to analyze the contact force signal. The expression for the composite contact force is as follows:

[0172]

[0173] in, , and The contact force components of the end effector on the coordinate axis;

[0174] S4.2: Multi-source information fusion is performed on the preprocessed data using the Extended Kalman Filter algorithm. The expression for calculating the filter gain of the Extended Kalman Filter algorithm is as follows:

[0175]

[0176] in, The covariance matrix is ​​estimated for the state. For the observation noise matrix, The observation matrix;

[0177] The filter state update expression for the extended Kalman filter algorithm is:

[0178]

[0179] in, for The robot's optimal state vector updated by observation at each moment.

[0180] S4.3: Input the data collected by each sensor into the global map coordinate system to obtain the robot's real-time accurate pose in the working environment.

[0181] S4.4: Based on the fused pose information and environmental perception data, construct an occupancy grid map and a dynamic cost map, and perform dilation processing on the obstacle grid.

[0182] S4.5: Calculate the cost map grid passage cost based on grid occupancy probability and obstacle distance information, and divide different passage areas, including safe passage areas, restricted passage areas and prohibited passage areas;

[0183] The calculation expression for the access cost of the cost map grid is as follows:

[0184]

[0185] in, In a raster map The access value of a grid , For cost weighting coefficients, The probability of grid occupancy. This is a penalty for obstacle distance.

[0186] S5: Integrate A* algorithm or RRT* for initial path planning, search for the optimal reference path from the current pose to the target point, and use dynamic window method or time elastic band algorithm for local trajectory planning. Dynamically adjust the trajectory evaluation function weight according to the navigation mode specified by the large model of the task to select the local optimal trajectory.

[0187] The total cost function expression of the A* algorithm is:

[0188]

[0189] in, The actual cost from the starting point to the current node. The price of inspiration for Euclidean distance This is the semantic constraint penalty coefficient. This is a penalty item for violating semantic constraints;

[0190] The formula for calculating the total path cost of the RRT* algorithm is as follows:

[0191]

[0192] in, The obstacle distance penalty coefficient, The spatial distance from the waypoint to the nearest obstacle;

[0193] The trajectory evaluation function expression of the dynamic window method is as follows:

[0194]

[0195] in, and They are linear velocity and angular velocity, respectively. , and These are the evaluation items: trajectory speed, safe distance, and target orientation. , , Assigning weights to trajectory evaluation;

[0196] The selection of the optimal local trajectory involves selecting the optimal local trajectory that has no collision risk, fits the global reference path, and faces the target point. The formula for determining trajectory collision is as follows:

[0197]

[0198] In the formula, For the first [item] on the robot's motion trajectory item, For the environment The coordinates of each obstacle point This is the safe distance threshold.

[0199] S6: Calculate the desired rotational speeds of the left and right drive wheels based on the local optimal trajectory and the differential chassis kinematic model. The expression for the differential chassis kinematic model is as follows:

[0200]

[0201] in, and These are the rotational speeds of the left and right wheels, respectively. The wheelbase of the robot;

[0202] The tracking error between the actual pose and the planned path is eliminated by a PID controller. The control law expression of the PID controller is as follows:

[0203]

[0204] in, To control the output, , as well as These are PID control parameters;

[0205] Real-time collection of feedback information (dynamic environmental changes, robot pose deviation data, collision detection signals, and task progress feedback) and setting of automatic trigger conditions, namely, when the path is obstructed by obstacles, the tracking error exceeds a threshold, or the reward value falls below a set lower limit. When the time comes, the path replanning and task plan correction process is automatically triggered, and the parameters are dynamically adjusted based on the feedback information. The automatic triggering condition expression is as follows:

[0206]

[0207] in, This is a global path blocking indicator. This represents the actual overall operational error value. To trigger the error threshold for replanning, To enhance the cumulative reward value for learning;

[0208] The The cumulative reward value in reinforcement learning is calculated based on the immediate reward function of reinforcement learning, the expression of which is:

[0209]

[0210] in, for Constantly reinforce learning with immediate reward values. As a safety reward for obstacle avoidance, As a reward for path efficiency, This is a collision penalty.

[0211] like Figure 3 As shown, the composite robot navigation and path autonomous planning system based on a large task model includes the following modules:

[0212] Modeling module: Used to construct a coordinated kinematic / dynamic integrated model of the composite robot vehicle-arm, and to identify parameters with high precision;

[0213] Control module: Used to design reactive motion controllers based on the integrated kinematic / dynamic model of the composite robot's vehicle-arm coordinated coupling, and to achieve task-oriented coordinated control by combining behavior trees;

[0214] Task processing module: Used to receive task instructions and realize task understanding and requirement modeling;

[0215] Environmental perception module: used to collect environmental data and realize environmental perception and real-time map construction;

[0216] Path planning module: used for global path planning and dynamic execution, and to select the local optimal trajectory;

[0217] Execution feedback module: used for motion control and feedback adjustment.

[0218] The above embodiments are merely illustrative of the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solutions based on the technical concept proposed in this invention shall fall within the scope of protection of this invention.

Claims

1. A method for navigation and autonomous path planning of a composite robot based on a large-scale task model, characterized in that, Includes the following steps: Step 1: Construct a vehicle-arm coordinated kinematic or dynamic integrated model, define the world coordinate system, chassis coordinate system, robot arm base coordinate system and end effector coordinate system, derive the relationship between end effector pose and joint velocity through kinematic derivation, and identify the parameters; Step 2: Design a reactive motion controller based on the integrated kinematic or dynamic model of vehicle-arm coordinated coupling, and perform task-oriented coordinated control through behavior tree; Step 3: Perform task understanding and requirements modeling; Step 4: Collect environmental data through sensors to perform environmental perception and real-time map construction; Step 5: Integrate A* algorithm or RRT* for initial path planning, search for the optimal reference path from the current pose to the target point, and use dynamic window method or time elastic band algorithm for local trajectory planning. Dynamically adjust the trajectory evaluation function weight according to the navigation mode specified by the large model of the task to select the local optimal trajectory. Step 6: Calculate the expected rotational speeds of the left and right drive wheels to eliminate tracking errors between the actual pose and the planned path. Collect feedback information in real time and set automatic triggering conditions to automatically trigger the path replanning and task plan correction process. Adjust parameters dynamically based on the feedback information.

2. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 1, characterized in that, The vehicle-arm coordinated coupling kinematic or dynamic integrated model uses the world coordinate system as a reference. It establishes the conversion relationship between joint velocities and end effector velocities through the relative poses of the moving chassis, the relative pose of the robotic arm base, and the pose of the end effector. Its expression is as follows: Where {0} represents the world coordinate system, {b} represents the relative pose of the mobile chassis, {a} represents the relative pose of the robotic arm base, and {e} represents the robotic arm end effector. For joint velocity, This refers to the terminal velocity.

3. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 1, characterized in that, The reactive motion controller employs a quadratic programming (QP) approach to construct an optimization problem, with the following expression for the optimization variables: in, For the joint speed of the robotic arm, For chassis joint speed; The objective functions include: joint velocity cost, relaxation cost, and the objective of maximizing maneuverability, as well as the objective of optimizing the base-end orientation. Their expressions are as follows: The constraints include joint position and velocity limits, and their expressions are as follows: Wherein, the relaxation cost coefficient is the reciprocal of the Euclidean distance error between the end effector and the desired posture, Q is a quadratic term matrix, C is a linear term matrix, J is the Jacobian matrix of the integrated composite robot, and A and B are constraint matrices. , These are the upper and lower limits for joint variables.

4. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 1, characterized in that, The specific steps of step 3 are as follows: Step 3.1: Combine the natural language task instructions, scene semantic description, robot motion constraints and environmental prior information into structured prompt text, and input it into the pre-trained task model; Step 3.2: The large-scale task model generates semantic feature vectors through an embedding layer, the expression of which is: in, Provide prompt text for structured tasks; Step 3.3: Decompose the overall task into a sequence of subtasks with sequential execution logic using a classification reasoning network, and extract navigation constraints. The function calculates the constraint class probability, and its expression is: in, For the first Navigation constraints, For the first The classification probability value of the navigation constraint. To constrain the weight matrix of the classification network, For bias terms; Step 3.4: Construct logical dependencies between subtasks through thought chain reasoning, forming a directed acyclic task relationship graph, and determine the execution order of subtasks through topological sorting; Step 3.5: Perform feasibility verification on the task sequence based on the environmental raster map information, remove unexecutable branches and adjust the task order. The feasibility verification score calculation expression is as follows: in, To determine the geometric feasibility of the task, For task completion rate, To estimate the execution time, , , As a weight for feasibility scoring; Step 3.6: Introduce feedback data from the execution process to iteratively optimize the task plan. The expression for calculating the optimization loss function value is as follows: in, For feedback penalty coefficient, , These are the initial generated and the optimized task plan vectors, respectively. To calculate the feedback deviation loss value.

5. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 1, characterized in that, The specific implementation steps of step 4 are as follows: Step 4.1: Collect environmental 3D point cloud data, RGB-D visual images, robot posture acceleration and angular velocity, drive wheel speed signals and contact force signals through LiDAR, depth camera, inertial measurement unit, wheel odometry and end force sensor respectively, and perform preprocessing; Step 4.2: Perform multi-source information fusion on the preprocessed data using the extended Kalman filter algorithm; Step 4.3: Input the data from each sensor into the global map coordinate system to obtain the robot's real-time accurate pose in the working environment; Step 4.4: Based on the fused pose information and environmental perception data, construct an occupancy grid map and a dynamic cost map, and perform dilation processing on the obstacle grid. Step 4.5: Calculate the cost map grid passage cost based on the grid occupancy probability and obstacle distance information, and divide different passage areas.

6. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 5, characterized in that, In step 4.1, the preprocessing includes: (1) Radial distortion correction is performed on the RGB-D visual image. The main process is as follows: ①Original pixel coordinates with normalized image coordinates The expression for parameter mapping is: in, , These are the original coordinates of the pixel. , The horizontal and vertical focal lengths of the camera. , The coordinates of the principal point in the camera image; ② Perform radial distortion correction on the image, the expression of which is: In the formula, , and The radial distortion coefficient of the lens. Let be the radial distance from the pixel to the center of the image, and ; ③ Map the corrected normalized coordinates back to pixel coordinates to obtain the final distortion-corrected pixel coordinates, the expression of which is: in, These are the pixel coordinates after distortion correction; (2) Outlier noise points are removed from the three-dimensional point cloud data using statistical filtering. The expression for identifying outlier points in the point cloud is as follows: in, For the point cloud neighborhood The spatial coordinate vector of a point, The coordinates are the mean coordinates. To count the number of neighboring points in a point cloud, the mean of the neighboring points is calculated, and point clouds exceeding a set threshold are identified as noise and removed. (3) Perform zero-bias error compensation and attitude calculation on the robot's attitude acceleration and angular velocity to generate real-time roll, pitch and yaw attitude data of the robot; (4) Perform kinematic integral calculation, time synchronization and cumulative error correction on the drive wheel speed signal to generate robot displacement and velocity data; (5) The robot contact state is determined by using the composite contact force to analyze the contact force signal. The expression for the composite contact force is as follows: in, , and The contact force components of the end effector on the coordinate axis; In step 4.2, the expression for calculating the filter gain of the extended Kalman algorithm is as follows: in, The state estimates the covariance matrix. For the observation noise matrix, The observation matrix; The filter state update expression of the extended Kalman filter algorithm is as follows: in, for The robot's optimal state vector updated by observation at each moment; In step 4.5, the calculation expression for the cost map grid passage cost is as follows: in, In a raster map The access value of a grid , For cost weighting coefficients, The probability of grid occupancy. This is a penalty for obstacle distance. The access areas include: safe access areas, restricted access areas, and prohibited access areas.

7. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 1, characterized in that, In step 5, the total cost function expression of the A* algorithm is: in, The actual cost from the starting point to the current node. The price of inspiration for Euclidean distance This is the semantic constraint penalty coefficient. Penalties for violating semantic constraints; The formula for calculating the total path cost of the RRT* algorithm is as follows: in, The obstacle distance penalty coefficient, The spatial distance from the waypoint to the nearest obstacle; The trajectory evaluation function expression of the dynamic window method is as follows: in, and They are linear velocity and angular velocity, respectively. , and These are the evaluation items: trajectory speed, safe distance, and target orientation. , , Assigning weights to trajectory evaluation; The selection of the optimal local trajectory involves selecting the optimal local trajectory that has no collision risk, fits the global reference path, and faces the target point. The formula for determining trajectory collision is as follows: In the formula, For the first [item] on the robot's motion trajectory item, For the environment The coordinates of the obstacle points This is the safe distance threshold.

8. The method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 1, characterized in that, In step 6, the desired rotational speeds of the left and right drive wheels are calculated based on the local optimal trajectory and the differential chassis kinematic model. The expression for the differential chassis kinematic model is as follows: in, and These are the rotational speeds of the left and right wheels, respectively. The wheelbase of the robot; The tracking error between the actual pose and the planned path is eliminated by a PID controller, and the control law expression of the PID controller is as follows: in, To control the output, , as well as These are PID control parameters; The feedback information includes: environmental dynamic change information, robot pose deviation data, collision detection signals, and operation progress feedback information; The automatic triggering conditions are when the path is obstructed by an obstacle, the tracking error exceeds a threshold, or the reward value is lower than a set lower limit. This automatically triggers the path replanning and task plan correction process, and its expression is: in, This is a global path blocking indicator. This represents the actual overall operational error value. To trigger the error threshold for replanning, To enhance learning, accumulate reward points.

9. A method for navigation and autonomous path planning of a composite robot based on a large task model according to claim 8, characterized in that, The The cumulative reward value in reinforcement learning is calculated based on the immediate reward function of reinforcement learning, the expression of which is: in, for Constantly reinforce learning with immediate reward values. As a safety reward for obstacle avoidance, As a reward for path efficiency, This is a collision penalty.

10. A composite robot navigation and path autonomous planning system based on a large-scale task model, characterized in that, For implementing the method as described in any one of claims 1 to 9, comprising: Modeling module: Used to construct a coordinated kinematic / dynamic integrated model of the composite robot vehicle-arm, and to identify parameters with high precision; Control module: Used to design reactive motion controllers based on the integrated kinematic / dynamic model of the composite robot's vehicle-arm coordinated coupling, and to achieve task-oriented coordinated control by combining behavior trees; Task processing module: Used to receive task instructions and realize task understanding and requirement modeling; Environmental perception module: used to collect environmental data and realize environmental perception and real-time map construction; Path planning module: used for global path planning and dynamic execution, and to select the local optimal trajectory; Execution feedback module: used for motion control and feedback adjustment.

Images