A mobile manipulator cooperative motion planning method and system

Abstract

The application provides a mobile manipulator cooperative motion planning method and system, relates to the robot control technology field in the strategic emerging industry, and has the technical solution points that: a convex optimization sub-problem is formed by convexing a non-convex coupling optimal control problem of a mobile chassis and a manipulator, a high algorithm planning unit solves a planning control quantity, an executable control quantity is solved by a real-time execution unit according to the planning control quantity, and coordinated feedback is carried out according to the two-side deviation, so that the subsequent planning control quantity adapts to the executable constraint, the problems that the planning end and the execution end lack coordinated feedback and the planning result exceeds the execution capability in the existing centralized solution are solved, and the real-time performance and the control precision of the cooperative motion planning in the limited space are improved.

Description

Technical Field

[0001] This application relates to the field of robot control technology in strategic emerging industries, and more specifically, to a method and system for collaborative motion planning of a mobile robotic arm. Background Technology

[0002] As strategic emerging industries continue to develop towards high-end and intelligent directions, industries such as high-end equipment manufacturing and robotics are placing higher demands on the mobility, environmental adaptability, and real-time control capabilities of autonomous operating equipment in complex environments. Mobile robotic arms combine the spatial transfer capabilities of mobile platforms with the operational capabilities of robotic arms, enabling them to complete complex tasks such as movement, detection, operation, and obstacle avoidance in work areas that are difficult for traditional fixed industrial robots to cover. This is directly related to the high-end equipment manufacturing and robotics technologies within strategic emerging industries. Especially in intelligent inspection, underground utility tunnel maintenance, hazardous environment detection, and other intelligent operation scenarios in confined spaces, mobile robotic arms are gradually becoming an important technological carrier for achieving equipment intelligence and autonomy.

[0003] Mobile robotic arms typically consist of a mobile chassis and a multi-degree-of-freedom robotic arm. In the confined space operation scenarios described above, the mobile chassis needs to adjust its position and orientation within narrow passages, while the robotic arm's end effector needs to track a specified spatial trajectory to complete detection or manipulation tasks. The mobile chassis and the robotic arm are not two independent moving objects; there is a clear dynamic coupling relationship between them: changes in the mobile chassis's orientation affect the positioning accuracy of the robotic arm's end effector, and the reaction force generated by the robotic arm's movement affects the stability of the mobile chassis. Therefore, it is necessary to plan the coordinated motion of the mobile chassis and the robotic arm as a whole.

[0004] When planning the overall motion of a mobile chassis and robotic arm, it is necessary to simultaneously consider system dynamics, actuator capabilities, joint range of motion, and collision avoidance relationships between the robot and its environment, thus forming a high-dimensional, non-convex coupled optimal control problem. The collision avoidance constraints in the confined space and some dynamic relationships exhibit significant nonlinear or non-convex characteristics. If centralized nonlinear optimization is directly performed, the solution scale and computation time will increase significantly as the state dimensions of the mobile chassis and robotic arm and the prediction time domain increase, making it difficult to balance planning accuracy and real-time control requirements.

[0005] To reduce the difficulty of solving non-convex optimal control problems, existing technologies have developed a sequential convex optimization approach. This approach involves linearizing the non-convex constraints at the current reference trajectory and limiting the applicability of local approximations through trust region constraints. This transforms the original non-convex problem into a series of solvable convex optimization subproblems. By iteratively updating the reference trajectory and repeatedly solving the subproblems, the solution to the original problem is gradually approximated. This approach can transform complex non-convex optimal control problems into convex quadratic programming or second-order cone programming subproblems that can be handled by numerical solvers.

[0006] However, as robotic equipment further develops towards autonomy, real-time operation, and edge intelligence, the control systems of mobile robotic arms are no longer typically handled by a single computing device for all planning and control functions. Instead, they form heterogeneous computing architectures with different computing capabilities and real-time characteristics. For example, motion planning and complex optimization can be deployed on edge computing units with high computing power, while underlying servo control, actuator protection, and local safety control need to be deployed on real-time processing units with defined control cycles. Therefore, mobile robotic arms not only face the computational complexity of the non-convex optimal control problem itself, but also the coordination problem between the high-computing-power planning side and the real-time execution side.

[0007] Specifically, high-performance planning units typically solve for the planning control quantities based on the overall motion planning objective, while real-time execution units directly interact with the underlying actuators. They need to satisfy executable constraints that directly limit the feasible range of the actual control quantities, such as actuator amplitude limits, joint limits, and local safety boundaries. Furthermore, they need to update the control quantities in real-time, considering their own computation cycle and planning data communication latency. Therefore, the executable constraints actually used by real-time execution units can be more stringent than the control constraints used by high-performance planning units during the planning process. At the same time, limited real-time computing power restricts the solution complexity that real-time execution units can employ.

[0008] Under these heterogeneous computing power conditions, traditional centralized or unidirectional planning execution mechanisms face further problems. On the one hand, when solving convex optimization subproblems, high-computing-power planning units typically determine ideal control quantities according to the constraints in the planning model, and their solutions may exceed the range that the real-time execution unit can actually achieve in the current state. On the other hand, when the real-time execution unit restricts or adjusts the planning control quantities due to its own executable constraints, a deviation may occur between the resulting actual executable control quantities and the planned control quantities, and traditional planning execution architectures usually lack a coordination mechanism to allow this deviation to be applied to subsequent planning solutions.

[0009] When the inconsistency between the planned control quantity and the actual executable control quantity persists, the planning side may continue to generate control results that are difficult for the real-time execution unit to achieve. In a confined space, this further manifests as increased trajectory tracking error, decreased positioning accuracy of the robotic arm end effector, actuator saturation, or increased collision risk. If the size of the convex optimization subproblem is further increased or the number of iterations is increased in order to improve planning accuracy, the solution time may be prolonged. If the planning scale is excessively compressed in order to meet real-time requirements, the ability to handle complex collision avoidance constraints and chassis-robotic arm coupling relationships may be reduced.

[0010] In the prior art, Chinese patent application publication number CN122645321A discloses a fast trajectory planning method for a mobile robotic arm based on two-layer optimization. This method uses sequential quadratic programming at the upper layer to solve for terminal configuration and interception timing, and generates trajectories at the lower layer through chassis quadratic programming and robotic arm quadratic programming respectively. The chassis trajectory is used as a time-varying parameter input into the robotic arm quadratic programming. The chassis target point and time window are adjusted according to the end-point interception position residual, and the solution is repeated to achieve coordination between the chassis and robotic arm trajectories.

[0011] The aforementioned method reduces planning complexity through hierarchical decision-making and trajectory generation, as well as residual compensation between the chassis and the robotic arm. Its feedback is used to coordinate the two moving parts to achieve the end-effector interception target. However, it does not address the mismatch between planning control requirements and executable constraints on the execution side.

[0012] To address the aforementioned issues, existing technologies urgently need improvement. Summary of the Invention

[0013] The purpose of this application is to provide a method and system for collaborative motion planning of a mobile robotic arm, which has the advantages of improving the real-time performance and control accuracy of collaborative motion planning of a mobile robotic arm under heterogeneous computing power conditions.

[0014] This application provides a collaborative motion planning method for a mobile robotic arm, the technical solution of which is as follows: The non-convex coupled optimal control problem of the mobile chassis and robotic arm is transformed into a convex optimization subproblem. The control variables for the overall motion of the mobile chassis and robotic arm are set as planning control variables on the planning side and executable control variables on the execution side, respectively. Consistency constraints are established between the two. The high-computing-power planning unit solves for the planning control variables under the motion planning objective, and the real-time execution unit solves for the executable control variables based on the planning control variables under its executable constraints. The solutions on both sides are coordinated based on the consistency deviation between the planning control variables and the executable control variables. The executable control variables provide feedback to the subsequent solutions of the high-computing-power planning unit, so that the subsequently obtained planning control variables adapt to the executable constraints of the real-time execution unit and obtain executable control variables that satisfy the executable constraints.

[0015] Furthermore, this application proposes a method for transforming the non-convex coupling optimal control problem of the mobile chassis and robotic arm into a convex optimization subproblem, including the following steps: The non-convex coupled optimal control problem is linearized at the current reference trajectory to construct a convex optimization subproblem; The deviation of the solution to the convex optimization subproblem from the current reference trajectory is limited by the trust domain constraint; The reference trajectory is updated based on the solution to the convex optimization subproblem, and the linearization process is continued based on the updated reference trajectory to construct and solve the convex optimization subproblem.

[0016] Furthermore, this application also proposes that the steps for transforming the non-convex coupling optimal control problem of the mobile chassis and the robotic arm into a convex optimization subproblem include: When a convex optimization subproblem becomes infeasible due to excessively tight collision avoidance constraints, non-negative relaxation variables are introduced to relax the collision avoidance constraints from hard constraints to soft constraints. A penalty term for the non-negative relaxation variables is added to the cost function of the convex optimization subproblem to restore its feasibility.

[0017] Furthermore, this application also proposes a step for coordinating the solutions on both sides based on the deviation between the planned control quantity and the executable control quantity, including: In the convex optimization subproblem, an auxiliary variable representing the executable control quantity is introduced, and a consistency constraint is established between the auxiliary variable and the planned control quantity to transform the convex optimization subproblem into a decomposition form with consistency constraints.

[0018] Furthermore, this application also proposes that the step of coordinating the solutions on both sides based on the deviation between the planned control quantity and the executable control quantity includes: For the decomposition form with consistency constraints, a dual variable corresponding to the consistency constraints is introduced, and the dual term containing the dual variable and the penalty term with penalty parameters are incorporated into the cost function of the convex optimization subproblem to construct an augmented Lagrangian function. In each alternating iteration, the current executable control quantity and the current dual variable are first fixed so that the high-computing-power planning unit solves for a new planning control quantity. Then, the new planning control quantity just obtained by the high-computing-power planning unit in this iteration and the current dual variable that has not yet been updated are fixed so that the real-time execution unit solves for a new executable control quantity. The dual variable is updated based on the deviation between the planned control variable and the executable control variable, and the deviation is converged through iteration.

[0019] Furthermore, this application also proposes that the steps for the real-time execution unit to solve for the executable control quantity based on the planned control quantity under its executable constraints include: The real-time execution unit independently projects the combined quantity formed by the planned control quantity, dual variables, and penalty parameters into the feasible region defined by the executable constraints at each control moment, and determines the executable control quantity in a closed manner.

[0020] Furthermore, this application also proposes that the method further includes the following steps: The real-time execution unit determines the consistency residual based on the deviation between the planned control quantity and the executable control quantity, and continuously monitors the consistency residual for exceeding limits. When the consistency residual exceeds the preset residual threshold for a preset number of consecutive times, the task reassignment condition is determined to be met, and task reassignment is triggered.

[0021] Furthermore, this application proposes that the steps of task reassignment include: The task reassignment steps include: After triggering task reassignment, the high-computing-power planning unit shortens the prediction time domain and increases the state trust domain radius, control trust domain radius, and penalty parameter. The real-time execution unit activates the local controller and takes over execution using the local control sequence generated by the local controller. When the consistency residual is lower than the recovery threshold, the reallocation mode is exited, and the collaborative execution mode in which the real-time execution unit determines the executable control quantity based on the planning control quantity obtained by the high-computing-power planning unit is restored, and the prediction time domain before the shortening is restored.

[0022] Furthermore, this application also proposes that the method further includes the following steps: When the control data from the high-performance planning unit is delayed, the real-time execution unit extrapolates the state based on the current state, the control quantity from the previous cycle, and the dynamic model of the moving robotic arm. Based on the results of state extrapolation and the dual variable of the previous cycle, the control quantity of the previous cycle is corrected by local closed projection to generate the transition control quantity. Transitional control variables are executed before new control data arrives in the high-computing-power planning unit.

[0023] In addition, this application also provides a collaborative motion planning system for a mobile robotic arm, the technical solution of which is as follows: Includes high-performance planning units and real-time execution units; The high-computing-power planning unit is used to transform the non-convex coupled optimal control problem of the mobile chassis and the robotic arm into a convex optimization subproblem, and solve the planning control quantity under the motion planning objective. The real-time execution unit is used to solve for executable control variables based on the planned control variables under its executable constraints; The high-performance planning unit and the real-time execution unit are configured to coordinate the solutions on both sides based on the deviation between the planned control quantity and the executable control quantity, so that the executable control quantity can provide feedback to the subsequent solutions of the high-performance planning unit, so that the subsequently obtained planned control quantity can adapt to the executable constraints of the real-time execution unit and obtain an executable control quantity that satisfies the executable constraints.

[0024] As can be seen from the above, the mobile robotic arm cooperative motion planning method and system provided in this application transforms the non-convex coupled optimal control problem of the mobile chassis and the robotic arm into a convex optimization subproblem. This enables the high-computing-power planning unit to solve the planning control quantity, and the real-time execution unit to solve the executable control quantity based on the planning control quantity. Furthermore, it provides coordination feedback based on the deviations on both sides, so that the subsequent planning control quantity adapts to the executable constraints. This solves the problem of lack of coordination feedback between the planning end and the execution end and the planning result exceeding the execution capability in the existing centralized solution, thereby improving the real-time performance and control accuracy of cooperative motion planning in confined spaces. Attached Figure Description

[0025] Figure 1 This is a flowchart illustrating the collaborative motion planning method for the mobile robotic arm proposed in this application.

[0026] Figure 2 This is a schematic diagram of the architecture of the mobile robotic arm collaborative motion planning system proposed in this application. Detailed Implementation

[0027] The following description, in conjunction with the technical solution of this application, provides a clearer and more complete explanation of the relevant content. It should be noted that the embodiments described herein are only a part of the implementation methods of this application, and not all of them. Other implementation methods obtained by those skilled in the art based on the embodiments of this application without creative effort should also fall within the protection scope of this application. Furthermore, in the description of this application, the terms "first," "second," etc., are mainly for distinction and should not be construed as indicating relative importance.

[0028] Reference Figure 1 This application proposes a cooperative motion planning method for a mobile robotic arm, which includes: The non-convex coupled optimal control problem of the mobile chassis and robotic arm is transformed into a convex optimization subproblem. The control variables for the overall motion of the mobile chassis and robotic arm are set as planning control variables on the planning side and executable control variables on the execution side, respectively. Consistency constraints are established between the two. The high-computing-power planning unit solves the planning control variables under the motion planning objective, and the real-time execution unit solves the executable control variables based on the planning control variables under its executable constraints. The solutions on both sides are coordinated according to the consistency deviation between the planning control variables and the executable control variables. The executable control variables provide feedback to the subsequent solutions of the high-computing-power planning unit, so that the subsequent planning control variables are adapted to the executable constraints of the real-time execution unit and obtain executable control variables that satisfy the executable constraints.

[0029] In this embodiment, both the planning control variables and the executable control variables correspond to the overall motion of the mobile chassis and the robotic arm. Each corresponding control component has the same controlled object and physical meaning. They are used to represent the solution results on the planning side and the solution results on the execution side under executable constraints, respectively. Thus, the same convex optimization subproblem is decomposed according to the solution responsibilities of the planning objective and the executable constraints. The consistency constraint between the two types of variables is used to coordinate the solution of the same control task on both sides. This consistency constraint does not require that the control quantities obtained in each intermediate iteration be immediately equal; deviations for subsequent coordination are allowed during the iteration process.

[0030] The above decomposition differs from the division of labor in CN122645321A, which separately solves for the chassis trajectory and the robotic arm trajectory. In that document, the chassis control sequence and the robotic arm control sequence correspond to different moving parts, and their end-effector position residuals reflect the positional error of the combined motion of the two parts relative to the task target. In this embodiment, the planned control quantity and the executable control quantity correspond to the same control task of the overall motion, and the consistency deviation reflects the inconsistency between the solution results on the planning side and the solution results under execution constraints.

[0031] When the executable constraints on the execution side limit the planning control requirements, these constraints are fed back to the high-computing-power planning unit through the executable control quantities and their consistency deviation with the planned control quantities, participating in subsequent planning solutions. The planning side adjusts the subsequent planning control quantities accordingly to adapt them to the executable constraints on the execution side. Therefore, this embodiment, while retaining the overall coupled planning of the chassis and robotic arm, establishes a consistency coordination relationship between the control quantities on the planning side and the execution side. The specific coordination solution method can be implemented as follows.

[0032] It should be noted that the mobile robotic arm described in this embodiment can consist of a mobile chassis and a robotic arm. The mobile chassis can adopt a wheeled or tracked structure, and the robotic arm can have multiple degrees of freedom. There is a dynamic coupling between the mobile chassis and the robotic arm; the attitude changes caused by the chassis movement will affect the positioning accuracy of the robotic arm's end effector, and the reaction torque generated by the robotic arm movement will also affect the stability of the chassis. When operating in a confined space, the movements of both need to be planned collaboratively.

[0033] The non-convex coupling optimal control problem of a mobile chassis and a robotic arm can be used to describe the process of determining control variables under conditions such as dynamic constraints, actuator constraints, joint limits, and collision avoidance constraints. Collision avoidance constraints typically exhibit non-convex characteristics, and dynamic constraints can also be nonlinear. Directly solving this problem as a whole results in a computational burden that increases with the state dimension, making it difficult to meet the requirements of high-frequency control cycles.

[0034] In one specific implementation, a unified dynamic model can be performed on the overall system consisting of the mobile chassis and the robotic arm. The mobile robotic arm includes a mobile chassis and a multi-degree-of-freedom robotic arm mounted on the mobile chassis. The robotic arm has n degrees of freedom, for example, n can be 6 or 7. The mobile chassis can include chassis position, heading angle, and one or more chassis joint degrees of freedom.

[0035] The generalized coordinates of the mobile robotic arm are defined as follows: ; in, The generalized coordinates representing the mobile chassis can include the chassis's position in a plane. Heading angle And k chassis joint angles; Indicates the joint angle of the robotic arm; The generalized coordinate dimension of the mobile robotic arm.

[0036] The system state vector can be represented as: ; Control input can be represented as: ; in, This indicates the control input applied to the mobile chassis. This represents the control input acting on the robotic arm. Under the generalized force representation in this embodiment, The control input can be the torque of the corresponding drive joint.

[0037] In one implementation, the overall system consisting of the mobile chassis and the robotic arm can be described in Lagrangian form, and its continuous-time dynamic equations are:

[0038] in, For generalized coordinates, For generalized speed, For generalized acceleration, the point above and the two points above represent the first and second derivatives with respect to continuous time, respectively; Represents the symmetric positive definite inertia matrix; Represents the matrix of Coriolis force and centrifugal force; Represents the gravity term; This represents the Jacobian matrix corresponding to the end effector of the robotic arm; Indicates environmental contact force; superscript This indicates transpose.

[0039] The above continuous-time dynamic system is arranged according to the sampling period Discretization yields discrete state transition relationships:

[0040] in, The sampling period is Indicates the number of prediction time-domain steps. Represents the discrete time step in the prediction time domain; Indicated by state and control input As independent variable, according to sampling period Discretization yields the state transition function for the next time step.

[0041] For motion planning in the prediction time domain, the following cost function can be defined:

[0042] in, Indicates the first The reference state at each control moment. This represents the reference state at the end of the prediction time domain; , and Let be the corresponding symmetric positive semidefinite weight matrix, and The aforementioned cost function can be used to comprehensively evaluate trajectory tracking error, control input magnitude, and state deviation at the end of the prediction time domain.

[0043] The non-convex coupled optimal control problem can be further included with the following constraints.

[0044] The dynamic constraints can be expressed as: ; The actuator limiting constraint can be expressed as: ; Joint limiting constraints can be expressed as: ; Collision avoidance constraints can be expressed as: ; in, and These represent the lower and upper limits of the control input, respectively. and These represent the lower and upper limits of the joint position, respectively. Represents the minimum distance between two spatial sets. The generalized coordinates are The space occupied by the mobile robotic arm Represents a set of obstacles. This indicates the preset safe distance.

[0045] Since the state variables and control variables of the mobile chassis and the robotic arm are coupled to each other through the overall dynamic relationship, and the collision avoidance constraints and some dynamic relationships have nonlinear or nonconvex characteristics, the above-mentioned optimal control problem belongs to the nonconvex coupled optimal control problem.

[0046] Based on this, the non-convex coupled optimal control problem will be made convex, forming a convex optimization subproblem. The method of convexification is not uniquely limited. In one implementation, the dynamic constraints and collision avoidance constraints can be linearized at the current reference trajectory, and a trust region constraint can be introduced to limit the degree to which the convexification result deviates from the current reference trajectory, thus constructing a convex optimization subproblem. The convex optimization subproblem can be expressed as a convex quadratic programming problem or a second-order cone programming problem.

[0047] In another implementation, other suitable approximation methods for transforming non-convex problems into convex problems can be used. Through convexification, subsequent solutions can be completed in a shorter time using mature convex optimization methods.

[0048] A high-performance planning unit (HMU) can be a processing unit with high computing power, running on hardware such as edge computing platforms or industrial control computers. The HMU solves for the planning control quantity under a motion planning objective. The motion planning objective can be reflected in minimizing the overall cost of indicators such as tracking error, control energy consumption, and deviation from the reference trajectory. The planning control quantity can be understood as the ideal control quantity obtained by the HMU for solving a convex optimization subproblem.

[0049] A real-time execution unit (RTU) can be a real-time processing unit oriented towards low-level control, running on hardware such as a microcontroller or a real-time processor. The RTU solves for executable control variables based on the planned control variables under its executable constraints. Executable constraints can include actuator limits, joint limits, torque rate of change limits, and local safety constraints.

[0050] Compared to the constraints included in convex optimization subproblems, executable constraints can have a more rigorous form to reflect the actual capabilities of real-time execution units at the physical execution level. Executable control quantities can be understood as the actual control quantities that a real-time execution unit can issue based on the planned control quantities and its own executable constraints.

[0051] When there are differences between the executable constraints and the constraints used by the high-performance planning unit for solving the problem, a deviation may occur between the planned control quantity and the executable control quantity. This deviation can, to some extent, reflect the degree to which the ideal control quantity given by the high-performance planning unit exceeds the actual execution capability of the real-time execution unit.

[0052] The solutions on both sides are coordinated based on the deviation between the planned control quantity and the executable control quantity. The two sides can be understood as the side where the high-computing-power planning unit is located and the side where the real-time execution unit is located. Coordination can be manifested in that the deviation is not only used to correct the control quantity in the current cycle, but also to enable the executable control quantity to provide feedback to the subsequent solutions of the high-computing-power planning unit.

[0053] Through the above feedback, the high-performance planning unit can perceive the limiting effect of the executable constraints of the real-time execution unit on the current control result based on the executable control quantity obtained by the real-time execution unit and its deviation from the planned control quantity. In subsequent optimization processes, it can adjust the new planned control quantity according to this limiting effect. Thus, through the executable control quantity and its deviation formed under the influence of executable constraints, the impact of execution-side constraints is incorporated into the subsequent solution on the planning side.

[0054] In one implementation, coordination between the solutions on both sides can be achieved through consistency constraints and iterative updates. Specifically, auxiliary variables representing the executable control variables can be introduced into the convex optimization subproblem, and consistency constraints can be established between the auxiliary variables and the planned control variables.

[0055] Consistency constraints are used to establish a coordination relationship between planned control variables and executable control variables, which eventually converge. During the intermediate process of alternating iterations, there may be temporary deviations between the two. After the real-time execution unit determines the executable control variables under its executable constraints, it can update the dual variables corresponding to the consistency constraints based on the deviations between the two, so that the deviations participate in the coordination feedback in subsequent iterations.

[0056] The updated dual variables participate in the subsequent solution of the high-performance planning unit, causing it to tend to provide planning control variables that are closer to the feasible range of the real-time execution unit. The real-time execution unit then continues to determine new executable control variables under its executable constraints based on the new planning control variables. The two sides solve alternately, and the deviation gradually converges.

[0057] In another implementation, coordination can be achieved by using penalty or compensation terms for deviations to address the aforementioned consistency constraints. For example, deviation information from the previous cycle can be incorporated into the cost information used by the high-performance planning unit in its current solution, enabling the high-performance planning unit to proactively avoid generating planning control quantities that exceed executable constraints during the optimization process.

[0058] The common point is that the executable control quantity participates in the subsequent solution process of the high-computing-power planning unit through the deviation, so that the solution results on both sides are closer to each other, and the subsequent planning control quantity gradually adapts to the executable constraints of the real-time execution unit.

[0059] It should be noted that this embodiment does not impose a unique limitation on the specific form of convexity or the specific solution method for coordination. As long as the non-convex coupled optimal control problem can be transformed into a convex optimization subproblem suitable for iterative solution, and planning control variables and executable control variables for the overall motion are set in the convex optimization subproblem and consistency constraints are established between the two, and the deviation between the planning control variables and the executable control variables provides feedback to the subsequent solution of the high-computing-power planning unit, this scheme can be used to implement it.

[0060] Furthermore, it should be noted that the coordination of the solutions on both sides mentioned in this embodiment refers to ensuring that the solutions of the high-performance planning unit and the real-time execution unit influence each other under the influence of deviations. Coordination can manifest as first obtaining the planning control quantity and the executable control quantity separately, and then adjusting the solutions on both sides according to the deviation; or it can manifest as directly embedding the deviation information into the solutions on both sides. While the above methods may differ in processing order and data representation, they all utilize deviations to allow the executable control quantity to provide feedback to the subsequent solutions of the high-performance planning unit, ultimately obtaining executable control quantities that satisfy executable constraints.

[0061] In this manner, the high-performance planning unit and the real-time execution unit each undertake solution tasks adapted to their respective computing power characteristics. The high-performance planning unit solves the convex optimization subproblems related to the motion planning objective, while the real-time execution unit solves the problems related to executable constraints. A closed loop is formed between the two through deviations. After closed-loop coordination, the subsequently obtained planning control quantities adapt to the executable constraints of the real-time execution unit, and the obtained executable control quantities satisfy these constraints. This allows the collaborative motion planning of the mobile robotic arm in a confined space to balance the motion planning objective with real-time execution capability.

[0062] Since the dynamic coupling between the mobile chassis and the robotic arm and the collision avoidance requirements usually make the original optimization problem non-convex, the computational cost is large when solving it directly, and a one-time approximation may deviate from the actual feasible range. Therefore, this application adopts a successive approximation method to limit the convexification process.

[0063] Furthermore, in some preferred approaches, transforming the non-convex coupled optimal control problem into a convex optimization subproblem includes: The non-convex coupled optimal control problem is linearized at the current reference trajectory to construct a convex optimization subproblem; The deviation of the solution to the convex optimization subproblem from the current reference trajectory is limited by the trust domain constraint; The solution to the convex optimization subproblem is updated to the reference trajectory, and the convex optimization subproblem is constructed and solved based on the updated reference trajectory.

[0064] The current reference trajectory can serve as the linearization benchmark for the current cycle. The current reference trajectory can include the state trajectory and control trajectory within the prediction time domain. Upon initial execution, the current reference trajectory can be derived from the current state of the mobile robotic arm, a preset initial trajectory, or the solution obtained from the previous control cycle. In continuous control cycles, the solution result from the previous control cycle can be used as the initial reference trajectory for the current cycle, helping to reduce trajectory jumps between cycles and decrease the number of iterations.

[0065] In one specific implementation, a warm start method can be used to initialize the current control cycle. The optimal state trajectory and optimal control trajectory obtained in the previous control cycle are denoted as... The result after time step translation can be used as the initial reference trajectory for the current control cycle. Since the state of the moving robot usually changes continuously between adjacent control cycles, this process can keep the current reference trajectory close to the feasible trajectory of the previous control cycle.

[0066] As one embodiment, when the control period is 20ms, the SCP outer loop can typically reach the set trajectory change threshold within 2 to 3 iterations. The control period and number of iterations are only used to illustrate one implementation method and do not constitute a unique limitation.

[0067] Linearization at the current reference trajectory transforms non-convex constraints into locally linear constraints in the vicinity of the current reference trajectory. Both dynamic constraints and collision avoidance constraints in non-convex coupled optimal control problems can be used as linearization objects.

[0068] Specifically, the current reference state and the current reference control quantity are respectively denoted as... and For discrete dynamic relationships, it is possible to... Perform a first-order Taylor expansion at this point: ; in: ; ;

[0069] Therefore, the nonlinear dynamic relationship near the current reference trajectory can be approximately transformed into a linear dynamic relationship.

[0070] To avoid the solution obtained after linearization deviating far from the current reference trajectory and causing local approximation failure, the following trust region constraint can be further set: ; ; in, Let the radius of the state trust domain be , To control the radius of the trust domain, the deviations of the state and control variables relative to the current reference trajectory are defined. These can be set and adjusted separately, and their values ​​do not need to be identical; the infinity norm represents the maximum absolute value of each component in the corresponding deviation vector.

[0071] For collision avoidance constraints, in one implementation, obstacles can be represented using a symbolic distance field. For obstacles... Define its symbolic distance function as And select the first one on the robot surface sampling points .in, Indicates the obstacle index. This represents the index of the robot's surface sampling point. It corresponds to the generalized coordinates of the current reference trajectory. At this point, the signed distance function can be linearized: ; in, This represents the signed distance function with respect to the location of sampling points on the robot surface. gradient, Let represent the Jacobian matrix of the sampling points with respect to the robot's generalized coordinates, and it can be expressed as: ; Through the above chain relationship, the constraint of obstacle distance on the robot's spatial position can be transformed into a linear constraint on the generalized coordinates of the mobile chassis and the robotic arm.

[0072] Combining the linearized dynamic constraints, collision avoidance constraints, actuator limiting, state or joint limiting, and trust region constraints, we can obtain the convex quadratic programming subproblem corresponding to the current SCP iteration: ; satisfy: ; ; ; ; ; ; in, Represents the dynamic linearization constant term determined by the current linearization point; and Let these represent the constraint coefficient matrix and constraint boundary vector, respectively, determined based on the linearization result of the collision avoidance constraints at the current reference trajectory, so that the linearized collision avoidance constraints are expressed as: ; and Representing the state vectors respectively The lower and upper bounds of the allowed values ​​can include boundaries formed by joint limits and other state constraints.

[0073] The above form is only one specific implementation of convex quadratic programming. Under the same convexity relation, other convex optimization subproblems such as second-order cone programming can also be formed.

[0074] For dynamic constraints, the state transition relationships can be expanded at the current reference trajectory, allowing the relationship between the chassis state, robot joint state, and control variables to be approximated linearly in the vicinity of the current reference trajectory. For collision avoidance constraints, linearization can be performed at the current reference trajectory based on the distance between the obstacle and the robot surface sampling points, transforming the collision avoidance requirement into a local linear constraint in the vicinity of the current reference trajectory. The resulting convex optimization subproblem has a convex optimization structure and can be solved using convex optimization solvers such as OSQP or qpOASES.

[0075] The trust region constraint restricts the solution range of the convex optimization subproblem to the vicinity of the current reference trajectory, thereby controlling the applicability of the linearization approximation. The state trajectory deviation is limited, and the control trajectory deviation is also limited; both are set according to the scale of the corresponding variables and the accuracy of the local approximation, and can be adjusted separately during iteration. The trust region is an internal constraint of the algorithm, distinct from physical constraints such as actuator limiting and joint limiting.

[0076] After obtaining the solution to the convex optimization subproblem, this solution can be updated to a new reference trajectory. The updated reference trajectory can serve as the benchmark for the next round of linearization, allowing the next round of the convex optimization subproblem to be constructed at a position closer to the target solution. As iterations proceed, the changes in the reference trajectory can gradually decrease, and the solution to the convex optimization subproblem can tend to stabilize, thereby obtaining a trajectory that satisfies the dynamic constraints and collision avoidance constraints.

[0077] For example, when a mobile robotic arm approaches an obstacle, the current reference trajectory may be close to the obstacle. After linearizing the collision avoidance constraints at the current reference trajectory, the convex optimization subproblem can find feasible trajectories near the current reference trajectory. Trust region constraints can limit significant changes to the chassis and robotic arm during this round of solution. After the solved trajectory is updated to the new reference trajectory, the next round of linearization can continue at a path closer to the avoidable obstacle, thus allowing the mobile robotic arm to gradually avoid the obstacle.

[0078] In this way, the convexification process can be transformed from a single approximation to a successive iterative approximation, which helps to achieve a balance between computational complexity and solution accuracy. The resulting convex optimization subproblems can also serve as the basis for subsequent planning control variable solutions and collaborative control.

[0079] When the moving robotic arm approaches an obstacle, the current reference trajectory may be too close to the obstacle. In one iteration of sequential convex programming, the linearized collision avoidance constraints may be too tight near the current reference trajectory, making the current convex optimization subproblem infeasible. Infeasibility here means that the current convex optimization subproblem has no feasible solution near the current linearization point, not necessarily that the original non-convex coupled optimal control problem has no solution. If the current convex optimization subproblem has no feasible solution, the sequential convex programming iteration will be interrupted, and the planned control variables cannot continue to be generated.

[0080] Furthermore, in some implementations, when a convex optimization subproblem becomes infeasible due to excessively tight collision avoidance constraints, a non-negative relaxation variable is introduced to relax the collision avoidance constraints from hard constraints to soft constraints. A penalty term for the non-negative relaxation variable is added to the cost function of the convex optimization subproblem to restore the feasibility of the current convex optimization subproblem that was infeasible due to the collision avoidance constraints.

[0081] In one implementation, the collision avoidance constraint can be expressed as: ; Introducing nonnegative relaxation variables Then, it can be adjusted to: ; ; In another implementation based on the symbolic distance field, the original collision avoidance constraint can be expressed as: ; Introducing nonnegative relaxation variables Then, it can be relaxed to: ; in, This represents the non-negative slack variable corresponding to the current collision avoidance constraint.

[0082] And add a penalty for slack variables to the cost function, for example: ; Alternatively, a form with a penalty coefficient can be used: or ; ; in, This represents the penalty coefficient. By setting a larger penalty coefficient, the slack variables can be preferentially kept at zero, and only allowed to take non-zero values ​​to avoid the current convex optimization subproblem becoming infeasible due to overly tight collision avoidance constraints. This approach relaxes the collision avoidance constraints that would render the current convex optimization subproblem infeasible, but does not imply that a feasible solution necessarily exists under any other combination of constraints.

[0083] Whether the current convex optimization subproblem is infeasible can be determined by the solution results. In one implementation, the optimization solver can return an infeasibility flag. In another implementation, the current convex optimization subproblem can be determined to be infeasible if no solution that simultaneously satisfies the dynamic constraints, actuator limiting, joint limiting, and collision avoidance constraints can be found near the current reference trajectory. These determination methods can be considered as optional approaches.

[0084] By introducing nonnegative relaxation variables and penalty terms, the current convex optimization subproblem can be solved again. The reference trajectory can then be updated based on the solution results, and the next linearization construction and solution can continue. As iterations continue, the reference trajectory can gradually move away from the region where the collision avoidance constraint is too tight, the values ​​of the nonnegative relaxation variables can tend to zero, and the collision avoidance constraint can be restored to an approximately hard constraint state.

[0085] Therefore, the solvability of convex optimization subproblems can be restored without directly removing collision avoidance constraints, and this helps maintain the continuity of the subsequent planning process. This application does not impose restrictions on the specific methods of introducing non-negative relaxation variables, the specific form of the penalty term, or the numerical value of the penalty coefficient.

[0086] Once a convex optimization subproblem has been formed, if the subproblem still contains only a single control variable, there will be a lack of clear comparison between the planning control quantity obtained by the high-performance planning unit and the executable control quantity that the real-time execution unit can actually execute. The coordination between the solutions on both sides will also easily remain at the external processing level.

[0087] Furthermore, in some preferred approaches, auxiliary variables representing executable control quantities are introduced into the convex optimization subproblem. Consistency constraints are established between the auxiliary variables and the planned control quantities to transform the convex optimization subproblem into a decomposition form with consistency constraints.

[0088] When a mobile robotic arm is operating in a confined space, the high-computing-power planning unit usually focuses more on the overall motion target, obstacle avoidance relationships, and the coupled motion relationship between the chassis and the robotic arm, while the real-time execution unit usually focuses more on joint boundaries, driving capabilities, and local safety constraints.

[0089] Even if the convex optimization subproblem can be solved, the planned control variables may not be directly executed by the real-time execution unit. To express this difference within the problem structure, auxiliary variables can be added to the convex optimization subproblem.

[0090] Auxiliary variables represent executable control quantities, that is, the control results that the real-time execution unit can actually execute under its executable constraints. Auxiliary variables exist alongside the control quantities used to obtain the planning control quantities, and do not replace the constraints and costs related to the motion planning objective in the original convex optimization subproblem.

[0091] In this way, the control objectives on the planning side and the executable capabilities on the execution side can be placed in the same problem structure. The organization of auxiliary variables can be determined according to the discretization method; for example, they can correspond to the control input sequence in the prediction time domain or to the control quantities at each time step, and their dimension can be determined according to the number of control quantities.

[0092] Consistency constraints are established between auxiliary variables and planning control quantities to require that the auxiliary variables and planning control quantities maintain a corresponding consistency.

[0093] In one implementation, it can be required that the value of the auxiliary variable at each control time be consistent with the value of the planned control variable at the corresponding control time. Alternatively, other constraint forms that can express the consistency between the two can be used.

[0094] Due to the existence of consistency constraints, the difference between the planned control quantity and the executable control quantity can be clearly identified. When auxiliary variables are affected by executable constraints and cannot be completely consistent with the planned control quantity, the deviation between the two has a clear source and can serve as the basis for subsequent coordination of the solutions on both sides.

[0095] Executable constraints can be applied to auxiliary variables, allowing the actual execution capability of the real-time execution unit to enter the optimization problem, reducing the need to truncate external control variables after planning is complete.

[0096] By introducing auxiliary variables and establishing consistency constraints, the convex optimization subproblem is transformed from a single problem structure into a decomposition form with consistency constraints.

[0097] In a specific mathematical implementation, the planning control variables on the high-computing-power planning unit side can be denoted as... The auxiliary variables used by the real-time execution unit to represent the actual executable control quantities are denoted as... And establish the following consistent relationship: ; The consistency relationship here does not represent the consistency obtained in each intermediate iteration. and Both must be strictly equal; rather, the target relationship needs to be gradually satisfied through iterative coordination between the two sides. During the iteration process, the dual variables corresponding to the consistency constraints are updated using the deviation between the control variables on both sides.

[0098] Introduce auxiliary variables and in the original planning cost In addition to the cost of execution-side control Subsequently, the collaborative optimization problem can be expressed as the following decomposition form. Here, the execution-side cost is an explicitly added cost term, not merely a variable substitution that keeps the original objective function value unchanged: ; satisfy: ; ; ; ; ; ; in, and The lower and upper bounds of the executable control variables on the execution side are defined; in this closed-form implementation, these two bounds form independent non-empty intervals between each time step and each control component. The execution-side bounds can be more stringent than the planning-side control bounds. The above decomposition form also retains the aforementioned state trust domain, control trust domain, and initial planning state constraints, avoiding the omission of constraints of the current SCP subproblem during variable splitting.

[0099] This decomposition form simultaneously includes variables related to the planning control variables, auxiliary variables, and consistency constraints between them. Therefore, the convex optimization subproblem, which originally required a global solution, now has a structural basis for being split into planning and execution sides. The high-performance planning unit can handle the parts related to the planning control variables within this decomposition form, while the real-time execution unit can handle the parts related to the auxiliary variables. This decomposition form also provides operational objects for subsequent alternating solutions of the planning and execution sides, enabling the planning control variables to adapt to the executable constraints of the real-time execution unit, and allowing the executable control variables to feed back and influence the subsequent solutions of the high-performance planning unit.

[0100] When the mobile robotic arm simultaneously adjusts the chassis attitude and extends the robotic arm in a narrow passage, the convex optimization subproblem has been transformed into a decomposition form with consistency constraints through auxiliary variables and consistency constraints. At this time, the planned control quantity and the executable control quantity are still coupled to each other by the consistency constraints.

[0101] If the high-performance planning unit solves only according to the motion planning objective, the resulting planning control quantity may exceed the amplitude, torque boundary, or safety boundary of the field actuator. If the real-time execution unit truncates only according to its own executable constraints, the resulting executable control quantity may deviate from the overall motion planning objective. Therefore, it is necessary to continuously exchange deviation information between the two sides during the solution process.

[0102] Furthermore, in some preferred embodiments, for decomposition forms with consistency constraints, the dual terms corresponding to the consistency constraints and the quadratic penalty terms with penalty parameters can be incorporated into the cost function of the original convex optimization subproblem to construct an augmented Lagrangian function.

[0103] The planning and control sequence in the prediction time domain is denoted as... The executable control sequence is denoted as Consistency constraints The corresponding unscaled dual variable is denoted as The penalty parameter is denoted as The dual variables at each time step are stacked as follows: Then the augmented Lagrange function can be expressed as: ; in: ; ; This represents the cost associated with the overall motion planning objective from the perspective of the high-computing-power planning unit. This represents the cost associated with executable control variables on the real-time execution unit side.

[0104] Using the augmented Lagrangian form described above, the high-computing-power planning unit side and the real-time execution unit side can be respectively formed into corresponding subproblems, and solved alternately within a control cycle.

[0105] by This represents the ADMM iteration index within the current fixed SCP convex problem. First, fix the first round of updates. The executable control sequence of the next iteration and dual variables Solved by high-computing-power planning units and : ; And satisfy: ; ; ; ; The high-computing-power planning unit side solution retains the aforementioned state trust domain, control trust domain, and initial state constraints of the current control cycle, and still belongs to convex quadratic programming. The size of its decision variables increases with the dimension of the control variables and the prediction time domain. The specific dimension depends on whether the state variables are eliminated through dynamic constraints and the optimization parameterization method adopted; it can be solved by the CPU or GPU of the high-computing-power edge computing platform.

[0106] In one implementation dataset, the typical solution time for this subproblem is 2 to 4 ms. This data is used to illustrate the feasibility under one hardware configuration and does not limit the scope of protection of this application.

[0107] After obtaining the new planning control sequence for this round Then, fix the new sequence and the dual variables that have not yet been updated. The new executable control sequence is solved by the real-time execution unit. What is fixed here is... Instead of the previous iteration : ; satisfy: ; When the objective function of the execution side is separable with respect to each prediction time step, and the executable constraints are also independent between time steps, the execution side subproblem can be solved step by step. The following closed-form clamping formula is further applicable to diagonal quadratic weights and interval constraints where each control component is independent; this formula is not directly applicable to control rate of change coupling constraints across time steps.

[0108] In one specific implementation: ; When executing the side weight matrix When it is a diagonal matrix, define ,in .index Represents the components of the control vector. Indicates the prediction time step. These three terms represent different iteration rounds of ADMM: ; For the One prediction time step, let ,in for An identity matrix of order 1. Under the aforementioned diagonal weights and interval constraints, the executable control vector is: ; Among them, vector operators Component-wise clamping; scalar form defined as ,and .because and It is a diagonal positive semi-definite matrix. Each diagonal element Since all numbers are positive, the inverse matrix exists, and only component division is needed for calculation.

[0109] The first Writing each component separately yields an expression equivalent to the vector formula above: ; The above denominator is obtained from the first-order optimal condition of the objective function on the execution side: the unconstrained minimum point satisfies Therefore, under the parameter definition in this embodiment, the denominator should be... For other feasible regions, the minimization problem of the same quadratic objective in that feasible region should be solved; only when its structure allows for analytical solution does it fall under the closed-form solution referred to in this application, and general coupling constraints cannot be directly clamped by components.

[0110] After obtaining the planned control variable and the executable control variable, update the dual variable based on the deviation between them: ; The updated dual variables continue to participate in the next round of the high-computational-power planning unit side problem and the real-time execution unit side problem. Therefore, the executable constraints of the real-time execution unit first apply to... Then through The resulting deviation affects the dual variable, and through the updated dual variable, it acts inversely on the next round of planning control quantity solution for the high-computing-power planning unit.

[0111] Therefore, this implementation allows the restrictive effect of executable constraints on executable control quantities to be incorporated into subsequent planning solutions through control quantity deviations and updates to dual variables. As alternating solutions continue, the deviation between the planned control quantity and the executable control quantity can be gradually reduced, thereby enabling the planning-side solution results to gradually adapt to the actual executable capabilities of the real-time execution unit.

[0112] Since the real-time execution unit does not need to run multiple rounds of numerical optimization solvers in the above separable implementation, closed projection can be directly calculated through deterministic operations, resulting in a low computational burden on the real-time execution unit. In one specific implementation, projection can achieve computation times on the order of microseconds.

[0113] The executable control quantities obtained in the above manner can be used for subsequent consistency residual monitoring, or as a basis for local correction when the control data of the high-computing-power planning unit is late.

[0114] When a mobile robotic arm enters a shaft, pipe gallery, or confined maintenance space, both the chassis and the robotic arm may need to simultaneously avoid obstacles and adjust their attitude. If personnel or obstacles suddenly appear ahead, the high-performance planning unit needs to update its planning control variables. Under normal collaborative execution, the deviation between the planning control variables obtained by the high-performance planning unit and the executable control variables obtained by the real-time execution unit under its own executable constraints can be gradually reduced through alternating solutions and dual updates.

[0115] When the environment changes, sensing data is delayed, the sensing computing load increases, or the solution time fluctuates, the planning control quantity given by the high-computing-power planning unit may continuously fall outside the executable constraints of the real-time execution unit for multiple control cycles. If the real-time execution unit simply waits for the next round of planning results to continue tracking, the deviation may continue to accumulate, leading to control timeouts, actuator saturation, increased risk of robotic arm jitter or collisions. Therefore, in addition to deviation coordination, a monitoring and triggering mechanism for continuous deviation exceeding limits needs to be added.

[0116] Furthermore, in some preferred embodiments, the real-time execution unit determines the consistency residual based on the deviation between the planned control quantity and the executable control quantity, and continuously monitors the consistency residual for exceeding limits; when the consistency residual continuously exceeds the preset residual threshold for a preset number of times, it determines that the task reassignment condition has been met, and triggers task reassignment.

[0117] In one specific implementation, the consistency residual at the k-th control time can be expressed as: ; For a prediction time domain, the following can be used: ; This represents the overall consistency residual level for the current period, and is compared with a preset residual threshold. Compare them.

[0118] To suppress false triggering caused by single-cycle disturbances, a continuous over-limit count variable `failure_counter` is maintained across control cycles. The count is incremented when the residual for the current cycle exceeds the threshold; the count is reset to zero when the residual for the current cycle does not exceed the threshold. ; Therefore, the count only represents the number of consecutive periods exceeding the limit; after a period without exceeding the limit occurs, subsequent periods exceeding the limit are re-accumulated.

[0119] when: Task reassignment is triggered at certain times. The above 3 is only one specific implementation parameter, and the preset number of times can be set according to the control cycle, actuator response capability, environmental risk level, and communication fluctuation characteristics.

[0120] In the above approach, the deviation can be understood as the difference between the planned control variable and the executable control variable at the control variable level. During the alternating solution process described above, this deviation can be used to update the dual variable, gradually harmonizing the solutions on both sides.

[0121] In this embodiment, the deviation can also serve as a basis for determining whether the current plan of the high-computing-power planning unit can be safely executed by the real-time execution unit. The real-time execution unit is directly related to actuator limiting, joint limiting, safety distance, and drive response capability, and therefore can sense whether the planned control quantity exceeds its own tolerance range in each control cycle. The high-computing-power planning unit's own judgment on whether the plan is overly optimistic may be affected by its computational load and perception latency; monitoring by the real-time execution unit allows for timely acquisition of the judgment result through the control cycle.

[0122] Specifically, the real-time execution unit can calculate the deviation level between the planned control quantity and the executable control quantity after obtaining them in each control cycle, and compare this deviation level with a preset deviation threshold. There are several ways to determine the deviation level. For example, the difference between the planned control quantity and the executable control quantity at each moment in the prediction time domain can be used as the moment deviation, and the largest moment deviation can be selected as the deviation level for the current cycle. Alternatively, the deviations at multiple moments can be summarized to obtain the overall deviation level representing the degree of deviation throughout the entire prediction time domain. The specific quantification form of the deviation can be configured according to the controlled object, actuator constraints, and safety margin.

[0123] When the deviation level of the current cycle exceeds a preset deviation threshold, the real-time execution unit can record an over-limit state. To distinguish between instantaneous fluctuations and continuous capability mismatch, continuous over-limit monitoring can maintain an over-limit count that spans control cycles. If the deviation of the current cycle exceeds the threshold, the over-limit count can be increased; if the deviation of the current cycle does not exceed the threshold, the over-limit count can be decreased or reset to zero. As an option, the over-limit count is incremented when an over-limit occurs and decremented when no over-limit occurs, maintaining a value no lower than zero. Alternatively, the over-limit count is reset to zero whenever a period without an over-limit occurs, and then re-accumulated when an over-limit occurs again. These counting rules can all be used to characterize whether deviation over-limits occur continuously over time.

[0124] It is worth noting that a single deviation exceeding the limit may be caused by communication jitter, state measurement noise, brief computation delays, or instantaneous changes in obstacle detection. If a single deviation exceeds the limit and triggers task reassignment, the control mode may switch frequently, causing instability in the execution process. Reaching a preset number of consecutive deviations can reduce the probability of such false triggers. When the deviation count reaches the preset number, the real-time execution unit can consider that the planned control quantity cannot be accommodated by the executable constraints within multiple consecutive control cycles, and the current plan provided by the high-computing-power planning unit has exceeded the existing real-time execution capability. In this case, continuous deviation exceeding the limit can serve as a basis for judging capability mismatch.

[0125] The preset number of attempts can be configured based on the control cycle length, actuator response speed, scenario hazard level, and obstacle movement speed. In scenarios with short control cycles and high safety risks, the preset number of attempts can be set lower to expedite the reassignment process. In scenarios with frequent intermittent communication link jitter or high sensor noise, the preset number of attempts can be set higher to suppress false triggering. For example, the preset number of attempts can be configured to exceed the limit for three consecutive control cycles. This value is for illustrative purposes only and does not constitute the only limitation.

[0126] Continuous over-limit monitoring uses a counter that is maintained across control cycles: it increments by one when the limit is exceeded in the current cycle and resets to zero when the limit is not exceeded. Only when the count reaches a preset number of times does it indicate that the corresponding number of consecutive over-limits has occurred; the cumulative count that only decreases when the limit is not exceeded cannot be interpreted as the number of consecutive over-limits.

[0127] In this way, the deviation, besides coordinating the solution results between the high-performance planning unit and the real-time execution unit, can also form an anomaly detection channel. Even if the dual variable update is still in progress, the real-time execution unit can determine whether the current plan is continuously unexecutable based on whether the deviation continuously exceeds the limit. This helps to identify abnormal states in advance before the deviation accumulates to the point of affecting the stability of the entire machine or triggering a collision, and allows time margin for subsequent processing. Since the monitoring object comes from the control results already generated by the high-performance planning unit and the real-time execution unit, there is no need to add sensors or additional sensing links, which helps to reduce engineering implementation costs. Continuous over-limit monitoring can also achieve a balance between anomaly sensitivity and disturbance resistance, making the triggering of task reassignment have configurable judgment criteria.

[0128] After consecutive exceedances of consistency residuals trigger task reassignment, if the high-performance planning unit continues to solve the problem using the original prediction time domain and parameters, it may be difficult to quickly obtain planning control quantities that can be executed by the real-time execution unit. If the real-time execution unit pauses and waits for new planning results, control instructions may be interrupted, increasing the risk of collisions or shutdowns. Therefore, after triggering task reassignment, both the high-performance planning unit and the real-time execution unit need to enter a working mode that matches the current abnormal state.

[0129] Furthermore, in some preferred embodiments, when the consistency residual continues to exceed the limit and the task redistribution condition is met, the working mode of the high-computing-power planning unit and the real-time execution unit can be adjusted simultaneously.

[0130] For high-computing-power planning units, the current prediction time domain can be shortened, and the radius of the state trust domain can be increased accordingly. Control the radius of the trust domain and the penalty parameters used for consistency coordination .

[0131] In one specific implementation, the original prediction time domain N can be shortened to: ; Shortening the prediction time domain reduces the number of decision variables in the current convex optimization subproblem; increasing the radii of the state and control trust regions allows for greater adjustments to the solution relative to the original reference trajectory, while still maintaining approximation within the range applicable to linearization; increasing the penalty parameter increases the penalty weight of the deviation between the two control variables in the coordinated solution. However, parameter adjustments do not inherently guarantee faster convergence in all scenarios.

[0132] At the same time, the real-time execution unit can activate the local controller set locally, and the local controller generates a local control sequence, temporarily taking over the generation of control quantities while the high-computing-power planning unit obtains a planning result suitable for execution.

[0133] In one specific implementation, the local controller may employ a Model Predictive Path Integral Controller (MPPI), and may employ: The local prediction time domain. MPPI can generate multiple candidate control sequences through gradient-free sampling, and determine the local control sequence based on the future state corresponding to the candidate control sequence, target tracking error, obstacle distance, and control change costs.

[0134] In one specific embodiment, 500 control sequences can be sampled in a single local planning, and the temperature parameter can be set to 0.1; the above parameters are specific implementation parameters and do not constitute a limitation on the local controller.

[0135] In task reassignment mode, the consistency residual between planned control variables and executable control variables can continue to be calculated. When the consistency residual falls below the recovery threshold again and meets the preset continuous recovery conditions, the reassignment mode is exited.

[0136] In one specific implementation, it can be required that the consistency residual remain below the recovery threshold for 10 control cycles before exiting the task redistribution mode. Upon exiting, the collaborative execution mode of the real-time execution unit determining the executable control quantity based on the planning control quantity obtained from the high-computing-power planning unit is first restored, and then the original prediction time domain before the high-computing-power planning unit shortens is restored. This avoids prematurely switching back to normal mode before the planning control quantity has stabilized and returned to the executable range of the real-time execution unit.

[0137] The recovery threshold can be the same as the residual threshold used when task redistribution is triggered, or different entry and exit thresholds can be set according to the stability requirements of control mode switching.

[0138] In an embedded operating environment, the high-performance planning unit may fail to provide new planning control values ​​before the start of the current control cycle of the real-time execution unit due to process scheduling, increased sensing task load, or other competition for computing resources. If the real-time execution unit continues to wait for new planning control values, control output interruption may occur; if the control values ​​from the previous cycle are used directly, the control values ​​may become mismatched with the current state because the actual state of the moving robotic arm has changed.

[0139] In some preferred embodiments, a state predictor can be set in the real-time execution unit. When the control data from the high-performance planning unit is delayed, the real-time execution unit uses the current state as the starting point for state prediction and performs short-term state extrapolation by combining the control quantity from the previous cycle and the dynamic model of the moving robotic arm.

[0140] In one specific implementation, the current state of the mobile robotic arm obtained by the real-time execution unit can be used as the initial state for state extrapolation: ; And perform short-term extrapolation according to the following state prediction relationship: ; in, Indicates the first The extrapolated state for each short-term prediction step, with the initial value being the currently obtained true state. ; This indicates the executable control quantity retained from the previous cycle for this prediction step; The next prediction step state is obtained from the extrapolated state and control quantity through a discrete state transition function. The right-hand side and the left-hand side of the recursion use the same extrapolated state sequence.

[0141] The state predictor can also use a simplified dynamics model or a kinematic model, and can perform one or more short-time prediction steps based on the actual data delay time. For example, a two-step short-time state extrapolation can be used.

[0142] It should be noted that the extrapolated state is the basis for determining the current motion state of the moving robotic arm during the data delay period, and does not directly project the state variable itself as the control variable. The real-time execution unit can determine the motion state on which the current control correction is based based on the extrapolated state, and combine it with the control quantity retained from the previous cycle, the dual variable from the previous cycle, and the currently executable constraints to form the control quantity to be executed.

[0143] In one implementation based on the aforementioned ADMM coordination relationship, when the new planned control quantity has not yet arrived, the dual variable retained from the previous cycle can continue to be used, and the control quantity of the previous cycle can be locally corrected using the extrapolation state. Then, the obtained control quantity to be executed is projected onto the feasible domain defined by the executable constraints of the real-time execution unit to form a transitional control quantity.

[0144] Therefore, the processing chain under the data late state can be represented as follows: First, obtain the extrapolated state based on the current state, the executable control quantity of the previous cycle, and the local dynamic model; second, determine the basis for the current control correction based on the extrapolated state; then, combine the control quantity of the previous cycle and the dual variable of the previous cycle to form the control quantity to be executed, and perform local closed projection correction on the control quantity to be executed to generate the transition control quantity that satisfies the executable constraints.

[0145] Transitional control quantities are sent to the mobile robotic arm for execution before the arrival of new control data from the high-computing-power planning unit. After the arrival of the new planned control quantities, the real-time execution unit can exit the data delay compensation state and re-execute the normal executable control quantity solution based on the newly arrived planned control quantities.

[0146] In one embodiment, the cerebellar state predictor can perform a very short time-domain state extrapolation using the planning and execution information received in the previous cycle, and perform a local closed-loop projection correction in conjunction with the dual variable of the previous cycle, thereby generating a transitional control quantity while new data from the high-computing-power end has not yet arrived. This delay compensation mechanism is used to solve the data delay problem caused by communication or computation time jitter, and belongs to a different branch of exception handling than the task reallocation mechanism triggered by the long-term inability of the planning control quantity to be accommodated by the executable constraints.

[0147] In a more specific implementation, a complete SCP outer loop, bilateral coordinated inner loop, residual monitoring, and task redistribution can be performed according to a control cycle.

[0148] The input for each control cycle can include the current real state. Reference trajectory The solution results of the previous control cycle and the dual variables retained from the previous control cycle The output is the executable control quantity of the first actual execution cycle. .

[0149] The number of SCP outer loop iterations can be set: ; ADMM inner loop iteration count: ; And set penalty parameters Residual threshold Initial state trust domain radius and initial control trust domain radius The specific values ​​are for illustrative purposes only.

[0150] In the current control cycle, the planning and control sequence solved by the high-computing-power planning unit and sent to the real-time execution unit is denoted as... , Corresponding to the aforementioned planning control quantity At the start of the current control cycle, the high-computing-power planning unit can perform the following initialization: ; ; ; Then the SCP outer loop is executed. In each SCP iteration, within the current reference trajectory... Linearize the dynamic constraints and collision avoidance constraints to construct the current convex quadratic programming subproblem.

[0151] Perform the ADMM inner loop within each fixed SCP bump problem. Each round begins with a fixed... and Obtained from high-computing-power planning units and The new planning control sequence is then sent to the real-time execution unit.

[0152] Under the aforementioned diagonal weight and interval constraint implementation method, the real-time execution unit executes for each prediction time step: ; Real-time execution unit gets Then, updates will be made within this round. The new executable control sequence and dual variables are returned to the high-performance planning unit for the next round of ADMM iteration. Each component Each prediction time step When executing side weights Only then does the above equation degenerate into a pair. The interval projection.

[0153] After completing the set number of ADMM coordinations, the high-performance planning unit updates the reference trajectory based on the current solution results: ; And continue the next SCP convexity; if the change in the reference trajectory is less than the set threshold, the SCP outer loop in the current control cycle can also be terminated in advance.

[0154] After obtaining the executable control sequence within the current prediction time domain, the real-time execution unit will execute the first-cycle executable control quantity. It is then sent to the underlying driver for execution.

[0155] Meanwhile, the real-time execution unit according to Determine the level of consistency residuals in the current prediction time domain, and then... Compared with the preset residual threshold A comparison is made, and the failure count variable `failure_counter` is updated based on the comparison result. The first... The consistency residual at each control moment is: .

[0156] When the overall consistency residual of the current period exceeds the preset residual threshold, failure_counter is incremented by one; otherwise, failure_counter is cleared to zero.

[0157] When failure_counter reaches the preset number of times, it enters the task redistribution mode. The high-computing-power planning unit shortens the prediction time domain in the next control cycle, increases the state trust domain radius, control trust domain radius and penalty parameter respectively, and the real-time execution unit enables the local controller.

[0158] In one implementation using MPPI as a local controller, five rounds of local sampling updates can be performed, and the first-cycle control variable obtained by MPPI can be used to cover v0 obtained by the normal coordination process.

[0159] In redistribution mode, the consistency residuals of the coordination results on both sides are continuously monitored. When the residuals are below the recovery threshold and remain below it for 10 consecutive control cycles, the redistribution mode is exited, normal collaborative execution is restored first, and then the high-computing-power planning unit is notified to restore the original prediction time domain. The recovery condition is not replaced by triggering the count to zero.

[0160] The above process describes a specific nested execution method. This application does not require all implementations to use the same number of SCP iterations, ADMM iterations, redistribution counts, or MPPI iterations.

[0161] In implementations that meet the corresponding optimization conditions, the convergence and stability of SCP, ADMM, and the task redistribution switching process can also be described.

[0162] For SCP iterations, convergence should be analyzed by considering the differentiability of dynamics and collision avoidance constraints, the consistency and feasibility of local approximations, and the specific reference trajectory and trust region update rules. The reference trajectory sequence is denoted as: ; in, This represents the outer loop iteration index of the SCP.

[0163] ADMM inner loop uses This indicates an iterative index; the two should be distinguished.

[0164] The state and control trust domains respectively satisfy the local approximation requirements of the adopted SCP method, and can be denoted as: ; in, and These represent upper bounds that correspond to the locally approximate effective ranges of the state and control variables, respectively. The constraint eligibility conditions and the small trust region radius alone cannot guarantee that any SCP update rule converges to the KKT point of the original problem.

[0165] For a fixed SCP subproblem, the objective function with constraints on both sides is a closed, normally convex function. The unaugmented Lagrangian function has a saddle point. All subproblems exist and their optimal solutions are obtained. A fixed SCP is used. Under the aforementioned condition of iterating sequentially in two synchronized ADMM blocks, the original consistency residuals satisfy: ; in, and They represent the first The norm of the predicted time-domain programming control sequence and the executable control sequence obtained after each round of updates is calculated over the entire predicted time-domain stacked vector. This conclusion describes asymptotic convergence and does not require the residual to decrease monotonically in each round.

[0166] Under the above conditions, the objective function value The convergence rate tends to the optimal value of this fixed convex subproblem. This conclusion does not directly guarantee that each original iteration point will satisfy a specific convergence rate, nor does it guarantee that arbitrary accuracy can be achieved in a fixed 3 to 5 iterations.

[0167] For the task redistribution process, if a local controller with local stability is used and the mode switching meets the appropriate dwell time condition, the closed-loop control system composed of the normal cooperative mode and the task redistribution mode can be analyzed as a switching system, and the system state can be kept bounded by conditions such as the average dwell time.

[0168] The above conclusions are used to distinguish between the iterative convergence of the fixed convex subproblem and the actual real-time control process. Changes in the prediction time domain, adjustment of penalty parameters, data delays, and local controller takeover will change the solution or execution conditions, and the conclusions of the fixed synchronous ADMM cannot be directly applied; closed-loop stability also needs to be analyzed according to the specific controller and switching conditions.

[0169] To verify the feasibility of the above-mentioned cooperative motion planning method for mobile robotic arms and its planning and control effects in confined spaces, a set of specific simulation examples are as follows:

[0170] In this embodiment, the mobile robotic arm consists of a four-wheeled, legged mobile chassis and a 6-DOF (degrees of freedom) collaborative robotic arm, with a total system degree of freedom of 10. The simulation environment is set as a confined passage with a width of 1.2m and a length of 8m. Five to eight columnar obstacles with diameters of 0.08m to 0.15m are randomly placed within the passage.

[0171] The task is to control a mobile robotic arm to move from a starting point A to an ending point B, a distance of approximately 6 meters. Simultaneously, the robotic arm's end effector must track a reference curve in three-dimensional space to simulate a contact-based inspection task during the movement of a mobile platform. During the movement, collisions between the mobile chassis and obstacles, collisions between the robotic arm and obstacles, and self-collisions between the mobile chassis and the robotic arm must be avoided.

[0172] In a set of test parameters, the control period was set to 20ms, the prediction time domain was set to 20, the number of SCP outer loop iterations was set to 3, and the number of ADMM inner loop iterations was set to 5.

[0173] To compare the effects, the following four methods can be set.

[0174] The first method is the centralized NMPC method, which treats the mobile chassis and robotic arm as a unified nonlinear system, uses nonlinear model predictive control and utilizes the IPOPT solver for centralized solution, without decomposing computing power between high-computing-power planning units and real-time execution units.

[0175] The second method is the decoupled planning method, which plans the mobile chassis and the robotic arm separately. The mobile chassis uses MPC to track the global path, and the robotic arm uses RRT-Connect combined with fifth-order polynomial interpolation to form the robotic arm's motion trajectory. Then, the results from both sides are spliced ​​together in time.

[0176] The third method is a global-local two-layer method, which uses a global planner and a local MPC planner, but does not use the SCP convexity and ADMM coordination based on the planning control quantity and the deviation of the executable control quantity of this application.

[0177] The fourth method is the implementation method of this application, namely, using the SCP outer loop, the ADMM two-sided coordinated inner loop, and the consistency residual monitoring and task redistribution mechanism.

[0178] The above four methods were repeated 30 times, and the average results were taken. The test results are as follows: Average solution time per step / ms 83.6 31.1 45.2 6.8 Effective control frequency / Hz 12 32 22 ≥100 End tracking root mean square error / cm 1.82 3.47 2.15 0.93 Obstacle avoidance success rate / % 76.7 83.3 88.3 96.7 Overall mission success rate / % 70.0 76.7 83.3 95.0 ; As can be seen from the above experimental results, under this simulation configuration, the average single-step solution time of the implementation method in this application is lower than that of the other three comparison methods. At the same time, the root mean square error of end-point tracking is reduced, and the obstacle avoidance success rate and the overall task success rate are improved. This indicates that under this specific test condition, coordinating the high-computing-power planning side and the real-time execution side according to different constraint roles can reduce the disconnect between planning and execution while maintaining the overall coupled motion planning.

[0179] To further verify the task redistribution mechanism, a dynamic obstacle sudden interference test can be designed.

[0180] In one specific experiment, the mobile robotic arm operated normally along its planned trajectory. When it reached the 3rd second, a dynamic obstacle suddenly appeared about 0.5m ahead of the previously unobstructed path to simulate the situation of an elevator door suddenly opening or a person entering the movement area.

[0181] In this experiment, the high-performance planning unit needed to replan the obstacle avoidance path, and it also undertook a high-load visual inference task, temporarily increasing its planning and solution time from approximately 4ms under normal conditions to 18ms. The real-time execution unit determined whether to enter the task reallocation mode by continuously monitoring the consistency residual between the planned control variable and the executable control variable.

[0182] The experimental procedure is as follows: 0～3.0s nominal operation Tracking error less than 0.5cm, consistency residual below the threshold. 3.0s Dynamic obstacles appear High-computing-power planning unit triggers replanning 3.0～3.018s High-performance computing latency The solution took 18ms, and no new planning instructions were issued. 3.004s First residual exceedance failure_counter=1 3.024s Second residual exceedance failure_counter=2 3.044s Third residual exceedance Triggering task reassignment, the real-time execution unit activates MPPI. 3.044～3.064s MPPI Local Planning Locally generated secure local control sequences 3.064s High-computing-power planning recovery The new planning results show that the consistency residual has decreased. 3.064～3.500s Redistribution recovery After the consistent residual remains low for 10 consecutive cycles, the task redistribution mode is exited. ; In this embodiment, the MPPI local controller employs 500 candidate control sequences and a 5-step local prediction time domain. The MPPI single planning time is approximately 0.8ms, the maximum position error of the robotic arm end effector during redistribution is approximately 3.2cm, the safety distance is set at 5cm, and no collisions occurred during the test.

[0183] In contrast, in the global-local dual-layer control scheme without the above task redistribution mechanism, under the same dynamic disturbance conditions, one collision event where the robotic arm end contacted an obstacle and one system shutdown protection event due to control timeout were recorded.

[0184] The above results demonstrate that, under the specific test conditions, the task reassignment triggered by continuous excess of consistency residuals and the local control takeover on the real-time execution unit side can maintain continuous control output and reduce the risk of collisions under abnormal conditions when the high-computing-power planning unit is temporarily unable to provide control results suitable for execution in a timely manner.

[0185] To test the adaptability of the cooperative motion planning method under different scene complexities, the number of obstacles in the restricted passage can be changed for repeated experiments.

[0186] In one experiment, the number of obstacles was set to 3, 5, 8, 10, and 12, and each configuration was tested 20 times. The results were recorded as follows: Centralized NMPC task success rate / % 83.3 76.7 66.7 60.0 53.3 Global-local dual-layer task success rate (%) 90.0 88.3 83.3 76.7 70.0 Success rate of the task in this application's implementation method / % 100 96.7 95.0 93.3 90.0 Average solution time of the implementation method in this application / ms 5.2 6.1 7.4 8.9 10.5 ; The above results show that in this specific test environment, the average solution time of the implementation method of this application increases with the increase of the number of obstacles, but still maintains a 90% task success rate with a configuration of 12 obstacles, thus indicating that the method has a certain adaptability under different obstacle densities.

[0187] To further verify the adaptability of this method to different high-computing-power planning hardware, the hardware configuration of the high-computing-power planning unit can be changed while keeping the parameters of other methods basically the same. In some specific implementations, the results are as follows: Jetson Orin 4.2 Not triggered 96.7 Jetson Xavier NX 8.7 Triggered in a few scenarios, approximately 2% 93.3 Raspberry Pi 4B 21.3 Triggered relatively frequently, in approximately 30% of scenarios. 86.7 ; Under low computing power configurations, the solution time of the high-computing-power planning unit increases, and the probability of consistency residual exceeding limits and task redistribution triggering increases accordingly. However, since the real-time execution unit can activate the local controller to take over when the task redistribution conditions are met, an 86.7% task success rate was still achieved when using the Raspberry Pi 4B as the high-computing-power planning unit in this experiment. Control schemes without task redistribution mechanisms under similar low-computing-power hardware have a task success rate of less than 60%.

[0188] This demonstrates that the task redistribution mechanism is not only used to handle sudden environmental changes, but also to provide real-time execution-side control compensation when high-computing-power planning units are unable to output planning results according to the expected time sequence due to insufficient hardware computing power or temporary increase in computing load.

[0189] Reference Figure 2 This application further illustrates the specific collaborative relationship of the mobile robotic arm collaborative motion planning system under heterogeneous computing power conditions. The high-computing-power planning unit, the real-time execution unit, and the robot's underlying hardware platform form a closed loop through a communication interface. The high-computing-power planning unit receives the current state feedback from the robot's underlying hardware platform and successively convexizes the nonlinear dynamic relationship between the mobile chassis and the robotic arm and the non-convex collision avoidance constraints in the outer loop of SCP, constructing a convex optimization subproblem near the current reference trajectory. Inside the current convex optimization subproblem, the planning control sequence is solved through the inner loop of ADMM.

[0190] The planned control sequence and dual variables are sent to the real-time execution unit. The real-time execution unit obtains the actual executable control sequence according to the aforementioned execution-side objective function and executable constraints, and sends the first executable control quantity to the robot's underlying hardware platform for execution. Figure 2 The projection operator in the figure is a schematic notation for the solution on the execution side. The specific calculation adopts the aforementioned closed formula containing weights and penalty parameters. Step 3 in the figure represents the coordination step of dual update. The real-time execution unit calculates the update result and feeds it back to the high-computing-power planning unit for subsequent iterations.

[0191] Meanwhile, the real-time execution unit determines the consistency residual based on the deviation between the planned control sequence and the actual executable control sequence, and updates the dual variables corresponding to the consistency constraints on both sides based on the deviation. The actual executable control sequence and the updated dual variables are returned to the high-computing-power planning unit through the communication interface to participate in the planning convex quadratic programming solution in the subsequent ADMM iteration.

[0192] thus, Figure 2 The architecture shown treats the planned control quantity obtained by the high-performance planning unit and the executable control quantity that the real-time execution unit can actually implement as corresponding split variables in the same convex optimization subproblem, so that both are solved by computing units adapted to their computing power characteristics under consistency constraints. The physical executable constraints faced by the real-time execution unit, such as actuator limiting and local safety boundaries, first act on the executable control quantity. The resulting constraints are then fed back into the subsequent solution of the high-performance planning unit through the deviation between the planned control quantity and the executable control quantity and the update of the dual variable.

[0193] Therefore, the execution side does not merely truncate the already formed planning results at the end, but can continuously influence the generation of subsequent planning control quantities with its own executability, so that the planning results gradually converge towards the actual executable range. Thus, while maintaining the overall coupled planning capability of the chassis and robotic arm, high-dimensional convex optimization solutions are configured in high-computing-power planning units, and separable low-complexity projection calculations are configured in real-time execution units, achieving synergy between planning accuracy, physical executability and real-time computing capabilities.

[0194] Furthermore, when the real-time execution unit detects that the consistency residual continuously exceeds the limit, this residual not only represents the instantaneous difference between the current planned control quantity and the actual executable capability, but also identifies a persistent capability mismatch between the high-computing-power planning side and the real-time execution side. At this time, the real-time execution unit triggers task reallocation, which on the one hand enables the high-computing-power planning unit to adjust the prediction time domain, trust domain, and consistency coordination parameters to reduce the current planning solution burden and enhance the degree to which the planning results approach the feasible range of the execution side; on the other hand, it activates the local controller to temporarily take over the generation of control quantity. Thus, the planning-execution consistency coordination under normal conditions and the task reallocation under abnormal conditions are not two isolated mechanisms, but rather form a continuous closed-loop control relationship with the degree of mismatch between the planned control quantity and the executable control quantity as the link.

[0195] The mathematical expressions described above in this application are used to illustrate a specific solution relationship between planning control variables, executable control variables, dual variables, penalty parameters, and executable constraints; each parameter is defined as explicitly in the corresponding formula.

[0196] Regarding the aforementioned And the definition of unscaled dual variables, performing the closed-form formula using the denominator If other weight definitions or scaling transformations are used, the objective function, dual variables, and closed-form formulas must be transformed simultaneously; these cannot be omitted when the parameter definitions remain unchanged. .

[0197] With a clear objective function, parameter definition, and consistency feedback relationship, the impact of executable constraints on the execution side is applied to the subsequent solution on the planning side through executable control variables, control variable deviations, and dual variable updates.

[0198] This application also proposes a collaborative motion planning system for a mobile robotic arm. This system includes a high-computing-power planning unit and a real-time execution unit, which cooperate to achieve collaborative motion planning for the mobile robotic arm; specifically:

[0199] The high-computing-power planning unit is used to transform the non-convex coupled optimal control problem of the mobile chassis and the robotic arm into a convex optimization subproblem, and solve the planning control quantity under the motion planning objective.

[0200] The real-time execution unit is used to solve for executable control variables based on the planned control variables under its executable constraints;

[0201] The high-performance planning unit and the real-time execution unit are configured to coordinate the solutions on both sides based on the deviation between the planned control quantity and the executable control quantity, so that the executable control quantity can provide feedback to the subsequent solutions of the high-performance planning unit, so that the subsequently obtained planned control quantity can adapt to the executable constraints of the real-time execution unit and obtain an executable control quantity that satisfies the executable constraints.

[0202] Through the above scheme, the high-computing-power planning unit is responsible for solving the planning control quantity under the motion planning objective, and the real-time execution unit is responsible for solving the executable control quantity under its executable constraints. The two coordinate according to the deviation between the planning control quantity and the executable control quantity, so that the executable control quantity can provide feedback to the subsequent solution of the high-computing-power planning unit, thereby adapting the subsequent planning control quantity to the executable constraints of the real-time execution unit, and finally obtaining the executable control quantity that satisfies the executable constraints.

[0203] The above description is merely an embodiment of this application and is not intended to limit the scope of protection of this application. Various modifications and variations can be made to this application by those skilled in the art. All modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A method for cooperative motion planning of a mobile robotic arm, characterized in that, The non-convex coupled optimal control problem of the mobile chassis and robotic arm is transformed into a convex optimization subproblem. The control variables for the overall motion of the mobile chassis and robotic arm are set as planning control variables on the planning side and executable control variables on the execution side, respectively. Consistency constraints are established between the two. The high-computing-power planning unit solves for the planning control variables under the motion planning objective, and the real-time execution unit solves for the executable control variables based on the planning control variables under its executable constraints. The solutions on both sides are coordinated based on the consistency deviation between the planning control variables and the executable control variables. The executable control variables provide feedback to the subsequent solutions of the high-computing-power planning unit, so that the subsequently obtained planning control variables adapt to the executable constraints of the real-time execution unit and obtain executable control variables that satisfy the executable constraints.

2. The mobile robotic arm cooperative motion planning method according to claim 1, characterized in that, The steps for transforming the non-convex coupled optimal control problem of the mobile chassis and robotic arm into a convex optimization subproblem include: The non-convex coupled optimal control problem is linearized at the current reference trajectory to construct a convex optimization subproblem; The deviation of the solution to the convex optimization subproblem from the current reference trajectory is limited by the trust domain constraint; The reference trajectory is updated based on the solution to the convex optimization subproblem, and the linearization process is continued based on the updated reference trajectory to construct and solve the convex optimization subproblem.

3. The mobile robotic arm cooperative motion planning method according to claim 2, characterized in that, The step of transforming the non-convex coupling optimal control problem of the mobile chassis and robotic arm into a convex optimization subproblem further includes: When the convex optimization subproblem becomes infeasible due to excessively tight collision avoidance constraints, non-negative relaxation variables are introduced to relax the collision avoidance constraints from hard constraints to soft constraints. A penalty term for the non-negative relaxation variables is added to the cost function of the convex optimization subproblem to restore the feasibility of the convex optimization subproblem.

4. The mobile robotic arm cooperative motion planning method according to claim 2, characterized in that, The step of coordinating the solutions on both sides based on the deviation between the planned control quantity and the executable control quantity includes: In the convex optimization subproblem, an auxiliary variable representing an executable control quantity is introduced, and a consistency constraint is established between the auxiliary variable and the planned control quantity to transform the convex optimization subproblem into a decomposition form with consistency constraints.

5. The mobile robotic arm cooperative motion planning method according to claim 4, characterized in that, The step of coordinating the solutions on both sides based on the deviation between the planned control quantity and the executable control quantity includes: For the decomposition form with consistency constraints, a dual variable corresponding to the consistency constraints is introduced, and the dual term containing the dual variable and the penalty term with penalty parameters are incorporated into the cost function of the convex optimization subproblem to construct an augmented Lagrangian function. In each alternating iteration, the current executable control quantity and the current dual variable are first fixed so that the high-computing-power planning unit solves for a new planning control quantity. Then, the new planning control quantity just obtained by the high-computing-power planning unit in this iteration and the current dual variable that has not yet been updated are fixed so that the real-time execution unit solves for a new executable control quantity. The dual variable is updated based on the deviation between the planned control variable and the executable control variable, and the deviation is converged through iteration.

6. The mobile robotic arm cooperative motion planning method according to claim 5, characterized in that, The steps by which the real-time execution unit solves for the executable control quantity based on the planned control quantity under its executable constraints include: The real-time execution unit independently projects the combined quantity formed by the planned control quantity, the dual variable, and the penalty parameter onto the feasible domain defined by the executable constraint at each control moment, and determines the executable control quantity in a closed manner.

7. The mobile robotic arm cooperative motion planning method according to claim 5, characterized in that, The method also includes the following steps: The real-time execution unit determines the consistency residual based on the deviation between the planned control quantity and the executable control quantity, and continuously monitors the consistency residual for exceeding limits. When the consistency residual exceeds the preset residual threshold for a preset number of consecutive times, it is determined that the task reassignment condition has been met, and task reassignment is triggered.

8. The mobile robotic arm cooperative motion planning method according to claim 7, characterized in that, The task reassignment steps include: After triggering task reassignment, the high-computing-power planning unit shortens the prediction time domain and increases the state trust domain radius, control trust domain radius, and penalty parameter. The real-time execution unit activates the local controller and takes over execution using the local control sequence generated by the local controller. When the consistency residual is lower than the recovery threshold, the reallocation mode is exited, and the collaborative execution mode in which the real-time execution unit determines the executable control quantity based on the planning control quantity obtained by the high-computing-power planning unit is restored, and the prediction time domain before the shortening is restored.

9. The mobile robotic arm cooperative motion planning method according to claim 6, characterized in that, The method also includes the following steps: When the control data of the high-computing-power planning unit is late, the real-time execution unit extrapolates the state based on the current state, the control quantity of the previous cycle, and the dynamic model of the moving robotic arm. Based on the extrapolation result of the state and the dual variable of the previous cycle, the control quantity of the previous cycle is corrected by local closed projection to generate the transition control quantity. The transition control variable is executed before the new control data arrives from the high-computing-power planning unit.

10. A cooperative motion planning system for a mobile robotic arm, characterized in that, Includes high-performance planning units and real-time execution units; The high-computing-power planning unit is used to transform the non-convex coupling optimal control problem of the mobile chassis and the robotic arm into a convex optimization subproblem, and solve the planning control quantity under the motion planning objective. The real-time execution unit is used to solve for the executable control quantity based on the planned control quantity under its executable constraints; The high-performance planning unit and the real-time execution unit are configured to coordinate the solutions on both sides based on the deviation between the planning control quantity and the executable control quantity, so that the executable control quantity provides feedback to the subsequent solutions of the high-performance planning unit, so that the subsequently obtained planning control quantity adapts to the executable constraints of the real-time execution unit and obtains an executable control quantity that satisfies the executable constraints.

Images