Coordinated motion control method of mobile manipulator based on maneuverability adaptive optimization

Abstract

The present invention discloses a coordinated motion control method for a mobile manipulator based on adaptive optimization of maneuverability. By designing a redundant kinematic zero space analysis method for a mobile manipulator, synchronous control of a primary task and a secondary task is achieved. By designing a task space motion allocation algorithm based on adaptive weighted pseudo-inverse of a Jacobian matrix, the end motion accuracy is ensured by considering the difference in vehicle-arm motion characteristics. By designing an extended maneuverability adaptive weighted optimization algorithm based on maneuverability and self-collision avoidance, the safety and maneuverability of the vehicle-arm coordinated motion are improved. By designing a coordinated motion control algorithm for a mobile manipulator based on adaptive estimation of kinematic parameters, finite-time convergence of the end tracking error is achieved. By using a Lyapunov design method, finite-time stability of a controller is achieved, and coordination, response speed and accuracy of continuous operations of the mobile manipulator are improved.

Description

Technical Field

[0001] The invention belongs to the field of robot motion control, and in particular relates to a coordinated motion control method for a mobile mechanical arm based on adaptive optimization of maneuverability. Background Art

[0002] Mobile robotic arms have become a research hotspot and development direction in the robotics industry in recent years. They are gradually being applied to agricultural harvesting, service guidance, industrial loading and unloading, and inspection, and have broad market prospects. Although the integration of full-space mobility and flexible operation capabilities gives mobile robotic arms outstanding environmental interaction capabilities, it also brings difficulties in high-precision coordinated operation planning and control. That is, kinematic redundancy enables mobile robotic arms to complete multiple tasks at the same time. How to consider the differences in motion characteristics of the mobile platform and the operating arm, and coordinate redundant degrees of freedom to achieve optimal operation control, is the key challenge facing the autonomous operation of mobile robotic arms.

[0003] Traditional coordinated motion control methods for mobile manipulators mostly focus on decoupling control strategies, that is, using an upper-level planner to directly assign task targets to the mobile platform and the manipulator, and then designing independent controllers for the two to complete the operation control. It can be seen that the upper-level planner cannot achieve coordinated allocation of redundant degrees of freedom for multi-task synchronization of the mobile manipulator, and can only be used for discrete sequential operation control of the mobile manipulator, such as mobile grasping, mobile picking, and mobile grinding and polishing, that is, the manipulator performs the operation task after the mobile platform moves to the target position. Another type of research direction for coordinated motion control of mobile manipulators focuses on joint control strategies, that is, by solving the whole-body redundant kinematics of the mobile manipulator to simultaneously complete the main task and the secondary task, it can meet the vehicle-arm coordinated motion control requirements for continuous operations of the mobile manipulator, such as continuous spraying and mobile cleaning, which is also a practical problem to be solved by the controller designed by the present invention. At present, methods such as multi-task hierarchical control and weighted least squares method have been applied to the redundant kinematics coordinated control of mobile manipulators, but these methods do not consider the differences in motion characteristics between the mobile platform and the manipulator, such as positioning accuracy differences, joint position limitations, and self-collision, which will affect the accuracy of the terminal operation. In addition, the above redundant kinematic coordination control methods mostly assume that the kinematic parameters of the mobile robot are precisely known, but this assumption is often difficult to establish due to the manufacturing errors and measurement errors of the connecting rods of the mobile robot, thus limiting the practical application of the relevant control methods. Summary of the invention

[0004] In order to solve the shortcomings of the prior art, while achieving the main task of finite time convergence of the terminal tracking error, the secondary task of expanding the maneuverability optimization is met, so as to effectively improve the accuracy of the continuous operation of the mobile manipulator and the coordination of the redundant degrees of freedom. The present invention adopts the following technical solutions:

[0005] The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization includes the following steps:

[0006] Step S1: construct the kinematic equation of the mobile manipulator, and design a redundant kinematic analytical algorithm based on the Jacobian matrix null space interior motion of the equation for synchronous control of the main task and the secondary task of the mobile manipulator;

[0007] Step S2: by using the redundant kinematics analytical algorithm and the Jacobian matrix of the kinematic parameter estimation value, a task space motion allocation algorithm based on the Jacobian matrix adaptive weighted pseudo-inverse is designed, and the motion allocation weights of the mobile platform and the manipulator are adaptively adjusted according to the difference in motion characteristics of the mobile platform and the manipulator arm, that is, most of the motion in the task space of the mobile manipulator arm is allocated to the mobile platform only when the position of the manipulator arm joint is near the upper and lower bounds, otherwise its task space motion is mainly allocated to the manipulator arm, thereby ensuring the motion accuracy of the end of the mobile manipulator arm;

[0008] Step S3: Based on the Jacobian matrix of the estimated values ​​of the kinematic parameters, an operability index based on an exponential growth rate is designed, and the mobile manipulator is kept away from the singularity point in the joint space by minimizing the operability index. The cost function of self-collision avoidance is designed by the Euclidean distance between the nearest links in the mobile manipulator. An adaptive optimization algorithm for the extended operability of the mobile manipulator is designed by the operability index and the self-collision cost function, and a zero-space interior point motion control law of the mobile manipulator is designed in combination with the task space motion allocation algorithm of the Jacobian matrix adaptive weighted pseudo-inverse, so as to adjust the joint space configuration of the mobile manipulator through the redundant degrees of freedom of the mobile manipulator, so as to achieve optimization of the operability of the vehicle-arm coordinated motion and self-collision avoidance, while not affecting the main task of the mobile manipulator end tracking, and improving the operability under the premise of ensuring the safety of the vehicle-arm coordinated motion;

[0009] Step S4: Based on the linearized kinematic equations of the mobile manipulator, auxiliary matrix variables and vectors are defined to design a coordinated motion control algorithm for the mobile manipulator and an adaptive finite time estimation algorithm for uncertain kinematic parameters, which are used to achieve the main task of finite time convergence of the end tracking error and the secondary task of extended operability optimization;

[0010] Step S5: Construct the Lyapunov equation, and based on the coordinated motion control algorithm of the mobile robotic arm, obtain the trajectory tracking error of the task space end, and coordinate the motion control of the mobile robotic arm through the convergence of the trajectory tracking error and the error of the kinematic parameter estimation within a finite time, so as to achieve the finite time stability of the coordinated motion controller of the mobile robotic arm and improve the vehicle-arm coordination, response speed and accuracy of the continuous operation of the mobile robotic arm.

[0011] Furthermore, in step S1, the kinematic equation of the mobile robot arm is as follows:

[0012]

[0013] Among them, x represents the pose vector of the end of the mobile robot in the task space, represents the generalized joint space coordinates of the mobile manipulator, q p =[x p ,y p ,θ p ] T and q a =[θ1,…,θ n ] T Represent the joint space coordinates of the mobile platform and the robotic arm, respectively, p ,y p Represents the horizontal and vertical coordinates of the mobile platform, θ p represents the rotation angle of the mobile platform, θ represents the joint angle of the robot arm, n represents the number of joints of the robot arm, and They represent the task space velocity and joint space velocity of the end of the mobile robot respectively; φ represents the kinematic parameters of the mobile robot, J(q,φ) represents the Jacobian matrix of the mobile robot and its elements satisfy the linearization relationship with φ.

[0014] Based on the kinematic equation of the mobile robot, the linearized kinematic equation of the mobile robot is obtained as follows:

[0015]

[0016] in, The kinematic state regression matrix represents the kinematics. Due to the influence of manufacturing assembly errors and load deformation in actual engineering, the kinematic parameters φ of the mobile robot cannot be accurately obtained. Therefore, accurate estimation of φ is the key to improving the accuracy of its task space operation.

[0017] The redundant kinematics analysis algorithm based on the point motion in the J(q,φ) null space is as follows:

[0018]

[0019] in, represents the joint space velocity corresponding to the J(q,φ) null-space secondary task, represents a positive definite weight matrix, represents the weighted pseudo-inverse matrix of J(q,φ), I represents the identity matrix, and it is easy to know the motion corresponding to the secondary task Will not affect the movement corresponding to the main task of the terminal The redundant kinematics analysis algorithm can realize the synchronous control of the primary task and secondary task of the mobile robot.

[0020] Furthermore, in step S2, the task space motion allocation algorithm based on Jacobian matrix adaptive weighted pseudo-inverse is as follows:

[0021]

[0022] in, represents the estimated values ​​of the kinematic parameters, represents the Jacobian matrix based on the kinematic parameter estimates, represents the weighted pseudo-inverse matrix, represents the joint space velocity corresponding to the J(q,φ) null-space secondary task, and ∑ represents the adaptive weighting matrix.

[0023] Furthermore, the formula of the adaptive weighting matrix ∑ is as follows:

[0024]

[0025] The adaptive weighting parameter η i The formula is as follows:

[0026]

[0027] Among them, θ i,min and θ i,max Respectively represent the position of the i-th joint of the robot arm θ i The lower and upper bounds of ρ1>0 represent the constant coefficient that determines the rate of change of the distribution weight. Represents a constant greater than 0.

[0028] Furthermore, in step S3, the adaptive optimization algorithm for the extended operability of the mobile robot arm is as follows:

[0029]

[0030] Where q represents the generalized joint space coordinates including the mobile platform and the robotic arm, ω(q) represents the operability index based on the exponential growth rate, Indicates the maneuverability of the mobile robot. represents the cost function of self-collision avoidance, It represents the Euclidean distance between the two closest links among all the links of the mobile robot, and v represents the adaptive weighting factor.

[0031] Furthermore, the formula of the adaptive weighting factor σ is as follows:

[0032]

[0033] in, Represents the self-collision safety distance of the mobile robot, that is, when Self-collision will occur when the robot moves. This algorithm can improve the operability of vehicle-arm coordinated motion while ensuring the self-collision safety of the mobile robot arm and the accuracy of vehicle-arm motion distribution.

[0034] Combined with the task space motion allocation algorithm based on Jacobian matrix adaptive weighted pseudo-inverse, the null space interior point motion control law of the mobile manipulator is as follows:

[0035]

[0036] Among them, κ0>0 represents the controller gain constant. This control law adjusts the joint space configuration of the mobile robot through the redundant degrees of freedom, realizes the optimization of the maneuverability of the vehicle-arm coordinated motion and self-collision avoidance, while not affecting the main task of the mobile robot end tracking.

[0037] Furthermore, in step S3, in order to avoid singular points in the joint space motion of the mobile robot, an operability index based on an exponential growth rate is designed as follows:

[0038]

[0039] Where q represents the generalized joint space coordinates including the mobile platform and the robotic arm, represents the estimated values ​​of the kinematic parameters, represents the Jacobian matrix based on the kinematic parameter estimates, represents the maneuverability of the mobile robot, ρ2>0 represents the change rate of the maneuverability index, It indicates the monitoring domain value where the maneuverability index increases sharply when the mobile robot approaches the singularity point. Therefore, minimizing ω(q) can make the mobile robot stay away from the singularity point in the joint space and have good maneuverability of vehicle-arm coordinated motion.

[0040] Furthermore, in step S3, in order to avoid the mobile robot from self-collision, the cost function of self-collision avoidance is designed as follows:

[0041]

[0042] Where q represents the generalized joint space coordinates including the mobile platform and the robotic arm, represents the Euclidean distance between the two closest links among all the links of the mobile robot, It represents the distance domain of the warning area where self-collision is about to occur, and ρ3 represents the constant coefficient greater than 0 that affects the rate of change of the cost function for self-collision avoidance.

[0043] Furthermore, in step S4, the auxiliary matrix variable is defined and vector And meet:

[0044]

[0045] Among them, through differentiation and Solved and Used to generate ζ>0 represents the forgetting factor constant;

[0046] The coordinated motion control algorithm of the mobile robot is as follows:

[0047]

[0048] in, represents the control law of the null space interior point motion of the mobile manipulator;

[0049] The adaptive finite-time estimation algorithm for the uncertain kinematic parameter φ is as follows:

[0050]

[0051] Among them, x r represents the expected trajectory of the mobile robot in the task space, e x =xx r represents the task space trajectory tracking error; κ1>0 and κ2>0 are both controller gain constants, and are all positive odd numbers and satisfy κ3>0 and κ4>0 are both gain constants of the kinematic parameter estimation algorithm, which can achieve the main task of finite time convergence of the spatial trajectory tracking error of the mobile manipulator task and the secondary task of extended maneuverability optimization.

[0052] Furthermore, in step S5, the task space trajectory tracking error e x The dynamic equation is as follows:

[0053]

[0054] in, represents the error in the estimation of the kinematic parameters of the mobile manipulator;

[0055] Constructing Lyapunov functions Calculate its first-order time differential and substitute it into the above formula to obtain:

[0056]

[0057] in, Representation Matrix The minimum eigenvalue of the mobile robot arm is obtained by estimating the kinematic parameters of the mobile robot arm. and the terminal trajectory tracking error e x All will converge to zero within a finite time, which proves the finite-time stability of the designed mobile robot arm coordinated motion controller, which can effectively improve the vehicle-arm coordination, response speed and accuracy of its continuous operation.

[0058] The advantages and beneficial effects of the present invention are:

[0059] The extended operability adaptive weighted optimization algorithm of the present invention can realize the adaptive optimization of the operability of the vehicle-arm coordinated motion under the premise of ensuring the self-collision safety of the mobile robotic arm; the designed mobile robotic arm coordinated motion control method can not only realize the main task of finite time convergence of task space trajectory tracking error and the secondary task of extended operability optimization, but also can realize adaptive high-precision motion allocation according to the differences in vehicle-arm motion characteristics, effectively improving the vehicle-arm coordination and response speed and accuracy of the mobile robotic arm's continuous operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 is a flow chart of a method in an embodiment of the present invention.

[0061] Figure 2a It is a schematic diagram of motion distribution results of the rotation angle of the mobile platform changing with time in an embodiment of the present invention.

[0062] Figure 2b The mobile platform in the embodiment of the present invention moves along x p Axis and y p Schematic diagram of the axis motion distribution results.

[0063] Figure 3 It is a schematic diagram of optimizing the operability of the mobile robot vehicle-arm coordinated motion in an embodiment of the present invention.

[0064] Figure 4a It is a schematic diagram of the convergence effect of the x-axis motion error of the mobile robot task space in an embodiment of the present invention.

[0065] Figure 4b It is a schematic diagram of the y-axis motion error convergence effect of the mobile robot task space in an embodiment of the present invention.

[0066] Figure 4c It is a schematic diagram of the convergence effect of the z-axis motion error of the mobile robot task space in an embodiment of the present invention. DETAILED DESCRIPTION

[0067] The specific implementation of the present invention is described in detail below in conjunction with the accompanying drawings. It should be understood that the specific implementation described here is only used to illustrate and explain the present invention, and is not used to limit the present invention.

[0068] A coordinated motion control method for a mobile manipulator based on adaptive optimization of maneuverability is proposed. This method first designs an adaptive weighted optimization algorithm for the extended maneuverability of the mobile manipulator to achieve the optimization of the maneuverability of the coordinated motion of the vehicle-arm, the adaptive adjustment of the weighted motion allocation of the vehicle-arm, and the self-collision avoidance; then a coordinated motion controller for the mobile manipulator based on adaptive estimation of uncertain kinematic parameters is designed. While achieving the main task of finite-time convergence of the end tracking error, the secondary task of extended maneuverability optimization is met, effectively improving the accuracy of the continuous operation of the mobile manipulator and the coordination of redundant degrees of freedom, such as Figure 1 As shown, the specific steps include:

[0069] Step S1: Establish a kinematic model of the mobile manipulator, design a redundant kinematic analysis method based on point motion in the null space, and form a synchronous control framework for the main and secondary tasks of the mobile manipulator;

[0070] The kinematic equations of the mobile robot are established as follows:

[0071]

[0072] In formula (1) Represents the pose vector of the end of the mobile robot in the task space, represents the generalized joint space coordinates of the mobile manipulator, where and Represent the joint space coordinates of the mobile platform and the robotic arm respectively, and represent the task space velocity and joint space velocity of the end of the mobile robot arm, respectively; are the kinematic parameters of the mobile manipulator, Represents the Jacobian matrix of the mobile manipulator and its elements are linearized with φ.

[0073] Based on this, the linearized kinematic equation of the mobile robot is derived as follows:

[0074]

[0075] In formula (2) Represents the state regression matrix of kinematics. Due to the influence of manufacturing assembly errors and load deformation in actual engineering, the kinematic parameters φ of the mobile robot cannot be accurately obtained, so accurate estimation of φ is the key to improving the accuracy of its task space operation.

[0076] Design a redundant kinematics analysis algorithm based on the point motion in the J(q,φ) null space as follows:

[0077]

[0078] In formula (3) and formula (4), represents the joint space velocity corresponding to the J(q,φ) null-space secondary task, is a positive definite weight matrix, represents the weighted pseudo-inverse matrix of J(q,φ). It is easy to know the motion corresponding to the secondary task Will not affect the movement corresponding to the main task of the terminal The redundant kinematics analysis algorithm can realize the synchronous control of the primary task and secondary task of the mobile robot.

[0079] Step S2: Design a task space motion allocation algorithm based on Jacobian matrix adaptive weighted pseudo-inverse, adaptively adjust the vehicle-arm motion allocation weight based on the motion characteristic difference between the mobile platform and the manipulator arm, and ensure the motion accuracy of the end of the mobile manipulator arm;

[0080] Due to problems such as uneven ground disturbance and incomplete motion constraints, the motion accuracy of the mobile platform is often worse than that of the robot arm. Therefore, the expected motion of the task space at the end of the mobile robot arm can be allocated more to the robot arm to ensure its operation motion accuracy. In order to achieve the above goals, define is the estimated error of the kinematic parameters of the mobile manipulator, where is the estimated value of the kinematic parameters, and the task space motion allocation algorithm based on the Jacobian matrix adaptive weighted pseudo-inverse is designed as follows:

[0081]

[0082] In formula (5) and formula (6), represents the Jacobian matrix based on the kinematic parameter estimates, is its weighted pseudo-inverse matrix, ∑ represents the adaptive weighting matrix and is designed as:

[0083]

[0084] The adaptive weighting parameter η in formula (7) i Designed to:

[0085]

[0086] In formula (8), θ i,min and θ i,max Represent the lower and upper bounds of the position of the i-th joint of the robot, respectively. ρ1>0 is a constant coefficient that determines the rate of change of the distribution weight. The algorithm adaptively adjusts the vehicle-arm motion allocation weight based on the difference in motion characteristics between the mobile platform and the manipulator, that is, only when the manipulator joint position is near the upper and lower bounds, most of the motion in the task space of the mobile manipulator is allocated to the mobile platform, otherwise, its task space motion is mainly allocated to the manipulator, thereby ensuring the motion accuracy of the end of the mobile manipulator.

[0087] Step S3: designing an extended maneuverability index of a mobile manipulator based on the maneuverability of vehicle-arm coordinated motion and self-collision avoidance and its adaptive optimization algorithm, so as to improve the maneuverability while ensuring the safety of vehicle-arm coordinated motion;

[0088] In order to avoid singularities in the joint space motion of the mobile robot, the maneuverability index based on exponential growth rate is designed as follows:

[0089]

[0090] In formula (9) and formula (10), represents the maneuverability of the mobile robot, ρ2>0 represents the change rate of the maneuverability index, The monitoring threshold that characterizes the sharp increase in the maneuverability index when the mobile robot approaches the singularity point. Therefore, minimizing ω(q) can make the mobile robot stay away from the singularity point in the joint space and have good maneuverability of vehicle-arm coordinated motion.

[0091] In order to avoid self-collision of the mobile robot, the cost function of self-collision avoidance is designed as follows:

[0092]

[0093] In formula (11) Represents the Euclidean distance between the two closest links among all the links of the mobile robot, Represents the distance threshold of the warning area where self-collision is about to occur, and ρ3>0 is a constant coefficient that affects the rate of change of the cost function for self-collision avoidance.

[0094] Design an adaptive optimization algorithm for the extended maneuverability of a mobile manipulator based on the maneuverability of vehicle-arm coordinated motion and the self-collision cost function as follows:

[0095]

[0096] And the adaptive weighting factor is designed as follows:

[0097]

[0098] In formula (13) Represents the self-collision safety distance of the mobile robot, that is, when The algorithm can improve the operability of vehicle-arm coordinated motion while ensuring the self-collision safety of the mobile robot arm and the accuracy of vehicle-arm motion distribution.

[0099] Combined with formula (5), the control law of the null space interior point motion of the mobile manipulator is designed as follows:

[0100]

[0101] In formula (14), κ0>0 represents the controller gain constant. This control law adjusts the joint space configuration of the mobile manipulator through the redundant degrees of freedom, achieving the optimization of the maneuverability of the vehicle-arm coordinated motion and self-collision avoidance, while not affecting the main task of the mobile manipulator end tracking.

[0102] Step S4: design a coordinated motion control algorithm for a mobile manipulator based on adaptive finite-time estimation of uncertain kinematic parameters, achieving the primary task of finite-time convergence of the end tracking error, and the secondary task of extended maneuverability optimization;

[0103] Define auxiliary matrix variables and vector And meet:

[0104]

[0105] In formula (15), ζ>0 represents the forgetting factor constant.

[0106] Based on this, the coordinated motion control algorithm of the mobile robot is designed as follows:

[0107]

[0108] And design an adaptive finite-time estimation algorithm for its uncertain kinematic parameters φ as follows:

[0109]

[0110] In formula (16) and formula (17), represents the expected trajectory of the mobile robot in the task space, e x =xx r represents the task space trajectory tracking error; κ1>0 and κ2>0 are controller gain constants, and are all positive odd numbers and satisfy k3>0 and κ4>0 are the gain constants of the kinematic parameter estimation algorithm. The algorithm can achieve the primary task of finite time convergence of the space trajectory tracking error of the mobile manipulator task, as well as the secondary task of extended maneuverability optimization.

[0111] Step S5: The finite-time stability of the coordinated motion controller of the mobile robot arm is realized by the Lyapunov design method, so as to improve the vehicle-arm coordination, response speed and accuracy of the continuous operation of the mobile robot arm.

[0112] The task space trajectory tracking error e can be derived from formula (16): x The dynamic equation is as follows:

[0113]

[0114] Design the Lyapunov function as Calculate its first-order time differential and substitute it into formula (18) to obtain

[0115]

[0116] In formula (19) Estimation error of kinematic parameters of mobile manipulator and the terminal trajectory tracking error e x All will converge to zero in a finite time. This proves the finite time stability of the designed mobile robot arm coordinated motion controller, which can effectively improve the vehicle-arm coordination, response speed and accuracy of its continuous operation.

[0117] The embodiment of the present invention is a mobile manipulator composed of a Mecanum wheeled mobile platform and a six-degree-of-freedom manipulator, whose kinematic parameters are l3=0.140, d3=0.300, d4=0.140, l5=0.375, l6=0.345, d7=0.122, d8=0.122, d9=0.083, and the limit of each joint motion is -πrad≤θ1≤πrad, -π / 2rad≤θ2≤π / 2rad, -7π / 18rad≤θ3≤22π / 18rad, -πrad≤θ4≤πrad, -πrad≤θ5≤πrad, -2πrad≤θ6≤2πrad.

[0118] In the implementation process of the present invention, the expected trajectory of the task space of the mobile robot is designed as follows: Where ω=0.02π, x r10 =3.10,x r20 =-0.12, x r30 = 0.77. The initial velocity of the mobile robot end is set to The initial position deviation is set to e x (0) = [-0.5, -0.6, 0.1] T (m).

[0119] A control law of a coordinated motion controller for a mobile manipulator based on adaptive weighted optimization of maneuverability is shown in formulas (5), (6), (11), (12), (13), (15), and (16). The values ​​of the main parameters of the controller are: g1 / h1=3 / 5、k0=0.5、k1=0.9、k2=0.7、k3=30、k4=20. Based on this, the implementation effect of the coordinated motion control of the mobile robot arm can be obtained. Figure 2a and Figure 2b This indicates that the rotation angle of the mobile platform changes little over time. p Axis and y p The movement of the axis is only to satisfy the large-scale movement of the task space trajectory, which verifies the effectiveness of the task space motion allocation algorithm; Figure 3 as well as Figures 4a to 4c It shows that the task space trajectory tracking error of the mobile robot arm can converge within a limited time. At the same time, the operability of the vehicle-arm coordinated motion can be rapidly increased to keep the joint space configuration of the mobile robot arm away from the singularity point, effectively improving the vehicle-arm coordination, response speed and accuracy of its continuous operation.

[0120] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A coordinated motion control method for a mobile manipulator based on maneuverability adaptive optimization, characterized in that The steps include: Step S1: construct the kinematic equation of the mobile manipulator, and design a redundant kinematic analytical algorithm based on the Jacobian matrix null space interior motion of the equation for synchronous control of the main task and the secondary task of the mobile manipulator; Step S2: by using the redundant kinematics analytical algorithm and the Jacobian matrix of the kinematic parameter estimation value, a task space motion allocation algorithm based on the Jacobian matrix adaptive weighted pseudo-inverse is designed, and the motion allocation weights of the mobile platform and the manipulator are adaptively adjusted according to the difference in motion characteristics of the mobile platform and the manipulator; Step S3: Based on the Jacobian matrix of the estimated values ​​of the kinematic parameters, an operability index based on an exponential growth rate is designed, the mobile manipulator is kept away from the singularity point in the joint space by minimizing the operability index, a cost function for self-collision avoidance is designed by the Euclidean distance between the nearest links in the mobile manipulator, an adaptive optimization algorithm for the extended operability of the mobile manipulator is designed by the operability index and the self-collision cost function, and a null space interior point motion control law of the mobile manipulator is designed in combination with the task space motion allocation algorithm of the Jacobian matrix adaptive weighted pseudo-inverse, so as to adjust its joint space configuration through the redundant degrees of freedom of the mobile manipulator; Step S4: Based on the linearized kinematic equations of the mobile manipulator, auxiliary matrix variables and vectors are defined to design a coordinated motion control algorithm for the mobile manipulator and an adaptive finite time estimation algorithm for uncertain kinematic parameters, which are used to achieve the main task of finite time convergence of the end tracking error and the secondary task of extended operability optimization; Step S5: construct the Lyapunov equation, obtain the trajectory tracking error of the task space end based on the coordinated motion control algorithm of the mobile robot, and coordinate the motion control of the mobile robot by converging the trajectory tracking error and the error of the kinematic parameter estimation within a finite time.

2. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 1 is characterized in that: In step S1, the kinematic equation of the mobile robot arm is as follows: Among them, x represents the pose vector of the end of the mobile robot in the task space, represents the generalized joint space coordinates of the mobile manipulator, q p =[x p ,y p ,θ p ] T and q a =[θ1,…,θ n ] T Represent the joint space coordinates of the mobile platform and the robotic arm, respectively, p ,y p Represents the horizontal and vertical coordinates of the mobile platform, θ p represents the rotation angle of the mobile platform, θ represents the joint angle of the robot arm, n represents the number of joints of the robot arm, and They represent the task space velocity and joint space velocity of the end of the mobile robot arm respectively; φ represents the kinematic parameters of the mobile robot arm, J(q,φ) represents the Jacobian matrix of the mobile robot arm and its elements satisfy the linearization relationship with φ; Based on the kinematic equation of the mobile robot, the linearized kinematic equation of the mobile robot is obtained as follows: in, The state regression matrix representing the kinematics; The redundant kinematics analysis algorithm based on the point motion in the J(q,φ) null space is as follows: in, represents the joint space velocity corresponding to the J(q,φ) null-space secondary task, represents a positive definite weight matrix, represents the weighted pseudo-inverse matrix of J(q,φ), and I represents the identity matrix.

3. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 2 is characterized in that: In step S2, the task space motion allocation algorithm based on Jacobian matrix adaptive weighted pseudo-inverse is as follows: in, represents the estimated values ​​of the kinematic parameters, represents the Jacobian matrix based on the kinematic parameter estimates, represents the weighted pseudo-inverse matrix, represents the joint space velocity corresponding to the J(q,φ) null-space secondary task, and Σ represents the adaptive weighting matrix.

4. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 3 is characterized in that: The formula of the adaptive weighting matrix ∑ is as follows: The adaptive weighting parameter η i The formula is as follows: Among them, θ i,min and θ i,max Respectively represent the position of the i-th joint of the robot arm θ i The lower and upper bounds of ρ1>0 represent the constant coefficient that determines the rate of change of the distribution weight. Represents a constant greater than 0.

5. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 3 is characterized in that: In step S3, the adaptive optimization algorithm for the extended operability of the mobile robot arm is as follows: Where q represents the generalized joint space coordinates including the mobile platform and the robotic arm, ω(q) represents the operability index based on the exponential growth rate, Indicates the maneuverability of the mobile robot. represents the cost function of self-collision avoidance, represents the Euclidean distance between the two closest links among all the links of the mobile robot, and σ represents the adaptive weighting factor.

6. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 5, characterized in that: The formula of the adaptive weighting factor σ is as follows: in, Represents the self-collision safety distance of the mobile robot, that is, when A self-collision will occur; Combined with the task space motion allocation algorithm based on Jacobian matrix adaptive weighted pseudo-inverse, the null space interior point motion control law of the mobile manipulator is as follows: Where κ0>0 represents the controller gain constant.

7. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 5, characterized in that: In step S3, the operability index based on the exponential growth rate is as follows: Where q represents the generalized joint space coordinates including the mobile platform and the robotic arm, represents the estimated values ​​of the kinematic parameters, represents the Jacobian matrix based on the kinematic parameter estimates, represents the maneuverability of the mobile robot, ρ2>0 represents the change rate of the maneuverability index, The monitoring threshold value indicating that the maneuverability index increases sharply when the mobile robot approaches the singularity point.

8. The coordinated motion control method of a mobile robot arm based on maneuverability adaptive optimization according to claim 5, characterized in that: In step S3, the cost function of self-collision avoidance is as follows: Where q represents the generalized joint space coordinates including the mobile platform and the robotic arm, represents the Euclidean distance between the two closest links among all the links of the mobile robot, It represents the distance domain of the warning area where self-collision is about to occur, and ρ3 represents the coefficient greater than 0 that affects the rate of change of the cost function of self-collision avoidance.

9. The coordinated motion control method of a mobile manipulator based on maneuverability adaptive optimization according to claim 3 is characterized in that: In step S4, the auxiliary matrix variable is defined and vector And meet: Among them, through differentiation and Solved and Used to generate ζ>0 represents the forgetting factor constant; The coordinated motion control algorithm of the mobile robot is as follows: in, represents the control law of the null space interior point motion of the mobile manipulator; The adaptive finite-time estimation algorithm for the uncertain kinematic parameter φ is as follows: Among them, x r represents the expected trajectory of the mobile robot in the task space, e x =xx r represents the task space trajectory tracking error; k1>0 and κ2>0 are both controller gain constants, and are all positive odd numbers and satisfy κ3>0 and κ4>0 are both gain constants of the kinematic parameter estimation algorithm.

10. The coordinated motion control method of a mobile mechanical arm based on maneuverability adaptive optimization according to claim 9, characterized in that: In step S5, the task space trajectory tracking error e x The dynamic equation is as follows: in, represents the error in the estimation of the kinematic parameters of the mobile manipulator; Constructing Lyapunov functions Calculate its first-order time differential and substitute it into the above formula to obtain: in, Representation Matrix The minimum eigenvalue of the mobile robot arm is obtained by estimating the kinematic parameters of the mobile robot arm. and the terminal trajectory tracking error e x will converge to zero in a finite time.

Images