A double-layer optimization-based fast trajectory planning method for mobile manipulators

Abstract

The application relates to the technical field of mobile operation robot motion planning, and discloses a mobile manipulator rapid trajectory planning method based on double-layer optimization, which is based on initial configuration and trajectory prediction, establishes a manipulator kinematics model and a blocking task constraint, adopts SQP combined with double integrators for rapid reachability screening in the upper layer, solves optimal terminal configurations and blocking time points that satisfy end blocking constraints, and adopts chassis and manipulator discrete QP layering cooperation in the lower layer to generate chassis and manipulator discrete optimal trajectory sequences, constructs continuous time trajectories through linear interpolation, designs a chassis-manipulator iterative cooperative mechanism, and fine-tunes chassis target points through end blocking residual feedback, so that the spatiotemporal coordination of the mobile manipulator system is realized. The application decouples high-dimensional non-convex trajectory optimization into low-dimensional decision and convex QP subproblems through a double-layer structure, avoids complex NLP iteration in complete trajectory space, reduces the calculation complexity, and is suitable for millisecond-level replanning requirements in a high-dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of motion planning technology for mobile robotic arms, and specifically to a rapid trajectory planning method for mobile robotic arms based on dual-layer optimization. Background Technology

[0002] In dynamic, unstructured environments, when a mobile robotic arm performs a high-speed target interception mission, it needs to complete motion planning within an extremely short time while simultaneously satisfying multiple constraints such as end-effector pose, kinematic limits, and collision safety. Current mainstream trajectory planning methods mostly employ a single-layer nonlinear programming (NLP) architecture to directly solve continuous-time trajectories, or use fixed-parameter curves to simplify the problem. However, in highly dynamic scenarios, these methods still suffer from the following technical limitations:

[0003] First, single-layer NLP has high computational complexity, making it difficult to meet real-time requirements. Complete NLP has high variable dimensionality and strong constraint non-convexity. The interior-point method or SQP iterative solution usually takes hundreds of milliseconds, which is difficult to meet the millisecond-level requirements of high dynamic tasks for replanning frequency.

[0004] Second, traditional trajectory parameterization lacks flexibility. While low-order polynomials or fixed velocity profiles are computationally efficient, they cannot adaptively adjust to changes in constraints, easily leading to acceleration jumps or trajectory overshoot. Increasing the polynomial order can cause Runge's phenomenon and makes it difficult to handle complex constraints in high-dimensional configuration spaces.

[0005] Third, the strong coupling between the chassis and the robotic arm makes it difficult to coordinate with a single optimization layer. The end-effector pose of the mobile robotic arm system is determined by both the chassis movement and the robotic arm motion. If a single NLP is used to simultaneously optimize the continuous trajectories of the chassis and the robotic arm, the dimensionality of variables and the number of constraints will increase dramatically, leading to uncontrollable solution time. Summary of the Invention

[0006] To address the technical problems of high computational complexity in single-layer optimization, insufficient flexibility in trajectory parameterization, and difficulties in chassis-arm motion coordination in existing technologies, this invention provides a rapid trajectory planning method for a mobile robotic arm based on two-layer optimization. The technical solution is as follows:

[0007] A fast trajectory planning method for a mobile robotic arm based on two-layer optimization includes the following steps:

[0008] Step 1: Input the initial configuration of the wheel-arm and predict the ball trajectory;

[0009] Step 2: Define the mobile robotic arm system configuration vector as an augmented vector of the chassis pose and the robotic arm joint angles: Establish a 9-DOF positive kinematic mapping and an enhanced Jacobian matrix. ;

[0010] Step 3: Establish a constraint model for the interception task that includes end-point interception constraints, spatiotemporal kinematic constraints, and collision constraints;

[0011] Step 4: Construct a high-level interception attitude planning model based on Sequential Quadratic Programming (SQP), which optimizes and reduces the high-dimensional continuous trajectory into a low-dimensional terminal configuration and interception timing decision problem. A simplified dual integrator model and a fast reachability screening mechanism based on conservative coefficient compensation are introduced to solve for the optimal interception configuration and timing that satisfy the terminal interception constraints and the rough feasibility constraints.

[0012] Step 5: Construct a chassis discrete trajectory generation model based on quadratic programming (QP). Under the dual integrator motion discretization, the chassis acceleration at the discrete time step is used as the optimization variable, and a continuous chassis trajectory is constructed by interpolation.

[0013] Step 6: Construct a discrete trajectory generation model for the robotic arm based on quadratic programming (QP). At the discrete time step, the joint acceleration of the robotic arm is used as the optimization variable. At the cost of the terminal joint tracking error and the rate of change of acceleration, a continuous robotic arm trajectory is constructed by interpolation.

[0014] Step 7: Design an iterative collaboration mechanism between the chassis and the robotic arm. Input the continuous trajectory of the chassis as a time-varying parameter into the QP layer of the robotic arm. If the residual at the end-point interception position exceeds the threshold, fine-tune the target point of the chassis along the residual gradient direction and resolve the chassis QP until the end-point interception constraint is satisfied.

[0015] Step 8: Integrate the continuous trajectory of the chassis and the continuous trajectory of the robotic arm to output the wheel-arm coordinated trajectory.

[0016] Further, in step 2, the Jacobian matrix is:

[0017] ,

[0018] Among them, the geometric Jacobian of the robotic arm Obtained by differentiating the forward kinematics chain rule; chassis contribution submatrix The derivation is as follows: the linear velocity of the end effector in the world coordinate system Based on chassis translation speed The speed of the chassis rotation and the components of the relative velocity in the robotic arm's base coordinate system after being mapped by the chassis rotation matrix. It is formed by superposition, and the formula is:

[0019] ,

[0020] in, Let the position vector of the end relative to the center of the chassis be denoted by ; then apply the above equation to the generalized velocity of the chassis. Taking the partial derivative, we obtain the chassis contribution submatrix. :

[0021] ,

[0022] in, The component representing the position in the local coordinate system of the chassis.

[0023] Furthermore, in step 3, the interception task constraint model includes end-position coincidence constraint, relaxed attitude alignment constraint, normal relative velocity range constraint, joint and chassis kinematic limit constraint, and collision constraint.

[0024] Furthermore, in step 4, the decision variable of the high-level interception attitude planning model is the terminal configuration. With the moment of interception Construct the following nonlinear programming problem:

[0025] ,

[0026] Wherein, objective function To minimize the overall motion of the system; , The initial orientation of the chassis and the joint angles of the robotic arm; , The engagement posture is the connection between the chassis position and the joint angle of the robotic arm; , These are the corresponding weighting coefficients; For positional coincidence constraints; To relax the posture alignment constraints; For attitude deviation limitation constraints; For the endpoint collision safety constraint; This is the reachability boundary constraint for the dual integrator.

[0027] Furthermore, in step 4, the fast reachability screening mechanism treats each joint as an independent dual integrator system, with the discrete state-space model as follows:

[0028] ,

[0029] in, The discrete time step; This is a joint acceleration command; , For joint position and velocity; for a given initial state and candidate target location During the time window Within the constraints, the optimal time trajectory is the Bang-Coast-Bang profile, and the upper bound of the maximum achievable displacement of joint i is... Introducing a conservative coefficient The compensation model simplifies the error and constructs a fast screening condition, as shown in the following formula:

[0030] .

[0031] Furthermore, the Bang-Coast-Bang profile consists of three segments: the first segment with maximum acceleration Perform Bang acceleration to increase the speed from the initial value. Ascend to maximum permissible speed The second segment is at maximum speed. The coasting motion proceeds at a constant speed; the third segment involves maximum deceleration. Perform a Bang to reduce speed to the terminal value. The explicit formulas for calculating acceleration and deceleration time are:

[0032] ,

[0033] like Then there exists a uniform velocity segment, the duration of which is ;like If there is no uniform velocity segment, it degenerates into a Bang-Bang profile, and the peak velocity needs to be recalculated.

[0034] Furthermore, in step 5, the chassis is... The QP problem is constructed at each time step, with the optimization variable being the discrete control input sequence. The control input is chassis acceleration. The objective function minimizes the rate of change of acceleration and the terminal tracking error, as shown in the following formula:

[0035] ,

[0036] The constraints of the formula include discrete dynamic evolution, endpoint boundaries, and physical motion limits; after obtaining the discrete state sequence of the chassis by solving, a continuous-time trajectory is constructed through linear interpolation. .

[0037] Furthermore, in step 6, the robotic arm... The QP problem is constructed over several time steps, with the optimization variable being a discrete joint acceleration sequence. ,in The objective function minimizes the rate of change of joint acceleration and the tracking error of the terminal joint angle, as shown in the following formula:

[0038] ,

[0039] The constraints of the formula include discrete double integrator dynamics, joint angle / velocity / acceleration limits, and endpoint boundaries; after solving to obtain the discrete state sequence of the robotic arm, a continuous-time trajectory is constructed through linear interpolation. .

[0040] Furthermore, in step 7, the iterative collaborative mechanism inputs the continuous trajectory of the chassis as a time-varying parameter into the QP layer of the robotic arm to calculate the residual at the end-point interception position; if the residual exceeds the threshold, the chassis target point is corrected along the residual gradient direction and the time window is simultaneously fine-tuned, and the chassis QP is solved again.

[0041] Furthermore, the collision constraint adopts a directed distance function smoothing aggregation strategy, the robotic arm links are capsule-shaped, and the envelope of the chassis and end effector is simplified to a cuboid, as shown in the following formula:

[0042] ,

[0043] in, For smoothing parameters, The original distances from the capsule and cuboid to the environmental geometric features are defined as follows:

[0044] ,

[0045] in, For a safe distance.

[0046] Beneficial effects

[0047] This invention decouples high-dimensional non-convex trajectory optimization into low-dimensional SQP decision-making and two convex QP subproblems through a two-layer architecture, avoiding complex NLP iterations throughout the process; it quickly generates continuous trajectories through interpolation and ensures end-point interception accuracy through iterative collaboration, thereby achieving millisecond-level replanning of mobile robotic arms in highly dynamic environments. Attached Figure Description

[0048] Figure 1 The flowchart shows a fast trajectory planning method for a mobile robotic arm based on two-layer optimization.

[0049] Figure 2 This is a schematic diagram of a collaborative planning and interception scenario for a mobile robotic arm according to an embodiment of the present invention;

[0050] Figure 3 This is a flowchart of the high-level SQP interception attitude planning in an embodiment of the present invention;

[0051] Figure 4 This is a schematic diagram of chassis QP discrete trajectory generation according to an embodiment of the present invention;

[0052] Figure 5aThis is a schematic diagram illustrating the generation and interpolation of the QP discrete trajectory of the robotic arm base rotation according to an embodiment of the present invention.

[0053] Figure 5b This is a schematic diagram of the generation and interpolation of the QP discrete trajectory of the robotic arm shoulder pitch in an embodiment of the present invention;

[0054] Figure 5c This is a schematic diagram of the generation and interpolation of the QP discrete trajectory of the robotic arm elbow pitch in an embodiment of the present invention;

[0055] Figure 5d This is a schematic diagram of the generation and interpolation of the QP discrete trajectory of the robotic arm wrist rotation according to an embodiment of the present invention;

[0056] Figure 5e This is a schematic diagram of the generation and interpolation of the QP discrete trajectory of the robotic arm wrist in an embodiment of the present invention;

[0057] Figure 5f This is a schematic diagram illustrating the generation and interpolation of the QP discrete trajectory of the robotic arm's end effector in an embodiment of the present invention.

[0058] Figure 6 This is a flowchart of the chassis-robotic arm iterative collaborative mechanism according to an embodiment of the present invention. Detailed Implementation

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

[0060] like Figure 1 As shown in the figure, this embodiment of the invention provides a fast trajectory planning method for a mobile robotic arm based on dual-layer optimization. The specific process is as follows:

[0061] Step 1: Input wheel-arm initial configuration and ball trajectory prediction:

[0062] In this embodiment, the ball trajectory prediction is obtained through external sensor perception and kinematic model prediction. The initial configuration of the wheel and arm is also obtained through the robot's own sensors, such as the click encoder. Subsequent steps are performed based on these two known parameters. Figure 2 As shown.

[0063] Step 2: Define the configuration vector of the mobile robotic arm system as an augmented vector of the chassis pose and the joint angles of the robotic arm, and establish a 9-DOF positive kinematic mapping and augmented Jacobian matrix:

[0064] In this embodiment, the MD-H model is used to perform kinematic modeling of the mobile robotic arm, and the Hayati-Roberts modified convention is adopted, with the parameter order as follows: This avoids singularity issues when adjacent parallel joint axes are involved. Define the system configuration vector. Establish a 9-DOF positive kinematic mapping and an enhanced Jacobian matrix. This is used to map the joint angles and the position of the robot's end effector, establishing the relationship between the joint and end effector positions. The robotic arm's geometric Jacobian is used in this context. Obtained by differentiating the forward kinematics chain rule; chassis contribution submatrix The derivation is as follows:

[0065] Linear velocity of the end effector in the world coordinate system It consists of three superimposed parts, namely the chassis translation speed. The speed of the chassis rotation And the components of the relative velocity in the robotic arm's base coordinate system after being mapped by the chassis rotation matrix. The formula is:

[0066] ,

[0067] in, Let be the position vector of the end relative to the center of the chassis. Applying the above equation to the generalized velocity of the chassis... Taking the partial derivative, we can obtain the chassis contribution submatrix. :

[0068] ,

[0069] in, This represents the component of the position in the local coordinate system of the chassis. This sub-matrix fully characterizes the contribution of the chassis's translational and rotational motions to the end effector's linear velocity, constituting a key link in the 9-DOF to 6-dimensional end effector spatial velocity mapping.

[0070] Step 3: Establish a constraint model for the interception task, including end-point interception constraints, spatiotemporal kinematic constraints, and collision constraints:

[0071] The interception task constraint model is used to describe the tasks observed in the objective world in mathematical form, including:

[0072] The constraint for coincident end positions requires that the end position at the moment of interception coincide with the predicted interception point. The formula is as follows:

[0073] .

[0074] Attitude alignment constraints require that the normal to the end effector's interception surface be aligned with the negative direction of the target velocity. If a three-dimensional cross product equation is directly applied... Since the cross product result is always perpendicular to and Zhang Cheng's plane, this three-dimensional vector equation mathematically provides only two independent scalar constraints (along...). The directional component is always zero, leading to a row rank deficiency in the constraint Jacobian matrix. In SQP solutions, this rank deficiency causes the row linear dependence of the constraint gradient matrix of the KKT system, resulting in numerical singularities and iteration failures. Therefore, a projection matrix is ​​introduced: Project the cross product result onto a plane perpendicular to the x-axis. The plane is reduced to a 2D equality constraint, thus ensuring the constraint Jacobian is full rank and guaranteeing stable convergence of the optimizer. The formula is:

[0075] .

[0076] The normal relative velocity constraint is used to control the relative approach velocity of the end effector and the flying target along the normal direction of the arresting surface at the moment of interception, in order to achieve a balance between efficient momentum transfer and impact suppression. If the normal relative velocity is too small, insufficient momentum transfer will occur during interception, and the target will easily slip along the arresting surface, leading to capture failure; if it is too large, the instantaneous impact will be severe, easily causing the robotic arm joint torque to exceed the limit, end effector vibration, or high-speed rebound of the target. Therefore, the normal relative velocity constraint is defined as:

[0077] ,

[0078] Among them, the lower limit Based on the target mass and desired capture momentum settings, ensure that the end effector applies sufficient normal impulse to the target to achieve stable contact; upper limit The settings are based on the maximum impact load and joint torque limits at the end of the robotic arm.

[0079] Spatiotemporal kinematic constraints are used to limit the motion limits of each joint and chassis.

[0080] Collision constraints are addressed using a Directed Distance Function (SDF) smoothing aggregation strategy. Traditional collision detection uses the GJK / EPA algorithm to directly calculate the Euclidean distance between the nearest points of two convex bodies. However, when robot motion causes abrupt changes in the nearest point pair (e.g., the nearest point of a capsule-shaped link jumps from one vertex to the other), the gradient of the distance function with respect to the joint configuration variables exhibits discontinuous jumps at the switching point. In nonlinear optimization, gradient discontinuities can lead to oscillations in the search direction of SQP or interior point methods, frequent step backtracking, and even iterative divergence. Therefore, the robotic arm link is considered as a capsule, and the envelope of the chassis and end effector is simplified to a cuboid. An SDF smoothing aggregation strategy is introduced to handle the gradient discontinuity problem caused by abrupt changes in the nearest point pair. The smoothing aggregation formula is:

[0081] ,

[0082] in, For smoothing parameters, This represents the original distance from the capsule and cuboid to the environmental geometry. This function is a smooth lower bound approximation of the maximum distance function, and its gradient is continuously differentiable in all regions, thus effectively eliminating numerical oscillations caused by abrupt changes in nearest-point pairs and ensuring stable convergence of the optimizer.

[0083] The method of abstracting the robotic arm links as capsules is as follows: for each link... Based on the link length parameters in its MD-H model Alternatively, determine the coordinates of the joint centers at both ends of the connecting rod in the world coordinate system based on the actual CAD geometric dimensions. and a line segment connecting the two endpoints The axis of the capsule body is defined by the radius of the largest circumscribed circle of the connecting rod. To construct the capsule body, define the capsule body radius. ;

[0084] The method to simplify the envelope of the chassis and end effector into a cuboid is as follows: taking the geometric center of the chassis as the origin, and based on the actual length of the chassis... ,Width ,high Define a half-size vector:

[0085] ,

[0086] Construct a cuboid aligned with the axes of the world coordinate system. Its 8 vertex coordinates are determined by the chassis pose. With half size The distances between capsules and between capsules and cuboids are determined by homogeneous transformation and are efficiently calculated using the line segment-line segment nearest point algorithm and the line segment-plane nearest point algorithm, respectively.

[0087] Collision constraints are defined as follows:

[0088] ,

[0089] in, For a safe distance.

[0090] Step 4: Construct a high-level interception attitude planning model based on Sequential Quadratic Programming (SQP):

[0091] like Figure 3 As shown, the decision variable for high-level SQP planning is the terminal configuration. With the moment of interception This paper introduces a dual integrator model and a fast reachability screening mechanism based on conservative coefficient compensation to construct a nonlinear programming problem. By performing a second-order Taylor expansion of the Lagrangian function at the current iteration point, the original NLP problem is transformed into a series of QP subproblems with linear constraints. Combining the BFGS matrix to approximate the Hessian, stable and fast convergence is achieved in a low-dimensional space, solving for the optimal gate configuration that satisfies the end-gate constraint and the coarse feasibility constraint. and time .

[0092] The nonlinear programming problem is:

[0093] ,

[0094] Wherein, objective function To minimize the overall motion of the system; , The initial orientation of the chassis and the joint angles of the robotic arm; , The engagement posture is the connection between the chassis position and the joint angle of the robotic arm; , These are the corresponding weighting coefficients; For positional coincidence constraints; To relax the posture alignment constraints; For attitude deviation limitation constraints; For the endpoint collision safety constraint; This is the reachability boundary constraint for the dual integrator.

[0095] The dual-integrator fast reachability screening mechanism treats each joint as an independent dual-integrator system, and the discrete state-space model is as follows:

[0096] ,

[0097] in, The discrete time step; This is a joint acceleration command; , For the joint position and velocity. Given the initial state... and candidate target location Within the time window, based on Pontryagin's minimum principle, the optimal time trajectory under control is the Bang-Coast-Bang profile, with joints... The upper bound of the maximum reachable displacement is .

[0098] The Bang-Coast-Bang profile is the time-optimal velocity curve of the control-constrained dual integrator system, consisting of three segments: the first segment is at maximum acceleration. Perform Bang acceleration to increase the speed from the initial value. Ascend to maximum permissible speed The second segment is at maximum speed. The coasting motion proceeds at a constant speed; the third segment involves maximum deceleration. Perform a Bang to reduce speed to the terminal value. Let the total time window be... The explicit formulas for calculating acceleration and deceleration time are:

[0099] ,

[0100] like Then there exists a uniform velocity segment, the duration of which is ;like If there is no uniform velocity segment, it degenerates into a Bang-Bang profile. In this case, the peak velocity needs to be recalculated. This analytical solution provides a closed-form criterion for rapid accessibility screening.

[0101] Because the dual integrator model treats each joint as an independent point mass, it neglects the dynamic coupling between the robotic arm links, joint friction, motor torque attenuation, gear transmission losses, and the interaction effects between chassis movement and robotic arm motion. Its theoretical limit is reached for the upper displacement limit. It is necessarily higher than the actual achievable capability of the real system. The aforementioned conservative coefficient is introduced. To compensate for the dynamic differences between the simplified model of the dual integrator and the real mobile robotic arm system, a conservative discount is applied to the theoretical boundary, and a fast screening condition is constructed: This ensures that candidate configurations deemed reachable remain physically feasible even under real dynamic conditions.

[0102] Step 5: Construct a chassis discrete trajectory generation model based on quadratic programming (QP):

[0103] chassis in A QP problem is constructed at each time step, with the optimization variable being a discrete acceleration sequence. The objective function minimizes the rate of change of acceleration and the terminal tracking error:

[0104] ,

[0105] The constraints of the objective function encompass discrete dynamic evolution, endpoint boundaries, and physical motion limits. This QP problem can be solved globally optimally in sub-millisecond time using an OSQP solver. After obtaining the discrete state sequence of the chassis, a continuous-time trajectory is constructed through linear interpolation. ,like Figure 4 As shown.

[0106] Step 6: Construct a discrete trajectory generation model for the robotic arm based on quadratic programming (QP):

[0107] robotic arm in The QP problem is constructed over several time steps, with the optimization variable being a discrete joint acceleration sequence. The objective function minimizes the rate of change of joint acceleration and the tracking error of the terminal joint angle:

[0108] ,

[0109] The constraints of the objective function include discrete double integrator dynamics, joint angle / velocity / acceleration limits, and endpoint boundaries. Since the objective function is a convex quadratic form and the constraints are all linear, this QP problem can be solved globally and quickly. After obtaining the discrete state sequence of the robotic arm, a continuous trajectory can be constructed through linear interpolation. To ensure that joint speed and acceleration are continuous throughout the entire process, such as Figures 5a to 5f As shown.

[0110] Step 7: Design the iterative collaboration and residual compensation mechanism between the chassis and the robotic arm:

[0111] like Figure 6 As shown, the continuous trajectory of the chassis is input as a time-varying parameter into the QP layer of the robotic arm to calculate the residual at the end-effector interception position:

[0112] ,

[0113] If the residual at the end-stop position exceeds the threshold of 0.01m, the chassis target point is fine-tuned along the residual gradient direction, and the time window is simultaneously fine-tuned. The chassis QP is then resolved until the end-stop constraint is satisfied. In this embodiment, the iteration converges in 1-2 iterations, and the chassis fine-tuning amplitude is less than 5%.

[0114] Step 8: Output wheel-arm coordinated trajectory:

[0115] The system integrates the continuous trajectory of the chassis and the robotic arm, outputs a wheel-arm collaborative trajectory that meets all time constraints, and sends it directly to the underlying controller for execution.

[0116] Experimental results show that the average cycle of a single complete replanning operation is 3.2ms, of which the high-level SQP interception attitude planning takes about 0.8ms, the low-level dual-QP discrete path generation takes about 1.5ms, and the interpolation and coordination takes about 0.9ms, which meets the millisecond-level real-time requirements of high dynamic interception.

[0117] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization, characterized in that, Includes the following steps: Step 1: Input the initial configuration of the wheel-arm and predict the ball trajectory; Step 2: Define the mobile robotic arm system configuration vector as an augmented vector of the chassis pose and the robotic arm joint angles: Establish a 9-DOF positive kinematic mapping and an enhanced Jacobian matrix. ; Step 3: Establish a constraint model for the interception task that includes end-point interception constraints, spatiotemporal kinematic constraints, and collision constraints; Step 4: Construct a high-level interception attitude planning model based on Sequential Quadratic Programming (SQP), which optimizes and reduces the high-dimensional continuous trajectory into a low-dimensional terminal configuration and interception timing decision problem. A simplified dual integrator model and a fast reachability screening mechanism based on conservative coefficient compensation are introduced to solve for the optimal interception configuration and timing that satisfy the terminal interception constraints and the rough feasibility constraints. Step 5: Construct a chassis discrete trajectory generation model based on quadratic programming (QP). Under the dual integrator motion discretization, the chassis acceleration at the discrete time step is used as the optimization variable, and a continuous chassis trajectory is constructed by interpolation. Step 6: Construct a discrete trajectory generation model for the robotic arm based on quadratic programming (QP). At the discrete time step, the joint acceleration of the robotic arm is used as the optimization variable. At the cost of the terminal joint tracking error and the rate of change of acceleration, a continuous robotic arm trajectory is constructed by interpolation. Step 7: Design an iterative collaboration mechanism between the chassis and the robotic arm. Input the continuous trajectory of the chassis as a time-varying parameter into the QP layer of the robotic arm. If the residual at the end-point interception position exceeds the threshold, fine-tune the target point of the chassis along the residual gradient direction and resolve the chassis QP until the end-point interception constraint is satisfied. Step 8: Integrate the continuous trajectory of the chassis and the continuous trajectory of the robotic arm to output the wheel-arm coordinated trajectory.

2. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 2, the Jacobian matrix is: ， Among them, the geometric Jacobian of the robotic arm Obtained by differentiating the forward kinematics chain rule; chassis contribution submatrix The derivation is as follows: the linear velocity of the end effector in the world coordinate system Based on chassis translation speed The speed of the chassis rotation and the components of the relative velocity in the robotic arm's base coordinate system after being mapped by the chassis rotation matrix. It is formed by superposition, and the formula is: ， in, Let the position vector of the end relative to the center of the chassis be denoted by ; then apply the above equation to the generalized velocity of the chassis. Taking the partial derivative, we obtain the chassis contribution submatrix. : ， in, The component representing the position in the local coordinate system of the chassis.

3. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 3, the interception task constraint model includes end-position coincidence constraint, relaxed posture alignment constraint, normal relative velocity range constraint, joint and chassis kinematic limit constraint, and collision constraint.

4. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 4, the decision variable of the high-level interception attitude planning model is the terminal configuration. With the moment of interception Construct the following nonlinear programming problem: ， Wherein, objective function To minimize the overall motion of the system; , The initial orientation of the chassis and the joint angles of the robotic arm; , The engagement posture is the connection between the chassis position and the joint angle of the robotic arm; , These are the corresponding weighting coefficients; For positional coincidence constraints; To relax the posture alignment constraints; As a constraint to limit attitude deviation; For the endpoint collision safety constraint; This is the reachability boundary constraint for the dual integrator.

5. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 4, the fast reachability screening mechanism treats each joint as an independent dual integrator system, and the discrete state-space model is as follows: ， in, The discrete time step; This is a joint acceleration command; , For joint position and velocity; for a given initial state and candidate target location During the time window Within the constraints, the optimal time trajectory is the Bang-Coast-Bang profile, and the upper bound of the maximum achievable displacement of joint i is... Introducing a conservative coefficient The compensation model simplifies the error and constructs a fast screening condition, as shown in the following formula: 。 6. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 5, characterized in that: The Bang-Coast-Bang profile consists of three segments: the first segment accelerates at maximum speed. Perform Bang acceleration to increase the speed from the initial value. Ascend to maximum permissible speed The second segment is at maximum speed. The coasting motion proceeds at a constant speed; the third segment involves maximum deceleration. Perform a Bang to reduce speed to the terminal value. The explicit formulas for calculating acceleration and deceleration time are: ， like Then there exists a uniform velocity segment, the duration of which is ;like If there is no uniform velocity segment, it degenerates into a Bang-Bang profile, and the peak velocity needs to be recalculated.

7. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 5, the chassis is The QP problem is constructed at each time step, with the optimization variable being a discrete control input sequence. The control input is chassis acceleration. The objective function minimizes the rate of change of acceleration and the terminal tracking error, as shown in the following formula: ， The constraints of the formula include discrete dynamic evolution, endpoint boundaries, and physical motion limits; after obtaining the discrete state sequence of the chassis by solving, a continuous-time trajectory is constructed through linear interpolation. .

8. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 6, the robotic arm is The QP problem is constructed over several time steps, with the optimization variable being a discrete joint acceleration sequence. ,in The objective function minimizes the rate of change of joint acceleration and the tracking error of the terminal joint angle, as shown in the following formula: ， The constraints of the formula include discrete double integrator dynamics, joint angle / velocity / acceleration limits, and endpoint boundaries; after solving to obtain the discrete state sequence of the robotic arm, a continuous-time trajectory is constructed through linear interpolation. .

9. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: In step 7, the iterative collaborative mechanism inputs the continuous trajectory of the chassis as a time-varying parameter into the QP layer of the robotic arm to calculate the residual of the end-point interception position. If the residual exceeds the threshold, the chassis target point is corrected along the residual gradient direction and the time window is adjusted synchronously to solve the chassis QP again.

10. The rapid trajectory planning method for a mobile robotic arm based on dual-layer optimization as described in claim 1, characterized in that: The collision constraints employ a directed distance function smoothing aggregation strategy. The robotic arm links are shaped like capsules, and the envelope of the chassis and end effector is simplified to a cuboid, as shown in the following formula: ， in, For smoothing parameters, The original distances from the capsule and cuboid to the environmental geometric features are defined as follows: ， in, For safe distance.

Images