Planning and control method for autonomous carrying of mobile manipulator and computer device

Abstract

The application discloses a kind of planning and control method and computer device of mobile manipulator autonomous handling, including based on planning domain definition language, the total handling task of mobile manipulator is decomposed into atomic action sequence containing navigation, operation and end operation;In response to the current atomic action in atomic action sequence, call fast exploration random tree algorithm in the redundant configuration space of mobile manipulator search discrete geometric path point sequence satisfying obstacle avoidance constraint;Discrete geometric path point sequence is carried out time parameterization processing, generates continuous reference trajectory, based on nonlinear model prediction control framework, trajectory tracking error, end pose keeping constraint, actuator physical constraint and the obstacle avoidance soft constraint constructed by environment obstacle distance field gradient are fused in objective function, the optimal control instruction of current time is solved by rolling horizon optimization, mobile manipulator is driven based on optimal control instruction, realize the efficient and safe handling of mobile manipulator in complex environment.

Description

Technical Field

[0001] This invention relates to the field of robotic arm technology, and in particular to a planning and control method and computer device for autonomous handling by a mobile robotic arm. Background Technology

[0002] Mobile robotic arms consist of a mobile chassis and a multi-degree-of-freedom robotic arm, combining wide-range mobility with precise manipulation capabilities. They have broad application prospects in warehousing and logistics, household services, and industrial manufacturing. Autonomous handling tasks performed by mobile robotic arms typically involve long sequences of multi-stage operations, encompassing navigation, grasping, and placement. The task logic is complex, and environmental constraints are diverse, placing extremely high demands on the robot's task planning, motion planning, and real-time control capabilities.

[0003] Traditional finite state machine (FSM) or behavior tree methods for mobile robotic arms suffer from poor logical generalization, state explosion, and disconnect between high-level semantics and low-level constraints when dealing with long sequences and complex cross-scene tasks, making it difficult to achieve automatic task derivation and hierarchical decoupling. Long-sequence handling tasks involve multiple stages of state transitions. Traditional FSM-based task planning methods are prone to state explosion when facing complex multi-stage tasks, and the disconnect between high-level semantic instructions and low-level environmental constraints can lead to execution failures. Mobile robotic arms have a high-dimensional redundant configuration space. Traditional low-dimensional path planning algorithms are prone to getting stuck in local optima or infeasible solutions in narrow spaces and complex obstacle environments, making it difficult to generate a global geometric path that satisfies obstacle avoidance constraints. In existing hierarchical control strategies, path planning and trajectory tracking are independent, lacking unified and coordinated optimization of multiple constraints such as end-effector attitude maintenance, actuator amplitude limiting, and dynamic obstacle avoidance. This results in insufficient trajectory tracking accuracy and large end-effector attitude fluctuations, making it difficult to meet the requirements of high-precision handling tasks. Summary of the Invention

[0004] This invention provides a planning and control method and computer device for autonomous handling by a mobile robotic arm, aiming to solve at least one of the technical problems existing in the prior art.

[0005] The technical solution of this invention is a planning and control method for autonomous handling by a mobile robotic arm, which includes:

[0006] Based on the planning domain definition language, the total handling task of the mobile robotic arm is decomposed into a sequence of atomic actions that include navigation, operation, and termination operations;

[0007] In response to the current atomic action in the atomic action sequence, the fast exploration random tree algorithm is invoked to search for a sequence of discrete geometric path points that satisfy the obstacle avoidance constraints in the redundant configuration space of the mobile robotic arm;

[0008] The discrete geometric path point sequence is time-parameterized to generate a continuous reference trajectory. Based on a nonlinear model predictive control framework, the trajectory tracking error, end-effector attitude maintenance constraint, actuator physical constraint, and obstacle avoidance soft constraint constructed by the distance field gradient of environmental obstacles are integrated into a unified optimization objective function. The optimal control command at the current moment is solved by rolling time-domain optimization, and the mobile robotic arm is driven based on the optimal control command.

[0009] According to some embodiments of the present invention, the decomposition of the total handling task of the mobile robotic arm into a sequence of atomic actions, including navigation actions, operation actions, and termination operation actions, based on the planning domain definition language, includes:

[0010] Based on the planning domain definition language, the dynamic changes from the initial state to the target state during the execution of the overall handling task of the mobile robotic arm are characterized by action, predicate and constraint elements, so as to formally model the overall handling task of the mobile robotic arm.

[0011] Within the formal modeling framework, the overall transport task of the mobile robotic arm consists of a series of atomic actions. Each atomic action corresponds to a specific operation of the mobile robotic arm in the task space. The types of operation actions include navigation actions, operation actions, and termination operation actions. Navigation represents the transition of the system state from the initial posture to the target posture. Operation represents controlling the mobile robotic arm to perform a specific task on the target object through action commands. Termination operation indicates that the mobile robotic arm has completed the interaction process with the target object, the task constraints are released, and the system expects the task space to return to a free state.

[0012] According to some embodiments of the present invention, it further includes:

[0013] A heuristic-based best-first search algorithm is used to solve the formal modeling based on the planning domain definition language. The best-first search algorithm starts from the initial state, takes a heuristic function and a cost limit as input, and initializes the state space through a priority queue.

[0014] During the initialization phase, the optimal priority search algorithm is used to evaluate whether the current state is the target state. If the target state has been reached, the optimal plan is returned immediately. If the target state has not been reached, the successor state is generated and added to the priority queue.

[0015] During subsequent node expansion, the optimal priority search algorithm continuously retrieves a new state from the priority queue and checks whether it is a failed state. If the expansion of the retrieved state fails, the retrieved state is marked as a dead node and skipped, and the next state is retrieved from the priority queue for expansion. When the priority queue is empty and the target state has not yet been reached, no solution is returned. If the expansion of the retrieved state is successful, the optimal priority search algorithm obtains the node corresponding to the current state.

[0016] According to some embodiments of the present invention, the heuristic function is defined as:

[0017]

[0018] in, This is the current state. The optimal planning path is obtained by solving a relaxation problem, which refers to finding a task path to the target state within a preset time by relaxing the environmental constraints in the planning domain definition language. It is the cost of the operator.

[0019] According to some embodiments of the present invention, the step of invoking the fast exploratory random tree algorithm to search for a sequence of discrete geometric path points in a redundant configuration space includes:

[0020] Sampling points are randomly generated in the free configuration space, or the target configuration is used as the sampling point with a preset probability;

[0021] Traverse the node set of the current search tree and find the existing node closest to the sampling point by measuring the Euclidean distance in the joint space. The existing node closest to the sampling point is defined as follows:

[0022]

[0023] in, The existing node that is closest to the sampling point. For sampling points, The joint space configuration of the mobile robotic arm;

[0024] Incrementally expand from the nearest existing node toward the sampling point with a preset step size to generate candidate new nodes, wherein the candidate new nodes are defined as:

[0025]

[0026] in, As a candidate new node, The existing node that is closest to the sampling point. For sampling points, Preset step size;

[0027] Using the forward kinematics model and simplified collision model of the mobile robotic arm, the system checks whether the local path from the nearest existing node to the candidate new node collides with environmental obstacles. The detection logic condition is as follows:

[0028]

[0029] in, For the positive kinematic mapping of the mobile robotic arm, As a heuristic regulator, As a candidate new node, The existing node that is closest to the sampling point. Areas with environmental obstacles;

[0030] If no collision occurs, the candidate new node and its connecting edges are added to the search tree.

[0031] According to some embodiments of the present invention, it further includes:

[0032] The sampling, nearest neighbor search, incremental growth, and collision detection operations are repeated until the candidate new node enters the neighborhood of the target configuration. Then, the termination condition is determined to be met. The termination condition is as follows:

[0033]

[0034] in, As a candidate new node, For the target configuration, This is the preset convergence threshold;

[0035] When the candidate new node reaches the neighborhood of the target configuration, the search tree is backtracked along the parent node pointers to extract a discrete geometric path point sequence connecting the start and end points. The discrete geometric path point sequence is as follows:

[0036]

[0037] in, It is a discrete geometric path point sequence. For the initial configuration, As a candidate new node, For the target configuration.

[0038] According to some embodiments of the present invention, the step of performing time parameterization processing on the discrete geometric path point sequence to generate a continuous reference trajectory includes:

[0039] The discrete geometric path point sequence is time-parameterized using cubic spline interpolation to generate a reference trajectory that is time-continuous and satisfies the continuity of higher-order derivatives.

[0040] The reference trajectory is used as the tracking benchmark of the nonlinear model predictive control framework. In each control cycle, the current state is obtained, and a multi-objective optimization problem is solved in the prediction time domain.

[0041] According to some embodiments of the present invention, it further includes:

[0042] Based on the system state vector, chassis state, mobile robot state, and control input vector of the mobile robot, a nonlinear model predictive control framework for the mobile robot is established. Within this framework, the continuous state equation of the mobile robot is:

[0043]

[0044] in, This represents the continuous state of the moving robotic arm. , These are the linear velocities of the left and right drive wheels, respectively. For heading angle, The chassis track. , These are the linear accelerations of the left and right drive wheels, respectively. This is the corresponding joint angular velocity vector. For control quantities of the mobile robotic arm;

[0045] The nonlinear model-based predictive control framework integrates trajectory tracking error, end-effector attitude maintenance constraints, actuator physical constraints, and obstacle avoidance soft constraints constructed from the distance field gradient of environmental obstacles in a unified optimization objective function, including:

[0046] The end-effector attitude constraint in the overall handling task of the mobile robotic arm is explicitly incorporated into the nonlinear model predictive control framework. The end-effector attitude constraint requires that the pitch and roll angle deviations of the end effector be kept within a given threshold during the handling process. The end-effector attitude is calculated using the forward kinematics of the mobile robotic arm, and the attitude constraint is expressed as:

[0047] , ,

[0048] in, and These are attitude functions related to roll angle and pitch angle, respectively. Given a threshold;

[0049] The mobile robotic arm satisfies a nonholonomic constraint on the chassis, which is expressed as follows:

[0050]

[0051] in, For the chassis nonholonomic constraint function, This represents the planar position of the chassis in the world coordinate system. For heading angle;

[0052] The actuator velocity and acceleration limiting constraints of the mobile robotic arm, as well as the singular configuration avoidance constraints, are expressed as follows:

[0053]

[0054] in, For the Jacobian matrix of the mobile robotic arm, For the preset safety singularity margin, and ;

[0055] The components of the mobile robotic arm are approximated as a set of collision spheres. The position of the center of each sphere in the world coordinate system is calculated using forward kinematics. The signed distance from any point to the nearest obstacle and its gradient are queried using a pre-constructed Euclidean signed distance field. The obstacle avoidance constraint is expressed as:

[0056] in, Forward kinematics, Let be the radius of the colliding sphere;

[0057] Introducing a relaxation barrier function transforms hard constraints into soft constraints, for constraints of the form The constraint, whose relaxation barrier function is:

[0058]

[0059] in, For obstacle parameters, The relaxation threshold, It is a quadratic smooth extension function below the threshold to ensure the continuous differentiability of the objective function near the constraint boundary.

[0060] According to some embodiments of the present invention, the step of solving for the optimal control command at the current moment through rolling time-domain optimization and driving the mobile robotic arm based on the optimal control command includes:

[0061] A nonlinear model predictive control framework acquires the current state in each control cycle, searches for the optimal control sequence in the prediction time domain, and uses constraint functions to cover end-effector attitude maintenance, actuator limiting, and obstacle avoidance requirements. The objective function is then minimized online, and its minimization is expressed as:

[0062]

[0063] in, To predict the time domain, The reference trajectory is generated by the front-end trajectory planner. It is a discrete geometric path point sequence. These are the weight matrices for state tracking error, control input penalty, and terminal error, respectively. That is, weighted quadratic form , Right now , Right now , For the first A relaxation barrier function, For the first One constraint function;

[0064] By applying only the first control variable from the optimization result at each sampling time and rolling the optimization forward over time, the nonlinear model predictive control framework generates a smooth and optimal control sequence that satisfies all constraints in real time in complex dynamic environments.

[0065] Take the control quantity corresponding to the current moment from the optimal control sequence, and generate the optimal control command based on the control quantity at the current moment;

[0066] The mobile robotic arm is driven based on the optimal control commands.

[0067] The present invention also relates to a computer device, including a memory and a processor, wherein the processor performs the above-described method when executing a computer program stored in the memory.

[0068] The present invention also relates to a computer-readable storage medium storing computer program instructions thereon, which, when executed by a processor, implement the above-described method.

[0069] The planning and control method and computer device for autonomous handling of mobile robotic arms provided in this invention embodiment have at least one of the following advantages or beneficial effects: by performing high-level semantic decomposition of the total handling task of the mobile robotic arm through a planning domain definition language, the complex multi-stage, cross-scene total handling task is structurally mapped into a logically coherent atomic action sequence of navigation, operation and termination operation, realizing automatic derivation of action sequence and decoupling between task level and motion control level, solving the problem of poor logical generalization of traditional state machines under complex tasks, and effectively avoiding execution failure caused by the disconnect between high-level semantic instructions and low-level environmental constraints.

[0070] For each atomic action call, a fast exploration random tree algorithm is used to search for global geometric paths that satisfy obstacle avoidance constraints in a redundant configuration space. This breaks through the bottleneck of traditional low-dimensional path planning, which is prone to getting stuck in local optima or infeasible solutions in narrow spaces and complex obstacle environments. It ensures the global reachability and completeness of discrete geometric path point sequences under obstacle avoidance constraints.

[0071] By performing time parameterization on the discrete geometric path point sequence, a reference trajectory with continuous position, velocity, and acceleration is generated, providing a trackable guidance signal for the nonlinear model predictive control framework. This framework integrates trajectory tracking error, end-effector attitude maintenance constraints, actuator physical constraints, and obstacle avoidance soft constraints constructed from the distance field gradient of environmental obstacles within a unified optimization objective function. This achieves coordinated optimization of multiple objectives and constraints, enabling closed-loop smooth tracking of the geometric path. By using rolling time-domain optimization to solve for the optimal control command at the current moment online, the system can adjust the coordinated motion of the base and the robotic arm in real time in a dynamically changing environment, balancing end-effector operation accuracy and overall obstacle avoidance safety.

[0072] Furthermore, additional aspects and advantages 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

[0073] Figure 1 This is a general flowchart of the planning and control method for autonomous handling by a mobile robotic arm provided in an embodiment of the present invention;

[0074] Figure 2 This is a multi-task search graph provided in an embodiment of the present invention;

[0075] Figure 3 This is a detailed flowchart of a planning and control method for autonomous handling by a mobile robotic arm provided in an embodiment of the present invention;

[0076] Figure 4 This is a detailed flowchart of step S200 in the planning and control method for autonomous handling by a mobile robotic arm provided in an embodiment of the present invention;

[0077] Figure 5 This is a simulation diagram of a long-sequence handling task of a mobile robotic arm provided in an embodiment of the present invention;

[0078] Figure 6 This is a schematic diagram of a mobile robotic arm transporting materials in a narrow, constrained space, provided in an embodiment of the present invention.

[0079] Figure 7 This is a schematic diagram of the simulation results of long task sequence planning in Scenario 1 provided by the embodiments of the present invention;

[0080] Figure 8 This is a schematic diagram of the parameters of the roll angle of the end effector after being controlled by the nonlinear model predictive control framework provided in this embodiment of the invention. Detailed Implementation

[0081] The following will provide a clear and complete description of the concept, specific structure, and technical effects of the present invention in conjunction with the embodiments and accompanying drawings, so as to fully understand the purpose, solution, and effects of the present invention.

[0082] It should be noted that, unless otherwise specified, when a feature is referred to as "fixed" or "connected" to another feature, it can be directly fixed or connected to the other feature, or indirectly fixed or connected to the other feature. The singular forms "a," "described," and "the" used herein are also intended to include the plural forms, unless the context clearly indicates otherwise. Furthermore, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used in this specification is for the purpose of describing particular embodiments only and not for limiting the invention. The term "and / or" as used herein includes any combination of one or more of the associated listed items.

[0083] It should be understood that although the terms first, second, third, etc., may be used to describe various elements in this disclosure, these elements should not be limited to these terms. These terms are used only to distinguish elements of the same type from one another. For example, a first element may also be referred to as a second element without departing from the scope of this disclosure, and similarly, a second element may also be referred to as a first element. Any and all instances or exemplary language (“e.g.,” “such as,” etc.) provided herein are intended only to better illustrate embodiments of the invention and, unless otherwise required, do not impose a limitation on the scope of the invention.

[0084] Reference Figures 1 to 8 As shown, the planning and control method and computer device for autonomous handling by a mobile robotic arm provided in the embodiments of the present invention will be further described.

[0085] Reference Figure 1 As shown, Figure 1 This is a flowchart illustrating the overall process of planning and controlling the autonomous handling of a mobile robotic arm according to an embodiment of the present invention. The planning and control method for autonomous handling of a mobile robotic arm includes, but is not limited to, steps S100 to S300. Specifically,

[0086] S100: Based on the planning domain definition language, the total handling task of the mobile robotic arm is decomposed into a sequence of atomic actions including navigation, operation and termination operations;

[0087] S200: In response to the current atomic action in the atomic action sequence, invoke the fast exploration random tree algorithm to search for a sequence of discrete geometric path points that satisfy the obstacle avoidance constraints in the redundant configuration space of the mobile robotic arm;

[0088] S300: The discrete geometric path point sequence is time-parameterized to generate a continuous reference trajectory. Based on the nonlinear model predictive control framework, the trajectory tracking error, end-effector attitude maintenance constraint, actuator physical constraint, and obstacle avoidance soft constraint constructed by the distance field gradient of environmental obstacles are integrated in the unified optimization objective function. The optimal control command at the current moment is solved by rolling time domain optimization, and the mobile robotic arm is driven based on the optimal control command.

[0089] To address the problem of long-sequence handling tasks for mobile robotic arms in complex environments, this invention proposes a planning and control method for autonomous handling of mobile robotic arms that integrates task planning, motion planning, and model predictive control. First, the overall handling task of the mobile robotic arm is decomposed into a high-level semantic decomposition using a planning domain definition language. The complex multi-stage, cross-scene overall handling task is structured and mapped into a logically coherent sequence of atomic actions, including navigation, operation, and termination. This achieves automatic derivation of action sequences and decoupling between the task level and the motion control level, solving the problem of poor logical generalization of traditional state machines under complex tasks and effectively avoiding execution failures caused by the disconnect between high-level semantic instructions and low-level environmental constraints.

[0090] Based on this, a fast exploration random tree algorithm is used for each atomic action call. According to the geometric target generated by the high-level action instructions, a global geometric path that satisfies the obstacle avoidance constraint is searched in the redundant configuration space. This ensures the accessibility of the target point in complex environments and breaks through the bottleneck of traditional low-dimensional path planning, which is prone to getting stuck in local optima or infeasible solutions in narrow spaces and complex obstacle environments. It also ensures the global accessibility and completeness of the discrete geometric path point sequence under obstacle avoidance constraints.

[0091] Furthermore, by time-parameterizing the discrete geometric path point sequence generated by the fast exploration random tree algorithm, a reference trajectory with continuous position, velocity, and acceleration is generated. This solves the bottleneck that traditional sampled paths cannot be directly used for continuous trajectory tracking control, improves the motion quality of the system, and provides a trackable guidance signal for the nonlinear model predictive control framework. This nonlinear model predictive control framework integrates trajectory tracking error, end-effector attitude maintenance constraints, actuator physical constraints, and obstacle avoidance soft constraints constructed from the distance field gradient of environmental obstacles in a unified optimization objective function, achieving coordinated optimization of multiple objectives and multiple constraints.

[0092] Finally, by constructing a nonlinear model predictive control framework that integrates obstacle distance field gradient and soft constraint modeling, closed-loop smooth tracking of the geometric path is achieved. Redundant degrees of freedom are dynamically allocated between the base and the robotic arm to cope with real-time environmental disturbances. Compared to the shortcomings of traditional hierarchical control strategies where path planning and trajectory tracking are independent and constraint processing is fragmented, this invention integrates task execution, motion planning, and real-time control into the same optimization system. The optimal control command for the current moment is solved online through rolling time-domain optimization, enabling the system to adjust the coordinated motion of the base and robotic arm in real time in dynamically changing environments, balancing end-effector accuracy and overall obstacle avoidance safety. Simultaneously, the explicit embedding of actuator physical constraints effectively suppresses control saturation and joint over-limit, extending the lifespan of the mechanical system. The introduction of soft constraints based on obstacle distance field gradient transforms collision risk into a differentiable penalty term without compromising problem feasibility, improving the numerical stability and computational efficiency of the optimization solution. Therefore, this invention improves the task completion rate, motion smoothness, and autonomous safety of the mobile robotic arm in complex unstructured environments.

[0093] As can be seen, the embodiments of this invention transform complex handling tasks into solvable logical chains through a planning domain definition language, effectively solving the problem of decision-making logic discontinuity in traditional control methods when facing long-sequence tasks, and ensuring the autonomy and rationality of the entire operation process. Furthermore, by utilizing the probabilistic completeness of the fast exploration random tree algorithm globally, geometric guidance with obstacle avoidance feasibility is provided. Simultaneously, combined with the prediction and optimization mechanism of the underlying nonlinear model, real-time feedback is effectively used to correct model errors, actuator response lag, and external disturbances, keeping the end-effector pose tracking error within a small range and ensuring the stability of the handling task.

[0094] In some embodiments of the present invention, step S100 of the autonomous handling planning and control method for a mobile robotic arm, based on a planning domain definition language, decomposes the total handling task of the mobile robotic arm into a sequence of atomic actions including navigation actions, operation actions, and termination operation actions. This includes, but is not limited to, steps S110 to S120.

[0095] S110: Based on the planning domain definition language, the dynamic changes from the initial state to the target state during the execution of the overall handling task of the mobile robotic arm are characterized by action, predicate and constraint elements, so as to formally model the overall handling task of the mobile robotic arm.

[0096] S120: Under the formal modeling framework, the total handling task of the mobile robotic arm consists of a series of atomic actions. Each atomic action corresponds to a specific operation action of the mobile robotic arm in the task space. The types of operation actions include navigation actions, operation actions, and termination operation actions. Among them, navigation represents the transition of the system state from the initial posture to the target posture, operation represents the control of the mobile robotic arm to perform a specific task on the target object through action commands, and termination operation means that the mobile robotic arm has completed the interaction process with the target object, the task constraints are released, and the system expects the task space to return to a free state.

[0097] In this embodiment of the invention, a formal modeling of long-sequence tasks is performed based on a planning domain definition language. Specifically, the planning domain definition language, through elements such as actions, predicates, and constraints, characterizes the dynamic changes from the initial state to the target state during the execution of a mobile robotic arm task, achieving a high degree of abstraction and unified expression of task semantics and execution logic. Under the formal modeling framework, the task sequence consists of a series of atomic actions, each corresponding to a specific operation of the mobile robotic arm in the task space. It can be understood that each atomic action corresponds to key stages such as state transitions, object interactions, and task termination for the mobile robotic arm in the task space, forming a hierarchical and semantically complete task decomposition structure. Through systematic modeling of these atomic actions and their logical dependencies, a high-level task sequence can be generated at the planning layer, and further parsed into an executable continuous trajectory by the motion planning and control layer. In the proposed whole-body planning framework for the mobile robotic arm, this invention predefines three types of operational actions:

[0098] navigation : Indicates that the state of the moving robotic arm is changed from its initial posture. Transition to target attitude During this process, the mobile robotic arm does not need to physically interact with the environment; its kinematic structure... It remains unchanged.

[0099] operate The robot arm moves towards the target object under this action command. Perform specific tasks To ensure the accuracy and controllability of the interaction process, the expected task space is adjusted accordingly based on the task type. The constraint set is used to generate a full-body motion trajectory that meets the conditions.

[0100] End operation This operation indicates that the mobile robotic arm has completed its interaction with the environmental object. During the interaction process, the task constraints are released, and the system expects the task space to return to a free state, providing initial conditions for the next task cycle and realizing the state synchronization between the action layer and the task layer.

[0101] The three types of states defined above define the interaction between the mobile robotic arm and the environment. On the one hand, they determine the expected task space, which enables the construction of path search and trajectory optimization problems. On the other hand, they establish the rules for the transition of environmental states between multiple tasks, which enables the long sequence multi-task planning problem to be modeled as a graph search problem.

[0102] For each task, this embodiment of the invention describes it as a joint state constituted by the mobile robotic arm and the environment. The feasibility of state transitions is represented by an environment model constructed using a planning domain definition language. Multi-task search, such as... Figure 2 As shown, a green line connecting two states indicates a possible state transition, while a red line indicates a non-transition. All state transitions are reversible, forming an undirected graph.

[0103] Consider three states: State 1: The robotic arm is free, in the original environment; State 2: The robotic arm's end effector grasps the door, the door's state changes in the environment; State 3: The robotic arm grabs a beverage from the refrigerator, the refrigerator door is closed in the environment. Between these three states, states 1 and 2 can be navigated and manipulated to move the robot's robotic arm near the door and grasp it, thus constructing two desired task spaces. However, a state transition cannot be established between states 1 and 3 because the refrigerator door is closed in the environment; the robotic arm must first complete the task of "opening the door" before it can retrieve the beverage. For general long-sequence multi-task planning, long-sequence task planning search algorithms use heuristic functions to guide the search process to achieve efficient task planning.

[0104] Reference Figure 3 As shown, Figure 3 This is a detailed flowchart of a planning and control method for autonomous handling by a mobile robotic arm provided in an embodiment of the present invention. The planning and control method for autonomous handling by a mobile robotic arm also includes, but is not limited to, steps S400 to S420. Specifically,

[0105] S400: The best-first search algorithm based on heuristic search is used to solve the formal modeling based on the planning domain definition language. The best-first search algorithm starts from the initial state, takes the heuristic function and cost limit as input, and initializes the state space through a priority queue.

[0106] S410: During the initialization phase, the best-first search algorithm is used to evaluate whether the current state is the target state. If the target state has been reached, the optimal plan is returned immediately; if the target state has not been reached, the successor state is generated and added to the priority queue.

[0107] S420: In the subsequent node expansion process, a new state is continuously taken from the priority queue using the best-first search algorithm, and it is checked whether it is a failed state. If the expansion of the taken state fails, the taken state is marked as a dead node and skipped, and the next state is taken from the priority queue for expansion. When the priority queue is empty and the target state has not yet been reached, no solution is returned. If the expansion of the taken state is successful, the best-first search algorithm obtains the node corresponding to the current state.

[0108] This invention employs a heuristic-based best-first search algorithm to solve the formal model of the planning domain definition language. It fully leverages the domain knowledge guidance capability of heuristic functions, prioritizing the state expansion process towards the target region, effectively avoiding blind traversal of the state space and significantly reducing the search complexity of the high-dimensional discrete state space. By managing nodes to be expanded through a priority queue, the algorithm consistently concentrates computational resources on the state transition path with the currently optimal evaluation cost, and performs efficient pruning based on cost limits, greatly improving solution efficiency while ensuring the optimality of the planning result.

[0109] During the initialization phase, the current state is used to determine the objective. If the initial state satisfies the task objective, the optimal planning result is immediately returned, completely eliminating redundant search overhead. If the objective is not achieved, the system generates a successor state and adds it to the priority queue in an orderly manner based on heuristic evaluation values, ensuring that state expansion has a clear direction and hierarchy. During subsequent node expansion, the algorithm continuously extracts new states from the priority queue and performs failure state detection. Once a state that has failed to expand or is stuck in a dead end is identified, a no-solution judgment is immediately returned or a backtracking mechanism is triggered, effectively preventing unnecessary computational resources from being consumed on infeasible branches. This solution mechanism combines completeness and optimality, enabling the generation of high-quality atomic motion sequence plans for complex handling tasks of mobile robotic arms within a limited time. This significantly improves the solution efficiency and real-time response capability of the task planning layer, providing reliable upper-level decision support for the rapid execution of underlying motion control.

[0110] In some embodiments of the present invention, the heuristic function is defined as:

[0111]

[0112] in, This is the current state. The goal is to find the optimal planning path by solving a relaxation problem. The relaxation problem refers to finding a task path to the target state within a preset time by relaxing the environmental constraints in the planning domain definition language. It is the cost of the operator.

[0113] By solving a relaxation problem to obtain the optimal planning path, a heuristic function is constructed using the sum of the costs of each operator in the path. This effectively leverages the acceptability of domain knowledge to provide a compact and computable lower bound estimate for the best-first search. This heuristic function, while ensuring optimality, significantly reduces the blind expansion of the state space, improves the efficiency and convergence speed of task planning, and enables the mobile robotic arm to quickly obtain high-quality atomic action sequences under complex constraints.

[0114] In some embodiments of the present invention, the heuristic function is used not only for state evaluation but also for determining which nodes to expand, prioritizing the expansion of nodes with lower heuristic values ​​to accelerate the search process. During node expansion, the heuristic best-first search algorithm calculates the heuristic cost of each successor state to determine whether to continue expanding the path. Whenever a new state is generated, the heuristic best-first search algorithm updates the statistics of the expanded nodes and continues to generate new successor states. If a state is evaluated as a dead node, the heuristic best-first search algorithm marks it as a dead zone and stops expanding, avoiding wasting computational resources. Finally, when the search finds a path that satisfies the objective, the heuristic best-first search algorithm returns the optimal planned path. If no path that satisfies the objective is found, it returns no solution.

[0115] Obtain the node corresponding to the current state and evaluate whether the node needs to be reopened. The reopening of the node is determined by the cost of the current state and the existing cost of the node, ensuring that the search process does not lead to deadlock.

[0116] In one embodiment, the best-first search algorithm for long-sequence heuristic search based on the planning domain definition language is as follows:

[0117] Input: Initial state Heuristic functions Cost Limit

[0118] Output: Optimal planning Or there is no solution

[0119] Initialization: Priority Queue Search space ;

[0120] Set the current state The current path cost ;

[0121] Evaluate the initial state and generate the evaluation context. ;

[0122] If the current logical state already satisfies the final goal defined in the planning domain definition language, then

[0123] Return: From Reconstruction planning path ;

[0124] Generate successor nodes for the initial state and add them. ;

[0125] when If not empty, execute the loop:

[0126] Obtain the optimal state to be expanded from the priority queue;

[0127] if If the return fails, then

[0128] Return: Planning failed flag;

[0129] Get node information for the corresponding state ;

[0130] Compute node reopen flag

[0131] ;

[0132] If the node is new or If true, then

[0133] The system is notified to perform a state transition;

[0134] Update assessment count metrics ;

[0135] Reassess the current status:

[0136] ;

[0137] if If it is determined not to be a dead node, then

[0138] If the current state is the initial state, then perform search initialization;

[0139] otherwise

[0140] if If true, then

[0141] Restart the closed node;

[0142] Updated node reopening statistics ;

[0143] otherwise

[0144] Start a new node;

[0145] Close the current node and mark it as expanded;

[0146] If the task has a logical progression, then

[0147] Priority queue Perform reward weighting operations;

[0148] Generate the successor state and update the queue;

[0149] Update extended node statistics ;

[0150] otherwise

[0151] Mark dead nodes and record the total number of dead nodes;

[0152] otherwise

[0153] Skip the current node and continue to the next iteration of the loop;

[0154] Returns: No solution.

[0155] Understandably, the best-first search algorithm for long-sequence heuristic search based on the planning domain definition language performs the following steps: initialize the priority queue and search space, set the initial state as the current state, evaluate the initial state and generate an evaluation context; if the current state already satisfies the final goal defined by the planning domain definition language, then reconstruct and return the planned path from the search space; otherwise, generate a successor node for the initial state and add it to the priority queue.

[0156] Repeat the following steps until the priority queue is empty:

[0157] Retrieve the optimal state to be expanded from the priority queue; if retrieval fails, return a "no solution" flag.

[0158] Obtain the node information corresponding to the state, calculate the node reopening flag, wherein the reopening flag indicates that the closed node needs to be reopened because the current path has a better cost value;

[0159] If the node is a new node or the reopen flag is true, then notify the system to perform a state transition, update the evaluation count, and re-evaluate the current state;

[0160] If, upon reassessment, the current state is determined not to be a dead node, then:

[0161] If the current state is the initial state, then perform search initialization;

[0162] Otherwise, if the reopen flag is true, the closed node is reopened and the reopened node statistics are updated; if it is not true, a new node is opened.

[0163] Close the current node and mark it as expanded;

[0164] If the task has logical progress, perform a reward weight operation on the priority queue, generate the successor state and update the queue, and update the expansion node statistics at the same time; if there is no logical progress, mark it as a dead node and record the total number of dead nodes.

[0165] If a node is neither a new node nor has a reopen flag, or if it is reassessed as a dead node, then skip the current node and continue to the next round of the loop;

[0166] If the priority queue is empty and the target state has not been found, return a no-solution flag.

[0167] By introducing a node reopening flag mechanism, closed nodes can be reopened during the search process with a better path cost, avoiding the loss of the globally optimal path due to early suboptimal expansion and significantly improving the optimality of task planning. Simultaneously, by combining this with a heuristic function to guide the search to prioritize the expansion of low-cost nodes, the blind expansion of the state space is effectively suppressed, reducing computational overhead.

[0168] By employing a dead node identification and skipping mechanism, unsolvable branches that cannot reach the target state can be identified and pruned in a timely manner, preventing invalid nodes from occupying priority queue resources and accelerating the search convergence speed. The reward weight operation further assigns higher expansion priority to nodes with logical progress, enhancing the algorithm's guidance in long-sequence, multi-stage tasks.

[0169] When a feasible task sequence exists, the optimal plan can be returned within a finite number of steps; if no solution exists, a "no solution" flag is explicitly returned to avoid infinite loops. In summary, the task planning and search method proposed in this invention achieves efficient, reliable, and optimal generation of atomic action sequences in complex, long-sequence transport scenarios, providing logically correct task guidance for subsequent motion planning and low-level control.

[0170] Reference Figure 4 As shown, Figure 4 This is a detailed flowchart of step S200 in the planning and control method for autonomous handling by a mobile robotic arm provided in this embodiment of the invention. In step S200, the fast exploration random tree algorithm is called to search for a sequence of discrete geometric path points in the redundant configuration space, including but not limited to the following steps:

[0171] S210: Randomly generate sampling points in the free configuration space, or use the target configuration as sampling points with a preset probability;

[0172] S220: Traverse the node set of the current search tree and find the existing node closest to the sampling point by measuring the Euclidean distance in the joint space. The existing node closest to the sampling point is defined as:

[0173]

[0174] in, The existing node that is closest to the sampling point. For sampling points, The joint space configuration of the mobile robotic arm;

[0175] Incrementally expand from the nearest existing node toward the sampling point with a preset step size to generate candidate new nodes, wherein the candidate new nodes are defined as:

[0176]

[0177] in, As a candidate new node, The existing node that is closest to the sampling point. For sampling points, Preset step size; direction vector The normalization process ensures that the search tree expands into unknown regions at a controlled rate, effectively preventing the algorithm from getting out of control during exploration in complex spaces.

[0178] Generate candidate new nodes Afterwards, rigorous safety verification must be performed using a local planner, utilizing the forward kinematic mapping of the mobile robotic arm. Check the existing node that is closest to the sampling point. To candidate new node The formed local path segment Is it related to environmental obstacles? The overlap occurs, and the specific logical judgment conditions are shown in step S240.

[0179] S240: Using the forward kinematics model and simplified collision model of the mobile robotic arm, check whether the local path from the nearest existing node to the candidate new node collides with environmental obstacles. The judgment condition for the check is:

[0180] ,

[0181] in, For the positive kinematic mapping of the mobile robotic arm, As a heuristic regulator, As a candidate new node, The existing node that is closest to the sampling point. Areas with environmental obstacles;

[0182] If the local path segment is completely in a free region within the environment space. If the nonholonomic constraint of the mobile base is satisfied, then the candidate new node will be selected. Stored as a new node in the node set. and establish edges Store in edge set This completes a successful expansion of the search tree.

[0183] S250: If no collision occurs, add the candidate new node and its connecting edges to the search tree.

[0184] In this embodiment of the invention, a target bias mechanism is adopted in the sampling strategy. The target configuration is directly used as the sampling point with a preset probability to guide the search tree to converge quickly to the target region, which significantly shortens the global path search time. At the same time, the full-space random sampling capability is retained to ensure that the algorithm has probabilistic completeness and avoids getting trapped in local minima.

[0185] By measuring the Euclidean distance in the joint space to find the nearest neighbor node, and incrementally expanding along the direction pointing to the sampling point with a fixed step size, the generated candidate new nodes have clear directionality and controllable step size, effectively balancing the breadth of exploration and the convergence speed, and avoiding the path crossing obstacles due to excessive step size or the computational redundancy caused by excessive step size.

[0186] By using forward kinematic mapping and simplified collision models (such as envelope spheres) to continuously detect local paths, the safety and kinematic feasibility of the entire path segment are ensured by verifying logical judgment conditions, thus avoiding the risk of missing intermediate collisions while only detecting endpoints.

[0187] As can be seen, the fast exploration random tree algorithm proposed in this invention can quickly and reliably generate discrete geometric paths that satisfy obstacle avoidance constraints in the high-dimensional redundant configuration space of a mobile robotic arm, providing a safe and continuous reference trajectory benchmark for the underlying controller.

[0188] In some embodiments of the present invention, the planning and control method for autonomous handling by the mobile robotic arm further includes:

[0189] The sampling, nearest neighbor search, incremental growth, and collision detection operations are repeated until the candidate new node enters the neighborhood of the target configuration. Then, the termination condition is determined to be met. The termination condition is as follows:

[0190]

[0191] in, As a candidate new node, For the target configuration, This is the preset convergence threshold;

[0192] When the candidate new node reaches the neighborhood of the target configuration, the search tree is backtracked along the parent node pointers to extract a discrete geometric path point sequence connecting the start and end points. The discrete geometric path point sequence is as follows:

[0193]

[0194] in, It is a discrete geometric path point sequence. For the initial configuration, As a candidate new node, For the target configuration.

[0195] By setting a convergence threshold and using the Euclidean distance between candidate nodes and the target configuration as the termination condition, excessive constraints requiring precise arrival at the target point are avoided, significantly improving search efficiency while ensuring path feasibility. This threshold can be flexibly adjusted according to task accuracy requirements, balancing rapid convergence with accurate end-point localization.

[0196] Once the termination condition is met, the parent node pointers stored in the search tree are used for reverse backtracking to fully extract the discrete geometric path point sequence from the initial configuration to the target configuration. This backtracking method ensures the connectivity and collision-free characteristics of the path and eliminates the need for repeated collision detection, resulting in high computational efficiency. The generated path maintains continuity in the configuration space. This path not only avoids static obstacles in the environment but also provides a globally continuous reference trajectory flow for the underlying control, providing a reliable discrete benchmark for subsequent cubic spline interpolation to generate smooth reference trajectories, effectively bridging global path search and underlying trajectory tracking control.

[0197] In some embodiments of the present invention, in step S300 of the planning and control method for autonomous handling by a mobile robotic arm, the discrete geometric path point sequence is subjected to time parameterization processing to generate a continuous reference trajectory, including but not limited to steps S310 to S320. Specifically,

[0198] S310: Use cubic spline interpolation to perform time parameterization on the discrete geometric path point sequence to generate a reference trajectory that is time-continuous and satisfies the continuity of higher-order derivatives;

[0199] S320: Using the reference trajectory as the tracking benchmark of the nonlinear model predictive control framework, the current state is obtained in each control cycle, and a multi-objective optimization problem is solved in the prediction time domain.

[0200] In the trajectory optimization scheme based on the combination of cubic spline interpolation and nonlinear model predictive control, cubic spline interpolation is used to parameterize the discrete geometric path points generated by the fast exploration random tree algorithm in time. The generated reference trajectory has the continuity of position, velocity and acceleration, eliminating the broken line turning and acceleration abrupt change in traditional sampling paths. It provides a smooth and differentiable tracking reference for the underlying controller, effectively reducing the impact and vibration of the robotic arm during start-up, stopping and turning, and extending the service life of the system.

[0201] Using the aforementioned continuous reference trajectory as the tracking benchmark for the nonlinear model predictive control framework, the current system state is acquired within each control cycle, and an optimization problem integrating multiple objectives (tracking error, end-effector attitude maintenance, obstacle avoidance, and actuator limiting) is solved within a finite prediction time domain. Through rolling time-domain optimization and feedback correction, high-precision tracking of the reference trajectory is achieved. Even with modeling errors or external disturbances, the end-effector pose error can be controlled within a minimal range, ensuring the stability and safety of the handling task. It is evident that this method balances the geometric feasibility of the global path with the dynamic optimality of the local trajectory, significantly improving the operational quality of the mobile robotic arm in complex environments.

[0202] In some embodiments of the present invention, the planning and control method for autonomous handling by a mobile robotic arm further includes establishing a nonlinear model predictive control framework for the mobile robotic arm, specifically including:

[0203] Based on the system state vector, chassis state, mobile robot state, and control input vector of the mobile robot, a nonlinear model predictive control framework for the mobile robot is established. Within this framework, the continuous state equation of the mobile robot is:

[0204]

[0205] in, This represents the continuous state of the moving robotic arm. , These are the linear velocities of the left and right drive wheels, respectively. For heading angle, The chassis track. , These are the linear accelerations of the left and right drive wheels, respectively. This is the corresponding joint angular velocity vector. For control quantities of the mobile robotic arm;

[0206] To achieve high-precision trajectory tracking and end-effector attitude maintenance in autonomous handling tasks, this invention employs a nonlinear model predictive control method that integrates obstacle gradient information and task constraints. This method treats the differential chassis and the multi-degree-of-freedom mobile robotic arm as a coupled system, and handles multiple constraints such as system kinematics, actuator amplitude limiting, and safety obstacle avoidance within a unified optimization framework. The optimal control input is solved in real time through rolling time-domain optimization.

[0207] First, a nonlinear model predictive control framework for the mobile robotic arm is established, defining the system state vector of the mobile robotic arm as... Among them, chassis status , This represents the planar position of the chassis in the world coordinate system. For heading angle, These represent the linear velocities of the left and right drive wheels, respectively; and the state of the moving robotic arm. , These are the angle vectors of the six joints. The corresponding joint angular velocity vector is the control input vector. Among them, chassis control volume , These are the linear accelerations of the left and right drive wheels, respectively; and the control parameters of the robotic arm. , For the first angular acceleration of each joint, Given the chassis wheelbase, the continuous state equation for the mobile robotic arm can be obtained as follows:

[0208]

[0209] By establishing a unified continuous state equation that includes factors such as differential chassis linear velocity, angular velocity, and robotic arm joint states, the coupled kinematic relationship between the mobile chassis and the robotic arm is accurately characterized. This provides a high-fidelity predictive model for the nonlinear model predictive control of the mobile robotic arm. This model can explicitly handle chassis nonholonomic constraints and actuator dynamic responses, enabling the controller to achieve coordinated motion between the base and the mobile robotic arm during rolling optimization. This effectively improves trajectory tracking accuracy and anti-disturbance capability, while avoiding control deviations caused by model mismatch, ensuring the stability and reliability of the handling process.

[0210] In some embodiments of the present invention, in step S300 of the planning and control method for autonomous handling by a mobile robotic arm, based on a nonlinear model predictive control framework, trajectory tracking error, end-effector attitude maintenance constraints, actuator physical constraints, and obstacle avoidance soft constraints constructed from the distance field gradient of environmental obstacles are integrated into a unified optimization objective function, including but not limited to the following steps:

[0211] The end-effector attitude constraint in the overall handling task of the mobile robotic arm is explicitly incorporated into the nonlinear model predictive control framework. The end-effector attitude constraint requires that the pitch and roll angle deviations of the end effector be kept within a given threshold during the handling process. The end-effector attitude is calculated using the forward kinematics of the mobile robotic arm, and the attitude constraint is expressed as:

[0212] , ,

[0213] in, and These are attitude functions related to roll angle and pitch angle, respectively. Given a threshold;

[0214] The mobile robotic arm satisfies a nonholonomic constraint on the chassis, which is expressed as follows:

[0215]

[0216] in, For the chassis nonholonomic constraint function, This represents the planar position of the chassis in the world coordinate system. For heading angle;

[0217] The actuator velocity and acceleration limiting constraints of the mobile robotic arm, as well as the singular configuration avoidance constraints, are expressed as follows:

[0218]

[0219] in, For the Jacobian matrix of the mobile robotic arm, For the preset safety singularity margin, and ;

[0220] The components of the mobile robotic arm are approximated as a set of collision spheres. The position of the center of each sphere in the world coordinate system is calculated using forward kinematics. The signed distance from any point to the nearest obstacle and its gradient are queried using a pre-constructed Euclidean signed distance field. The obstacle avoidance constraint is expressed as:

[0221] in, Forward kinematics, Let the radius of the colliding sphere be . The signed distance to the nearest obstacle;

[0222] To stably handle a large number of inequality constraints in numerical optimization, a relaxation barrier function is introduced to transform hard constraints into soft constraints, such as those of the form... The constraint, whose relaxation barrier function is:

[0223]

[0224] in, For obstacle parameters, The relaxation threshold, It is a quadratic smooth extension function below the threshold to ensure the continuous differentiability of the objective function near the constraint boundary.

[0225] By explicitly incorporating end-effector attitude constraints into the nonlinear model predictive control framework of the mobile robotic arm, the roll and pitch angles of the end effector are calculated in real time using forward kinematics, and their deviations are controlled within a given threshold. This effectively ensures the attitude stability of the load during handling and avoids the risk of objects slipping or tipping over due to end-effector tilting. It is particularly suitable for high-precision scenarios such as handling in narrow passages or on desktops.

[0226] Strictly satisfying the chassis nonholonomic constraints ensures that the planned control commands conform to the actual kinematic laws of the differential chassis, eliminating slippage or infeasible commands caused by model mismatch and improving the feasibility of trajectory tracking. Simultaneously, by using singular configuration avoidance constraints to limit the determinant of the Jacobian matrix to above a safety margin, the dangerous situation of joint velocities tending to infinity when the robotic arm approaches singular configurations is effectively prevented, ensuring the safe operation of the system.

[0227] By combining the collision sphere model with the Euclidean signed distance field to construct obstacle avoidance constraints, the signed distance and its gradient from any point to the obstacle can be queried in real time. This enables the nonlinear model predictive control of the mobile robotic arm to actively move away from the obstacle during rolling optimization, achieving collaborative obstacle avoidance between the base and the robotic arm. Moreover, it eliminates the need for pre-planning of obstacle avoidance paths, thus enhancing environmental adaptability.

[0228] By introducing a relaxation barrier function, the aforementioned multiple hard constraints are uniformly transformed into a smooth differentiable penalty term in the objective function. By designing a quadratic extension function near the constraint boundary, the problem of the traditional interior point method being prone to no solution when the constraint is violated is solved. At the same time, the continuous differentiability of the objective function is guaranteed, and the robustness and convergence speed of nonlinear programming solutions are improved.

[0229] As can be seen, the multi-constraint fusion and obstacle relaxation function scheme proposed in this invention enables the mobile robotic arm to simultaneously meet multiple requirements such as end-effector posture maintenance, actuator amplitude limiting, singularity avoidance and real-time obstacle avoidance in a dynamically constrained environment, thus achieving high-precision and high-safety autonomous handling control.

[0230] In some embodiments of the present invention, in step S300 of the planning and control method for autonomous handling by a mobile robotic arm, the optimal control command at the current moment is obtained through rolling time-domain optimization, and the mobile robotic arm is driven based on the optimal control command. This includes, but is not limited to, the following steps:

[0231] A nonlinear model predictive control framework acquires the current state in each control cycle, searches for the optimal control sequence in the prediction time domain, and uses constraint functions to cover end-effector attitude maintenance, actuator limiting, and obstacle avoidance requirements. The objective function is then minimized online, and its minimization is expressed as:

[0232]

[0233] in, To predict the time domain, The reference trajectory is generated by the front-end trajectory planner. It is a discrete geometric path point sequence. These are the weight matrices for state tracking error, control input penalty, and terminal error, respectively. That is, weighted quadratic form , Right now , Right now , For the first A relaxation barrier function, For the first One constraint function;

[0234] By applying only the first control variable from the optimization result at each sampling time and rolling the optimization forward over time, the nonlinear model predictive control framework generates a smooth and optimal control sequence that satisfies all constraints in real time in complex dynamic environments.

[0235] Extract the control quantity corresponding to the current moment from the optimal control sequence, and generate the optimal control command based on the control quantity at the current moment;

[0236] The mobile robotic arm is driven by optimal control commands.

[0237] In this embodiment of the invention, the objective function integrates state tracking error, control input penalty, and a multi-constraint soft penalty term constructed from a relaxed obstacle function, and introduces terminal cost to ensure closed-loop stability. It can be understood that the first term of the objective function measures trajectory tracking accuracy, the second term suppresses abrupt changes in control input to ensure smooth motion, the third term uses RBF soft constraints to uniformly handle end-effector attitude maintenance, actuator amplitude limiting, and environmental obstacle avoidance requirements, and the terminal term ensures closed-loop stability.

[0238] By adjusting the weight matrix, a flexible trade-off can be struck between tracking accuracy and control smoothness, adapting to the dynamic performance requirements of different handling tasks. After solving for the optimal control sequence in the predicted time domain within each control cycle, only the first control term is applied, and the optimization proceeds forward with the system state. This mechanism utilizes real-time feedback to correct model mismatch and external disturbances, enabling the generated control commands to adapt to changes in complex dynamic environments while ensuring that all constraints (end-effector attitude, actuator limiting, obstacle avoidance) are explicitly satisfied. The relaxation obstacle function summation term in the objective function handles inequality constraints in a continuously differentiable manner, avoiding the predicament of no solution at the boundary in traditional hard-constraint optimization, ensuring the robustness of nonlinear programming solutions, and ultimately generating a smooth, safe, and highly accurate optimal control sequence to drive the mobile robotic arm.

[0239] In this embodiment of the invention, the multi-object trajectory optimization algorithm for a mobile robotic arm based on a nonlinear model predictive control framework is as follows:

[0240] Input: Reference trajectory Current state

[0241] Output: The optimal control command at the current moment.

[0242] Initialization: Loading the continuous state prediction model of the mobile robotic arm system Euclidean distance field map, constraint set and state tracking weight matrix Control input weight matrix Terminal weight matrix With the parameters of the relaxation barrier function ,in, For state vectors, To control the input vector, For the first Constraint functions, As a penalty factor, The relaxation threshold;

[0243] The loop executes while the task is not finished:

[0244] Constructing the finite-time optimal control problem: in the prediction time domain Define the objective function internally;

[0245] Solving the above nonlinear programming problem yields the optimal control sequence. ;

[0246] Take the control quantity corresponding to the current time in the sequence. ;

[0247] Will Applying to the mobile robotic arm system, the drive status is updated to ;

[0248] Waiting for the next control cycle, update time

[0249] return .

[0250] Understandably, cubic spline interpolation is used to perform time parameterization on the output discrete geometric path point sequence, generating a reference trajectory where both position and velocity are continuous. This serves as the tracking benchmark for the nonlinear model predictive control framework of the mobile robotic arm. Subsequently, within each control cycle, the current state is acquired. In the prediction time domain The objective optimization problem is to minimize the objective function in the inner solution form, where the constraint function is... Covering end-effector attitude maintenance, actuator limiting, and obstacle avoidance requirements, through obstacle relaxation functions. The constraint is transformed into a soft constraint. The optimal control sequence is obtained through online solution. Fetch the current instruction It is applied to a mobile robotic arm to achieve high-precision tracking control of the entire body.

[0251] In some embodiments of the present invention, the long-sequence handling planning and control algorithm for the mobile robotic arm is as follows:

[0252] Input: Overall objective of the transport task initial state Environmental map

[0253] Output: Driver execution instructions

[0254] Task Planning: Invoke the task planner of the planning domain definition language to plan the total handling task of the mobile robotic arm. Decomposed into atomic action sequences ;

[0255] Sequential execution of atomic actions: For each action in the sequence of atomic actions Execution loop:

[0256] Geometric path search: Determine the action Target configuration It then invokes the fast exploratory random tree algorithm to search for a sequence of discrete geometric path points that satisfy the obstacle avoidance constraints in the redundant configuration space. ;

[0257] Trajectory parameterization: using cubic spline interpolation to parameterize discrete point sequences Transform into a reference trajectory that is time-continuous and satisfies the continuity of higher-order derivatives. ;

[0258] Real-time closed-loop execution: when the current state Not achieved When the termination condition is met, execute:

[0259] Status feedback: Real-time acquisition of information from the base odometer and robotic arm encoder to update the current status. ;

[0260] Rolling time-domain optimization: The nonlinear model predictive control framework of the mobile robotic arm is invoked to construct an objective function in the prediction time domain that includes tracking error, end-effector attitude maintenance, singular configuration avoidance, and environmental obstacle avoidance penalty;

[0261] Command Solving and Issuance: Solving nonlinear programming problems and extracting the first term of the optimal control sequence. This is converted into motor control signals and sent to the hardware for execution.

[0262] Action Update: Determine the current atomic action. Complete, update the initial configuration and proceed to the next action. ;

[0263] Return to the original location; transport task complete.

[0264] The task planner, defined by a domain definition language, automatically decomposes macroscopic transportation goals into atomic action sequences such as navigation, operation, and termination operations, solving the problem of state explosion that traditional finite state machines are prone to in complex long-sequence tasks. The task layer and the motion layer are tightly integrated, ensuring consistency between high-level logic and low-level geometric constraints, thus avoiding execution failures caused by planning conflicts.

[0265] For each atomic action, a fast-exploration random tree algorithm is invoked to search for a discrete geometric path that satisfies obstacle avoidance constraints in the redundant configuration space, ensuring the probabilistic completeness of the global path. Subsequently, cubic spline interpolation is used to transform the discrete point sequence into a reference trajectory where position, velocity, and acceleration are all continuous, eliminating path inflections and providing a smooth benchmark for dynamic control.

[0266] During the real-time closed-loop execution phase, a nonlinear model predictive control framework is employed for rolling optimization. This framework integrates trajectory tracking error, end-point attitude maintenance, singular configuration avoidance, and Euclidean sign range field obstacle avoidance penalty terms to solve for the optimal control sequence that satisfies all constraints online. Only the first instruction is applied in each control cycle, and rolling correction is performed using state feedback. This effectively compensates for model mismatch and external disturbances, achieving high-precision tracking of the reference trajectory.

[0267] This algorithm achieves deep collaboration between task decision-making, path planning, and underlying control, significantly improving the robustness and operational quality of mobile robotic arms in completing long-sequence autonomous handling tasks in complex and constrained environments.

[0268] To verify the effectiveness of the proposed autonomous handling planning and control method for mobile robotic arms in complex scenarios, this invention utilizes the Gazebo physics simulation environment and the Rviz visualization platform to build typical operating scenarios and systematically verifies the logic of long sequence tasks, the efficiency of confined space planning, and the accuracy of end-effector pose tracking.

[0269] Simulation environment and experimental object settings:

[0270] The experiment designed two typical simulation scenarios:

[0271] Scenario 1 (Long sequence transport task): such as Figure 5 The environment shown includes a refrigerator, bottled objects to be transported, and a door. The task objective is for the robot's mobile robotic arm to autonomously complete the complete logical chain of "moving to the refrigerator, opening the refrigerator door, grasping the object, moving to the dining table, placing the object, and closing the refrigerator door."

[0272] Scenario 2 (Transportation in confined spaces): such as Figure 6 As shown, the environment includes the object to be moved and a narrow, height-limited table. The mobile robotic arm must navigate through a narrow passage with limited height and lateral space, while the end effector must maintain a high-precision horizontal orientation to prevent the container from tipping over.

[0273] Validation of the effectiveness of long sequence task planning methods:

[0274] In Experiment Scenario 1, for planning long-sequence tasks, the proposed task planning algorithm successfully solved the task of taking a can of cola out of the refrigerator and placing it on the dining table. Table 1 lists the planning results for different tasks. Figure 7 The results of this long sequence task were demonstrated in the simulation, covering multiple subtasks from the refrigerator handle to the dining table.

[0275]

[0276] Table 1

[0277] Simulation results demonstrate that the proposed long-sequence task planning algorithm successfully solves the task of retrieving a cola from the refrigerator and placing it on the dining table, proving its efficiency and flexibility in handling multi-constraint, long-sequence tasks. The algorithm exhibits good performance in both unconstrained tasks and tasks with geometric and orientation constraints, providing an effective path planning solution for practical robot operation tasks.

[0278] Comparison of confined space planning performance:

[0279] In Experiment Scenario 2, for transport in a narrow, constrained space, the proposed planning algorithm, the FR-RRT (First-Order Retraction Rapidly-exploring Random Tree) algorithm, and the BI-RRT-Connect (Bidirectional Rapidly-exploring Random TreeConnect) algorithm were compared and evaluated. All three algorithms are based on the MoveIt framework publicly implemented by Felix, ensuring the consistency and reproducibility of the experimental environment. Table 2 shows the attitude tolerances at different end points (…). and ) and desktop height ( Under the given conditions, the performance comparison results of various path planning algorithms are presented. Each set of data is the average of 20 independent runs of the algorithm. The comparison indicators include path search time, path length, and success rate (Succ). The target space type (Tgt Coord.) used by each algorithm during sampling is also labeled, including joint space and task space.

[0280]

[0281] Table 2

[0282] As can be seen from Table 2, the planning method proposed in this invention exhibits superior performance compared to the comparative algorithms under all test conditions.

[0283] High-precision pose tracking and obstacle avoidance control:

[0284] against Figure 6 In the narrow desktop obstacle scenario shown, we set the end effector pose constraint tolerance. We then use the planning and control method for autonomous handling by a mobile robotic arm proposed in this invention to generate an initial trajectory. Subsequently, we employ a nonlinear model predictive control method to perform real-time optimization and tracking control on the generated trajectory.

[0285] Gazebo simulation results are as follows: Figure 8As shown in the figure, this graph illustrates the changes in the roll and pitch angles of the end effector after applying RRT planning (dashed lines) and further applying a nonlinear model predictive control method. The left vertical axis represents the attitude error (roll, pitch) after applying the nonlinear model predictive control method, while the right vertical axis represents the attitude error when only fast exploratory random tree algorithm planning is used. Figure 8 The results show that without using nonlinear model predictive control, the attitude oscillations of the end effector are quite significant, especially in the pitch direction, where the maximum error reaches [missing value]. Left and right; in the roll direction, the error range also reaches After applying nonlinear model predictive control to optimize and control the trajectory, the attitude error of the end effector was effectively controlled within a certain range. Within this range, it shows a significant improvement effect, ensuring the trend of pitch angle change. Figure 8 The solid line represents the end effector attitude error after optimization using the nonlinear model predictive control method (left ordinate), while the dashed line represents the attitude error change when only the fast exploration random tree algorithm was used for planning (right ordinate). This demonstrates the high accuracy and stability of the end effector attitude. The nonlinear model predictive control method operates at a frequency of 50Hz, meeting real-time control requirements.

[0286] In summary, the simulation results fully verify that the nonlinear model predictive control method proposed in this paper can significantly improve the performance of end-effector pose control and achieve real-time optimization and stable tracking of the initial planned trajectory.

[0287] It should be understood that the method steps in the embodiments of the present invention can be implemented or carried out by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer-readable storage medium. The method can use standard programming techniques. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, if necessary, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. Furthermore, for this purpose, the program can run on a programmed application-specific integrated circuit (ASIC).

[0288] Furthermore, the procedures described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by the context. The procedures described herein (or variations and / or combinations thereof) may be executed under the control of one or more computer systems configured with executable instructions, and may be implemented by hardware or a combination thereof as code (e.g., executable instructions, one or more computer programs, or one or more applications) that commonly executes on one or more processors. The computer program comprises a plurality of instructions executable by one or more processors.

[0289] Furthermore, the method can be implemented in any suitable type of computing platform, including but not limited to personal computers, minicomputers, mainframes, workstations, networked or distributed computing environments, standalone or integrated computer platforms, or in communication with charged particle tools or other imaging devices, etc. Aspects of the invention can be implemented as machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, optical read and / or write storage medium, RAM, ROM, etc., such that it is readable by a programmable computer, and when the storage medium or device is read by the computer, it can be used to configure and operate the computer to perform the processes described herein. Furthermore, the machine-readable code, or portions thereof, can be transmitted via wired or wireless networks. The invention described herein includes these and other different types of non-transitory computer-readable storage media when such media comprises instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor. When programmed according to the methods and techniques described in the invention, the invention may also include the computer itself.

[0290] A computer program can be applied to input data to perform the functions described herein, thereby transforming the input data to generate output data stored in non-volatile memory. The output information can also be applied to one or more output devices, such as a display. In a preferred embodiment of the invention, the transformed data represents physical and tangible objects, including specific visual depictions of physical and tangible objects generated on the display.

[0291] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention, as long as they achieve the technical effects of the present invention by the same means, should be included within the scope of protection of the present invention. Within the scope of protection of the present invention, the technical solutions and / or implementation methods can have various modifications and variations.

Claims

1. A planning and control method for autonomous handling by a mobile robotic arm, characterized in that, include: Based on the planning domain definition language, the total handling task of the mobile robotic arm is decomposed into a sequence of atomic actions that include navigation, operation, and termination operations; In response to the current atomic action in the atomic action sequence, the fast exploration random tree algorithm is invoked to search for a sequence of discrete geometric path points that satisfy the obstacle avoidance constraints in the redundant configuration space of the mobile robotic arm; Based on the system state vector, chassis state, mobile robot state, and control input vector of the mobile robot, a nonlinear model predictive control framework for the mobile robot is established. Within this framework, the continuous state equation of the mobile robot is: , in, This represents the continuous state of the moving robotic arm. , These are the linear velocities of the left and right drive wheels, respectively. For heading angle, The chassis track. , These are the linear accelerations of the left and right drive wheels, respectively. This is the corresponding joint angular velocity vector. For control quantities of the mobile robotic arm; The discrete geometric path point sequence is time parameterized to generate a continuous reference trajectory. Based on a nonlinear model predictive control framework, the trajectory tracking error, end attitude maintenance constraint, actuator physical constraint, and obstacle avoidance soft constraint constructed by the distance field gradient of environmental obstacles are integrated into the unified optimization objective function. The optimal control command at the current moment is solved by rolling time domain optimization, and the mobile robotic arm is driven based on the optimal control command. The nonlinear model-based predictive control framework integrates trajectory tracking error, end-effector attitude maintenance constraints, actuator physical constraints, and obstacle avoidance soft constraints constructed from the distance field gradient of environmental obstacles in a unified optimization objective function, including: The end-effector attitude constraint in the overall handling task of the mobile robotic arm is explicitly incorporated into the nonlinear model predictive control framework. The end-effector attitude constraint requires that the pitch and roll angle deviations of the end effector be kept within a given threshold during the handling process. The end-effector attitude is calculated using the forward kinematics of the mobile robotic arm, and the attitude constraint is expressed as: ， ， in, and These are attitude functions related to roll angle and pitch angle, respectively. Given a threshold; The mobile robotic arm satisfies a nonholonomic constraint on the chassis, which is expressed as follows: ， in, For the chassis nonholonomic constraint function, , This represents the planar position of the chassis in the world coordinate system. For heading angle; The actuator velocity and acceleration limiting constraints of the mobile robotic arm, as well as the singular configuration avoidance constraints, are expressed as follows: ， in, For the Jacobian matrix of the mobile robotic arm, For the preset safety singularity margin, and ; The components of the mobile robotic arm are approximated as a set of collision spheres. The position of the center of each sphere in the world coordinate system is calculated using forward kinematics. The signed distance from any point to the nearest obstacle and its gradient are queried using a pre-constructed Euclidean signed distance field. The obstacle avoidance constraint is expressed as: ， in, Forward kinematics, Let be the radius of the colliding sphere; Introducing a relaxation barrier function transforms hard constraints into soft constraints, for constraints of the form The constraint, whose relaxation barrier function is: ， in, For obstacle parameters, The relaxation threshold, It is a quadratic smooth extension function below the threshold to ensure the continuous differentiability of the objective function near the constraint boundary.

2. The planning and control method for autonomous handling by a mobile robotic arm according to claim 1, characterized in that, The planning domain definition language decomposes the overall handling task of the mobile robotic arm into a sequence of atomic actions, including navigation actions, operation actions, and termination actions, including: Based on the planning domain definition language, the dynamic changes from the initial state to the target state during the execution of the overall handling task of the mobile robotic arm are characterized by action, predicate and constraint elements, so as to formally model the overall handling task of the mobile robotic arm. Within the formal modeling framework, the overall transport task of the mobile robotic arm consists of a series of atomic actions. Each atomic action corresponds to a specific operation of the mobile robotic arm in the task space. The types of operation actions include navigation actions, operation actions, and termination operation actions. Navigation represents the transition of the system state from the initial posture to the target posture. Operation represents controlling the mobile robotic arm to perform a specific task on the target object through action commands. Termination operation indicates that the mobile robotic arm has completed the interaction process with the target object, the task constraints are released, and the system expects the task space to return to a free state.

3. The planning and control method for autonomous handling by a mobile robotic arm according to claim 2, characterized in that, Also includes: A heuristic-based best-first search algorithm is used to solve the formal modeling based on the planning domain definition language. The best-first search algorithm starts from the initial state, takes a heuristic function and a cost limit as input, and initializes the state space through a priority queue. During the initialization phase, the optimal priority search algorithm is used to evaluate whether the current state is the target state. If the target state has been reached, the optimal plan is returned immediately. If the target state has not been reached, the successor state is generated and added to the priority queue. During subsequent node expansion, the optimal priority search algorithm continuously retrieves a new state from the priority queue and checks whether it is a failed state. If the expansion of the retrieved state fails, the retrieved state is marked as a dead node and skipped, and the next state is retrieved from the priority queue for expansion. When the priority queue is empty and the target state has not yet been reached, no solution is returned. If the expansion of the retrieved state is successful, the optimal priority search algorithm obtains the node corresponding to the current state.

4. The planning and control method for autonomous handling by a mobile robotic arm according to claim 3, characterized in that, The heuristic function is defined as follows: ， in, This is the current state. The optimal planning path is obtained by solving a relaxation problem, which refers to finding a task path to the target state within a preset time by relaxing the environmental constraints in the planning domain definition language. It is the cost of the operator.

5. The planning and control method for autonomous handling by a mobile robotic arm according to claim 1, characterized in that, The invocation of the fast exploratory random tree algorithm to search for discrete geometric path point sequences in the redundant configuration space includes: Sampling points are randomly generated in the free configuration space, or the target configuration is used as the sampling point with a preset probability; Traverse the node set of the current search tree and find the existing node closest to the sampling point by measuring the Euclidean distance in the joint space. The existing node closest to the sampling point is defined as follows: ， in, The existing node that is closest to the sampling point. For the joint space configuration of the mobile robotic arm, For sampling points; Incrementally expand from the nearest existing node toward the sampling point with a preset step size to generate candidate new nodes, wherein the candidate new nodes are defined as: ， in, As a candidate new node, Preset step size; Using the forward kinematics model and simplified collision model of the mobile robotic arm, the system checks whether the local path from the nearest existing node to the candidate new node collides with environmental obstacles. The detection logic condition is as follows: ， in, For the positive kinematic mapping of the mobile robotic arm, As a heuristic regulator, Areas with environmental obstacles; If no collision occurs, the candidate new node and its connecting edges are added to the search tree.

6. The planning and control method for autonomous handling by a mobile robotic arm according to claim 5, characterized in that, Also includes: The sampling, nearest neighbor search, incremental growth, and collision detection operations are repeated until the candidate new node enters the neighborhood of the target configuration. Then, the termination condition is determined to be met. The termination condition is as follows: ， in, For the target configuration, This is the preset convergence threshold; When the candidate new node reaches the neighborhood of the target configuration, the search tree is backtracked along the parent node pointers to extract a discrete geometric path point sequence connecting the start and end points. The discrete geometric path point sequence is as follows: ， in, It is a discrete geometric path point sequence. This is the initial configuration.

7. The planning and control method for autonomous handling by a mobile robotic arm according to claim 1, characterized in that, The step of performing time parameterization processing on the discrete geometric path point sequence to generate a continuous reference trajectory includes: The discrete geometric path point sequence is time-parameterized using cubic spline interpolation to generate a reference trajectory that is time-continuous and satisfies the continuity of higher-order derivatives. The reference trajectory is used as the tracking benchmark of the nonlinear model predictive control framework. In each control cycle, the current state is obtained, and a multi-objective optimization problem is solved in the prediction time domain.

8. The planning and control method for autonomous handling by a mobile robotic arm according to claim 3, characterized in that, The step of solving for the optimal control command at the current moment through rolling time-domain optimization, and driving the mobile robotic arm based on the optimal control command, includes: A nonlinear model predictive control framework acquires the current state in each control cycle, searches for the optimal control sequence in the prediction time domain, and uses constraint functions to cover end-effector attitude maintenance, actuator limiting, and obstacle avoidance requirements. The objective function is then minimized online, and its minimization is expressed as: ， in, To predict the time domain, The reference trajectory is generated by the front-end trajectory planner. It is a discrete geometric path point sequence. These are the weight matrices for state tracking error, control input penalty, and terminal error, respectively. That is, weighted quadratic form , Right now , Right now , For the first A relaxation barrier function, For the first One constraint function; By applying only the first control variable from the optimization result at each sampling time and rolling the optimization forward over time, the nonlinear model predictive control framework generates a smooth and optimal control sequence that satisfies all constraints in real time in complex dynamic environments. Take the control quantity corresponding to the current moment from the optimal control sequence, and generate the optimal control command based on the control quantity at the current moment; The mobile robotic arm is driven based on the optimal control commands.

9. A computer device comprising a memory and a processor, characterized in that, When the processor executes a computer program stored in the memory, it performs the method as described in any one of claims 1 to 8.

Images