Artificial potential field-based constraint processing planning method and system for visual servo of mobile operation arm

Abstract

The invention relates to a constraint processing planning method and system based on an artificial potential field for visual servo of a moving operation arm, and the method comprises the following steps: obtaining initial parameters, and carrying out the system modeling; on the basis of system modeling, global path planning based on sampling is carried out, and an initial feasible path is obtained; verifying the initial feasible path based on a local planner of the APF and a preset multi-constraint requirement to obtain a discrete feature path; performing trajectory smoothing and time parameterization on the discrete feature path to obtain a final image feature trajectory; and on the basis of the final image feature track, carrying out online image servo execution and tracking on the mobile operation arm. The robot can be widely applied to the technical field of robots.

Description

Technical Field

[0001] The present invention relates to a constraint processing planning method and system based on artificial potential field (APF) for visual servoing of a mobile manipulator, belonging to the technical field of robots. Background Art

[0002] With the rapid development of robotics technology, mobile manipulators, as integrated systems combining mobile platforms and robotic arms, have been widely used in intelligent manufacturing, warehousing logistics, and service robots. Mobile manipulators use mobile chassis to achieve large-scale displacement, and then use robotic arms to complete fine manipulation tasks. Among many control methods, visual servoing (VS) technology uses cameras to obtain image features of the environment and targets, and converts image feature errors into control instructions to guide the end-effector of the mobile manipulator to accurately reach the desired position and posture. Image-Based Visual Servoing (IBVS) technology has received particular attention. It has strong robustness to model and camera calibration errors and can achieve high-precision control in more complex and uncertain environments.

[0003] However, in actual application scenarios, mobile manipulators often face the following constraints and problems when performing visual servoing tasks:

[0004] 1. Joint position limitation: Each joint of the robot arm must be kept within the safe range, otherwise it may cause equipment damage or safety hazards.

[0005] 2. Field of view limitation: Image features must always be within the visible range of the camera. Once the target features leave the camera's field of view, the visual servoing process will not be able to continue.

[0006] 3. Singularity problem: including the singularity of the dynamic Jacobian matrix and the singularity of the image interaction matrix. When the system approaches a singular point, the control command may be too large, divergent or unstable.

[0007] 4. Occlusion and collision risks: The mobile manipulator may collide with obstacles in the environment or the structure of the mobile platform itself during movement; the target object or feature point may be occluded, resulting in loss of image features.

[0008] 5. Local minimum trap: Traditional local path planning methods that rely on simple interpolation or optimization often fall into local minima when considering multiple constraints, making it impossible to find a feasible global path.

[0009] 6. Mobile platform and robotic arm coordination constraints: Compared with the fixed base robotic arm, the planning of the mobile manipulator needs to consider both the kinematic constraints of the mobile chassis and the joint constraints of the robotic arm. In image servo, this increases the difficulty of control and planning.

[0010] Existing methods mostly focus on visual servoing of stationary robotic arms, and lack efficient and robust global planning and local control solutions for mobile manipulators under the above-mentioned multiple constraints. Summary of the invention

[0011] In view of the above problems, the purpose of the present invention is to provide a constraint processing planning method and system based on artificial potential field for a mobile manipulator. The method combines sampling-based global path planning with an APF local controller to meet multiple constraint requirements such as joint position, field of view, singularity, occlusion, collision and mobile platform characteristics, and quickly generate a feasible visual servo trajectory that takes into account multiple types of constraints for the mobile manipulator in a complex environment.

[0012] To achieve the above object, the present invention adopts the following technical solutions:

[0013] In a first aspect, the present invention provides a constraint processing planning method based on artificial potential field for visual servoing of a mobile manipulator, comprising the following steps:

[0014] Obtain initial parameters and perform system modeling;

[0015] Based on system modeling, global path planning based on sampling is carried out to obtain the initial feasible path;

[0016] Based on the APF local planner and preset multiple constraints, the initial feasible path is verified to obtain a discrete characteristic path;

[0017] Perform trajectory smoothing and time parameterization on the discrete feature path to obtain the final image feature trajectory;

[0018] Based on the final image feature trajectory, online image servo execution and tracking of the mobile manipulator are performed.

[0019] Further, the obtaining of initial parameters and performing system modeling includes:

[0020] Obtain the kinematic model of the mobile chassis and the robotic arm, and calculate the kinematic Jacobian matrix;

[0021] Collect initial image features s0 and expected image features s * ;

[0022] Obtain the camera's intrinsic parameter matrix K and extrinsic parameter matrix [R|t] for 3D reconstruction and posture estimation of image features;

[0023] Obtain the three-dimensional obstacle information of the working environment, the location distribution of the target object and the potential occlusion area, and perform obstacle modeling.

[0024] Furthermore, based on the system modeling, the sampling-based global path planning is performed to obtain an initial feasible path, including:

[0025] Define the search space state and combine the comprehensive degrees of freedom of the mobile chassis and the robotic arm to form a high-dimensional planning space

[0026] Starting from the initial image feature s0, a new image feature state point s is obtained by random sampling in the high-dimensional planning space S. rand , and find the image feature state point s in the search tree rand The nearest node s near , expand to get new node s new ;

[0027] Use the constraint checker to check the nearest node s near and the new node s new The transition path between the initial image feature s0 and the expected image feature s * A feasible path is found or the preset maximum number of iterations is reached.

[0028] Furthermore, the new node s new The calculation formula is:

[0029]

[0030] Among them, δ is the expansion step size.

[0031] Further, the constraint checker is used to check the nearest node s near and the new node s new The transition path between the initial image feature s0 and the expected image feature s * A feasible path is found or the preset maximum number of iterations is reached, including:

[0032] Using the constraint checker, near To the new node s new To perform constraint checking on transition paths between

[0033] If the transition path does not meet the constraint verification conditions, the new node s is discarded new and re-sample;

[0034] Repeat the above two steps until the initial image feature s0 is reached to the desired image feature s *A feasible path is found or the preset maximum number of iterations is reached, and the initial feasible path of the global planning is obtained.

[0035] Furthermore, the APF-based local planner and preset multiple constraints verify the initial feasible path to obtain a discrete characteristic path, including:

[0036] Define the attractive potential function U att (s) makes the desired image feature s * Become the lowest point of the potential field;

[0037] According to the current joint configuration q, calculate the joint repulsion potential field U JL (q);

[0038] According to the feature point position p, calculate the visual field exclusion potential field U FOV (p);

[0039] Thresholds are set for the determinant of the dynamic Jacobian matrix J(q) and the image interaction matrix L to detect and calculate the singular repulsive potential field U JS (q) and U IS (s);

[0040] Based on the obstacle model, the constraint checker verification function is used to detect whether the local transition path causes occlusion or collides with obstacles;

[0041] According to the reachability and speed limit of the mobile platform, the kinematic repulsive potential field of the mobile platform is calculated;

[0042] The attractive potential function is superimposed with the gradients of various repulsive potential fields to form a combined force, and the feature position is iteratively updated by the gradient descent method.

[0043] Furthermore, the discrete feature path is subjected to trajectory smoothing and time parameterization to obtain the final image feature trajectory, including:

[0044] The discrete feature path S = {s0, s1, ..., s N} to simplify and obtain the simplified path S′={s0,s2,…,s N};

[0045] Perform curve fitting and smoothing on the simplified path S′ to obtain a smooth trajectory s(t);

[0046] According to the speed v max and acceleration a max Constraints are set, and the smooth trajectory s(t) is parameterized in time to obtain the final image feature trajectory.

[0047] Furthermore, the simplified path S′ is subjected to curve fitting and smoothing to obtain a smooth trajectory s(t), including:

[0048] The simplified path S′ is converted into a continuous and differentiable smooth trajectory s(t) using cubic spline interpolation or polynomial interpolation method;

[0049] The final smooth trajectory s(t) is obtained by curve fitting by minimizing the following objective function;

[0050]

[0051] Where λ is the weight of the smoothing term.

[0052] Furthermore, the online image servo execution and tracking of the mobile operating arm based on the final image feature trajectory includes:

[0053] The final image feature trajectory s(t) is input into the classic image-based visual servo control law or the improved controller based on interactive matrix feedback;

[0054] The controller adjusts the joint speed of the mobile platform and the robotic arm according to the real-time image feature error, so that the end effector can smoothly approach the desired feature position;

[0055] During the approximation process, an adaptive control strategy is used to cope with dynamic environmental changes and ensure the robustness of the control process.

[0056] In a second aspect, the present invention provides a constraint processing planning system based on artificial potential field for visual servoing of a mobile manipulator, comprising:

[0057] Data acquisition module, used to obtain initial parameters and perform system modeling;

[0058] The global planning module is used to perform sampling-based global path planning based on system modeling to obtain the initial feasible path;

[0059] The local planning module is used to verify the initial feasible path based on the APF local planner and preset multiple constraints to obtain a discrete feature path;

[0060] The trajectory smoothing and time parameterization module is used to perform trajectory smoothing and time parameterization on the discrete feature path to obtain the final image feature trajectory;

[0061] The servo control module is used to perform online image servo execution and tracking of the mobile manipulator based on the final image feature trajectory.

[0062] The present invention adopts the above technical solution, which has the following advantages:

[0063] 1. High success rate and robustness: After comprehensively considering multiple types of constraints, the present invention can quickly generate feasible trajectories even if the initial image features are significantly different from the expected image features, and there are obstacles and model errors.

[0064] 2. Improved computational efficiency: This invention combines sampling-based global search with an efficient local planner, which can find feasible solutions within seconds and significantly reduce planning time.

[0065] 3. Avoid local minimum traps: The present invention can repeatedly try different paths by resampling the global framework, thereby reducing the probability of falling into local minimums.

[0066] 4. Adapt to complex environments: In the working scenario of the mobile operating arm, the present invention considers various real constraints such as chassis kinematics, obstacle environment, camera field of view, etc., making the solution more practical.

[0067] Therefore, the present invention can be widely applied to the field of robotics. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present invention. Throughout the accompanying drawings, the same reference numerals are used to represent the same components. In the accompanying drawings:

[0069] Figure 1 It is a flow chart of a constraint processing planning method based on artificial potential field for visual servoing of a mobile manipulator provided by an embodiment of the present invention;

[0070] Figure 2 This is a schematic diagram of path planning provided by an embodiment of the present invention:

[0071] Figure 3 It is a schematic diagram of verification in an actual robot environment. DETAILED DESCRIPTION

[0072] In order to make the purpose, technical solution and advantages of the embodiment of the present invention clearer, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings of the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all of the embodiments. Based on the described embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field belong to the scope of protection of the present invention.

[0073] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0074] In some embodiments of the present invention, a constraint processing planning method based on artificial potential field for visual servoing of a mobile manipulator is provided, which combines sampling-based global path planning with an APF local controller to meet multiple constraint requirements such as joint position, field of view, singularity, occlusion, collision and mobile platform characteristics. The present invention comprehensively considers a variety of actual constraints, so that feasible trajectories with high success rate and low time consumption can be generated under complex environments and large feature differences. Compared with traditional visual servoing methods that only rely on simple interpolation or do not fully consider constraints, the present invention not only improves the feasibility and execution efficiency of path planning, but also enhances the robustness and safety of the system, and has broad application prospects.

[0075] Correspondingly, in some other embodiments of the present invention, a constraint processing planning system based on artificial potential field for visual servoing of a mobile manipulator is provided.

[0076] Example 1

[0077] like Figure 1 and Figure 2 As shown, the present invention provides a constraint processing planning method based on artificial potential field for visual servoing of a mobile manipulator, which comprises the following steps:

[0078] 1) Obtain initial parameters and perform system modeling;

[0079] 2) Based on system modeling, perform sampling-based global path planning to obtain the initial feasible path;

[0080] 3) Based on the APF local planner and preset multiple constraints, the initial feasible path is verified to obtain a discrete characteristic path;

[0081] 4) Smoothing and time parameterizing the discrete feature path to obtain the final image feature trajectory;

[0082] 5) Based on the final image feature trajectory, online image servo execution and tracking of the mobile manipulator are performed.

[0083] Furthermore, the above step 1) includes the following steps:

[0084] 1.1) Obtain the kinematic model of the mobile chassis and the robotic arm, and calculate the kinematic Jacobian matrix.

[0085] In this embodiment, the kinematic model of the mobile chassis and the manipulator includes kinematic equations and joint parameters, and the joint parameters include upper and lower limits of joint angles, etc. In this embodiment, the kinematic model is represented by the Jacobian matrix J(q) reflecting differential kinematics, which is used to describe the relationship between the joint motion speed and the terminal motion speed, which is expressed as:

[0086]

[0087] Where x is the position and posture vector of the end effector; q is the joint angle vector.

[0088] 1.2) Collect initial image features s0 and expected image features s * , where the expected image feature s * It refers to the coordinate set of feature points of the target object;

[0089] 1.3) Obtain the camera’s intrinsic parameter matrix K and extrinsic parameter matrix [R|t] for 3D reconstruction and posture estimation of image features.

[0090]

[0091] Among them, f x and f y is the focal length, (c x ,c y ) is the main point, R and t are the rotation matrix and translation vector respectively.

[0092] 1.4) Obtain the three-dimensional obstacle information of the working environment, the location distribution of the target object and the potential occlusion area, and perform obstacle modeling.

[0093] Furthermore, the above step 1.4) includes the following steps:

[0094] 1.4.1) Obtain the three-dimensional obstacle information of the working environment and build an environmental map to check whether the robot collides with the environment during subsequent planning.

[0095] The method for constructing the environment map may adopt a technique known to those skilled in the art, such as an octree map, etc., and the present invention does not limit this.

[0096] 1.4.2) Determine the field of view (FOV) of the camera, as well as the location distribution and potential occlusion areas of the target object, so as to check whether the robot has blocked the target object during subsequent planning.

[0097] 1.4.3) Use sensor fusion technology, such as data fusion of LiDAR and camera, to obtain accurate information about the working environment, which is used to improve the accuracy and real-time performance of environmental perception in steps 1.4.1) and 1.4.2) and obtain the final obstacle model.

[0098] Furthermore, in the above step 2), the sampling-based global path planning includes the following steps:

[0099] 2.1) Define the search space state and combine the comprehensive degrees of freedom of the mobile chassis and the robotic arm to form a high-dimensional planning space

[0100] In this embodiment, the search space state is defined as the image feature state point s, which is expressed as s = {s1, s2, ..., s n}, where s i =(u i ,v i ),u i ,v i Refers to the pixel coordinates of the feature point in the image, that is, the pixel coordinates of the feature point in the image with the horizontal axis v i Pixels, vertical axis u i Pixel.

[0101] Combine the comprehensive degrees of freedom of the mobile chassis and the robotic arm to form a high-dimensional planning space

[0102]

[0103] in, represents the degrees of freedom of the mobile chassis, represents the joint degrees of freedom of the robot arm, Indicates the state of a visual feature.

[0104] 2.2) Starting from the initial image feature d0, randomly sample the new image feature state point s in the high-dimensional planning space S rand , and find the image feature state point s in the search tree rand The nearest node s near and new nodes s that meet the preset requirements new ;

[0105] 2.3) Use the constraint checker to check the nearest node s near and the new node s new The transition path between the initial image feature s0 and the expected image feature s * A feasible path is found or the preset maximum number of iterations is reached.

[0106] Furthermore, the above step 2.2) includes the following steps:

[0107] 2.2.1) Using RRT or its improved algorithm (such as RRT*) starting from the initial image feature s0, Random sampling obtains a new image feature state point s rand ;

[0108]

[0109] 2.2.2) Find the state point s corresponding to the image feature in the search tree rand The nearest nodes near , and expand to generate new node s new :

[0110]

[0111] Among them, δ is the expansion step size.

[0112] Furthermore, the above step 2.3) includes the following steps:

[0113] 2.3.1) Use the constraint checker to check the nearest node s near To the new node s new The transition path between them is constrained and checked, which can be expressed as:

[0114] Valid(s near ,s new )=True / False

[0115] 2.3.2) If the transition path does not meet the constraint verification conditions, the new node s is discarded new And return to step 2.2.1) to resample;

[0116] 2.3.3) Repeat steps 2.3.1) to 2.3.2) until the initial image feature s0 is reached to the desired image feature s * A feasible path is found or the preset maximum number of iterations is reached, and the initial feasible path of the global planning is obtained.

[0117] Furthermore, in the above step 3), based on the local planner of APF and the preset multiple constraint requirements, the initial feasible path is verified, including the following steps:

[0118] 3.1) Construction of artificial potential field: Definition of attractive potential function U att (s) makes the desired image feature s * Become the lowest point of the potential field:

[0119]

[0120] Among them, k att is the attractive potential field coefficient.

[0121] 3.2) Joint position restriction: Calculate the joint repulsive potential field U according to the current joint configuration q JL (q).

[0122] In this embodiment, when the robot arm joint configuration q is close to the joint limit, the joint repulsion potential function U is defined as JL (q):

[0123]

[0124] Among them, k JL is the joint repulsion potential field coefficient, m is the number of joints, q i,min and q i,max are the minimum and maximum angle limits of the i-th joint, respectively.

[0125] 3.3) Field of view limitation (FOV): Calculate the field of view exclusion potential field U according to the feature point position p FOV (p).

[0126] In this embodiment, the effective working area of ​​the image plane is defined, and the visual field exclusion potential function U is introduced when the feature point is close to the image boundary. FOV (p):

[0127]

[0128] Among them, d i is the feature point p i Minimum distance to the image boundary, k FOV is the field of view repulsion potential coefficient.

[0129] 3.4) Singularity constraint: Detect and calculate the singularity repulsion potential field U according to the preset threshold JS (q) and U IS (s).

[0130] Set a threshold for the determinant of the dynamic Jacobian matrix J(q) and the image interaction matrix L, and define the singularity exclusion potential function U JS (q) and U IS (s):

[0131]

[0132] Among them, k JS and k IS is the singularity repulsion potential field coefficient, ∈ is the threshold.

[0133] 3.5) Occlusion and collision detection: The constraint checker verification function is used to detect whether the local transition path causes occlusion or collides with obstacles:

[0134]

[0135] If the path is invalid, the branch is discarded at the global planning level.

[0136] 3.6) Mobile platform kinematic constraints: Incorporate the reachability and speed limits of the mobile platform into the potential field or constraints:

[0137]

[0138] Among them, d reachable is the distance from the mobile platform to the unreachable area, k MP is the repulsive potential field coefficient of the mobile platform.

[0139] 3.7) The attractive potential function is superimposed with the gradients of various repulsive potential fields to form a combined force, and the feature position is iteratively updated by the gradient descent method.

[0140] Furthermore, the above step 3.7) includes the following steps:

[0141] 3.7.1) Superimpose the gradients of the attractive potential and various repulsive potential fields to form a resultant force F:

[0142]

[0143] 3.7.2) Iteratively update feature positions by gradient descent:

[0144] s k+1 =s k +αF(s k )

[0145] Among them, α is the step size coefficient.

[0146] 3.7.3) If the local planning is blocked (such as falling into a local minimum), the expansion is marked as failed, and the global planning retry new random points until the new image feature position s(t) converges to the target image feature s * Or reach the iteration limit to improve the overall success rate.

[0147] Furthermore, in the above step 4), the discrete feature path is trajectory smoothed and time parameterized to obtain the final image feature trajectory, which includes the following steps:

[0148] 4.1) The discrete feature path S = {s0, s1, ..., s N} to simplify and obtain the simplified path S′={s0,s2,…,s N}.

[0149] Specifically, in this embodiment, a simplification algorithm (such as the Douglas-Peucker algorithm) is applied to subdivide and reconnect the discrete feature paths generated by the global planning, remove redundant nodes, and simplify the paths to maintain the geometric shape of the paths while reducing the number of nodes:

[0150] S={s0,s1,…,s N}→S′={s0,s2,…,s +}

[0151] 4.2) Perform curve fitting and smoothing on the simplified path S′ to obtain a smooth trajectory s(t).

[0152] 4.3) According to the speed v max and acceleration a max Constraints are used to time parameterize the smooth trajectory s(t).

[0153] Furthermore, the above step 4.2) includes the following steps:

[0154] 4.2.1) Use cubic spline interpolation or other polynomial interpolation methods to convert the simplified path S′ into a continuous and differentiable smooth trajectory s(t):

[0155]

[0156] In the formula, a i represents the fixed parameters in spline interpolation or other parameter representation methods, and t represents the variable parameters.

[0157] 4.2.2) Curve fitting is performed by minimizing the following objective function:

[0158]

[0159] Where λ is the weight of the smoothness term, which is used to control the smoothness of the trajectory.

[0160] Furthermore, the above step 4.3) includes the following steps:

[0161] 4.3.1) Assign a reasonable time distribution to the smooth feature trajectory so that it satisfies the speed v max and acceleration a max Constraints:

[0162]

[0163] 4.3.2) Apply time optimization algorithms (such as minimum time path planning) to determine the timestamps of each node to ensure that the trajectory can be smoothly executed under physical constraints.

[0164] Furthermore, in the above step 5), based on the final image feature trajectory, the mobile operating arm is subjected to online image servo execution and tracking, including the following steps:

[0165] 5.1) Input the final image feature trajectory s(t) into the classic image-based visual servoing (IBVS) control law or the improved controller based on interactive matrix feedback:

[0166] v=-λL + (s(t)-s desired (t))

[0167] Where v is the control velocity vector, λ is the gain coefficient, L + is the pseudo-inverse of the interaction matrix, s desired (t) is the expected feature position.

[0168] 5.2) The controller adjusts the joint speed of the mobile platform and the robotic arm according to the real-time image feature error, so that the end effector smoothly approaches the desired feature position.

[0169] 5.3) Implement adaptive control strategies to cope with dynamic environmental changes and ensure the robustness of the control process:

[0170]

[0171] Wherein, γ is the adaptive gain adjustment coefficient.

[0172] Example 2

[0173] like Figure 3 As shown, this embodiment has been proved through simulation tests that, under the conditions of joint restrictions, FOV restrictions and singularity, the present invention can generate a feasible trajectory within about 3 seconds, with a success rate of more than 94%. The specific experimental steps and results are as follows:

[0174] 1) Experimental setup

[0175] 1.1) Use a simulation environment to simulate the movement of the mobile manipulator in a complex warehouse environment.

[0176] 1.2) Set up multiple obstacles and dynamic targets to test the adaptability and robustness of the method in different environments.

[0177] 2) Experimental process

[0178] 2.1) Initial feature s0 and target feature s * Set them at different locations to ensure that path planning can bypass obstacles.

[0179] 2.2) Record the time consumption and success rate of each path planning.

[0180] 2.3) Test the effect of different λ and γ values ​​on the control performance.

[0181] 3) Experimental results

[0182] 3.1) Path planning time: The average planning time is about 3 seconds, which meets the real-time control requirements.

[0183] 3.2) Success Rate: Feasible trajectories are successfully generated in 90% of the test scenarios, indicating that the method has high robustness.

[0184] 3.3) Trajectory smoothness: The generated trajectory is smooth, the joint movement is stable, and there is no obvious oscillation.

[0185] 3.4) Control accuracy: The end effector successfully reaches the target feature position with an acceptable error.

[0186] 4) Performance analysis

[0187] 4.1) Computational efficiency: The combination of sampling-based global path planning and APF local planner significantly improves the computational efficiency and shortens the path planning time.

[0188] 4.2) Robustness: Our method can work stably in the presence of multiple constraints and dynamic obstacles, which proves its robustness.

[0189] 4.3) Adaptability: It can adapt to environments of different complexity and adjust planning strategies according to actual conditions, showing good adaptability.

[0190] Through detailed description and experimental verification of specific embodiments, it is proved that the constraint processing planning method based on artificial potential field of the present invention has significant advantages in the visual servoing task of mobile manipulators, and can efficiently and robustly generate feasible trajectories that meet multiple constraints, ensuring that the mobile manipulators can complete tasks safely and accurately in complex environments.

[0191] Example 3

[0192] The above-mentioned embodiment 1 provides a constraint processing planning method based on artificial potential field for visual servoing of a mobile manipulator arm. Correspondingly, this embodiment provides a constraint processing planning system based on artificial potential field for visual servoing of a mobile manipulator arm. The system provided in this embodiment can implement the constraint processing planning method based on artificial potential field for visual servoing of a mobile manipulator arm of embodiment 1, and the system can be implemented by software, hardware, or a combination of software and hardware. For example, the system may include integrated or separate functional modules or functional units to execute the corresponding steps in each method of embodiment 1. Since the system of this embodiment is basically similar to the method embodiment, the process described in this embodiment is relatively simple, and the relevant parts can refer to the partial description of embodiment 1. The embodiment of the system provided in this embodiment is merely illustrative.

[0193] The artificial potential field-based constraint processing planning system for mobile manipulator visual servoing provided in this embodiment includes:

[0194] Data acquisition module, used to obtain initial parameters and perform system modeling;

[0195] The global planning module is used to perform sampling-based global path planning based on system modeling to obtain the initial feasible path;

[0196] The local planning module is used to verify the initial feasible path based on the APF local planner and preset multiple constraints to obtain a discrete feature path;

[0197] The trajectory smoothing and time parameterization module is used to perform trajectory smoothing and time parameterization on the discrete feature path to obtain the final image feature trajectory;

[0198] The servo control module is used to perform online image servo execution and tracking of the mobile manipulator based on the final image feature trajectory.

[0199] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A constraint processing planning method based on artificial potential field for visual servoing of mobile manipulators, characterized in that: The following steps are involved: Obtain initial parameters and perform system modeling; Based on system modeling, global path planning based on sampling is carried out to obtain the initial feasible path; Based on the APF local planner and preset multiple constraints, the initial feasible path is verified to obtain a discrete characteristic path; Perform trajectory smoothing and time parameterization on the discrete feature path to obtain the final image feature trajectory; Based on the final image feature trajectory, online image servo execution and tracking of the mobile manipulator are performed.

2. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 1, characterized in that: The obtaining of initial parameters and system modeling includes: Obtain the kinematic model of the mobile chassis and the robotic arm, and calculate the kinematic Jacobian matrix; Collect initial image features s0 and expected image features s * ; Obtain the camera's intrinsic parameter matrix K and extrinsic parameter matrix [R|t] for 3D reconstruction and posture estimation of image features; Obtain the three-dimensional obstacle information of the working environment, the location distribution of the target object and the potential occlusion area, and perform obstacle modeling.

3. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 2, characterized in that: The above-mentioned method is based on system modeling, and performs sampling-based global path planning to obtain an initial feasible path, including: Define the search space state and combine the comprehensive degrees of freedom of the mobile chassis and the robotic arm to form a high-dimensional planning space Starting from the initial image feature s0, in the high-dimensional planning space Random sampling inside to get new image feature state point s rand , and find the image feature state point s in the search tree rand The nearest node s near , expand to get new node s new ; Use the constraint checker to check the nearest node s near and the new node s new The transition path between the initial image feature s0 and the expected image feature s * A feasible path is found or the preset maximum number of iterations is reached.

4. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 3, characterized in that: The new node s new The calculation formula is: Among them, δ is the expansion step size.

5. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing as claimed in claim 3, characterized in that: The constraint checker is used to check the nearest node s near and the new node s new The transition path between the initial image feature s0 and the expected image feature s * A feasible path is found or the preset maximum number of iterations is reached, including: Use the constraint checker to check the nearest node s near To the new node s new To perform constraint checking on transition paths between If the transition path does not meet the constraint verification conditions, the new node s is discarded new and re-sample; Repeat the above two steps until the initial image feature s0 is reached to the desired image feature s * A feasible path is found or the preset maximum number of iterations is reached, and the initial feasible path of the global planning is obtained.

6. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 2, characterized in that: The APF-based local planner and preset multiple constraints verify the initial feasible path to obtain a discrete feature path, including: Define the attractive potential function U att (s) makes the desired image feature s * Become the lowest point of the potential field; According to the current joint configuration q, calculate the joint repulsion potential field U JL (q); According to the feature point position p, calculate the visual field exclusion potential field U FOV (p); Thresholds are set for the determinant of the dynamic Jacobian matrix J(q) and the image interaction matrix L to detect and calculate the singular repulsive potential field U JS (q) and U IS (s); Based on the obstacle model, the constraint checker verification function is used to detect whether the local transition path causes occlusion or collides with obstacles; According to the reachability and speed limit of the mobile platform, the kinematic repulsive potential field of the mobile platform is calculated; The attractive potential function is superimposed with the gradients of various repulsive potential fields to form a combined force, and the feature position is iteratively updated by the gradient descent method.

7. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 2, characterized in that: The step of performing trajectory smoothing and time parameterization on the discrete feature path to obtain the final image feature trajectory includes: The discrete feature path S = {s0, s1, ..., s N } to simplify and obtain the simplified path S′={s0,s2,…,s N }; Perform curve fitting and smoothing on the simplified path S′ to obtain a smooth trajectory s(t); According to the speed v max and acceleration a max Constraints are set, and the smooth trajectory s(t) is parameterized in time to obtain the final image feature trajectory.

8. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 7, characterized in that: The curve fitting and smoothing of the simplified path S′ to obtain a smooth trajectory s(t) includes: The simplified path S′ is converted into a continuous and differentiable smooth trajectory s(t) using cubic spline interpolation or polynomial interpolation method; The final smooth trajectory s(t) is obtained by curve fitting by minimizing the following objective function; Where λ is the weight of the smoothing term.

9. The artificial potential field-based constraint processing planning method for mobile manipulator visual servoing according to claim 1, characterized in that: The online image servo execution and tracking of the mobile operating arm based on the final image feature trajectory includes: The final image feature trajectory s(t) is input into the classic image-based visual servo control law or the improved controller based on interactive matrix feedback; The controller adjusts the joint speed of the mobile platform and the robotic arm according to the real-time image feature error, so that the end effector can smoothly approach the desired feature position; During the approximation process, an adaptive control strategy is used to cope with dynamic environmental changes and ensure the robustness of the control process.

10. A constraint processing planning system based on artificial potential field for visual servoing of mobile manipulators, characterized in that: include: Data acquisition module, used to obtain initial parameters and perform system modeling; The global planning module is used to perform sampling-based global path planning based on system modeling to obtain the initial feasible path; The local planning module is used to verify the initial feasible path based on the APF local planner and preset multiple constraints to obtain a discrete feature path; The trajectory smoothing and time parameterization module is used to perform trajectory smoothing and time parameterization on discrete feature paths to obtain the final image feature trajectory; The servo control module is used to perform online image servo execution and tracking of the mobile manipulator based on the final image feature trajectory.

Images