Collaborative motion planning method and system for unmanned material grabbing machine

Abstract

The invention relates to a cooperative motion planning method and system for an unmanned material grabbing machine, and belongs to the technical field of motion planning of material grabbing machines. The method comprises the following steps: defining a path planning task; under collision constraint, joint constraint and incomplete constraint, an optimal path searching scheme with the shortest path length, the maximum distance between a path node and an obstacle and the minimum joint configuration change of adjacent path nodes as targets is established; an improved fast expansion random tree algorithm with target offset is adopted to generate a target path, focusing processing is carried out on a moving area of a chassis of the material grabbing machine, and then a random number growth method is improved to obtain the target path; and the obtained target path is subjected to smoothing processing, including rotating angle optimization processing of a mechanical arm of the material grabbing machine and redundant node processing, and then the final motion path of the unmanned material grabbing machine is obtained. The system is based on the method.

Description

Technical Field

[0001] The invention belongs to the technical field of motion planning of a material grabber, and relates to a collaborative motion planning method and system for an unmanned material grabber. Background Art

[0002] Grab machines were originally a type of engineering machinery equipment used for material loading and unloading, which evolved from excavators. With the advancement of industrialization and the increase in demand for material handling, grab machines have gradually developed into an independent and specialized engineering equipment. In China, the development of grab machines can be traced back to the end of the last century and the beginning of this century. With the introduction and digestion of technology, domestic enterprises began to independently develop and produce grab machines, which are widely used in various industrial fields.

[0003] Grab machines are widely used in various factories and material handling occasions, such as textile mills, weaving mills, steel mills, coal mines, cement plants, petrochemical plants, etc. In textile mills, grab machines are mainly used to handle raw materials; in steel mills, they are used to handle various iron ores, iron bauxite, scrap steel and other materials. In addition, grab machines are also widely used in food processing, pharmaceutical manufacturing, cosmetics production and other fields to achieve automated and efficient material handling.

[0004] Modern material grabbers are usually equipped with advanced control systems that can receive operating instructions, monitor equipment status, and coordinate the work of various parts. Through the synergy of components such as sensors, controllers, and actuators, the automatic and intelligent operation of the material grabber can be achieved. With the development of unmanned research on engineering machinery such as material grabbers, the continuous improvement of operation decision-making and environmental perception capabilities, higher requirements are placed on the diversity of the motion trajectory of the material grabber, which also puts forward new requirements for the actual motion trajectory description method during the operation process.

[0005] In particular, there are challenges such as how to achieve the coordinated movement of the gripper robot arm and the mobile chassis in a complex dynamic environment, how to effectively avoid collisions and dynamic obstacles, how to ensure safe and efficient operations, how to perform task allocation and resource optimization to optimize the overall performance of the system, and how to improve the adaptability and execution efficiency of the automation system in a complex environment through real-time collaborative decision-making. These issues need to be resolved. Summary of the invention

[0006] In view of this, the purpose of the present invention is to provide a collaborative motion planning method and system for an unmanned material grabber. The present invention effectively avoids collisions and dynamic obstacles through intelligent path planning, ensuring safe and efficient operation. At the same time, the present invention also covers task allocation and resource optimization to optimize the overall performance of the system, and improve the adaptability and execution efficiency of the automation system in complex environments through real-time collaborative decision-making.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] A collaborative motion planning method for an unmanned material grabber comprises the following steps:

[0009] S1. Define the path planning task, determine the collision-free path in the joint configuration space based on the initial joint configuration and target configuration of the gripper, as well as the workspace and environmental obstacles;

[0010] S2. Under collision constraints, joint constraints and non-holonomic constraints, establish an optimal path search scheme with the goal of shortest path length, maximum distance between path nodes and obstacles, and minimum change in joint configuration of adjacent path nodes;

[0011] S3, using an improved fast-expanding random tree algorithm with target bias to generate a target path, wherein the moving area of ​​the gripper chassis is first focused, and then the random number growth method is improved to obtain the target path;

[0012] S4. Smoothing the obtained target path, including optimizing the angle of the gripper robot arm and processing redundant nodes, to obtain the final motion path of the unmanned gripper.

[0013] Furthermore, in step S1, the joint space of the gripper is used as the working space to plan the motion path, wherein:

[0014] The working space of the gripper robot arm is represented by X∈R 3 , the obstacles in the workspace are represented by X obs ∈X; the joint configuration of the mobile robot is q=[q b ,q m ] T =[x,y,φ,θ1,θ2,θ3] T , where q b = {x, y, Φ} represents the position of the mobile chassis, q m ={θ1,θ2,θ3} represents the joint configuration of the robot arm, and each joint configuration corresponds to a position of the end effector of the gripper robot arm;

[0015] For each joint configuration q of the grabber, including the chassis and the movement of the manipulator, its geometric configuration is represented by a set of links M(q)∈X; the joint configuration space C of the grabber manipulator contains all its feasible joint configurations. obs Represents the joint configuration that collides with the obstacle, C free represents the set of collision-free joint configurations in the configuration space, with C free ={q∈C|C obs}; According to the forward kinematic transformation of the gripper robot arm, its joint configuration is converted into a Cartesian space state.

[0016] Given the initial joint configuration q start and target configuration q goal , as well as the workspace X and environmental obstacles X obs , the goal is to find a collision-free path in the joint configuration space where q0 = q start ,q l =q goal , l is the number of nodes in the planned path.

[0017] Further, in step S1, the task execution stage of the gripper is determined according to whether the distance between the gripper and the grasping target exceeds a threshold distance. When the distance between the gripper and the grasping target exceeds the threshold distance, the gripper executes the first task, that is, only the gripper chassis moves and the robotic arm remains stationary; when the distance between the gripper and the grasping target is less than or equal to the threshold distance, the gripper executes the second task, that is, the gripper robotic arm and chassis move in coordination and grasp the target.

[0018] Further, in step S2, the shortest path length means that the path with the shortest distance from the grabber to the target point is preferentially selected within the constraint range; the maximum distance between the path node and the obstacle means that the path with the largest distance Δd between each node in the path and the obstacle is preferentially selected within the constraint range; the minimum change in the joint configuration of adjacent path nodes means that the path with the minimum change in the joint configuration between adjacent path points is preferentially selected within the constraint range, wherein, assuming that the joint configurations of two adjacent path points are q1 and q2 respectively, the change in the joint configuration between adjacent path points is:

[0019] Δq=||q2-q1||

[0020] where ||·|| is the Frobenius norm;

[0021] The obstacle avoidance path planning of the grabber manipulator is regarded as the process of finding the extreme value of the function under the known function constraints: the constraints are to avoid collision with obstacles in the environment, joint limit constraints and system non-holonomic constraints, and the objective function is the cost of path planning; then its mathematical model is expressed as:

[0022] minf(q),q∈C free ,g i (q)≤b i ,i=1,2...,p

[0023] Where f(q) represents the objective function of the planned path, g i (q) represents the constraint function, b i represents constraints, and p represents the number of constraints.

[0024] Furthermore, in step S2, in the process of establishing collision constraints, the irregular obstacles are simplified into sphere or cube models, the chassis of the material grabber is simplified into a cylinder model, and the robotic arm is simplified into a series of sphere models with a radius of R; for the mobile chassis part of the material grabber, a collision detection method based on the cylinder envelope is adopted, and the collision detection strategy based on the envelope sphere is adopted for the robotic arm part and the obstacle;

[0025] The safe distance between the robot arm and the obstacle is d safe , the gripper arm is in joint configuration q l When the distance from the center of the sphere to the obstacle is:

[0026]

[0027] in Represents the position of the obstacle in the world coordinate system. Indicates the center position of the envelope sphere in the world coordinate system;

[0028] For each node in the planned path, the distance d from the center of the envelope sphere to the obstacle must be j (q l ) is greater than the sum of the envelope sphere radius and the set safety distance; the relationship between the center position of the envelope sphere and its coordinate system is expressed by the following formula:

[0029]

[0030] in is the transformation from the world coordinate system to the coordinate system {i}, represents the position of the sphere center in the coordinate system {i};

[0031] Joint constraints refer to the constraints caused by the joint limit of the grabber arm itself, including the joint limit of the arm joint and the space limit of the environment where the grabber chassis is located. When the grabber arm moves in a given environment, the chassis movement range is limited to a given workspace, with the following restrictions in the x and y directions:

[0032] L min ≤[x l ,y l ] T ≤L max

[0033] Where L min and L max They represent the two-dimensional boundaries of the chassis workspace respectively. The heading angle range of the chassis of the gripper is set between [0,2π]. The rotation range of each joint of the robotic arm meets the following restrictions:

[0034] θ i·min ≤θ i≤θ i·max

[0035] where θ i·min and θ i·max They represent the range of joint angles of the robot arm joint i respectively; according to the single joint limit of the gripper robot arm, its overall configuration joint limit is obtained as q∈[q min ,q max ];

[0036] The non-holonomic constraint means that the chassis of the material grabber adopts a differential drive wheel structure and can only move in the forward direction and rotate. The change Δφ between the chassis heading angles Φ1 and Φ2 of two consecutive path points of the chassis of the material grabber is defined as:

[0037] Δφ=|φ2-φ1|

[0038] Among them, the change of ΔΦ between two consecutive path points is used as an indicator of path evaluation.

[0039] Further, in step S3, an improved rapidly expanding random tree algorithm with target bias is used to generate a target path, and the process is as follows:

[0040] First, use the initial node q start As the root node of the random tree, initialize the random tree T;

[0041] The initialized random tree contains only the initial node, and the random tree is continuously expanded through iteration until the target node q is reached. goal ;

[0042] During the random tree growth process, a sampling point q is randomly selected in the sampling space according to the ChooseSampling() function. rand , and then use the NearestNeighbor() nearest node function to find the distance q in the search tree rand The node closest to the node q nearest Finally, the Extend() function is used to add a new node q to the random tree. new ;

[0043] In the process of randomly selecting sampling points, the target preference feature is added to the search of the robot arm's partial configuration. The expression of the modified ChooseSampling() sampling function to generate random nodes is:

[0044]

[0045] In the formula, q b = {x, y, φ} represents the position of the mobile chassis, q m ={θ1,θ2,θ3} represents the joint configuration of the robot arm;

[0046] By continuously iterating and expanding the random number, the new node q expanded within the allowed time new The distance q from the target node goal When the distance is less than the given DISTANCE_THRESHOLD value, a random tree and path are returned, and the planning is successful.

[0047] Further, in step S3, during the expansion process, the chassis moving area is first focused according to the initial position and target position of the chassis of the material grabber, and the moving focus area of ​​the chassis of the material grabber is determined by generating a series of discs from the initial position to the target position in the chassis moving plane, specifically:

[0048] The initial position of the grabber chassis is contained in the first disk, and the target position is contained in the last disk. The radius r of each disk is the difference between the distance from the center of the grabber chassis to the nearest obstacle and the radius of the inner circle of the grabber chassis. The obstacle is projected on a two-dimensional plane to calculate the distance from the mobile chassis to the nearest obstacle. The radius of the inner circle of the grabber chassis is the radius of the circle that encloses the entire grabber chassis.

[0049] The center of the first disc is the starting position of the gripper chassis (x s ,y s );

[0050] After determining the radius of the disk, k sampling points are randomly selected on the circumference of the disk;

[0051] Take the sampling point closest to the target position among the k sampling points as the center to create the next disk and determine the radius of the disk;

[0052] This process is repeated until the target position (x g ,y g ) appears on the disk.

[0053] Furthermore, in step S3, the random tree growth method is improved:

[0054] Assume that the previous path point moving chassis pose is q1.q b =(x1,y1,Φ1), the gripper chassis posture of the next path point corresponding to this node is q2.q b =(x2,y2,Φ2), the position and posture changes between the two nodes satisfy the following relationship:

[0055]

[0056] Where d is the change in chassis position between two adjacent path points, and ΔΦ is the change in chassis heading angle between two adjacent path points;

[0057] When planning the path, the metric function of the nearest node and the random tree growth method are improved:

[0058] In selecting a random sampling node q rand The nearest node q nearest When the adjacent nodes of the grabber chassis configuration are first determined: compare the position of the grabber chassis (x r ,y r ) and the node pose Φ r , when the postures are similar, the node is used as a neighboring node, that is:

[0059] Randomly sample the node position (x r ,y r ) in q near ·q b The coordinate position is the vertex, the radius is r, and the heading angle is the center angle of the 2α fan-shaped area, that is, the position change of the chassis between two adjacent nodes should be within the fan-shaped range; β = |φ r -φ n |<γ, that is, the change β of the heading angle between two adjacent nodes should be limited to the range of γ, where α, β, γ and r are preset parameters;

[0060] Grab machine robot arm node configuration q rand ·q m The nearest node is determined by weighting the configuration changes of each joint of the grabber robot arm: First, determine the node that satisfies the chassis neighboring node constraint and the distance q rand ·q b The nearest K nodes, then select q from among the K nodes near ·q m and q rand · m The closest node between them, finally getting the node closest to the randomly sampled node;

[0061] In generating random sampling nodes q rand And find its nearest node q nearest After that, the expansion function adds the new node q new Added to the random tree, the chassis configuration of the expansion node is q new ·q b =(x new ,y new ,z new ), the mobile chassis is at the node position (x n ,y n ) rotates to point to (x r ,y r ) and moves along the line connecting the two points for a distance d before reaching position (x new ,y new ), the heading angle of the extended node is:

[0062] φ new =λ(φ r -φ n )+φ n

[0063] in

[0064] The robotic arm from q nearesr ·q m To q rand ·q m Expand and insert new nodes, and check whether there is any collision when connecting the new node configuration to obtain the configuration q of the robot arm new ·q m ; Set the new configuration node q new Add to the random tree and update the random tree until the target path is generated.

[0065] Furthermore, in step S4, during the path smoothing process, a rotation constraint is added to the expanded new node:

[0066] The rotation constraint α is a vector and vector The angle is calculated as follows:

[0067]

[0068] Set the gripper arm rotation angle constraint threshold When the rotation angle α is less than or equal to the rotation angle constraint When the newly added node satisfies the rotation angle constraint, the node is added to the random tree; when the rotation angle α exceeds the rotation angle constraint When , the node is discarded and re-sampled;

[0069] Then remove redundant nodes from the path:

[0070] The line of sight algorithm is used to smooth the path, that is, to determine whether there will be obstacles between two nodes. If there is no collision, the two nodes are connected. If a collision occurs, the two nodes are not connected and the subsequent nodes are optimized. The specific execution steps are as follows:

[0071] First, use node q start For node q t Initialize and then follow the generated path to the next node q i Search and detect the node and the initial node q of the smoothing process t Whether there are obstacles on the connection line or whether there is a situation that exceeds the environmental boundary;

[0072] If there is no collision, continue to the next node q i+1 Conduct a search;

[0073] If a collision or out-of-bounds occurs, node q t and the previous node q of this node i-1 Connect to generate a new path, remove redundant nodes in the path, and finally use node q i-1 Q t Make a replacement and continue searching along the path;

[0074] Each time a collision occurs, the node q t Update and repeat the search and replace process until the target node q is found goal .

[0075] The present invention also proposes a system based on the collaborative motion planning method of the unmanned material grabber, which includes an information input module, a path planning module and a path output module, wherein:

[0076] The information input module is used to define the path planning task, and determine the collision-free path in the joint configuration space based on the initial joint configuration and target configuration of the gripper, as well as the workspace and environmental obstacles;

[0077] The path planning module is used to execute the aforementioned collaborative motion planning method of the unmanned material grabber, which at least includes establishing an optimal path search scheme with the shortest path length, the maximum distance between the path node and the obstacle, and the minimum change in the joint configuration of adjacent path nodes under collision constraints, joint constraints, and non-holonomic constraints; the path planning module uses an improved fast-expanding random tree algorithm with target bias to generate a target path, wherein the path planning module focuses on the moving area of ​​the chassis of the material grabber, and then improves the random number growth method to obtain the target path;

[0078] The path output module smoothes the target path given by the path planning module, including the optimization of the gripper arm angle and redundant node processing, and then the path output module outputs the final unmanned gripper motion path.

[0079] The beneficial effects of the present invention are:

[0080] Enhanced safety: Through intelligent path planning and dynamic obstacle avoidance, the risk of collision is effectively reduced, improving the safety of unmanned material handling machine operation.

[0081] Efficiency optimization: Optimize path selection and task allocation, maximize resource utilization, and ensure the high efficiency and rapid response capabilities of the overall system when performing various tasks.

[0082] Improved system integration: Through collaborative motion planning solutions, seamless collaboration between the robot arm and the crawler is achieved, improving the overall performance and stability of the system.

[0083] Enhanced environmental adaptability: Through real-time collaborative decision-making, the solution enables the unmanned material grabber to flexibly respond to complex and dynamically changing working environments, improving the system's adaptability and execution efficiency in complex environments.

[0084] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0086] Figure 1 Planning staging diagrams for long-distance missions;

[0087] Figure 2 This is an extended schematic diagram of the standard RRT algorithm;

[0088] Figure 3 A schematic diagram of the chassis moving area focus;

[0089] Figure 4 It is a schematic diagram of the chassis adjacent nodes;

[0090] Figure 5 This is a schematic diagram of the chassis expansion node;

[0091] Figure 6 Schematic diagram for extending node corner optimization. DETAILED DESCRIPTION

[0092] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0093] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0094] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0095] See also Figure 1 to Figure 6 , which is a collaborative motion planning method and system for an unmanned material grabber.

[0096] Example 1

[0097] This embodiment provides a collaborative motion planning method for an unmanned material grabber, which includes the following steps:

[0098] S1. Define the path planning task, determine the collision-free path in the joint configuration space based on the initial joint configuration and target configuration of the gripper, as well as the workspace and environmental obstacles;

[0099] S2. Under collision constraints, joint constraints and non-holonomic constraints, establish an optimal path search scheme with the goal of shortest path length, maximum distance between path nodes and obstacles, and minimum change in joint configuration of adjacent path nodes;

[0100] S3, using an improved Rapidly-exploring RandomTrees with Goal Bias (RRT GoalBias) algorithm to generate a target path, wherein the moving area of ​​the gripper chassis is first focused, and then the random number growth method is improved to obtain the target path;

[0101] S4. Smoothing the obtained target path, including optimizing the angle of the gripper robot arm and processing redundant nodes, to obtain the final motion path of the unmanned gripper.

[0102] This embodiment describes step S1 in detail as follows: When planning the path of the grabber robot arm, it can be done in the joint space or the task space. When planning in the joint space, planning is performed based on the initial joint configuration and target joint configuration of the grabber robot arm. Each joint of the grabber robot arm has a corresponding range of variation, and there is a fixed boundary in the sampling space. Sampling within the range of variation of each joint can effectively avoid exceeding the range of the joint configuration, and at the same time, no complex inverse kinematics calculations are required for each sampling point; when performing motion planning in the task space, planning is performed based on the initial position and target position of the end effector of the grabber robot arm. This method can intuitively display the motion path. The grabber chassis and robot arm have different motion characteristics and the overall inverse kinematics solution process is cumbersome and time-consuming. Therefore, in this embodiment, motion planning is performed based on the joint space.

[0103] The working space of the gripper robot arm is represented by X∈R 3 , the obstacles in the workspace are represented by X obs ∈X. The joint configuration of the mobile robot is q=[q b ,q m ] T =[x,y,φ,θ1,θ2,θ3] T , where q b = {x, y, Φ} represents the position of the mobile chassis, q m ={θ1,θ2,θ3} represents the joint configuration of the manipulator, and each joint configuration corresponds to a position of the end effector of the gripper manipulator. For each feasible joint configuration q of the gripper that includes chassis movement and manipulator movement, its geometric configuration can be represented by a set of links M(q)∈X. The joint configuration space C of the gripper manipulator contains all its feasible joint configurations. In its configuration space C obs Represents the joint configuration that collides with the obstacle, C free represents the set of collision-free joint configurations in the configuration space, with C free ={q∈C|C obs According to the forward kinematic transformation of the gripper robot arm, its joint configuration can be converted into a state in Cartesian space.

[0104] Given the initial joint configuration q start and target configuration q goal , as well as the workspace X and environmental obstacles X obs , the goal is to find a collision-free path in the joint configuration space where q0 = q start ,q l =q goal, l is the number of nodes in the planned path, and any two consecutive points in the path need to satisfy the non-holonomic constraints of the mobile chassis.

[0105] Additionally, in step S1, when the gripper arm performs a grasping task, the gripper chassis may need to move in a larger working area. When performing a long-distance grasping task, the gripper arm moves from the initial joint state q s ·q m The joint pose q reaching the target g ·q m The time taken is less than that taken by the chassis of the material grabber. Therefore, it is usually hoped that the mechanical arm of the material grabber will move as late as possible when performing long-distance operation tasks.

[0106] When the grabber's robotic arm performs a long-distance operation task, the task is divided into two sections. That is, when the distance between the grabber's chassis and the grasping target exceeds the threshold, the grabber's path will be divided into two sections. For example, the threshold can be set to 8, 10 or 12m. The specific setting size is related to the scene and the placement of the material pile, and can be adjusted at will. The path is divided into two sections, which means that when the first section of the task is executed when the distance is greater than the threshold, the grabber's chassis moves and the robotic arm remains stationary; after the grabber's chassis is less than or equal to the threshold distance from the target position, the second section of the task begins to be executed, that is, the grabber's robotic arm performs overall planning and coordinated movement to the target position. Figure 1 As shown in the figure, when executing long-distance tasks, the tasks are processed in segments. After the first segment of the task is completed and the two segments are reached, the grabber robot arm coordinates the motion planning. When executing short-distance tasks, the coordinated planning is directly performed.

[0107] The detailed description of step S2 in this embodiment is as follows: When planning the obstacle avoidance path of the material grabber robot arm, according to specific evaluation indicators, such as the shortest path length, the shortest time, the lowest energy consumption, etc., a collision-free optimal path from the starting point to the target point is searched in an environment with obstacles. This embodiment proposes the following evaluation indicators based on the characteristics of the material grabber robot arm.

[0108] (1) Path length: There are multiple feasible paths when the gripper arm moves from the starting point to the target point. The lengths of different paths are usually different, and the paths with longer total lengths are more time-consuming to execute in actual execution. Therefore, the path length is used as an indicator for path planning evaluation. Using the total path length l as an evaluation indicator will make the generated path length as short as possible and reduce the movement of the robot during task execution.

[0109] (2) Safety performance: The grabber arm has a more complex hardware structure. The grabber arm in the steel industry has a larger size. In actual operation scenarios, it should be kept away from obstacles as much as possible to reduce possible collisions with the environment. Therefore, safety performance is used as a path evaluation indicator, and the planned path should avoid being too close to obstacles. The distance Δd between each point in the path and the obstacle is used as an evaluation indicator to keep the generated path away from obstacles.

[0110] (3) Path smoothness: The change in the joints of the grabber arm between two adjacent path points in the path should be as small as possible, so that the path of the grabber arm is smooth when it moves, and the situation of drastic changes in the joint state during path peaks is reduced. The joint configurations of two adjacent path points are q1 and q2 respectively, and the change in joint configuration between adjacent path points is:

[0111] Δq=||q2-q1||

[0112] Where ||·|| is the Frobenius norm. The change in joint configuration between two adjacent path points is as small as possible, and the generated path will be smoother.

[0113] There are corresponding constraints in the process of path planning, and the planned path cannot violate the constraints. There are the following constraints in the process of path planning.

[0114] (1) Collision constraints

[0115] Collision constraints mean that the gripper arm needs to avoid collisions with environmental obstacles and the robot itself. In motion planning, collision constraints limit the gripper arm's choice of planned paths. Obstacles and the gripper arm in real scenes have irregular shapes, which requires a lot of calculations and is inefficient when performing collision detection. Therefore, it is impossible to check every point on the surface of the mobile arm, and the model of the detection object needs to be simplified.

[0116] For irregular obstacles, the obstacles can be simplified into spheres or cube models. For the mobile chassis of the grabber arm, a collision detection method based on cylindrical envelope is used to simplify the chassis of the grabber into a cylindrical model of appropriate height and diameter. The robot arm and obstacles use a collision detection strategy based on an enclosing sphere. The robot arm is replaced by a series of spheres with a radius of R. The equivalent replacement of spheres can avoid real-time updates caused by rotation, and the centers of the spheres are distributed on the robot arm connecting rods. The safe distance between the robot arm and the obstacle is d safe , the gripper arm is in joint configuration q l When the distance from the center of the sphere to the obstacle is:

[0117]

[0118] in Represents the position of the obstacle in the world coordinate system. Indicates the center position of the enveloping sphere in the world coordinate system.

[0119] For each node in the planned path, the above relationship needs to be satisfied, that is, the distance d from the center of the envelope sphere to the obstacle j (q l ) is greater than the sum of the envelope sphere radius and the set safety distance. The relationship between the center position of the envelope sphere and its coordinate system is shown in the following formula:

[0120]

[0121] in is the transformation from the world coordinate system to the coordinate system {i}, Represents the sphere center position in the coordinate system {i}.

[0122] (2) Joint constraints

[0123] Joint constraints refer to the constraints caused by the joint limit of the grabber's arm, including the joint limit of the arm joint and the space limit of the environment where the grabber chassis is located. Similar to collision constraints, joint constraints cannot violate the following conditions:

[0124] When the gripper arm moves in a given environment, the chassis movement range is limited to a given workspace, with the following restrictions in the x and y directions:

[0125] L min ≤[x l ,y l ] T ≤L max

[0126] Where L min and L max They represent the two-dimensional boundaries of the chassis working space respectively. The heading angle range of the chassis of the grabber should be set between [0,2π].

[0127] The rotation range of each joint of the robot arm also has corresponding restrictions. Each joint should meet the following restrictions:

[0128] θ i·min ≤θ i ≤θ i·max

[0129] where θ i·min and θ i·max They respectively represent the joint angle value range of the robot joint i.

[0130] According to the single joint limit of the gripper robot arm, its overall configuration joint limit is obtained as q∈[qmin ,q max ], for each node in path planning, the above restrictions should be met.

[0131] (3) Non-holonomic constraints

[0132] The gripper chassis adopts a differential drive wheel structure, which satisfies the non-complete constraint when moving, that is, the gripper chassis can only move in the forward direction and rotate. The evaluation index of the non-complete characteristic of the gripper chassis is that the lateral movement of the gripper chassis between two adjacent path points in the path is as small as possible. Therefore, the position of the gripper chassis between adjacent path points in the path (x b ,y b ) changes as small as possible, while the heading angle changes cannot be too large.

[0133] The change ΔΦ between the chassis heading angles Φ1 and Φ2 of two consecutive path points of the grabber chassis is defined as:

[0134] Δφ=|φ2-φ1|

[0135] The change of ΔΦ between two consecutive path points is used as an indicator for path evaluation, which will make the generated path meet the non-holonomic characteristics of the mobile chassis.

[0136] Therefore, from a mathematical perspective, the obstacle avoidance path planning of the grabber robot can be regarded as the process of finding the extreme value of a function with known function constraints: the constraints are to avoid collisions with obstacles in the environment, joint limits, and system incomplete constraints, and the objective function is the cost of path planning. Therefore, its mathematical model can be written in the following form:

[0137] minf(q),q∈C free ,g i (q)≤b i ,i=1,2...,p

[0138] Where f(q) represents the objective function of the planned path, g i (q) represents the constraint function, b i Represents the constraint condition, and p represents the number of constraint conditions. Through the proposed evaluation index and constraints, the path planning algorithm of the grabber manipulator is improved to generate the optimal path.

[0139] The detailed process of the content in step S3 in this embodiment is described as follows: First, this embodiment describes the conventional fast random number algorithm RRT and the fast extended random number RRT GoalBias algorithm with target bias.

[0140] like Figure 2The extended process of the standard RRT algorithm shown in the figure: The RRT algorithm includes processes such as initializing the tree, generating random nodes, finding the nearest node, and expanding the tree. To facilitate the description of the RRT algorithm, the definitions of various related functions are introduced as follows:

[0141] ChooseSampling() sampling function: randomly select nodes q that meet the requirements in the joint space of the grabber robot arm rand , a random sampling method is often used, that is, the sampling nodes obey a uniform random distribution in the configuration space.

[0142] NearestNeighbor() nearest node function: Assume that the generated random sampling node q rand It is outside the random tree T, traverses the entire random tree, and finds the distance node q in the random tree according to the metric function ρ rand The nearest node q nearest .

[0143] Extend() extension function: The direction of the vector is q rand Find a new node q with a certain step length ε as the starting point new , if from q rand to q new If no collision occurs during the expansion process, the new node q new Added to random tree.

[0144] First, use the initial node q start As the root node of the random tree, the random tree T is initialized. The initialized random tree only contains the initial node, and the random tree is continuously expanded through iteration until the target node q is reached. goal In the random tree growth process, firstly, a sampling point q is randomly selected in the sampling space according to the ChooseSampling() function. rand , and then use the NearestNeighbor() nearest node function to find the distance q in the search tree rand The node closest to the node q nearest Finally, the Extend() function is used to add a new node q to the random tree. new By continuously iterating and expanding the random number, the number of new nodes q expanded within the allowed time new The distance q from the target node goal When the distance is less than the given DISTANCE_THRESHOLD value, a random tree and path are returned, and the planning is successful.

[0145] The RRT algorithm expands the random tree by random sampling in the sampling space, generating random uniform random points in the entire configuration space. This search method ensures that the entire configuration space is effectively searched, but the random tree approaches the target area slowly, and the random tree often expands to a position that deviates from the target. In order to speed up the search for the target, this embodiment uses target preference to speed up the search process.

[0146] The RRT GoalBias algorithm improves the random sampling function of the RRT algorithm and generates random sampling points q rand When goal Replace the random sampling point q rand , accelerating the expansion of the random tree to the target area. After preprocessing, the search area of ​​the gripper chassis can converge to the target area quickly. Therefore, the random sampling points of the gripper chassis do not add the target preference feature, and only the search of the robot arm configuration is added with the target preference feature. The expression for generating random nodes by the modified ChooseSampling() sampling function is:

[0147]

[0148] When planning paths in complex high-dimensional dynamic environments, planning methods based on random sampling have excellent results. As a representative of random sampling algorithms, the rapidly expanding random tree (RRT) algorithm uses an incremental single query search method to quickly and efficiently search for a collision-free path from the initial point to the target point in high-dimensional space path planning. Based on the shortest path, chassis incompleteness characteristics, and path smoothness indicators in the above path evaluation indicators, the RRTGoalBias algorithm is improved to generate the optimal path.

[0149] The RRT algorithm has probabilistic integrity and randomness. Direct sampling search in the configuration space will have a high cost. At the same time, the non-holonomic constraints of the mobile robot chassis should be satisfied between two adjacent search nodes. Therefore, in this section, the mobile area of ​​the grabber chassis is first focused to accelerate the mobile chassis search process, and then the random tree growth method is improved to meet the non-holonomic constraints of the system.

[0150] The actual operation scene of the grabber arm usually has a large working space, and the grabber chassis can perform mobile tasks in the two-dimensional plane of the entire operation area. When searching for the path of the grabber chassis, directly searching the entire operation area will increase the difficulty of the search task and increase the path planning time of the grabber arm. This section will focus on the chassis movement area based on the initial position and target position of the grabber chassis.

[0151] The moving focus area of ​​the grabber chassis is determined by generating a series of disks from the initial position to the target position in the chassis moving plane. The initial position of the grabber chassis is contained in the first disk, and the target position is contained in the last disk. The radius r of each disk is obtained by subtracting the inner circle radius of the grabber chassis from the distance from the center of the grabber chassis to the nearest obstacle. The obstacles in the actual working grabbing operation scene of the grabber manipulator arm are generally loading vehicles, pillars, etc. The obstacles can be projected in a two-dimensional plane to calculate the distance from the mobile chassis to the nearest obstacle; the inner circle radius of the grabber chassis is the radius of the circle that encloses the entire grabber chassis.

[0152] The center of the first disc is the starting position of the gripper chassis (x s ,y s ). After determining the disk radius as above, randomly select k sampling points on the circumference of the disk. Create the next disk with the sampling point closest to the target position as the center, and determine the disk radius as above; iterate this process until the target position (x g ,y g ) appears on the disk.

[0153] The focus picture of chassis moving area is as follows Figure 3 As shown in the figure, a series of discs from the initial position to the target position are generated through regional focusing. When the gripper arm runs in the disc channel, the disc area provides a collision-free space for the gripper arm; the position search area of ​​the gripper chassis is limited to the disc space, which accelerates the path search speed.

[0154] This embodiment also improves the random tree growth method. The grabber chassis has non-complete constraints. Therefore, the posture transformation of the grabber chassis between adjacent nodes of the path should satisfy the non-complete constraints, that is, the position change in the posture transformation of the grabber chassis cannot be too large, and the change of the heading angle should be limited to a certain range.

[0155] Assume that the previous path point moving chassis pose is q1.q b =(x1,y1,Φ1), the gripper chassis posture of the next path point corresponding to this node is q2.q b =(x2,y2,Φ2), the position and posture changes between the two nodes need to satisfy the following relationship:

[0156]

[0157] Where d is the change in chassis position between two adjacent path points, and ΔΦ is the change in chassis heading angle between two adjacent path points. Therefore, when planning a path, it is necessary to improve the metric function of the nearest node and the random tree growth method in the algorithm.

[0158] In selecting a random sampling node qrand The nearest node q nearest When the measurement function is defined based on the distance of the composite distance, it is necessary to first determine the neighboring nodes of the gripper chassis configuration. Due to the non-holonomic constraints of the gripper chassis, it is necessary to compare the position of the gripper chassis (x r ,y r ) and the node pose Φ r , only when the postures are similar can they become adjacent nodes of the gripper chassis configuration. Figure 4 It represents the selection criteria of the nodes adjacent to the chassis of the material grabber. The position of the randomly sampled node (x r ,y r ) in q near ·q b The coordinate position is the vertex, the radius is r, and the heading angle is the center angle of the 2α fan-shaped area, that is, the position change of the chassis between two adjacent nodes should be within the fan-shaped range; β = |φ r -φ n |<γ, that is, the change β of the heading angle between two adjacent nodes should be limited to the range of γ. Among them, α, β, γ and r are parameters that need to be set.

[0159] Grab machine robot arm node configuration q rabd ·q m The nearest node is determined by weighting the configuration changes of each joint of the grabber robot arm. According to the above-mentioned mobile chassis neighboring node selection criteria, first determine the neighboring node that meets the chassis neighboring node constraint and has a distance q rand ·q b The nearest K nodes, then select q from among the K nodes near ·q m and q rand ·q m The nearest node between them is finally obtained.

[0160] The above steps generate a random sampling node q rand And found its nearest node q nearest After that, the expansion function adds the new node q new Added to the random tree. The chassis configuration of the extended node is q new ·q b =(x new ,y new ,z new ), the generated new node diagram is as follows Figure 5 As shown, the mobile chassis is at the node position (x n ,y n ) rotates to point to (x r ,y r) and moves along the line connecting the two points for a distance d before reaching position (x new ,y new ), the heading angle of the extended node is:

[0161] φ new =λ(φ r -φ n )+φ n

[0162] in

[0163] The robotic arm from q nearesr ·q m To q rand ·q m Expand and insert new nodes, and check whether there is any collision when connecting the new node configuration to obtain the configuration q of the robot arm new ·q m . Set the new configuration node q new Add to the random tree and update the random tree until the target path is generated.

[0164] The RRT GoalBias algorithm is improved by the above method, which accelerates the search process of the random tree and enables it to meet the non-holonomic constraints of the mobile manipulator.

[0165] In this embodiment, the path is smoothed in step S4. In order to improve the smoothness and fluency of the grabber robot arm task execution process and reduce the loss of the grabber robot arm during operation, an angle constraint is added to the expanded new node to make the planned path smoother. Figure 6 shown.

[0166] The rotation constraint α is a vector and vector The angle is calculated as follows:

[0167]

[0168] Set the gripper arm rotation angle constraint threshold When the rotation angle α is less than or equal to the rotation angle constraint When the newly added node satisfies the rotation angle constraint, the node is added to the random tree; when the rotation angle α exceeds the rotation angle constraint When , the node is discarded and re-sampled. The setting of the angle threshold of the grabber robot arm ensures the continuity between nodes.

[0169] Then the path is processed to remove redundant nodes. The RRT algorithm quickly generates a path from the initial position to the target position. The above improvements can improve the efficiency of path planning, but there are still corner nodes in the planned path, which have an adverse effect on the movement of the mobile robot. In order to reduce unnecessary mechanical losses at the turning points, the path needs to be smoothed. The line of sight algorithm is used to smooth the path, that is, to determine whether there will be obstacles between two nodes. If there is no collision, the two nodes are connected. If a collision occurs, they are not connected and the subsequent nodes are optimized. The specific execution steps are as follows:

[0170] First, use node q start For node q t Initialize and then follow the generated path to the next node q i Search and detect the node and the initial node q of the smoothing process t Is there an obstacle on the line between them or is there a situation of exceeding the environmental boundary? If there is no collision, continue to the next node q i+1 Search; if a collision or out-of-bounds occurs, node q t and the previous node q of this node i-1 Connect to generate a new path, remove redundant nodes in the path, and finally use node q i-1 Q t Replace it and continue searching along the path. Each time a collision occurs, node q is t Update and repeat the search and replace process until the target node q is found goal .

[0171] After smoothing, unnecessary turning points are eliminated from the path, and the number of peaks in the generated path is reduced, making it more suitable for the actual movement of the unmanned material grabber.

[0172] Example 2

[0173] This embodiment proposes a planning system based on the collaborative motion planning method of the unmanned material gripper of Embodiment 1.

[0174] In this embodiment, the planning system of the collaborative motion planning method of the unmanned material grabber includes an information input module, a path planning module and a path output module, wherein the information input module is used to define the path planning task, and determine a collision-free path in the joint configuration space based on the initial joint configuration and target configuration of the material grabber, as well as the workspace and environmental obstacles;

[0175] The path planning module is used to establish an optimal path search scheme with the goal of shortest path length, maximum distance between path nodes and obstacles, and minimum change in joint configuration of adjacent path nodes under collision constraints, joint constraints and non-holonomic constraints; more specifically, the path planning module uses an improved rapidly expanding random tree algorithm with target bias to generate the target path, wherein the path planning module focuses on the moving area of ​​the chassis of the material grabber, and then improves the random number growth method to obtain the target path;

[0176] The path output module smoothes the target path given by the path planning module, including the optimization of the gripper arm angle and redundant node processing, and then the path output module outputs the final unmanned gripper motion path.

[0177] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A collaborative motion planning method for an unmanned material grabber, characterized in that: It includes the following steps: S1. Define the path planning task, determine the collision-free path in the joint configuration space based on the initial joint configuration and target configuration of the gripper, as well as the workspace and environmental obstacles; S2. Under collision constraints, joint constraints and non-holonomic constraints, establish an optimal path search scheme with the goal of shortest path length, maximum distance between path nodes and obstacles, and minimum change in joint configuration of adjacent path nodes; S3, using an improved fast-expanding random tree algorithm with target bias to generate a target path, wherein the moving area of ​​the gripper chassis is first focused, and then the random number growth method is improved to obtain the target path; S4. Smoothing the obtained target path, including optimizing the angle of the gripper arm and processing redundant nodes, to obtain the final motion path of the unmanned gripper.

2. The method for collaborative motion planning of an unmanned material grabber according to claim 1, characterized in that: In step S1, the motion path is planned using the joint space of the gripper as the working space, where: The working space of the gripper robot arm is represented by X∈R 3 , the obstacles in the workspace are represented by x obs ∈X; the joint configuration of the mobile robot is q=[q b ,q m ] T =[x,y,φ,θ1,θ2,θ3] T , where q b = {x, y, Φ} represents the position of the mobile chassis, q m ={θ1,θ2,θ3} represents the joint configuration of the robot arm, and each joint configuration corresponds to a position of the end effector of the gripper robot arm; For each gripper robot joint configuration q including chassis and robot arm movement, its geometric configuration is represented by a set of links M(q)∈X; the gripper robot joint configuration space C contains all its feasible joint configurations. obs Represents the joint configuration that collides with the obstacle, C free represents the set of collision-free joint configurations in the configuration space, with C free ={q∈C|C obs }; According to the forward kinematic transformation of the gripper robot arm, its joint configuration is converted into a Cartesian space state; Given the initial joint configuration q start and target configuration q goal , as well as the workspace X and environmental obstacles X obs , the goal is to find a collision-free path in the joint configuration space where q0 = q start ,q l =q goal , l is the number of nodes in the planned path.

3. The method for collaborative motion planning of an unmanned material grabber according to claim 2, characterized in that: In step S1, the task execution stage of the gripper is determined according to whether the distance between the gripper and the grasping target exceeds the threshold distance. When the distance between the gripper and the grasping target exceeds the threshold distance, the gripper executes the first task, that is, only the gripper chassis moves and the robotic arm remains stationary; when the distance between the gripper and the grasping target is less than or equal to the threshold distance, the gripper executes the second task, that is, the gripper robotic arm and chassis move in coordination and grasp the target.

4. The method for collaborative motion planning of an unmanned material grabber according to claim 3, characterized in that: In step S2, the shortest path length means that the path with the shortest distance from the grabber to the target point is preferentially selected within the constraint range; the maximum distance between the path node and the obstacle means that the path with the largest distance Δd between each node in the path and the obstacle is preferentially selected within the constraint range; the minimum change in the joint configuration of adjacent path nodes means that the path with the minimum change in the joint configuration between adjacent path points is preferentially selected within the constraint range. Among them, assuming that the joint configurations of two adjacent path points are q1 and q2 respectively, the joint configuration changes between adjacent path points are: Δq=||q2-q1|| where ||·|| is the Frobenius norm; The obstacle avoidance path planning of the grabber manipulator is regarded as the process of finding the extreme value of the function under the known function constraints: the constraints are to avoid collision with obstacles in the environment, joint limit constraints and system non-holonomic constraints, and the objective function is the cost of path planning; then its mathematical model is expressed as: min f(q),q∈C free ,g i (q)≤b i ,i=1,2...,P Where f(q) represents the objective function of the planned path, g i (q) represents the constraint function, b i represents constraints, and p represents the number of constraints.

5. The method for collaborative motion planning of an unmanned material grabber according to claim 4, characterized in that: In step S2, in the process of establishing collision constraints, the irregular obstacles are simplified into sphere or cube models, the chassis of the material grabber is simplified into a cylindrical model, and the robotic arm is simplified into a series of sphere models with a radius of R; for the mobile chassis part of the material grabber, a collision detection method based on the cylindrical envelope is adopted, and the collision detection strategy based on the envelope sphere is adopted for the robotic arm part and the obstacle; The safe distance between the robot arm and the obstacle is d safe , the gripper arm is in joint configuration q l When the distance from the center of the sphere to the obstacle is: in Represents the position of the obstacle in the world coordinate system. Indicates the center position of the envelope sphere in the world coordinate system; For each node in the planned path, the distance d from the center of the envelope sphere to the obstacle must be j (q l ) is greater than the sum of the envelope sphere radius and the set safety distance; the relationship between the center position of the envelope sphere and its coordinate system is expressed by the following formula: in is the transformation from the world coordinate system to the coordinate system {i}, represents the position of the sphere center in the coordinate system {i}; Joint constraints refer to the constraints caused by the joint limit of the grabber arm itself, including the joint limit of the arm joint and the space limit of the environment where the grabber chassis is located. When the grabber arm moves in a given environment, the chassis movement range is limited to a given workspace, with the following restrictions in the x and y directions: L min ≤[x l ,y l ] T ≤L max Where L nin and L max They represent the two-dimensional boundaries of the chassis workspace respectively. The heading angle range of the chassis of the gripper is set between [0,2π]. The rotation range of each joint of the robotic arm meets the following restrictions: i i·min ≤θ i ≤θ i·max where θ i · min and θ i · max They represent the range of joint angles of the robot arm joint i respectively; according to the single joint limit of the gripper robot arm, its overall configuration joint limit is obtained as q∈[q min ,q max ]; The non-holonomic constraint means that the chassis of the material grabber adopts a differential drive wheel structure and can only move in the forward direction and rotate. The change ΔΦ between the chassis heading angles Φ1 and Φ2 of two consecutive path points of the chassis of the material grabber is defined as: Δφ=|φ2-φ1| Among them, the change of ΔΦ between two consecutive path points is used as an indicator of path evaluation.

6. The method for collaborative motion planning of an unmanned material grabber according to claim 4, characterized in that: In step S3, the target path is generated by using an improved rapidly expanding random tree algorithm with target bias, and the process is as follows: First, use the initial node q start As the root node of the random tree, initialize the random tree T; The initialized random tree contains only the initial node, and the random tree is continuously expanded through iteration until the target node q is reached. goal ; During the random tree growth process, a sampling point q is randomly selected in the sampling space according to the ChooseSampling() function. rand , and then use the NearestNeighbor() nearest node function to find the distance q in the search tree rand The node closest to the node q nearest Finally, the Extend() function is used to add a new node q to the random tree. new ; In the process of randomly selecting sampling points, the target preference feature is added to the search of the robot arm's partial configuration. The expression of the modified ChooseSampling() sampling function to generate random nodes is: In the formula, q b = {x, y, Φ} represents the position of the mobile chassis, q m ={θ1,θ2,θ3} represents the joint configuration of the robot arm; By continuously iterating and expanding the random number, the new node q expanded within the allowed time new The distance q from the target node goal When the distance is less than the given DISTANCE_THRESHOLD value, a random tree and path are returned, and the planning is successful.

7. The method for collaborative motion planning of an unmanned material grabber according to claim 6, characterized in that: In step S3, during the expansion process, the chassis moving area is first focused according to the initial position and target position of the chassis of the material grabber, and the moving focus area of ​​the chassis of the material grabber is determined by generating several disks from the initial position to the target position in the chassis moving plane, specifically: The initial position of the grabber chassis is contained in the first disk, and the target position is contained in the last disk. The radius r of each disk is the difference between the distance from the center of the grabber chassis to the nearest obstacle and the radius of the inner circle of the grabber chassis. The obstacle is projected on a two-dimensional plane to calculate the distance from the mobile chassis to the nearest obstacle. The radius of the inner circle of the grabber chassis is the radius of the circle that encloses the entire grabber chassis. The center of the first disc is the starting position of the gripper chassis (x s ,y s ); After determining the radius of the disk, k sampling points are randomly selected on the circumference of the disk; Take the sampling point closest to the target position among the k sampling points as the center to create the next disk and determine the radius of the disk; This process is repeated until the target position (x g ,y g ) appears on the disk.

8. The method for collaborative motion planning of an unmanned material grabber according to claim 7, characterized in that: In step S3, the random tree growing method is further improved: Assume that the previous path point moving chassis pose is q1.q b =(x1,y1,Φ1), the gripper chassis posture of the next path point corresponding to this node is q2.q b =(x2,y2,Φ2), the position and posture changes between the two nodes satisfy the following relationship: Where d is the change in chassis position between two adjacent path points, and ΔΦ is the change in chassis heading angle between two adjacent path points; When planning the path, the metric function of the nearest node and the random tree growth method are improved: In selecting a random sampling node q rand The nearest node q nearest When the adjacent nodes of the grabber chassis configuration are first determined: compare the position of the grabber chassis (x r ,y r ) and the node pose Φ r , when the postures are similar, the node is used as a neighboring node, that is: Randomly sample the node position (x r ,y r ) in q near ·q b The coordinate position is the vertex, the radius is r, and the heading angle is the center angle of the 2α fan-shaped area, that is, the position change of the chassis between two adjacent nodes should be within the fan-shaped range; β = |φ r -φ n |<γ means the change β of the heading angle between two adjacent nodes should be limited to the range of γ, where α, β, γ and r are preset parameters; Grab machine robot arm node configuration q rand ·q m The nearest node is determined by weighting the configuration changes of each joint of the grabber robot arm: First, determine the node that satisfies the chassis neighboring node constraint and the distance q rand ·q b The nearest K nodes, then select q from among the K nodes near ·q m and q rand ·q m The closest node between them, finally getting the node closest to the randomly sampled node; In generating random sampling nodes q rand And find its nearest node q nearest After that, the expansion function adds the new node q new Added to the random tree, the chassis configuration of the expansion node is q new ·q b =(x new ,y new ,z new ), the mobile chassis is at the node position (x n ,y n ) rotates to point to (x r ,y r ) and moves along the line connecting the two points for a distance d before reaching position (x new ,y new ), the heading angle of the extended node is: f new =λ(φ r -f n )+φ n in The robotic arm from q nearesr ·q m To q rand ·q m Expand and insert new nodes, and check whether there is any collision when connecting the new node configuration to obtain the configuration q of the robot arm new ·q m ; Set the new configuration node q new Add to the random tree and update the random tree until the target path is generated.

9. The method for collaborative motion planning of an unmanned material grabber according to claim 6, characterized in that: In step S4, during the path smoothing process, a rotation constraint is added to the expanded new node: The rotation constraint α is a vector and vector The angle is calculated as follows: Set the gripper arm rotation angle constraint threshold When the rotation angle α is less than or equal to the rotation constraint When the newly added node satisfies the rotation angle constraint, the node is added to the random tree; when the rotation angle α exceeds the rotation angle constraint When , the node is discarded and re-sampled; Then remove redundant nodes from the path: The line of sight algorithm is used to smooth the path, that is, to determine whether there will be obstacles between two nodes. If there is no collision, the two nodes are connected. If a collision occurs, the two nodes are not connected and the subsequent nodes are optimized. The specific execution steps are as follows: First, use node q start For node q t Initialize and then follow the generated path to the next node q i Search and detect the node and the initial node q of the smoothing process t Whether there are obstacles on the connection line or whether there is a situation beyond the environmental boundary; If there is no collision, continue to the next node q i+1 Conduct a search; If a collision or out-of-bounds occurs, node q t and the previous node q of this node i-1 Connect to generate a new path, remove redundant nodes in the path, and finally use node q i-1 Q t Make a replacement and continue searching along the path; Each time a collision occurs, t Update, repeat the search and replace process until the target node q is found goal .

10. A collaborative motion planning system for an unmanned material grabber, characterized in that: It includes an information input module, a path planning module and a path output module, wherein: The information input module is used to define the path planning task, and determine the collision-free path in the joint configuration space based on the initial joint configuration and target configuration of the gripper, as well as the workspace and environmental obstacles; The path planning module is used to execute the collaborative motion planning method of the unmanned material grabber as described in any of the aforementioned claims 1-9, at least including establishing an optimal path search scheme with the goal of shortest path length, maximum distance between path nodes and obstacles, and minimum change in joint configuration of adjacent path nodes under collision constraints, joint constraints and incomplete constraints; the path planning module uses an improved rapidly expanding random tree algorithm with target bias to generate a target path, wherein the path planning module focuses on the moving area of ​​the chassis of the material grabber, and then improves the random number growth method to obtain the target path; The path output module smoothes the target path given by the path planning module, including the optimization of the gripper arm angle and redundant node processing, and then the path output module outputs the final unmanned gripper motion path.

Images