Moving mechanical arm whole body configuration optimization method based on hierarchical quadratic programming

Abstract

The invention discloses a method for optimizing the whole-body configuration of a mobile manipulator based on hierarchical quadratic programming, which comprises the following steps of: firstly, selecting generalized coordinates capable of completely representing the configuration of the mobile manipulator, deducing the relationship between generalized speed and tail end speed, and establishing a method for establishing a whole-body kinematics model of the mobile manipulator; secondly, establishing a hierarchical quadratic programming framework, designing physical constraints such as joint limiting, obstacle avoidance and tail end trajectory tracking into equality and inequality constraint forms, taking a tail end tracking slack variable as a first-layer optimization function, taking operability as a second-layer optimization function, and taking a second-layer optimization function as a second-layer optimization function; and introducing slack variables into a tail end trajectory tracking task of the hierarchical quadratic programming framework to design the slack variables into an equality constraint form. The method for optimizing the whole-body configuration of the mobile mechanical arm is safer and more accurate in movement, the priority of the task is designed and executed through the hierarchical quadratic programming technology, and a technical scheme is provided for the high-precision task of the mobile mechanical arm in a complex environment.

Description

Technical Field

[0001] This invention relates to, and more particularly to, a method for optimizing the overall configuration of a mobile robotic arm based on hierarchical quadratic programming. Background Technology

[0002] Mobile robotic arm configuration optimization technology is a current research hotspot in the field of robotics, and its application background is closely related to the rapid development of industrial automation, intelligent services, and special operations. With the increasing demand for intelligent manufacturing and flexible production, the limitations of traditional fixed-base robotic arms are becoming increasingly apparent, while composite systems integrating mobile platforms and multi-degree-of-freedom robotic arms are showing significant advantages. In industrial scenarios, mobile robotic arms need to complete complex tasks such as material handling, precision assembly, and quality inspection. Their performance is highly dependent on configuration optimization technology—that is, coordinating the joint movements of the robotic arm with the posture of the mobile platform to achieve efficient, stable, and safe operation. For example, in an automotive manufacturing workshop, a mobile robotic arm needs to adjust its posture in a confined space to complete welding or screw tightening. Configuration optimization technology can calculate the optimal motion trajectory in real time, avoiding singular configurations of the robotic arm or collisions with the environment, while simultaneously considering energy consumption and efficiency. Similarly, in the logistics and warehousing field, facing dynamically changing shelves and diverse items, mobile robotic arms must quickly generate gripping configurations and plan movement paths. Optimization algorithms need to comprehensively consider the accessibility of the robotic arm, the stability of the platform, and the overall movement time to meet the stringent requirements of the e-commerce industry for sorting speed and accuracy.

[0003] In non-industrial fields such as medical rehabilitation, home services, and specialized operations, mobile robotic arm configuration optimization technology also plays a crucial role. Medical surgical robots require sub-millimeter precision within limited operating spaces; configuration optimization must not only ensure accurate instrument positioning but also guarantee that the robotic arm avoids sensitive areas during movement, ensuring patient safety. Service robots, such as those assisting the elderly in their homes or food delivery equipment, rely on configuration optimization technology to adaptively adjust the extension angle and movement speed of the robotic arm in dynamic home environments, avoiding collisions with obstacles or pedestrians. However, this technology still faces challenges such as multi-object conflicts, insufficient real-time performance, and environmental uncertainties, requiring continuous exploration of more efficient algorithms and more robust control architectures. Summary of the Invention

[0004] The technical problem to be solved by this invention is to address the shortcomings of existing technologies in handling conflict tasks and to provide a method for optimizing the whole-body configuration of a mobile robotic arm based on hierarchical quadratic programming, taking into account both physical and environmental constraints.

[0005] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0006] A method for optimizing the overall configuration of a mobile robotic arm based on hierarchical quadratic programming includes the following steps:

[0007] Step 1): Select generalized coordinates that can fully characterize the configuration of the mobile robotic arm. Calculate the first derivative of the generalized coordinates to obtain the generalized velocity of the mobile robotic arm. Calculate the second derivative of the generalized coordinates to obtain the generalized acceleration of the mobile robotic arm. Fit the relationship between the generalized velocity, generalized acceleration, and the desired acceleration of the end effector of the mobile robotic arm to establish a whole-body kinematic model of the mobile robotic arm.

[0008]

[0009] in, Let l represent the pose of the end effector in the world reference coordinate system, where l is its dimension. and These represent the end effector's velocity and acceleration, respectively. For the generalized coordinates of the moving robotic arm, ξ is the generalized coordinate vector of the mobile platform of the mobile robotic arm in the world coordinate system, where ξ is the coordinate vector of q. b Dimensions m is the generalized coordinate vector of the mobile robotic arm in the world coordinate system; q m The dimension is also the number of joints in the robotic arm; These represent the generalized velocity and generalized acceleration of the moving robotic arm, respectively. It is the Jacobian matrix for mobile platforms. It is the Jacobian matrix of the robotic arm. It is the Jacobian matrix of the mobile robotic arm. It is the matrix obtained by differentiating J with respect to time;

[0010] Step 2) Establish a hierarchical quadratic programming framework, design physical constraints such as joint limits, obstacle avoidance, and end-point trajectory tracking as equal and inequality constraints, use end-point tracking slack variables as the first-level optimization index, and use operability as the second-level optimization index; and introduce slack variables into the end-point trajectory tracking task of the hierarchical quadratic programming framework and design it as equal constraints.

[0011] The structure of the first level quadratic programming framework is as follows:

[0012]

[0013] Where s represents the slack variable for the end-effector trajectory tracking task; W represents the motion assignment matrix, which determines which degrees of freedom of the mobile robotic arm are activated to complete the desired task. This represents the adaptive control law for acceleration of the mobile platform, and the influence function of end-effector acceleration. This indicates the impact of the additional acceleration imparted to the mobile platform by adaptive motion assignment technology on the end-effector acceleration. Let B represent the projected Jacobian matrix, and let B represent the adaptive acceleration constraint factor. These represent the upper and lower bounds of the generalized acceleration correction, respectively.

[0014] The input to the second-level quadratic programming structure in the hierarchical quadratic programming framework is the optimal slack variable value s obtained from the first-level quadratic programming structure. * The optimization goal is to improve operability, specifically as follows:

[0015]

[0016] Where H represents the weight matrix. Equivalent to Indicates the direction of acceleration for improved operability;

[0017] Step 4): Solve for the generalized acceleration of the mobile robotic arm to be optimized based on the hierarchical quadratic programming framework. The generalized velocity of the mobile robotic arm is obtained through numerical integration. As a control signal, it enables the control of the mobile robotic arm.

[0018] As a further optimization scheme of the hierarchical quadratic programming-based whole-body configuration optimization method for mobile robotic arms of the present invention, the specific steps of step 1) are as follows:

[0019] Step 1.1), let ∑ w Σ m Let q represent the world reference coordinate system and the robotic arm reference coordinate system, respectively; b =[x b ,y b ,θ b ] T Indicates the mobile platform in ∑ w The following pose, x b y b θ b These represent the mobile platform in the world reference coordinate system ∑ w The X-axis coordinate, Y-axis coordinate, and angle around the Z-axis are shown below.

[0020] The Jacobian matrix of the mobile platform e y e z They represent the ends in ∑ m The y-axis and z-axis coordinates are shown below.

[0021] Step 1.2), by analyzing the contribution of the velocities of each joint of the robotic arm to the end effector velocity and angular velocity, we obtain the following: m Jacobian matrix of the robotic arm When all joints of the robotic arm are rotary joints z j The Z-axis of the j-th joint coordinate system of the robotic arm is in ∑ m The following represents p e The end position is in ∑ m The following represents p j The position of the origin of the coordinate system of the j-th joint of the robotic arm is in ∑ m The following is a representation;

[0022] Through ∑ m With ∑ w Rotation matrix between The robotic arm is in ∑ m Jacobian matrix under Transformed into ∑ w Jacobian matrix under

[0023] Step 1.3) By combining the kinematic models of the mobile platform and the robotic arm, the forward kinematic model of the mobile robotic arm at the velocity level is obtained. This refers to the full-body Jacobian matrix of the mobile robotic arm;

[0024] Differentiating the forward kinematics model of the mobile robotic arm at the velocity level yields the forward kinematics model of the mobile robotic arm at the acceleration level:

[0025] As a further optimization scheme of the hierarchical quadratic programming-based whole-body configuration optimization method for mobile robotic arms of the present invention, the specific steps of step 2) are as follows:

[0026] Step 2.1): Obtain the position q of each joint of the robotic arm at intervals T. i With speed Calculate the current moment by combining the physical constraints of joint limits, joint speed limits, and joint acceleration limits of the robotic arm. and Specifically as follows:

[0027]

[0028] Among them, Q max Q min V represents the maximum and minimum physical hard boundary constraints of the joint position, respectively. max V min A represents the physical hard boundary constraints for the maximum and minimum joint velocities, respectively. max A min ξ and q represent the maximum and minimum physical hard boundary constraints of the joint acceleration, respectively; i = 1, 2, ..., ξ; T is the sampling time; q i and Let $\mathbf{i}$ represent the position and velocity of the $i$-th joint at the current moment, respectively. The maximum function $max{·}$ represents the maximum value among all variables, and the minimum function $min{·}$ represents the minimum value among all variables. The upper boundary of the safety judgment is $Q$. upper,i With the lower boundary of safety determination Q lower,i The definition is as follows:

[0029]

[0030] Step 2.2): Select the collision inspection feature point of the mobile robotic arm, calculate the minimum circumscribed sphere of the obstacle using sensors such as vision, obtain the position of the feature point, the center position of the obstacle, and the radius d1 of the circumscribed sphere of the obstacle, and then obtain the feature point-obstacle center position vector. Feature point - obstacle center position unit vector Feature point - minimum position vector of obstacle boundary obstacle approach speed v is the velocity at the feature point;

[0031] Calculate the projected Jacobian matrix J based on the full-body Jacobian matrix of the mobile robotic arm. Derivative of the projected Jacobian matrix And the adaptive acceleration constraint factor B, as detailed below:

[0032]

[0033] Step 2.3), according to the robotic arm's Jacobian matrix J m Calculate the first derivative of the square operability γ And further, the second derivative is obtained. Comprehensive robotic arm joint speed feedback Specifically as follows:

[0034]

[0035] Among them, square operability and Represent γ to q respectively m First and second partial derivatives, 0 ξ×1 Let ξ be a column vector with all elements equal to 0. α1 is a preset operability gradient weight coefficient and α2 is a preset damping weight coefficient. Both α1 and α2 are positive numbers.

[0036] Step 2.4): The motion assignment matrix W is used to determine which degrees of freedom of the mobile robotic arm are activated to complete the desired task; initially, the motion assignment matrix is ​​set to... The mobile platform is not assigned motion; when the robotic arm's Jacobian matrix J mWhen the minimum singular value is less than a specific value, the robotic arm is determined to be in a singular configuration. At this time, the element W(i,i) = 1, i = 1, 2, ..., ξ in the i-th row and i-th column of the motion assignment matrix W is set.

[0037] The adaptive control law for acceleration of the mobile platform is shown in the following equation:

[0038]

[0039] Step 2.5), based on the motion transmission relationship of the rigid body, obtain The influence of terminal acceleration is determined to determine the terminal acceleration influence function.

[0040] Based on the whole-body Jacobian matrix J of the mobile robotic arm and its differential Furthermore, a slack variable s is introduced to rewrite the forward kinematics model of the mobile robotic arm, serving as an equality constraint in the hierarchical quadratic programming framework:

[0041]

[0042] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0043] 1. The full-body configuration optimization method for mobile robotic arms based on hierarchical quadratic programming designed in this invention can handle environmental constraints including joint physical limits and obstacles, and can execute different tasks in order of priority. As a comprehensive configuration optimization method, it is not limited to the double-layer quadratic programming structure proposed in this invention, and can be extended according to actual task needs. This method has strong versatility.

[0044] 2. Compared to traditional optimization frameworks based on single-level quadratic programming, this approach is less effective because it cannot prioritize different tasks and struggles to handle conflicting tasks to achieve optimal performance. Furthermore, compared to task-priority-based motion planning methods, it cannot handle inequality constraints or effectively address infeasibility constraints. Additionally, the computation time required for the generalized inverse involved in the solution process is longer than that of quadratic programming structures.

[0045] 3. The hierarchical quadratic programming-based whole-body configuration optimization method for mobile robotic arms designed in this invention can handle the trajectory tracking and configuration optimization problems of robotic arms in complex environments, ensuring that the robotic arm meets joint physical constraints and obstacle avoidance, and guaranteeing the safety and reliability of the robotic arm operation process. Attached Figure Description

[0046] Figure 1 Mobile robotic arm and ∑ w ,∑ b ,∑ m ,∑ee Schematic diagram of coordinate system;

[0047] Figure 2 This is a schematic diagram of the top-down view and related kinematic parameters of a mobile robotic arm;

[0048] Figure 3 This is a diagram showing the coordinate system settings of the robotic arm used in the example;

[0049] Figure 4 This is a 3D schematic diagram of the parameters involved in the obstacle avoidance algorithm of this invention;

[0050] Figure 5 This is a schematic diagram of the actual trajectory of the robotic arm's end effector in the example working condition;

[0051] Figure 6 This is a schematic diagram of the tracking error of the robotic arm's end effector in a specific working condition.

[0052] Figure 7 This is a schematic diagram of the expected trajectory and the actual trajectory of the robotic arm's end effector in Example 2.

[0053] Figure 8 This is a schematic diagram of the operability of the robotic arm in the control group of Example Working Condition 2;

[0054] Figure 9 This is a schematic diagram of the operability of the robotic arm under the configuration optimization framework of this invention in Example 2. Detailed Implementation

[0055] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:

[0056] This invention can be implemented in many different forms and should not be considered limited to the embodiments described herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully express the scope of the invention to those skilled in the art. In the drawings, components are enlarged for clarity.

[0057] It should be understood that although the terms first, second, third, etc., may be used herein to describe various elements, components, and / or parts, these elements, components, and / or parts are not limited by these terms. These terms are merely used to distinguish elements, components, and / or parts from one another. Therefore, the first element, component, and / or part discussed below may be a second element, component, or part without departing from the teachings of this invention.

[0058] This invention discloses a method for optimizing the whole-body configuration of a mobile robotic arm based on hierarchical quadratic programming. Based on the whole-body kinematic model of the mobile robotic arm, a hierarchical quadratic programming framework is proposed, which comprehensively considers physical constraints and environmental constraints. Furthermore, an adaptive motion allocation technique is proposed to correct the forward kinematic model of the mobile robotic arm and introduce relaxation variables. Operability is used as the optimization objective, which enables different tasks to be executed in order of priority.

[0059] The present invention includes the following steps:

[0060] Step 1) Select generalized coordinates that can fully characterize the configuration of the mobile robotic arm. Take the first derivative of the generalized coordinates to obtain the generalized velocity of the mobile robotic arm. Take the second derivative of the generalized coordinates to obtain the generalized acceleration of the mobile robotic arm. Fit the relationship between the generalized velocity, generalized acceleration and the expected acceleration of the end effector of the mobile robotic arm to establish a whole-body kinematic model of the mobile robotic arm.

[0061] In step 1), the overall kinematic model of the mobile robotic arm can be obtained from the kinematic models of the two subsystems: the mobile platform and the robotic arm. Kinematic models of the mobile platform and the robotic arm are established separately and then transformed into representations in a unified coordinate system using rotation matrices. For example... Figure 1 As shown, ∑ w , ∑ b , ∑ m , ∑ ee Let these represent the world reference coordinate system, the mobile platform coordinate system, the robotic arm reference coordinate system, and the end effector coordinate system, respectively. The end effector is defined in ∑... w The pose in the middle is represented as ι is its dimension. The generalized coordinate vector of the mobile robotic arm is defined as follows: in It is the generalized coordinate vector of the mobile platform in the world coordinate system. It is the generalized coordinate vector of the robotic arm, and m is q. m The dimension is also the number of joints in the robotic arm. It's worth noting that the generalized velocity / acceleration of the robotic arm is the same as the velocity / acceleration of its joints. We choose the generalized coordinate vector q of the moving robotic arm to ensure it can completely represent the moving robotic arm in ∑ w The configuration in.

[0062] This invention aims to establish a universal symbolic representation method for different mobile platforms without loss of generality. Figure 1 , Figure 2 Taking the Jacobian matrix J of a mobile platform as an example, this paper introduces the Jacobian matrix J of a mobile platform. b The derivation method. Assume q b =[x b ,y b ,θ b ] TGeneralized speed of mobile platforms P b =[x b ,y b ] T This indicates that the mobile platform is in ∑ w The position of P in the middle m The reference coordinate system of the robotic arm is represented by ∑. m The origin of P, d represents P b With P m The horizontal distance between them, the robotic arm end effector relative to the robotic arm reference coordinate system ∑ m The position can be obtained by combining the joint position feedback of the robotic arm with the forward motion model of the robotic arm. ∑ w , ∑ b , ∑ m , ∑ ee direction such as Figure 1 Define the Jacobian matrix J for mobile platforms. b The details are as follows:

[0063]

[0064] Where e y e z They represent the ends in ∑ m The y-axis and z-axis coordinates below.

[0065] By analyzing the contribution of the velocities of each joint of the robotic arm to the end effector velocity and angular velocity, we can obtain the following: m Jacobian matrix of the robotic arm Assuming all joints of the robotic arm are rotary joints, Specifically as follows:

[0066]

[0067] in,

[0068]

[0069] z j The Z-axis of the j-th joint coordinate system is in ∑ m The following represents p e The end position is in ∑ m The following represents p j The position of the origin of the j-th joint coordinate system is in ∑ m The following represents the definition of the coordinate system of each joint of the robotic arm: Figure 3 As shown.

[0070] Through ∑ m With ∑ w Rotation matrix between The robotic arm is in ∑ mJacobian matrix under Transformed into ∑ w Jacobian matrix J under m :

[0071]

[0072] Next, by combining the kinematic models of the mobile platform and the robotic arm, we can obtain the forward kinematic model of the mobile robotic arm at the velocity level:

[0073]

[0074] in This is the whole-body Jacobian matrix of the mobile robotic arm. Furthermore, by differentiating the above equation, we obtain the forward kinematic model of the mobile robotic arm at the acceleration level:

[0075]

[0076] Step 2) Establish a hierarchical quadratic programming framework, design physical constraints such as joint limits, obstacle avoidance, and end-point trajectory tracking as equal and inequality constraints, use end-point tracking slack variables as the first-level optimization index, and use operability as the second-level optimization index; and introduce slack variables into the end-point trajectory tracking task of the hierarchical quadratic programming framework and design it as equal constraints.

[0077] The structure of the first level of the hierarchical quadratic programming framework is shown below:

[0078]

[0079] The first-level quadratic programming structure introduces slack variables into the equality constraints corresponding to the terminal desired trajectory tracking task. The terminal tracking task is prioritized after the obstacle avoidance task, and 's' is used as the sole optimization metric. Therefore, even when the terminal tracking task and the obstacle avoidance task conflict, the terminal tracking error can be minimized while ensuring obstacle avoidance.

[0080] The structure of the second level of the hierarchical quadratic programming framework is shown below:

[0081]

[0082] The minimum slack variable s obtained by solving the first-level quadratic programming structure * As a constant, s is used to replace the original s, thereby solving the first-level quadratic programming structure to obtain the minimum slack variable s. * All corresponding As the feasible region of the second-level quadratic programming structure, the task of tracking the desired end-effector trajectory is prioritized over minimizing the generalized velocity of the mobile robotic arm and maximizing the operability optimization task.

[0083] The following section will elaborate on the detailed derivation of the expressions for physical constraints, obstacle avoidance, and operability optimization in step 2) within the hierarchical quadratic programming framework:

[0084] In step 2.1), the motion of the mobile robotic arm is planned at the acceleration level. Therefore, physical constraints such as joint limits, joint velocity limits, and joint acceleration limits need to be uniformly transformed into acceleration-level constraints, as follows:

[0085]

[0086] For each degree of freedom of motion of a mobile robotic arm, the upper and lower bounds of its acceleration are defined as follows:

[0087]

[0088] Among them, Q max / Q min V max / V min and A max / A min These represent the maximum / minimum physical hard boundary constraints for joint position, velocity, and acceleration, respectively. i = 1, 2, ..., ξ defines the i-th element of the corresponding vector, and the sampling time is defined as T. i and These represent the position and velocity of the corresponding joint at the current moment, respectively. The maximum function `max{·}` represents the maximum value among all variables, and the minimum function `min{·}` represents the minimum value among all variables. The upper boundary of the safety judgment is Q. upper,i With the lower boundary of safety determination Q lower,i The definition is as follows:

[0089]

[0090] Q upper,i With Q lower,i The design ensures that at the current joint velocity, if the current joint position is within the judgment range (q) i ≤Q lower,i or q i ≥Q upper,i The robotic arm can move to Q max / Q min Stop before.

[0091] In step 2.2), when the robotic arm performs its task, contacting an obstacle at a high speed may damage its mechanical structure or electronic components, reducing the accuracy of the task and even forcing its termination. Therefore, obstacle avoidance takes priority over the robotic arm's trajectory tracking. Feature points on each link are selected, ensuring that the acceleration of each feature point relative to the obstacle's center position vector is always less than a specific negative value. This ensures that the approach speed of each feature point to the obstacle continuously decreases, and the approach speed must decrease to 0 before contacting the obstacle. Furthermore, the above obstacle avoidance strategy is incorporated into a hierarchical quadratic programming framework in the form of inequality constraints, as follows:

[0092]

[0093]

[0094] Among them, the projected Jacobian matrix Derivative of the projected Jacobian matrix Feature point - obstacle center position vector Feature point - obstacle center position unit vector Feature point - minimum position vector of obstacle boundary obstacle approach speed v is the velocity at the feature point, and the geometric description of each parameter is as follows: Figure 4 As shown.

[0095] To avoid excessive response of the mobile robotic arm to obstacles, obstacle avoidance only takes effect when the reference point is d2 distance from the obstacle's center, where d1 represents the radius of the obstacle's envelope sphere. In the above formula... The physical meaning is that the Cartesian acceleration at the feature point is in the direction vector The design purpose of the adaptive acceleration constraint factor B on the projection is to ensure that, under the current conditions... If but When the feature point - obstacle boundary minimum position vector component At this point, obstacle avoidance can be achieved simply by constraining the projected acceleration in other directions, therefore design B... i =0; when That is, when v is below ∑, the feature point moves closer to the obstacle, and at this time the direction component of the adaptive acceleration constraint factor... Ensure the acceleration direction remains above ∑, the feature point decelerates towards the obstacle and stops approaching before contact; when When v is above ∑, the feature point moves away from the obstacle. At this time, the acceleration direction can be below ∑ to reduce the loss of available workspace for the robotic arm due to obstacle avoidance. Thus, when obstacle avoidance conflicts with other tasks, it can maximize the optimization performance for other tasks. ensure Always satisfy the condition to avoid feature points moving back and forth towards obstacles, causing acceleration to oscillate back and forth.

[0096] In step 2.3), to avoid the moving robotic arm approaching singular configurations, a squared operability is selected. As the optimization objective, solve for γ with respect to q. m First-order partial derivatives With second-order partial derivatives Design operability optimized for acceleration direction Specifically as follows:

[0097]

[0098] Here, α1 and α2 are two positive numbers. The operability optimization task is... The form is written into the second-level quadratic programming structure of the hierarchical quadratic programming framework, thereby maximizing Guarantee solution vector Move as close as possible to the direction where operability increases the fastest.

[0099] In step 2.4), during the movement of the mobile robotic arm, the positioning accuracy of the mobile platform is much lower than that of the robotic arm due to inertial sensor errors and slippage. Therefore, it is considered to allocate more motion to the robotic arm joints to improve the end-effector trajectory tracking accuracy. This invention proposes a motion adaptive allocation method, which aims to preferentially allocate motion to the robotic arm. Motion will only be allocated to the mobile platform when the robotic arm cannot complete the task independently, such as when the desired tracking trajectory exceeds the robotic arm's workspace or is near a singular configuration.

[0100] In the proposed adaptive allocation method, a motion allocation matrix W is first designed. W determines which degrees of freedom of the mobile robotic arm are activated to complete the desired task. W is an n-dimensional diagonal matrix. If the i-th diagonal element of W is 1, then the i-th degree of freedom of the mobile robotic arm is activated; otherwise, if the i-th diagonal element of W is 0, then the i-th degree of freedom of the mobile robotic arm does not participate in the execution of the desired task.

[0101] Initially, the motion assignment matrix is ​​set to The mobile platform is not assigned motion. The Jacobian matrix J for the robotic arm... m Perform SVD decomposition when J m The minimum singular value σ minWhen <δ, the robot arm is determined to be near a singular configuration (when the robot arm is close to the boundary of the workspace, it is also a singular configuration). At this time, the motion allocation factor W(i,i)=1,i=1,2,...,ξ corresponding to the motion degree of freedom of the mobile platform is set, and the mobile platform is assigned motion to improve the robot arm configuration.

[0102] Motion planning for the mobile robotic arm is performed at the acceleration level. When the adaptive motion planning algorithm no longer assigns motion to the mobile platform, i.e., the acceleration of the mobile platform in all directions is zero, the mobile platform will move at a constant speed. However, our goal is for the mobile platform to tend to come to a standstill when no motion is assigned, thereby achieving high end-effector motion accuracy. The adaptive acceleration control law for the mobile platform is designed as follows:

[0103]

[0104] Step 2.5): When W(i,i)≠0, i=1,2,...,ξ, the mobile platform is required to be assigned motion, and the acceleration of the mobile platform is equal to the value solved under the optimization framework. When W(i,i)=0, i=1,2,...,ξ, the acceleration of the moving platform solved under the optimization framework is zero. It is worth noting that... and The design ensures that the speed of the moving platform changes to zero at most after one time step without changing its sign, thus preventing unwanted oscillations. Because... This is an additional acceleration applied to the mobile platform; this motion will affect the end-effector acceleration. The combined motion allocation matrix W and the end-effector acceleration influence function... Furthermore, a slack variable s is introduced to rewrite the forward kinematics model of the mobile robotic arm, serving as an equality constraint in the hierarchical quadratic programming framework:

[0105]

[0106] Step 3) Solve for the generalized acceleration of the mobile robotic arm to be optimized based on the hierarchical quadratic programming framework. The generalized velocity of the mobile robotic arm is obtained through numerical integration. As a control signal, it enables the control of the mobile robotic arm.

[0107] This embodiment uses numerical simulation to illustrate the overall configuration optimization effect of the proposed hierarchical quadratic programming-based mobile robotic arm. The mobile platform used in the simulation achieves motion based on two-wheel differential speed, and the mobile robotic arm adopts a seven-degree-of-freedom redundant robotic arm.

[0108] First, the generalized degrees of freedom that can characterize the configuration of the mobile robotic arm are determined, each coordinate system is set, and the homogeneous transformation matrix between each coordinate system is derived based on the DH method. The influence of each generalized velocity on the end effector velocity is analyzed to obtain the Jacobian matrix J of the mobile robotic arm, and a whole-body kinematic model of the mobile robotic arm is established.

[0109] In this example, the mobile robotic arm has 9 generalized degrees of freedom, and the generalized coordinates represent the rotation angles of the two wheels of the mobile platform and the joint positions of the seven joints of the robotic arm. The coordinate system settings for each joint of the robotic arm are as follows: Figure 3 As shown.

[0110] Secondly, the upper and lower bounds of the bilateral constraints are determined, and the positions q of each joint of the robotic arm are obtained at sampling intervals T. i With speed Calculate the current moment by combining the physical constraints of joint limits, joint speed limits, and joint acceleration limits of the robotic arm. and

[0111] Furthermore, collision inspection feature points are selected by the mobile robotic arm. The minimum circumsphere of the obstacle is calculated using sensors such as vision. The positions of the feature points, the center position of the obstacle, and the radius d1 of the circumsphere of the obstacle are obtained. The projection Jacobian matrix is ​​then calculated. Derivative of the projected Jacobian matrix And the adaptive acceleration constraint factor B.

[0112] For the direction of acceleration in the operability optimization within the second-level quadratic programming framework The calculation first involves calculating the first derivative of the square operability γ. Further, the second derivative is obtained. Designing α1, α2 > 0, and obtaining the combined robotic arm joint velocity feedback...

[0113] Finally, the weight matrix H and adaptive motion allocation control law in the optimization function are designed. H determines the weights of motion allocation for each joint. H is generally set as a diagonal matrix; the larger the elements on the diagonal corresponding to each joint, the greater the amount of motion allocated to that joint during motion allocation. In this example, the weight matrix H is designed as an identity matrix. For adaptive motion allocation, when the minimum eigenvalue of the robotic arm's Jacobian matrix is ​​greater than 0.2, the weights corresponding to the degrees of freedom of the moving platform in the motion allocation matrix W are designed to be 0, and no motion is allocated to the moving platform. Conversely, when the minimum eigenvalue is less than 0.2, the moving platform moves to avoid the robotic arm approaching the workspace boundary and causing singularities.

[0114] To verify the actual effect of the algorithm, the following two sets of working conditions were designed for numerical simulation verification.

[0115] Working Condition 1: End-effector tracking ellipsoidal trajectory. Assuming no obstacles appear within the robot arm's working area, verify the motion accuracy of the mobile robot arm in free space. The end-effector trajectory is as follows: Figure 5 As shown.

[0116] To ensure the convergence of tracking error, a feedback control law is introduced, assuming the desired trajectory follows a path of x. d This indicates that the expected velocity and the expected acceleration are respectively and The actual tracking acceleration of the robotic arm End-point tracking accuracy under operating condition 1 is as follows Figure 6 As shown, ignoring the large initial tracking error caused by the initial position error, the subsequent tracking error remains at 5×10. -4 Within this range, the tracking performance is good.

[0117] Condition 2: The desired trajectory at the end point intersects with the obstacle, that is, the end point trajectory tracking task and the obstacle avoidance task conflict, which verifies the "hierarchical quadratic programming based on hierarchical quadratic programming can realize the execution of each task according to priority" mentioned in this invention.

[0118] like Figure 7 As shown, the red sphere represents the smallest circumscribed sphere of the obstacle, the yellow sphere represents the obstacle avoidance decision sphere with a radius of d², the solid blue line represents the desired trajectory of the terminal, and the dotted green line represents the actual trajectory of the terminal. The desired trajectory of the terminal intersects with the smallest circumscribed sphere of the obstacle; that is, if the terminal performs the tracking task according to the desired trajectory, it will inevitably collide with the obstacle. Because the configuration optimization method proposed in this invention introduces slack variables into the terminal's desired trajectory tracking task, and obstacle avoidance is treated as a constraint, the obstacle avoidance task has a higher priority than the terminal's desired trajectory tracking task. Therefore, the terminal will prioritize avoiding collisions with obstacles, even though this will result in a larger tracking error. Figure 7 This reflects the logic of prioritizing execution. When the end effector enters the yellow obstacle avoidance judgment ball, it will continue to approach the obstacle as it moves along the expected trajectory. At this time, the end effector gradually deviates from the expected trajectory and bypasses the obstacle from above. When the end effector leaves the yellow obstacle avoidance judgment ball, it will resume tracking the expected trajectory.

[0119] In addition, to demonstrate operability and performance optimization, a set of comparative simulations was designed. Based on condition two, the simulations removed the second-level quadratic programming framework. The item serves as a control group, and the operability of the control group is as follows: Figure 8 As shown, the operability under the original framework is as follows: Figure 9 As shown, the control group moved into a singular configuration during the tracking process, while the improved operability avoided this risk.

[0120] This invention applies a hierarchical quadratic programming method to the whole-body configuration optimization of a mobile robotic arm, providing operators with a safer and simpler framework for mobile robotic arm configuration optimization. This framework has strong versatility and can be easily extended to other tasks according to the needs of operators. At the same time, it enables different tasks to be executed in order of priority, avoids complex pseudo-inverse calculations, improves computational efficiency, and reduces the requirements of onboard computing units.

[0121] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.

[0122] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for optimizing the overall configuration of a mobile robotic arm based on hierarchical quadratic programming, characterized in that, Includes the following steps: Step 1): Select generalized coordinates that can fully characterize the configuration of the mobile robotic arm. Calculate the first derivative of the generalized coordinates to obtain the generalized velocity of the mobile robotic arm. Calculate the second derivative of the generalized coordinates to obtain the generalized acceleration of the mobile robotic arm. Fit the relationship between the generalized velocity, generalized acceleration, and the desired acceleration of the end effector of the mobile robotic arm to establish a whole-body kinematic model of the mobile robotic arm. in, ι represents the pose of the end effector in the world reference coordinate system, where ι is its dimension. and These represent the end effector's velocity and acceleration, respectively. For the generalized coordinates of the moving robotic arm, ξ is the generalized coordinate vector of the mobile platform of the mobile robotic arm in the world coordinate system, where ξ is the coordinate vector of q. b Dimensions m is the generalized coordinate vector of the mobile robotic arm in the world coordinate system; q m The dimension is also the number of joints in the robotic arm; These represent the generalized velocity and generalized acceleration of the moving robotic arm, respectively. It is the Jacobian matrix for mobile platforms. It is the Jacobian matrix of the robotic arm. It is the Jacobian matrix of the mobile robotic arm. It is the matrix obtained by differentiating J with respect to time; Step 2) Establish a hierarchical quadratic programming framework, design physical constraints such as joint limits, obstacle avoidance, and end-point trajectory tracking as equal and inequality constraints, use end-point tracking slack variables as the first-level optimization index, and use operability as the second-level optimization index; and introduce slack variables into the end-point trajectory tracking task of the hierarchical quadratic programming framework and design it as equal constraints. The structure of the first level quadratic programming framework is as follows: Where s represents the slack variable for the end-effector trajectory tracking task; W represents the motion assignment matrix, which determines which degrees of freedom of the mobile robotic arm are activated to complete the desired task. This represents the adaptive control law for acceleration of the mobile platform, and the influence function of end-effector acceleration. This indicates the impact of the additional acceleration imparted to the mobile platform by adaptive motion assignment technology on the end-effector acceleration. Let B represent the projected Jacobian matrix, and let B represent the adaptive acceleration constraint factor. These represent the upper and lower bounds of the generalized acceleration correction, respectively. The input to the second-level quadratic programming structure in the hierarchical quadratic programming framework is the optimal slack variable value s obtained from the first-level quadratic programming structure. * The optimization goal is to improve operability, specifically as follows: Where H represents the weight matrix. Equivalent to Indicates the direction of acceleration for improved operability; Step 3) Solve for the generalized acceleration of the mobile robotic arm to be optimized based on the hierarchical quadratic programming framework. The generalized velocity of the mobile robotic arm is obtained through numerical integration. As a control signal, it enables the control of the mobile robotic arm.

2. The method for optimizing the whole-body configuration of a mobile robotic arm based on hierarchical quadratic programming according to claim 1, characterized in that, The specific steps of step 1) are as follows: Step 1.1), let ∑ w ,∑ m These represent the world reference coordinate system and the robotic arm reference coordinate system, respectively. Select q b =[x b ,y b ,θ b ] T Indicates the mobile platform in ∑ w The following pose, x b y b θ b These represent the mobile platform in the world reference coordinate system ∑ w The X-axis coordinate, Y-axis coordinate, and angle around the Z-axis are shown below. Then the Jacobian matrix of the mobile platform e y e z They represent the ends in ∑ m The y-axis and z-axis coordinates are shown below. Step 1.2): By analyzing the contribution of the velocities of each joint of the robotic arm to the end effector velocity and angular velocity, we obtain the result in Σ m Jacobian matrix of the robotic arm When all joints of the robotic arm are rotary joints z j The Z-axis of the j-th joint coordinate system of the robotic arm is in Σ m The following represents p e The end position is in ∑ m The following represents p j The position of the origin of the coordinate system of the j-th joint of the robotic arm is in ∑ m The following is a representation; Through ∑ m With ∑ w Rotation matrix between The robotic arm is in ∑ m Jacobian matrix under Transformed into Σ w Jacobian matrix under Step 1.3) By combining the kinematic models of the mobile platform and the robotic arm, the forward kinematic model of the mobile robotic arm at the velocity level is obtained. This refers to the full-body Jacobian matrix of the mobile robotic arm; Differentiating the forward kinematics model of the mobile robotic arm at the velocity level yields the forward kinematics model of the mobile robotic arm at the acceleration level:

3. The method for optimizing the whole-body configuration of a mobile robotic arm based on hierarchical quadratic programming according to claim 2, characterized in that, The specific steps of step 2) are as follows: Step 2.1): Obtain the position q of each joint of the robotic arm at intervals T. i With speed Calculate the current moment by combining the physical constraints of joint limits, joint speed limits, and joint acceleration limits of the robotic arm. and Specifically as follows: Among them, Q max Q min V represents the maximum and minimum physical hard boundary constraints of the joint position, respectively. max V min A represents the physical hard boundary constraints for the maximum and minimum joint velocities, respectively. max A min ξ and q represent the maximum and minimum physical hard boundary constraints of the joint acceleration, respectively; i = 1, 2, ..., ξ; T is the sampling time; q i and Let $\mathbf{i}$ represent the position and velocity of the $i$-th joint at the current moment, respectively. The maximum function $max{·}$ represents the maximum value among all variables, and the minimum function $min{·}$ represents the minimum value among all variables. The upper boundary of the safety judgment is $Q$. upper,i With the lower boundary of safety determination Q lower,i The definition is as follows: Step 2.2): Select the collision inspection feature point of the mobile robotic arm, calculate the minimum circumscribed sphere of the obstacle using sensors such as vision, obtain the position of the feature point, the center position of the obstacle, and the radius d1 of the circumscribed sphere of the obstacle, and then obtain the feature point-obstacle center position vector. Feature point - obstacle center position unit vector Feature point - minimum position vector of obstacle boundary obstacle approach speed v is the velocity at the feature point; Calculate the projected Jacobian matrix J based on the full-body Jacobian matrix of the mobile robotic arm. Derivative of the projected Jacobian matrix And the adaptive acceleration constraint factor B, as detailed below: Step 2.3), according to the robotic arm's Jacobian matrix J m Calculate the first derivative of the square operability γ And further, the second derivative is obtained. Comprehensive robotic arm joint speed feedback Specifically as follows: Among them, square operability and Represent γ to q respectively m First and second partial derivatives, 0 ξ×1 Let ξ be a column vector with all elements equal to 0. α1 is a preset operability gradient weight coefficient and α2 is a preset damping weight coefficient. Both α1 and α2 are positive numbers. Step 2.4): The motion assignment matrix W is used to determine which degrees of freedom of the mobile robotic arm are activated to complete the desired task; initially, the motion assignment matrix is ​​set to... The mobile platform is not assigned motion; when the robotic arm's Jacobian matrix J m When the minimum singular value is less than a specific value, the robotic arm is determined to be in a singular configuration. At this time, the element W(i,i) = 1, i = 1, 2, ..., ξ in the i-th row and i-th column of the motion assignment matrix W is set. The adaptive control law for acceleration of the mobile platform is shown in the following equation: Step 2.5), based on the motion transmission relationship of the rigid body, obtain The influence of terminal acceleration is determined to determine the terminal acceleration influence function. Based on the whole-body Jacobian matrix J of the mobile robotic arm and its differential Furthermore, a slack variable s is introduced to rewrite the forward kinematics model of the mobile robotic arm, serving as an equality constraint in the hierarchical quadratic programming framework:

Images