A method and system for constraint handling programming based on artificial potential field for visual servoing of mobile manipulators

Abstract

The application relates to a kind of artificial potential field-based constraint processing planning method and system for mobile operating arm visual servo, comprising the following steps: obtaining initial parameters, and carrying out system modeling;Based on system modeling, global path planning based on sampling is carried out, and initial feasible path is obtained;The initial feasible path is checked based on the local planner of APF and the preset multiple constraint requirements, and discrete feature path is obtained;Trajectory smoothing and time parameterization are carried out on the discrete feature path, and the final image feature trajectory is obtained;Based on the final image feature trajectory, the mobile operating arm is executed and tracked on-line image servo.This application can be widely applied in the field of robot technology.

Description

TECHNICAL FIELD

[0001] The application relates to an artificial potential field (APF) based constraint handling planning method and system for visual servoing of a mobile manipulator, and belongs to the technical field of robots. BACKGROUND

[0002] With the rapid development of robot technology, a mobile manipulator, which is a comprehensive system combining a mobile platform and a manipulator, has been widely applied in intelligent manufacturing, warehouse logistics and service robots. The mobile manipulator realizes large-scale displacement by using a mobile chassis, and then completes fine operation tasks through a manipulator. Among many control methods, visual servoing (VS) technology obtains image features of the environment and the target through a camera, and converts image feature errors into control instructions to guide the end-effector of the mobile manipulator to accurately reach the desired position and attitude. Image-based visual servoing (IBVS) technology is particularly concerned, which has strong robustness to model and camera calibration errors, and can realize high-precision control in a relatively complex and uncertain environment.

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

[0004] 1. Joint position limit: each joint of the manipulator must be kept within a safe allowable range, otherwise it may cause equipment damage or safety hazards.

[0005] 2. Field of view limit: image features must always be within the visible range of the camera. Once the target feature is out of the camera's field of view, the visual servoing process will not continue.

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

[0007] 4. Obstruction and collision risk: 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 blocked, resulting in image feature loss.

[0008] 5. Local minimum value trap: traditional local path planning methods relying on simple interpolation or optimization often fall into local minimum values when considering multiple constraints, thus failing to find a feasible global path.

[0009] 6. Mobile platform and mechanical arm collaborative constraint: Compared with the mechanical arm of fixed base, the planning of mobile operating arm needs to consider the kinematic constraint of mobile chassis and the joint limit of mechanical arm. In image servo, this increases the difficulty of control and planning.

[0010] The existing method is mainly for visual servo of stationary mechanical arm, and lacks efficient and robust global planning and local regulation scheme for mobile operating arm under multiple constraints. SUMMARY

[0011] To solve the above problems, the purpose of the present application is to provide an artificial potential field based constraint processing planning method and system for mobile operating arm, which combines global path planning based on sampling and APF local controller to meet the requirements of multiple constraints such as joint position, field of view, singularity, occlusion, collision and mobile platform characteristics, and quickly generates feasible visual servo trajectory considering multiple constraints for mobile operating arm in complex environment.

[0012] To achieve the above purpose, the present application adopts the following technical solutions:

[0013] In the first aspect, the present application provides an artificial potential field based constraint processing planning method for visual servo of mobile operating arm, comprising the following steps:

[0014] Obtain initial parameters and perform system modeling;

[0015] Based on system modeling, perform global path planning based on sampling to obtain initial feasible path;

[0016] Based on the local planner of APF and the preset multiple constraint requirements, check the initial feasible path to obtain discrete feature path;

[0017] Smooth the discrete feature path and parameterize the time to obtain the final image feature trajectory;

[0018] Based on the final image feature trajectory, perform online image servo execution and tracking for the mobile operating arm.

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

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

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

[0022] Obtain the intrinsic matrix K and extrinsic matrix [R|t] of the camera, which are used for three-dimensional reconstruction and pose estimation of image features;

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

[0024] Furthermore, based on the system modeling, 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 is constrained and checked. * 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] Where δ is the expansion step size.

[0031] Furthermore, 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 is constrained and checked. * A feasible path is found or the preset maximum number of iterations is reached, including:

[0032] Use the constraint checker to check the nearest node s near To the new node s new Perform constraint checking on the transition paths between them;

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

[0034] Repeat the above two steps until the initial image feature s0 is converted to the desired image feature s *The initial feasible path of the global planner is obtained by finding a feasible path or reaching a preset maximum number of iterations.

[0035] Further, the APF-based local planner and preset multiple constraint requirements are used to check the initial feasible path to obtain a discrete feature path, including:

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

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

[0038] According to the feature point position p, calculate the field of view repulsive potential field U FOV (p);

[0039] Set a threshold for the determinant of the dynamic Jacobian matrix J(q) and the image interaction matrix L, detect and calculate the singularity repulsive potential field U JS (q) and U IS (s);

[0040] Based on the obstacle model, verify whether the local transition path causes occlusion or collision with obstacles through the constraint checker verification function;

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

[0042] Superimpose the gradient of the attractive potential function and various repulsive potential fields to form the resultant force, and update the feature position by gradient descent method.

[0043] Further, the trajectory smoothing and time parameterization of the discrete feature path are performed to obtain the final image feature trajectory, including:

[0044] Simplify the discrete feature path S = {s0, s1, …, s N} generated by the global planner to obtain the simplified path S' = {s0, s2, …, s N};

[0045] Curve fitting and smoothing are performed on the simplified path S' to obtain a smooth trajectory s(t);

[0046] According to the speed v max and acceleration a max constraints, time parameterize the smooth trajectory s(t) to obtain the final image feature trajectory.

[0047] Further, the curve fitting and smoothing of the simplified path S' to obtain a smooth trajectory s(t) includes:

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

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

[0050]

[0051] In the formula, λ is the weight of the smoothing term.

[0052] Further, the mobile manipulator is executed and tracked online based on the final image feature trajectory, including:

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

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

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

[0056] In a second aspect, the present application provides an artificial potential field-based constraint processing planning system for visual servoing of a mobile manipulator, including:

[0057] A data acquisition module is used to acquire initial parameters and perform system modeling;

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

[0059] A local planning module is used to check the initial feasible path based on an APF-based local planner and a preset multiple constraint requirement to obtain a discrete feature path;

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

[0061] A servo control module is used to execute and track the mobile manipulator online based on the final image feature trajectory.

[0062] The present application has the following advantages due to the above technical solutions:

[0063] 1. High success rate and robustness: The present application considers multiple constraints comprehensively, even if the initial image features and desired image features differ greatly, there are obstacles and model errors, it can still quickly generate a feasible trajectory.

[0064] 2. Improved computational efficiency: The present application combines global search based on sampling with an efficient local planner, which can find a feasible solution in seconds, significantly reducing planning time.

[0065] 3. Avoid local minimum value trap: The present application can repeatedly try different paths by resampling the global framework, reducing the probability of falling into a local minimum.

[0066] 4. Adapt to complex environment: The present application considers chassis kinematics, obstacle environment, camera field of view and other real-world constraints in the mobile manipulator working scene, making the solution more practical.

[0067] Therefore, the present application can be widely applied in the field of robot technology. BRIEF DESCRIPTION OF DRAWINGS

[0068] Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The accompanying drawings are included to provide a description of the preferred embodiments and are not meant to limit the present application. Throughout the drawings, the same reference numerals are used for the same components. In the drawings:

[0069] Figure 1 is a flowchart of the artificial potential field-based constraint processing planning method for mobile manipulator visual servo provided by the embodiments of the present application;

[0070] Figure 2 is a path planning schematic diagram provided by the embodiments of the present application:

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

[0072] To make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions of the embodiments of the present application will be described clearly and completely below in conjunction with the drawings of the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all. Based on the described embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art belong to the scope of protection of the present application.

[0073] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, elements, components, and / or groups thereof, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and / or groups thereof.

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

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

[0076] Embodiment 1

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

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

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

[0080] 3) Based on the APF local planner and the preset multiple constraint requirements, check the initial feasible path to obtain a discrete feature path;

[0081] 4) Perform trajectory smoothing and time parameterization on the discrete feature path to obtain a final image feature trajectory;

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

[0083] Further, in the above step 1), the following steps are included:

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

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

[0086]

[0087] In the formula, x is the position and attitude vector of the end effector; q is the joint angle vector.

[0088] 1.2) Collect the initial image feature s0 and the expected image feature s * , wherein the expected image feature s * refers to a set of feature point coordinates of the target object;

[0089] 1.3) Obtain the intrinsic matrix K and the extrinsic matrix [R|t] of the camera, which are used for three-dimensional reconstruction and pose estimation of the image feature.

[0090]

[0091] , wherein f x and f y are focal lengths, (c x , c y ) are principal points, R and t are rotation matrix and translation vector respectively.

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

[0093] Further, in the above step 1.4), the following steps are included:

[0094] 1.4.1) Obtain the three-dimensional obstacle information of the working environment, construct an environment map, which is used to check whether the robot collides with the environment in subsequent planning.

[0095] Wherein, the method of constructing the environment map can adopt the technology known to those skilled in the art, such as octree map, etc., which is not limited in the present application;

[0096] 1.4.2) Determine the field of view (FOV) of the camera, and the position distribution and potential occlusion area of the target object, which are used to check whether the robot occludes the target object in subsequent planning.

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

[0098] Furthermore, in step 2) above, 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 Pixels.

[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 robotic 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 is constrained and checked. * 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) 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 rand ;

[0108]

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

[0110]

[0111] where δ is the expansion step size.

[0112] Further, in the above step 2.3), the following steps are included:

[0113] 2.3.1) Use the constraint checker to check the transition path between the nearest node s near and the new node s new , denoted as:

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

[0115] 2.3.2) If the transition path does not satisfy the constraint checking condition, discard the new node s new and return to step 2.2.1) to resample;

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

[0117] Further, in the above step 3), the initial feasible path is checked based on the APF-based local planner and the preset multiple constraints, including the following steps:

[0118] 3.1) Artificial potential field construction: define an attractive potential function U att (s) such that the desired image feature s * becomes the lowest point of the potential field:

[0119]

[0120] where k att is the attractive potential field coefficient.

[0121] 3.2) Joint position limit: Compute joint repulsive potential U JL (q).

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

[0123]

[0124] where k JL is the joint repulsive potential 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 limit (FOV): Compute field of view repulsive potential U FOV (p).

[0126] In this embodiment, define the effective working area of the image plane, and introduce the field of view repulsive potential function U FOV (p) :

[0127]

[0128] where d i is the minimum distance from the feature point p i to the image boundary, k FOV is the field of view repulsive potential coefficient.

[0129] 3.4) Singularity constraint: Detect and compute singularity repulsive potential U 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 repulsive potential function U JS (q) and U IS (s) :

[0131]

[0132] where k JS and k IS are the singularity repulsive potential coefficients, and ∈ is the threshold.

[0133] 3.5) Occlusion and collision detection: Verify whether the local transition path causes occlusion or collision with obstacles by the constraint checker verification function:

[0134]

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

[0136] 3.6) Kinematic constraints of mobile platform: Incorporate the reachability and speed limit of mobile platform into potential field or constraint condition:

[0137]

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

[0139] 3.7) Superimpose the gradient of attractive potential function and various repulsive potential fields to form the resultant force, and update the feature position iteratively by gradient descent method.

[0140] Further, in the above step 3.7), the following steps are included:

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

[0142]

[0143] 3.7.2) Update the feature position iteratively by gradient descent method:

[0144] s k+1 = s k + aF(s k )

[0145] where a is the step size coefficient.

[0146] 3.7.3) If the local planning is blocked (e.g. trapped in local minimum), mark the expansion as failed, and the global planning tries a new random point until the new image feature position s(t) converges to the target image feature s * or reaches the iteration limit, to improve the overall success rate.

[0147] Further, in the above step 4), the trajectory smoothing and time parameterization are performed on the discrete feature path to obtain the final image feature trajectory, including the following steps:

[0148] 4.1) Simplify the discrete feature path S = {s0, s1, …, s N} generated by global planning to obtain the simplified path S' = {s0, s2, …, s N}.

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

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

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

[0152] 4.3) Time parameterization of the smooth trajectory s(t) according to the velocity v max and acceleration a max constraints.

[0153] Further, in step 4.2) above, the following steps are included:

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

[0155]

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

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

[0158]

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

[0160] Further, in step 4.3) above, the following steps are included:

[0161] 4.3.1) Assign a reasonable time distribution to the smooth feature trajectory to satisfy the velocity v max and acceleration a max constraints:

[0162]

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

[0164] Further, in step 5) above, based on the final image feature trajectory, the moving operation arm is executed online image servo 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 interaction matrix feedback-based controller:

[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 desired feature position.

[0168] 5.2) The controller adjusts the moving platform and mechanical arm joint speed 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 strategy to cope with dynamic environmental changes and ensure the robustness of the control process:

[0170]

[0171] Where γ is the adaptive gain adjustment coefficient.

[0172] Embodiment 2

[0173] As Figure 3 shown, this embodiment is proved by simulation test that under the conditions of joint limit, FOV limit and singularity, the present application can generate a feasible trajectory in 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 operation arm in a complex warehouse environment.

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

[0177] 2) Experimental process

[0178] 2.1) The initial feature s0 and the target feature s * are respectively set at different positions to ensure that the path planning needs to bypass obstacles.

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

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

[0181] 3) Experimental results

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

[0183] 3.2) Success rate: In 90% of the test scenarios, a feasible trajectory is successfully generated, indicating that the method has high robustness.

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

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

[0186] 4) Performance analysis

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

[0188] 4.2) Robustness: The method still works stably in the presence of multiple constraints and dynamic obstacles, demonstrating its robustness.

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

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

[0191] Example 3

[0192] The embodiment 1 provides the artificial potential field based constraint processing planning method for the mobile manipulator visual servoing, and the embodiment provides an artificial potential field based constraint processing planning system for the mobile manipulator visual servoing. The system provided by the embodiment can implement the artificial potential field based constraint processing planning method for the mobile manipulator visual servoing of the embodiment 1. The system can be implemented by software, hardware or a combination of software and hardware. For example, the system can include integrated or separated functional modules or functional units to perform the corresponding steps in the method of the embodiment 1. Since the system of the embodiment is basically similar to the method embodiment, the process of the embodiment is relatively simple, and the related parts can be referred to the part of the description of the embodiment 1. The system embodiment provided by the embodiment is only illustrative.

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

[0194] The data acquisition module is configured to acquire initial parameters and perform system modeling.

[0195] The global planning module is configured to perform global path planning based on sampling on the basis of the system modeling to obtain an initial feasible path.

[0196] The local planning module is configured to check the initial feasible path based on an APF local planner and preset multiple constraint requirements to obtain a discrete feature path.

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

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

[0199] Finally, it should be noted that: the above embodiments are used to illustrate the technical solutions of the present application rather than limit them, and although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that the specific embodiments of the present application can be modified or replaced equivalently without departing from the spirit and scope of the present application, and any modification or equivalent replacement without departing from the spirit and scope of the present application should be covered in the protection scope of the claims of the present application.

Claims

1. A method for artificial potential field based constraint handling planning for mobile manipulator visual servoing, characterized in that, The method comprises the following steps: obtaining initial parameters and performing system modeling; based on the system modeling, performing global path planning based on sampling to obtain an initial feasible path; based on an APF local planner and preset multiple constraint requirements, checking the initial feasible path to obtain a discrete feature path; performing trajectory smoothing and time parameterization on the discrete feature path to obtain a final image feature trajectory; based on the final image feature trajectory, performing online image servo execution and tracking on the mobile manipulator.

2. The artificial potential field based constraint handling planning method for visual servoing of a mobile manipulator according to claim 1, wherein, The obtaining of the initial parameters and the system modeling comprises: obtaining the kinematic model of the mobile chassis and the manipulator and calculating the kinematic Jacobian matrix; collecting an initial image feature s0 and a desired image feature s * ; obtaining the intrinsic matrix K and the extrinsic matrix [R|t] of the camera, which are used for three-dimensional reconstruction and pose estimation of the image feature; obtaining three-dimensional obstacle information of the working environment and position distribution and potential occlusion area of the target object, and performing obstacle modeling.

3. The artificial potential field based constraint handling programming method for visual servoing of a mobile manipulator according to claim 2, wherein, The global path planning based on sampling comprises: The search space state is defined, and combined with the comprehensive freedom of the mobile chassis and the mechanical arm, a high-dimensional planning space is formed 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 ; s near and new node s new is performed by the constraint checker until a feasible path from the initial image feature s0to the desired image feature s * is found or a preset maximum number of iterations is reached.

4. The artificial potential field based constraint handling programming method for visual servoing of a mobile manipulator of claim 3, wherein, The new node s new The calculation formula is: wherein δ is an expansion step length.

5. The artificial potential field based constraint handling planning method for visual servoing of a mobile manipulator of claim 3, wherein, The constraint checker is used to check the transition path between the last node s near and the new node s new , until a feasible path from the initial image feature s0 to the desired image feature s * is found or a preset maximum number of iterations is reached, comprising: using the constraint checker to check constraints on a transition path from a recent node s near to a new node s new ; If the transition path does not satisfy the constraint checking condition, discard the new node s new And resample; The above two steps are repeated until a feasible path from the initial image feature s0to the desired image feature s * is found or a preset maximum number of iterations is reached, obtaining an initial feasible path of the global plan.

6. The artificial potential field based constraint handling programming method for visual servoing of a mobile manipulator according to claim 2, wherein, The checking of the initial feasible path based on the APF local planner and the preset multiple constraint requirements comprises: defining an attractive potential function U att (s) such that the desired image feature s * becomes a minimum of the potential field; Calculate joint repulsive potential field U according to current joint configuration q JL (q); According to the feature point position p, the field of view repulsive potential field U is calculated FOV (p); A threshold is set for the determinant of the kinetic Jacobian matrix J(q) and the image interaction matrix L, singularity repulsive potential U is detected and calculated JS (q) and U IS (s); based on the obstacle model, detecting whether the local transition path causes occlusion or collision with the obstacle through a constraint checker verification function; calculating the kinematic repulsive potential field of the mobile platform according to the reachability and speed limit of the mobile platform; superimposing the gradient of the attractive potential function and various repulsive potential fields to form a resultant force, and iteratively updating the feature position through the gradient descent method.

7. The artificial potential field based constraint handling programming method for visual servoing of a mobile manipulator according to claim 2, wherein, The trajectory smoothing and time parameterization of the discrete feature path comprise: Simplify the discrete feature path S = {s0, s1,..., s N} generated by global planning to obtain a simplified path S' = {s0, s2,..., s N} performing curve fitting and smoothing on the simplified path S' to obtain a smooth trajectory s(t); According to the velocity v max and the acceleration a max constraints, the smooth trajectory s(t) is time parameterized to obtain the final image feature trajectory.

8. The artificial potential field based constraint handling programming method for visual servoing of a mobile manipulator of claim 7, wherein, The curve fitting and smoothing of the simplified path S' to obtain the smooth trajectory s(t) comprise: adopting a cubic spline interpolation or a polynomial interpolation method to convert the simplified path S' into a continuous and differentiable smooth trajectory s(t); performing curve fitting by minimizing the following objective function to obtain the final smooth trajectory s(t); wherein λ is a smoothing item weight.

9. The artificial potential field based constraint handling planning method for visual servoing of a mobile manipulator according to claim 1, wherein, The online image servo execution and tracking of the mobile manipulator based on the final image feature trajectory comprise: inputting the final image feature trajectory s(t) into a classical image-based visual servo control law or an improved controller based on interaction matrix feedback; the controller adjusts the speed of the mobile platform and the joints of the manipulator according to real-time image feature errors, so that the end effector smoothly approaches the desired feature position; in the approaching process, an adaptive control strategy is adopted to cope with dynamic environmental changes, so as to ensure the robustness of the control process.

10. A potential field based constraint handling planning system for mobile manipulator visual servoing, characterized in that, The method comprises: a data acquisition module for obtaining initial parameters and performing system modeling; a global planning module for performing global path planning based on sampling based on the system modeling to obtain an initial feasible path; a local planning module for checking the initial feasible path based on an APF local planner and preset multiple constraint requirements to obtain a discrete feature path; The trajectory smoothing and time parameterization module is configured to perform trajectory smoothing and time parameterization on the discrete feature path to obtain a final image feature trajectory. The servo control module is configured to perform online image servo execution and tracking on the moving operation arm based on the final image feature trajectory.

Images