Repetitive motion planning method and system for three-wheel omnidirectional mobile manipulator with limited motion space

Abstract

The application relates to a three-wheel omnidirectional mobile manipulator motion space limited repetitive motion planning method and system, wherein the method comprises the following steps: considering the pose limit of a mobile platform and the joint angle limit of a manipulator, establishing a kinematics equation of the three-wheel omnidirectional mobile manipulator under the condition of motion space limitation; converting the kinematics equation into a joint nonlinear equation group to determine a general form of a velocity layer motion planning scheme of the three-wheel omnidirectional mobile manipulator; constructing a velocity layer criterion capable of realizing repetitive motion; based on the general form of the velocity layer motion planning scheme and the velocity layer criterion, constructing a repetitive motion planning scheme based on pseudo-inverse description; and a lower machine controller drives three omnidirectional wheels of the mobile platform and joints of the manipulator to complete a given end operation task according to the calculation result of the repetitive motion planning scheme. Compared with the prior art, the application can make the mobile platform and the manipulator with limited motion space return to the initial state of each other after completing the task.

Description

Technical Field

[0001] This invention relates to the field of motion planning and control of mobile robotic arms, and in particular to a method and system for repetitive motion planning of a three-wheeled omnidirectional mobile robotic arm with limited motion space. Background Technology

[0002] Due to their combination of platform mobility and robotic arm maneuverability, three-wheeled omnidirectional redundant mobile robotic arms have been widely used in various fields such as industry, medicine, education, and the military. Repetitive motion planning is one of the hot topics in the application research of three-wheeled omnidirectional mobile robotic arms; that is, after completing a given end-effector task, both the mobile platform and the robotic arm need to return to their respective initial states simultaneously. Currently, several repetitive motion planning schemes based on pseudo-inverse descriptions have been proposed and successfully applied to three-wheeled omnidirectional mobile robotic arms. However, these schemes are designed without considering the physical constraints of the mobile robotic arm (including the pose limits of the mobile platform and the joint angle limits of the robotic arm). Clearly, this is not suitable for three-wheeled omnidirectional mobile robotic arms with limited motion space, and naturally, it cannot enable the robotic arm to successfully complete repetitive end-effector tasks within a limited operating space. Therefore, considering and implementing the handling of physical constraints is essential in the research of repetitive motion planning for mobile robotic arms. Summary of the Invention

[0003] The purpose of this invention is to provide a repetitive motion planning method and system for a three-wheeled omnidirectional mobile robotic arm that is simple in structure, easy to implement, requires little computation, and has limited motion space.

[0004] The objective of this invention can be achieved through the following technical solutions:

[0005] A method for repetitive motion planning of a three-wheeled omnidirectional robotic arm with limited motion space includes the following steps:

[0006] Step 1: Considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, establish the kinematic equations of the three-wheeled omnidirectional mobile robotic arm under the condition of limited motion space;

[0007] Step 2: Transform the kinematic equations into a joint nonlinear equation system to determine the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional mobile robotic arm;

[0008] Step 3: Construct a velocity layer criterion that enables repetitive motion;

[0009] Step 4: Based on the general form of the velocity layer motion planning scheme and the velocity layer criterion, construct a repetitive motion planning scheme based on pseudo-inverse description;

[0010] Step 5: The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm to complete the given end-effector operation task based on the calculation results of the repetitive motion planning scheme.

[0011] Step 1 specifically involves: for a three-wheeled omnidirectional mobile robotic arm with limited motion space, kinematic modeling is performed using the Denavit-Hartenberg parametric method, while considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, and kinematic equations are established.

[0012] The kinematic equations are:

[0013]

[0014] Where f(·) represents a nonlinear mapping function, Let the joint position vector of the three-wheeled omnidirectional robotic arm be defined as follows: p xy ∈R 2 This indicates the position of the mobile platform in the XY plane, i.e., the position where the robotic arm base is mounted on the mobile platform. φ∈R represents the orientation angle of the mobile platform, and θ∈R. n The joint angle r of the robotic arm d ∈R m This represents the expected motion trajectory of the end effector of the mobile robotic arm in m-dimensional space. This indicates the upper and lower limits of the movement space of the mobile robotic arm.

[0015] Step 2 specifically involves: introducing a non-negative vector to transform the constrained kinematic equations into a joint nonlinear equation system, and using an exponential decay formula to derive the general form of the velocity layer motion planning scheme for a three-wheeled omnidirectional mobile robotic arm.

[0016] The general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional mobile robotic arm is expressed as follows:

[0017]

[0018] in, The combined speed of the moving robotic arm is defined as follows: This indicates the rotational angular velocity of the drive wheels of the mobile platform. This indicates the joint speed of the robotic arm; Represents the state vector v∈R 6 +2n Time derivative, Indicates r d Time derivative; C∈R (6+2n)×(3+n) Let C represent the augmented coefficient matrix and be defined as C = [-I 3+n ;I 3+n ], I3+n ∈R (3+n)×(3+n) Denotes the identity matrix; D∈R (6+2n)×(6+2n) Let D represent a diagonal matrix and define it as D = diag{v1, v2, ..., v (6+2n)};d∈R 6+2n Let the vector of augmented coefficients be defined as follows: W∈R (m+6+2n)×(9+3n) Let W represent the augmented coefficient matrix and define it as W = [JM 0; CM 2D], J∈R m×(3+n) The Jacobian matrix representing the mobile robotic arm, M∈R (3+n)×(3+n) Let M represent the augmented coefficient matrix and define it as M = [A 0; 0I]. n ], I n ∈R n×n Represents the identity matrix; A∈R 3×3 The coefficient matrix of the mobile platform structural parameters is represented as follows:

[0019]

[0020] r∈R represents the radius of the omnidirectional drive wheel of the mobile platform, l∈R represents the distance from the center point of the mobile platform to the omnidirectional drive wheel; W∈R (9+3n)×(m+6+2n) Let I represent the pseudo-inverse matrix of W. M ∈R (9+3n)×(9+3n) Denotes the identity matrix, z∈R 9+3n This refers to the speed-level criteria derived from preset optimization indicators to achieve different planning objectives.

[0021] Based on the idea of ​​minimizing the error norm between the current state and the initial state of a three-wheeled omnidirectional robotic arm, a velocity layer criterion that enables repetitive motion is constructed using the negative gradient descent formula.

[0022] The velocity layer criterion is:

[0023]

[0024] Where, p xy0 ∈R 2 φ0∈R represents the initial position and initial orientation angle of the mobile platform in the XY plane, respectively, and θ0∈R n This represents the initial value of the robotic arm joint angle; ρ>0∈R represents the preset design parameters, 1 v ∈R 6+2n A vector whose all elements are 1, a joint vector This indicates the initial state of the three-wheeled omnidirectional robotic arm.

[0025] Step 4 specifically involves:

[0026] Combining the kinematic equations and velocity layer criteria of the mobile platform, let z = ηHvRMP Construct a repetitive motion planning scheme for a three-wheeled omnidirectional mobile robotic arm based on pseudo-inverse description:

[0027]

[0028] Where η>0∈R represents the repetitive motion coefficient, H=[A - ,0;0,I N ]∈R (9+3n)×(9+3n) Let I represent the augmented coefficient matrix. N ∈R (6+3n)×(6+3n) Let A represent the identity matrix. - ∈R 3×3 This represents the inverse matrix of the mobile platform structural parameter coefficient matrix A.

[0029] Step 5 specifically involves the lower-level controller driving the three omnidirectional wheels of the mobile platform and the joints of the robotic arm according to the calculation results of the repetitive motion planning scheme, completing the given end-effector operation task, and realizing repetitive motion planning under limited motion space conditions. That is, the three-wheeled omnidirectional mobile robotic arm with limited motion space returns to the initial state after completing the task.

[0030] A repetitive motion planning system for a three-wheeled omnidirectional robotic arm with limited motion space includes:

[0031] The repetitive motion planning module performs the following steps: considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, establishing the kinematic equations of the three-wheeled omnidirectional robotic arm under limited motion space; transforming the kinematic equations into a joint nonlinear equation set to determine the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional robotic arm; constructing a velocity layer criterion that enables repetitive motion; and constructing a repetitive motion planning scheme based on pseudo-inverse description based on the general form of the velocity layer motion planning scheme and the velocity layer criterion.

[0032] The drive module is used to perform the following steps: The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm to complete the given end-effector operation task based on the calculation results of the repetitive motion planning scheme.

[0033] Compared with the prior art, the present invention has the following beneficial effects:

[0034] 1. This invention can effectively overcome the shortcomings of existing methods. Considering the physical constraints of the mobile robotic arm (including the pose limits of the mobile platform and the joint angle limits of the robotic arm), it provides a pseudo-inverse repetitive motion planning method designed at the velocity level, which enables the mobile platform and robotic arm to achieve the purpose of repetitive motion planning under the condition of limited motion space. This has important significance and value for the motion planning of mobile robotic arms in complex environments.

[0035] 2. This invention adopts a pseudo-inverse method. When the structure of the mobile robotic arm is fixed, the angle or position of the joint can be directly calculated based on the position and orientation of the end effector after modeling. It does not require a large number of calculations of the optimal value in multiple solutions, which is easy to implement and requires less computation. Attached Figure Description

[0036] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0037] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.

[0038] This embodiment provides a repetitive motion planning method for a three-wheeled omnidirectional mobile robotic arm with limited motion space. First, modeling is performed using the Denavit-Hartenberg parametric method, considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, to establish the kinematic equations of the three-wheeled omnidirectional mobile robotic arm under limited motion space conditions. An exponential decay formula is used to derive the general form of the velocity layer motion planning scheme. A gradient descent formula is used to design a velocity layer criterion that enables repetitive motion. Combining the kinematic equations of the mobile platform and the velocity layer repetitive motion criterion, a repetitive motion planning scheme based on pseudo-inverse description is proposed. The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm according to the calculation results of the scheme to complete the given end-effector operation task. The repetitive motion planning scheme designed in this invention enables the mobile platform and robotic arm, with limited motion space, to simultaneously return to their respective initial states after completing the task (i.e., the three-wheeled omnidirectional redundant mobile robotic arm achieves repetitive motion planning under limited motion space conditions).

[0039] Specifically, such as Figure 1 As shown, it includes the following steps:

[0040] Step 1: Considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, establish the kinematic equations of the three-wheeled omnidirectional mobile robotic arm under the condition of limited motion space.

[0041] Specifically, for a three-wheeled omnidirectional robotic arm with limited motion space, kinematic modeling is performed using the Denavit-Hartenberg parametric method, considering both the pose limits of the mobile platform and the joint angle limits of the robotic arm, to establish the kinematic equations:

[0042]

[0043] Where f(·) represents a nonlinear mapping function, Let the joint position vector of the three-wheeled omnidirectional robotic arm be defined as follows: p xy ∈R 2 This indicates the position of the mobile platform in the XY plane, i.e., the position where the robotic arm base is mounted on the mobile platform. φ∈R represents the orientation angle of the mobile platform, and θ∈R. n The joint angle r of the robotic arm d ∈R m This represents the expected motion trajectory of the end effector of the mobile robotic arm in m-dimensional space. This indicates the upper and lower limits of the movement space of the mobile robotic arm.

[0044] Step 2: Transform the kinematic equations into a joint nonlinear equation system to determine the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional mobile robotic arm.

[0045] Specifically, a nonnegative vector is introduced to transform the constrained kinematic equation (1) into a joint nonlinear equation system, and the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional robotic arm is derived using the exponential decay formula:

[0046]

[0047] in, The combined speed of the moving robotic arm is defined as follows: This indicates the rotational angular velocity of the drive wheels of the mobile platform. This indicates the joint speed of the robotic arm; Represents the state vector v∈R 6 +2n Time derivative, Indicates r d Time derivative; C∈R (6+2n)×(3+n) Let C represent the augmented coefficient matrix and be defined as C = [-I 3+n ;I 3+n ], I 3+n ∈R (3+n)×(3+n) Denotes the identity matrix; D∈R (6+2n)×(6+2n) Let D represent a diagonal matrix and define it as D = diag{v1, v2, ..., v (6+2n)};d∈R 6+2n Let the vector of augmented coefficients be defined as follows: W∈R (m+6+2n)×(9+3n) Let W represent the augmented coefficient matrix and define it as W = [JM 0; CM 2D], J∈R m×(3+n) The Jacobian matrix representing the mobile robotic arm, M∈R (3+n)×(3+n) Let M represent the augmented coefficient matrix and define it as M = [A 0; 0I]. n ], I n ∈R n×nRepresents the identity matrix; A∈R 3×3 The coefficient matrix of the mobile platform structural parameters is represented as follows:

[0048]

[0049] r∈R represents the radius of the omnidirectional drive wheel of the mobile platform, l∈R represents the distance from the center point of the mobile platform to the omnidirectional drive wheel; W∈R (9+3n)×(m+6+2n) Let I represent the pseudo-inverse matrix of W. M ∈R (9+3n)×(9+3n) Denotes the identity matrix, z∈R 9+3n This refers to the velocity layer criteria derived from preset optimization indicators to achieve different planning objectives (such as repetitive motion).

[0050] Step 3: Using the gradient descent formula, construct a velocity layer criterion that enables repetitive motion.

[0051] Based on the idea of ​​minimizing the error norm between the current state and the initial state of a three-wheeled omnidirectional robotic arm, a velocity layer criterion for achieving repetitive motion is constructed using the negative gradient descent formula:

[0052]

[0053] Where, p xy0 ∈R 2 φ0∈R represents the initial position and initial orientation angle of the mobile platform in the XY plane, respectively, and θ0∈R n This represents the initial value of the robotic arm joint angle; ρ > 0 ∈ R indicates a design parameter with a small preset value (e.g., ρ = 0.01), 1 v ∈R 6+2n A vector whose all elements are 1, a joint vector This indicates the initial state of the three-wheeled omnidirectional robotic arm (i.e., the initial state when performing the end-effector planning task).

[0054] Step 4: Based on the general form of the velocity layer motion planning scheme and the velocity layer criterion, construct a repetitive motion planning scheme based on pseudo-inverse description.

[0055] Combining the kinematic equations of the mobile platform and the velocity layer criterion, the velocity layer repetitive motion criterion (3) is substituted into the general form (2) of the velocity layer motion planning scheme, that is, let z = ηHv RMP Construct a repetitive motion planning scheme for a three-wheeled omnidirectional robotic arm based on pseudo-inverse description:

[0056]

[0057] Where η>0∈R represents the repetitive motion coefficient, H=[A - ,0;0,I N]∈R (9+3n)×(9+3n) Let I represent the augmented coefficient matrix. N ∈R (6+3n)×(6+3n) Let A represent the identity matrix. - ∈R 3×3 This represents the inverse matrix of the coefficient matrix A of the mobile platform structural parameters.

[0058] Step 5: The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm to complete the given end-effector operation task based on the calculation results of the repetitive motion planning scheme.

[0059] Specifically, the lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm according to the calculation results of the repetitive motion planning scheme (4) to complete the given end-effector operation task and realize repetitive motion planning under limited motion space. That is, the three-wheeled omnidirectional mobile robotic arm with limited motion space returns to the initial state after completing the task.

[0060] This embodiment also provides a repetitive motion planning system for a three-wheeled omnidirectional mobile robotic arm with limited motion space, including:

[0061] The repetitive motion planning module performs the following steps: considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, establishing the kinematic equations of the three-wheeled omnidirectional robotic arm under limited motion space; transforming the kinematic equations into a joint nonlinear equation set to determine the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional robotic arm; constructing a velocity layer criterion that enables repetitive motion; and constructing a repetitive motion planning scheme based on pseudo-inverse description based on the general form of the velocity layer motion planning scheme and the velocity layer criterion.

[0062] The drive module is used to perform the following steps: The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm to complete the given end-effector operation task based on the calculation results of the repetitive motion planning scheme.

[0063] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working process of the described module can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0064] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A method for repetitive motion planning of a three-wheeled omnidirectional robotic arm with limited motion space, characterized in that, Includes the following steps: Step 1: Considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, establish the kinematic equations of the three-wheeled omnidirectional mobile robotic arm under the condition of limited motion space; Step 2: Transform the kinematic equations into a joint nonlinear equation system to determine the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional robotic arm. Specifically, introduce a non-negative vector to transform the constrained kinematic equations into a joint nonlinear equation system, and use the exponential decay formula to derive the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional robotic arm. The general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional mobile robotic arm is expressed as follows: ； in, The combined speed of the moving robotic arm is defined as follows: , This indicates the rotational angular velocity of the drive wheels of the mobile platform. This indicates the joint speed of the robotic arm; State vector Time derivative, express The time derivative; Let the augmented coefficient matrix be defined as follows: , Represents the identity matrix; Denotes a diagonal matrix and is defined as follows: ; Let the vector of augmented coefficients be defined as follows: ; Let the augmented coefficient matrix be defined as follows: , The Jacobian matrix represents the moving robotic arm. Let the augmented coefficient matrix be defined as follows: , Represents the identity matrix; This is the coefficient matrix of the mobile platform structural parameters; Represents a nonlinear mapping function. Let the joint position vector of the three-wheeled omnidirectional robotic arm be defined as follows: , This indicates the position of the mobile platform in the XY plane, that is, the position where the robotic arm base is mounted on the mobile platform. Indicates the orientation angle of the mobile platform. Indicates the joint angles of the robotic arm; Indicates the end effector of the mobile robotic arm at The desired trajectory of motion in 3D space; Step 3: Based on the idea of ​​minimizing the error norm between the current state and the initial state of the three-wheeled omnidirectional mobile robotic arm, the negative gradient descent formula is used to construct a velocity layer criterion that enables repetitive motion; The velocity layer criterion is: ； in, and These represent the initial position and initial orientation angle of the mobile platform in the XY plane, respectively. This represents the initial value of the robotic arm joint angle; This indicates the preset design parameters. A vector whose all elements are 1, a joint vector This indicates the initial state of the three-wheeled omnidirectional robotic arm; Step 4: Based on the general form and velocity-level criteria of the velocity-level motion planning scheme, construct a repetitive motion planning scheme based on pseudo-inverse description, specifically as follows: Combining the kinematic equations and velocity layer criteria of the mobile platform, let Construct a repetitive motion planning scheme for a three-wheeled omnidirectional robotic arm based on pseudo-inverse description: ； in, Indicates the repetitive motion coefficient. Represents the augmented coefficient matrix. Represents the identity matrix. Represents the coefficient matrix of mobile platform structural parameters The inverse matrix; Step 5: The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm to complete the given end-effector operation task based on the calculation results of the repetitive motion planning scheme.

2. The repetitive motion planning method for a three-wheeled omnidirectional robotic arm with limited motion space according to claim 1, characterized in that, Step 1 specifically involves: for a three-wheeled omnidirectional mobile robotic arm with limited motion space, kinematic modeling is performed using the Denavit-Hartenberg parametric method, while considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, and kinematic equations are established.

3. The repetitive motion planning method for a three-wheeled omnidirectional robotic arm with limited motion space according to claim 2, characterized in that, The kinematic equations are: ； in, This indicates the upper and lower limits of the movement space of the mobile robotic arm.

4. The repetitive motion planning method for a three-wheeled omnidirectional robotic arm with limited motion space according to claim 1, characterized in that, Mobile platform structural parameter coefficient matrix It is expressed as follows: ； This indicates the radius of the omnidirectional drive wheels of the mobile platform. This indicates the distance from the center point of the mobile platform to the omnidirectional drive wheel; express The pseudo-inverse matrix, Represents the identity matrix. This refers to the speed-level criteria derived from preset optimization indicators to achieve different planning objectives.

5. The repetitive motion planning method for a three-wheeled omnidirectional robotic arm with limited motion space according to claim 1, characterized in that, Step 5 specifically involves the lower-level controller driving the three omnidirectional wheels of the mobile platform and the joints of the robotic arm according to the calculation results of the repetitive motion planning scheme, completing the given end-effector operation task, and realizing repetitive motion planning under limited motion space conditions. That is, the three-wheeled omnidirectional mobile robotic arm with limited motion space returns to the initial state after completing the task.

6. A repetitive motion planning system for a three-wheeled omnidirectional robotic arm with limited motion space, characterized in that, The method of claim 1 includes: The repetitive motion planning module performs the following steps: considering the pose limits of the mobile platform and the joint angle limits of the robotic arm, establishing the kinematic equations of the three-wheeled omnidirectional robotic arm under limited motion space; transforming the kinematic equations into a joint nonlinear equation set to determine the general form of the velocity layer motion planning scheme for the three-wheeled omnidirectional robotic arm; constructing a velocity layer criterion that enables repetitive motion; and constructing a repetitive motion planning scheme based on pseudo-inverse description based on the general form of the velocity layer motion planning scheme and the velocity layer criterion. The drive module is used to perform the following steps: The lower-level controller drives the three omnidirectional wheels of the mobile platform and the joints of the robotic arm to complete the given end-effector operation task based on the calculation results of the repetitive motion planning scheme.

Images