Path planning method for cooperative control of crown block moving mechanical arm

Abstract

The invention discloses a path planning method for cooperative control of a crown block and a movable mechanical arm, and relates to the field of path planning of the movable mechanical arm, and the method comprises the steps: obtaining the position information of the movable mechanical arm and the crown block based on a factory two-dimensional plane graph, and combining the position information with a path planning algorithm to generate a feasible path, calculating the path length of the feasible path; taking the path length of the feasible path as a long axis to establish a hyper-ellipse intelligent constraint sampling condition, and obtaining sampling points based on the hyper-ellipse intelligent constraint sampling condition to generate an ellipse constraint; combining the elliptical constraint with the bidirectional growth direct connection strategy to generate a direct connection path, calculating the path length of the direct connection path, and obtaining an optimal planning path according to the path length of the direct connection path and the optimization strategy; and a driving instruction is generated based on the optimal planning path, and the moving path of the moving mechanical arm and the crown block is controlled. The situation that the moving speed of the crown block exceeds the following capacity of the mechanical arm due to insufficient path planning speed can be avoided.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot arm path planning, and in particular to a path planning method for coordinated control of a mobile robot arm of an overhead crane. Background Art

[0002] In recent years, the research on path planning has continuously introduced new theories and technologies, which is particularly evident in the RRT series of path planning algorithms. In order to improve the performance of the RRT series of algorithms, the existing technology has optimized the performance of the path planning algorithm in terms of path length, time cost, search efficiency and robustness by introducing new theories or new technologies.

[0003] Some optimize the path search strategy. For example, a three-source rapidly expanding random tree GT-RRT algorithm combined with a gravitational field is proposed in the prior art. The gravitational field is used to guide the generation of new nodes, which greatly reduces the search path length. Therefore, it is believed that the research on the robustness improvement of probability-based algorithms for path planning is still relatively rare. In current research, path planning mainly focuses on the optimization of path length, time cost, and search efficiency, as well as the fusion of different algorithms according to specific needs. This method can better adapt to the needs of different scenarios and problems and improve the efficiency and accuracy of path planning.

[0004] For example, in the field of path planning technology for overhead crane mobile robotic arms, path planning is widely used. However, in the prior art, the path search efficiency of the overhead crane mobile robotic arms under complex road conditions is low, resulting in the inability of the overhead crane and the robotic arm to effectively coordinate, that is, the low efficiency of path planning causes the overhead crane to move before the mobile robotic arm.

[0005] Currently, no effective solution has been proposed for the problems in the related technologies. Summary of the invention

[0006] 1. Technical issues to be resolved

[0007] In view of the deficiencies in the prior art, the present invention provides a path planning method for the collaborative control of an overhead crane and a mobile robotic arm, which is capable of collaborative operation between the robotic arm and the overhead crane during the transportation of heavy objects, thereby avoiding the situation where the crane's movement speed exceeds the robotic arm's following capability due to insufficient path planning speed, thereby solving the problem of low path planning efficiency causing the overhead crane to move before the mobile robotic arm.

[0008] (II) Technical solution

[0009] In order to achieve the above purpose, the specific technical solution adopted by the present invention is as follows:

[0010] A path planning method for coordinated control of a mobile robot arm of an overhead crane, the path planning method comprising:

[0011] S1. Obtain the position information of the mobile robot arm and the overhead crane based on the two-dimensional plan of the factory, combine the position information with the path planning algorithm to generate a feasible path, and calculate the path length of the feasible path;

[0012] S2. Establish a super ellipse intelligent constraint sampling condition with the path length of the feasible path as the major axis, and obtain sampling points based on the super ellipse intelligent constraint sampling condition to generate an ellipse constraint;

[0013] S3, combining the ellipse constraint with the bidirectional growth direct connection strategy to generate a direct connection path, and calculating the path length of the direct connection path, and obtaining the optimal planning path according to the path length of the direct connection path and the optimization strategy;

[0014] S4. Generate driving instructions based on the optimal planned path to control the moving paths of the mobile robot arm and the overhead crane, and introduce a dynamic environment monitoring mechanism to monitor and adjust the moving status of the mobile robot arm and the overhead crane in real time.

[0015] Preferably, obtaining the position information of the mobile robot arm and the overhead crane based on the two-dimensional plan view of the factory, combining the position information with the path planning algorithm to generate a feasible path, and calculating the path length of the feasible path includes:

[0016] S11, extracting a two-dimensional plan view of the factory, obtaining the initial position and target position of the mobile robot arm and the overhead crane, and the position information of obstacles inside the factory;

[0017] S12, setting the initial position of the mobile robot arm as the starting position, the target position as the end position, and using the path planning algorithm to generate an initialization search tree, defining the starting position as the root point of the search tree;

[0018] S13, generating a random sampling point according to the starting position and the end position, searching for a node whose distance from the random sampling point is a threshold value in the search tree, and extending a preset length in the direction from the node to the random sampling point to generate a new node;

[0019] S14, judging whether there is a collision between the new node and the path between the nodes according to the obstacle position information, if a collision occurs, the path is discarded, if no collision occurs, the new node is added to the search tree, and the random sampling point is connected to the new node;

[0020] S15. Determine the distance relationship between the new node and the end point, generate a feasible path based on the distance result, and record the path length of the feasible path.

[0021] Preferably, establishing a super ellipse intelligent constraint sampling condition with the path length of the feasible path as the major axis, and obtaining sampling points based on the super ellipse intelligent constraint sampling condition to generate an ellipse constraint includes:

[0022] S21, defining a superellipse based on superellipse intelligent constraint sampling, performing random sampling in the unit circle according to the sampling rules to obtain random points, and converting the random points into the superellipse;

[0023] S22, determining the center point of the superellipse according to the starting position and the end position of the mobile mechanical arm on the horizontal axis and the vertical axis, and defining the radius information of the superellipse by using the path length of the feasible path and the straight-line distance from the starting position to the end position of the mobile mechanical arm, and determining the position of the superellipse based on the radius information and the center;

[0024] S23. Determine super ellipse constraint sampling parameters using the super ellipse position, and determine sampling points within the super ellipse according to the super ellipse constraint sampling parameters.

[0025] Preferably, the ellipse constraint is combined with the bidirectional growth direct connection strategy to generate a direct connection path, and the path length of the direct connection path is calculated. The optimal planning path is obtained according to the path length of the direct connection path and the optimization strategy, including:

[0026] S31, constructing a forward growth tree and a reverse growth tree respectively with the starting position and the end position of the mobile robot arm as the root points, and generating forward random sampling points and reverse random sampling points by using the forward growth tree and the reverse growth tree based on the ellipse constraint;

[0027] S32, searching for a forward node and a reverse node in the forward growth tree and the reverse growth tree respectively, the distance between the forward random sampling point and the reverse random sampling point being a threshold value;

[0028] S33, extending a preset length from the forward node to the forward random sampling point, and extending a preset length from the reverse node to the reverse random sampling point, and generating a forward new node and a reverse new node according to the extension result;

[0029] S34, generating a direct path based on the forward new node and the reverse new node, calculating the path length of the direct path after determining the existence of obstacles, and combining the optimization strategy to obtain the optimal planning path.

[0030] Preferably, generating a direct path based on the forward new node and the reverse new node, calculating the path length of the direct path after determining the existence of obstacles, and obtaining the optimal planning path in combination with the optimization strategy includes:

[0031] S341, connecting the forward new node and the forward random sampling point to obtain a forward search path, and connecting the reverse new node and the reverse random sampling point to obtain a reverse search path, and respectively determining the existence of obstacles on the forward search path and the reverse search path;

[0032] S342, if there is an obstacle, the corresponding path is abandoned; if there is no obstacle, the forward new node and the reverse new node are added to the forward growth tree and the reverse growth tree respectively, and the forward new node and the reverse new node are connected to obtain a direct path;

[0033] S343, respectively calculating the path lengths of the forward search path, the reverse search path and the direct path, and determining the traversal optimization parameters based on the optimization strategy, and obtaining the optimal planning path according to the traversal optimization parameters and the path length.

[0034] Preferably, the path length calculation formula of the forward search path is:

[0035]

[0036] The path length calculation formula for the reverse search path is:

[0037]

[0038] The formula for calculating the path length of a direct path is:

[0039] L z =d(P znew ,P fnew );

[0040] Where L1 represents the path length of the forward search path, L2 represents the path length of the reverse search path, and L z represents the path length of the direct path, n represents the number of dimensions, d represents the Euclidean distance, and P zi represents the forward node, P zi+1 represents the i+1th forward node, P fi Represents the reverse node, P fi+1 represents the i+1th reverse node, P znew Indicates the new node, P fnew Represents a new reverse node, and i represents the number of nodes.

[0041] Preferably, generating a driving instruction based on the optimal planned path, controlling the moving paths of the mobile manipulator and the overhead travelling crane, and introducing a dynamic environment monitoring mechanism to monitor and adjust the moving states of the mobile manipulator and the overhead travelling crane in real time include:

[0042] S41, generating a driving instruction based on the optimal planned path, and sending the driving instruction to the mobile robot arm driving controller, so that the mobile robot arm and the overhead travelling crane move according to the optimal planned path;

[0043] S42, using sensors and radars in the factory to monitor the movement process of the mobile robot arm and the environmental information and obstacle information in the factory, and transmit the monitoring results to the central processor;

[0044] S43, the central processing unit receives the monitoring result to identify the abnormal state, generates a decision instruction to control the mobile robot arm according to the abnormal state, and regenerates the planned path to control the moving state of the mobile robot arm.

[0045] Preferably, generating a driving instruction based on the optimal planned path, and sending the driving instruction to the mobile mechanical arm driving controller, and the mobile mechanical arm and the overhead travelling crane moving according to the optimal planned path includes:

[0046] S411, generating a path point based on the optimal planned path, analyzing the joint angles of each joint of the mobile robot arm when executing the path point, and determining the target speed according to the joint angles and the initial joint angles of the mobile robot arm;

[0047] S412, combining the joint angle and the target speed as a driving instruction for the mobile robotic arm, and sending the driving instruction to the mobile robotic arm driving controller using the communication interface;

[0048] S413: The mobile robot arm driving controller receives the driving instruction to control the movement of the mobile robot arm, and the overhead crane follows the movement of the mobile robot arm during the movement.

[0049] Preferably, generating a path point based on the optimal planned path, analyzing the joint angles of each joint of the mobile robot arm when executing the path point, and determining the target speed according to the joint angles and the initial joint angles of the mobile robot arm include:

[0050] S4111, generating a grid map based on the two-dimensional plan of the factory, and generating path Voronoi points on the grid map according to the optimal planning path and the Voronoi diagram generation rule;

[0051] S4112, traversing the Voronoi points of the path to select intersection points, and connecting adjacent intersections, extracting guide points based on the relationship between the connecting lines of the intersection points and obstacles, and using the guide points as path points;

[0052] S4113. Establish a finite element model of the mobile robot arm using computer simulation technology, and simulate the target joint angles of the mobile robot arm when it moves along the path points according to inverse kinematics in the finite element model;

[0053] S4114. Obtain an initial joint angle of the mobile robotic arm, and determine a joint angular velocity of the mobile robotic arm based on the initial joint angle and the target joint angle.

[0054] Preferably, the calculation formula of the joint angular velocity of the moving manipulator is:

[0055] T=k p (T target -T cur );

[0056] Where T represents the joint angular velocity of the mobile robot, k p represents the proportional gain, T target represents the target joint angle, T cur Represents the initial joint angle.

[0057] (III) Beneficial effects

[0058] Compared with the prior art, the present invention provides a path planning method for coordinated control of a mobile robot arm of an overhead crane, which has the following beneficial effects:

[0059] (1) The present invention optimizes the path planning algorithm of the mobile robot arm to achieve efficient path search, which can effectively improve the ability of the overhead crane to follow the mobile robot arm, ensure the coordinated operation between the robot arm and the overhead crane during the transportation of heavy objects, and avoid the situation where the movement speed of the overhead crane exceeds the following ability of the robot arm due to insufficient path planning speed, thereby reducing the transportation time and improving the overall operation efficiency.

[0060] (2) The path planning method for the coordinated control of the overhead crane and the mobile robot arm proposed in the present invention obtains a feasible path at the fastest speed by searching for the low time cost of the first feasible path, and calculates its path length as the undetermined variable of the super ellipse constraint, thereby quickly establishing the sampling constraint, which can optimize the tracking ability of the mobile robot arm and solve the problem of coordinated operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0062] Figure 1 is a flow chart of a path planning method for coordinated control of a mobile mechanical arm of an overhead traveling crane according to an embodiment of the present invention;

[0063] Figure 2 is a bidirectional growth defect diagram according to an embodiment of the present invention;

[0064] Figure 3 is an elliptical constraint defect map according to an embodiment of the present invention;

[0065] Figure 4 is a flow chart of an algorithm according to an embodiment of the present invention;

[0066] Figure 5 is a reconnaissance map based on the IBTO-RRT algorithm in an embodiment of the present invention;

[0067] Figure 6 is a bidirectional search graph of the IBTO-RRT algorithm according to an embodiment of the present invention;

[0068] Figure 7 is a bidirectional direct connection diagram of the IBTO-RRT algorithm according to an embodiment of the present invention;

[0069] Figure 8 is a sampling constraint graph of the IBTO-RRT algorithm according to an embodiment of the present invention;

[0070] Fig. 9 is a simulation diagram of path planning in a simple RRT environment according to an embodiment of the present invention;

[0071] Fig.10 is a simulation diagram of path planning in a simple RRT* environment according to an embodiment of the present invention;

[0072] Fig.11 is a path planning simulation diagram in a simple environment of Informed-RRT* according to an embodiment of the present invention;

[0073] Fig.12 is a simulation diagram of path planning in a simple environment of BITO-RRT* according to an embodiment of the present invention;

[0074] Fig.13 is a simulation diagram of path planning in a simple IBTO-RRT* environment according to an embodiment of the present invention;

[0075] Fig.14 is a simulation diagram of path planning in a complex RRT environment according to an embodiment of the present invention;

[0076] Fig.15 is a simulation diagram of path planning in a complex environment of RRT* according to an embodiment of the present invention;

[0077] Fig.16 is a simulation diagram of path planning in a complex environment of Informed-RRT* according to an embodiment of the present invention;

[0078] Fig.17 is a simulation diagram of path planning in a complex environment of BITO-RRT* according to an embodiment of the present invention;

[0079] Fig.18 is a simulation diagram of path planning in a complex environment of IBTO-RRT* according to an embodiment of the present invention;

[0080] Fig.19 is a comparison diagram of path lengths according to an embodiment of the present invention;

[0081] Fig. 20 is a time cost comparison diagram according to an embodiment of the present invention;

[0082] Fig.21 4 is a comprehensive performance comparison chart according to an embodiment of the present invention. DETAILED DESCRIPTION

[0083] To further illustrate each embodiment, the present invention provides drawings, which are part of the disclosure of the present invention and are mainly used to illustrate the embodiments. They can be used in conjunction with the relevant descriptions in the specification to explain the operating principles of the embodiments. By referring to these contents, ordinary technicians in this field should be able to understand other possible implementation methods and advantages of the present invention.

[0084] According to an embodiment of the present invention, a path planning method for coordinated control of an overhead travelling crane and a mobile mechanical arm is provided.

[0085] The present invention will now be further described with reference to the accompanying drawings and specific embodiments. Figure 1 As shown, according to an embodiment of the present invention, a path planning method for coordinated control of a mobile mechanical arm of an overhead traveling vehicle comprises:

[0086] S1. Based on the two-dimensional plan of the factory, the position information of the mobile robot arm and the overhead crane is obtained, the position information is combined with the path planning algorithm to generate a feasible path, and the path length of the feasible path is calculated.

[0087] In this embodiment, the position information of the mobile robot arm and the overhead crane is obtained based on the two-dimensional plan view of the factory, the position information is combined with the path planning algorithm to generate a feasible path, and the path length of the feasible path is calculated, including:

[0088] S11, extracting a two-dimensional plan view of the factory, obtaining the initial position and target position of the mobile robot arm and the overhead crane, and the position information of obstacles inside the factory;

[0089] S12, setting the initial position of the mobile robot arm as the starting position, the target position as the end position, and using the path planning algorithm to generate an initialization search tree, defining the starting position as the root point of the search tree;

[0090] S13, generating a random sampling point according to the starting position and the end position, searching for a node whose distance from the random sampling point is a threshold value in the search tree, and extending a preset length in the direction from the node to the random sampling point to generate a new node;

[0091] S14, judging whether there is a collision between the new node and the path between the nodes according to the obstacle position information, if a collision occurs, the path is discarded, if no collision occurs, the new node is added to the search tree, and the random sampling point is connected to the new node;

[0092] S15. Determine the distance relationship between the new node and the end point, generate a feasible path based on the distance result, and record the path length of the feasible path.

[0093] S2. Establish a super ellipse intelligent constraint sampling condition with the path length of the feasible path as the major axis, and obtain sampling points based on the super ellipse intelligent constraint sampling condition to generate an ellipse constraint.

[0094] In this embodiment, the path length of the feasible path is used as the major axis to establish a super ellipse intelligent constraint sampling condition, and the sampling points are obtained based on the super ellipse intelligent constraint sampling condition to generate an ellipse constraint, which includes:

[0095] S21, defining a superellipse based on superellipse intelligent constraint sampling, performing random sampling in the unit circle according to the sampling rules to obtain random points, and converting the random points into the superellipse;

[0096] S22, determining the center point of the superellipse according to the starting position and the end position of the mobile mechanical arm on the horizontal axis and the vertical axis, and defining the radius information of the superellipse by using the path length of the feasible path and the straight-line distance from the starting position to the end position of the mobile mechanical arm, and determining the position of the superellipse based on the radius information and the center;

[0097] S23. Determine super ellipse constraint sampling parameters using the super ellipse position, and determine sampling points within the super ellipse according to the super ellipse constraint sampling parameters.

[0098] S3. Combining the ellipse constraint with the bidirectional growth direct connection strategy to generate a direct connection path, and calculating the path length of the direct connection path, and obtaining the optimal planning path according to the path length of the direct connection path and the optimization strategy.

[0099] In this embodiment, the ellipse constraint is combined with the bidirectional growth direct connection strategy to generate a direct connection path, and the path length of the direct connection path is calculated. The optimal planning path is obtained according to the path length of the direct connection path and the optimization strategy, including:

[0100] S31, constructing a forward growth tree and a reverse growth tree with the starting position and the end position of the mobile robot arm as the root points respectively, and generating forward random sampling points and reverse random sampling points by using the forward growth tree and the reverse growth tree based on the ellipse constraint;

[0101] S32, searching for a forward node and a reverse node in the forward growth tree and the reverse growth tree respectively, the distance between the forward random sampling point and the reverse random sampling point being a threshold value;

[0102] S33, extending a preset length from the forward node to the forward random sampling point, and extending a preset length from the reverse node to the reverse random sampling point, and generating a forward new node and a reverse new node according to the extension result;

[0103] S34, generating a direct path based on the forward new node and the reverse new node, calculating the path length of the direct path after determining the existence of obstacles, and combining the optimization strategy to obtain the optimal planning path.

[0104] Specifically, a direct path is generated based on the forward new node and the reverse new node, the path length of the direct path is calculated after determining the existence of obstacles, and the optimal planning path is obtained by combining the optimization strategy, including:

[0105] S341, connecting the forward new node and the forward random sampling point to obtain a forward search path, and connecting the reverse new node and the reverse random sampling point to obtain a reverse search path, and respectively determining the existence of obstacles on the forward search path and the reverse search path;

[0106] S342, if there is an obstacle, the corresponding path is abandoned; if there is no obstacle, the forward new node and the reverse new node are added to the forward growth tree and the reverse growth tree respectively, and the forward new node and the reverse new node are connected to obtain a direct path;

[0107] S343, respectively calculating the path lengths of the forward search path, the reverse search path and the direct path, and determining the traversal optimization parameters based on the optimization strategy, and obtaining the optimal planning path according to the traversal optimization parameters and the path length.

[0108] S4. Generate driving instructions based on the optimal planned path to control the moving paths of the mobile robot arm and the overhead crane, and introduce a dynamic environment monitoring mechanism to monitor and adjust the moving status of the mobile robot arm and the overhead crane in real time.

[0109] In this embodiment, the driving instructions are generated based on the optimal planning path to control the moving paths of the mobile robot arm and the overhead crane, and a dynamic environment monitoring mechanism is introduced to monitor and adjust the moving states of the mobile robot arm and the overhead crane in real time, including:

[0110] S41, generating a driving instruction based on the optimal planned path, and sending the driving instruction to the mobile robot arm driving controller, so that the mobile robot arm and the overhead travelling crane move according to the optimal planned path;

[0111] S42, using sensors and radars in the factory to monitor the movement process of the mobile robot arm and the environmental information and obstacle information in the factory, and transmit the monitoring results to the central processor;

[0112] S43, the central processing unit receives the monitoring result to identify the abnormal state, generates a decision instruction to control the mobile robot arm according to the abnormal state, and regenerates the planned path to control the moving state of the mobile robot arm.

[0113] Specifically, generating a driving instruction based on the optimal planned path, and sending the driving instruction to the mobile robot arm driving controller, and the mobile robot arm and the overhead crane moving according to the optimal planned path include:

[0114] S411, generating a path point based on the optimal planned path, analyzing the joint angles of each joint of the mobile robot arm when executing the path point, and determining the target speed according to the joint angles and the initial joint angles of the mobile robot arm;

[0115] S412, combining the joint angle and the target speed as a driving instruction for the mobile robotic arm, and sending the driving instruction to the mobile robotic arm driving controller using the communication interface;

[0116] S413: The mobile robot arm driving controller receives the driving instruction to control the movement of the mobile robot arm, and the overhead crane follows the movement of the mobile robot arm during the movement.

[0117] Among them, generating a path point based on the optimal planning path, analyzing the joint angles of each joint of the mobile robot when executing the path point, and determining the target speed according to the joint angle and the initial joint angle of the mobile robot include:

[0118] S4111, generating a grid map based on the two-dimensional plan of the factory, and generating path Voronoi points on the grid map according to the optimal planning path and the Voronoi diagram generation rule;

[0119] S4112, traversing the Voronoi points of the path to select intersection points, and connecting adjacent intersections, extracting guide points based on the relationship between the connecting lines of the intersection points and obstacles, and using the guide points as path points;

[0120] S4113. Establish a finite element model of the mobile robot arm using computer simulation technology, and simulate the target joint angles of the mobile robot arm when it moves along the path points according to inverse kinematics in the finite element model;

[0121] S4114. Obtain an initial joint angle of the mobile robotic arm, and determine a joint angular velocity of the mobile robotic arm based on the initial joint angle and the target joint angle.

[0122] Specifically, the calculation formula for the joint angular velocity of the moving robot arm is:

[0123] T=k p (T target -T cur );

[0124] Where T represents the joint angular velocity of the mobile robot, k p represents the proportional gain, T target represents the target joint angle, T cur Represents the initial joint angle.

[0125] It should be explained that the BITO-RRT* proposed in this embodiment speeds up the search speed by using a bidirectional growth strategy. Although it reduces the search time, it increases the cost of finding the first feasible path. Figure 2 As shown in , Informed-RRT* relies on existing feasible paths to establish superellipse constraints, significantly reducing invalid sampling points, thereby speeding up the search speed. However, the search speed of the first feasible path in a complex environment is similar to that of RRT, which weakens its advantage and affects the robustness of the time cost, as shown in Figure 3 As shown in the figure, overall, the robustness of Informed-RRT* in path length has been improved, while the robustness of time cost has been reduced. The robustness of BITO-RRT* in time cost has been improved, but the robustness of path length still needs to be improved. Therefore, the ideal new algorithm should optimize the search time and path length at the same time and ensure high robustness to meet the needs of different scenarios and reduce dependence on specific algorithms.

[0126] This embodiment is mainly based on the BITO-RRT* algorithm and the Informed-RRT* algorithm. It points out that the former has few valid nodes and low search efficiency due to the randomness of sampling, while the latter is difficult to establish super ellipse constraints in complex situations. Therefore, the sampling constraint method of the Informed-RRT* algorithm in the embodiment of this article is used to improve the sampling efficiency of the BITO-RRT algorithm. At the same time, the BITO-RRT* algorithm is used to solve the problem of high time cost of the Informed-RRT* algorithm. In addition, the RRT algorithm is used to first search and establish ellipse constraints to cope with the difficulty of establishing sampling constraints in bidirectional search. Finally, an improved algorithm IBTO-RRT* based on ellipse constraints and bidirectional search is proposed to solve the coordination problem of the overhead crane and the mobile robot arm.

[0127] Based on the traditional algorithm, this embodiment proposes the IBTO-RRT* algorithm by taking advantage of its strengths and making up for its weaknesses. First, the RRT algorithm has the characteristic of short search time to first find a non-optimal path to lay the foundation for constrained sampling, that is, the first detection method. Then, the bidirectional growth strategy of direct connection detection is adopted to accelerate the convergence speed. At the same time, the sampling constraint strategy of super ellipse constraint sampling is introduced to reduce the acquisition of invalid points. Then, the direct connection path is optimized by traversing the optimization steps. Finally, the nodes are reselected in the asymptotic convergence area to shorten the path length. The advantage of the IBTO-RRT algorithm is that it performs very well in all aspects, and the restrictions imposed by demand are greatly reduced. While optimizing the time cost and path search, the robustness of the path planning algorithm is increased. Its design flow chart is as follows Figure 4 shown.

[0128] The IBTO-RRT algorithm mainly covers the first reconnaissance strategy, super ellipse sampling constraints, bidirectional search, bidirectional direct connection and traversal optimization. First, in the RRT first reconnaissance strategy, the RRT algorithm is used to determine a non-optimal path length and assign it to the major axis of the super ellipse, thereby accelerating the formation of sampling constraints, reducing random sampling time, and reducing the time cost of the algorithm. Figure 5 As shown; then introduce bidirectional search, generate forward tree and reverse tree from the starting point and end point respectively, and use the information of both sides to search simultaneously to improve efficiency, especially in complex search space, such as Figure 6 As shown in the figure, the bidirectional direct connection strategy generates a direct path between the starting point and the end point immediately when the direct connection is detected, which significantly reduces the computational cost. If the direct connection fails, an intermediate node is generated to try again. Figure 7 As shown; the superellipse sampling constraint limits the sampling space by dynamically adjusting the major axis (current path length) and the minor axis (based on the straight-line distance between the starting point and the target point), reducing invalid sampling points and accelerating path search, as shown Figure 8 As shown; finally, in the traversal optimization step, the total length of the forward, reverse and direct paths is calculated, and the updated path length is recorded until the iteration is completed and the optimal path length is finally obtained. By combining these methods, the IBTO-RRT algorithm can efficiently perform path planning.

[0129] In order to facilitate understanding of the above technical solutions of the present invention, the working principle or operation mode of the present invention in the actual process is described in detail below.

[0130] Step 1: Extract the two-dimensional plan of the factory and obtain the initial position, target position and obstacle position information of the mobile robot and overhead crane.

[0131] Step 2: Use the RRT algorithm (path planning algorithm) to quickly find a non-optimal feasible path and calculate the path length to speed up the construction of sampling constraints, namely the first detection method, specifically:

[0132] (1) Determine the starting position and the end position, take the initial position of the mobile robot as the starting position, and the target position as the end position, initialize an empty search tree with the starting position of the mobile robot as the root point, and generate a random sampling point q in the two-dimensional plane where the starting position and the end position of the mobile robot exist rand .

[0133] (2) Find the value corresponding to q in the search tree rand The nearest node q neares , from q nearest To q rand The direction is extended by 1 meter using the Euclidean distance to obtain the new node q new , the Euclidean distance calculation formula is as follows:

[0134]

[0135] In the formula, x and y represent the coordinates of two points, n represents the number of dimensions, which is 1 here, and the detection starts from q neares to q new Whether the path collides with the obstacle, if a collision occurs, the extension is discarded, if the path does not collide, the new state q new Add to the tree and connect q neares and q new .

[0136] (3) Each time a new node is added, check whether the new node is close to the target position. If so, try to connect the new node to the target position and perform collision detection. Otherwise, repeat step (2). If the connection to the target position is successful and there is no collision on the path, build a path from the starting position to the end position and record the path length. At this time, the mobile robot arm is still at the starting position.

[0137] Step 3: Introduce super ellipse intelligent constraint sampling. After constructing the sampling constraint based on the path length obtained by the first reconnaissance strategy as the super ellipse construction condition, the sampling area from the initial position of the mobile manipulator to the target position is limited to reduce the acquisition of invalid sampling points, thereby improving the sampling quality. Specifically:

[0138] (1) Define the superellipse constraint. The superellipse is a generalized form of the ellipse, and its equation can be expressed as:

[0139]

[0140] Where a and b are the radii of the superellipse in the x-axis and y-axis directions respectively, and n is a positive integer used to control the shape of the superellipse. When n=2, the above formula represents a standard ellipse equation.

[0141] (2) Random sampling is performed within the unit circle, and the coordinates of the sampling points (u, v) satisfy: u 2 +v 2 Less than or equal to 1, and u and v are randomly drawn from a uniform distribution, and the random points in the unit circle are converted to the superellipse. The transformation steps are as follows: x = a*u 1 / n , y=b*v 1 / n , (u, v) after transformation to (x, y) will be located on the super ellipse defined by the parameters a, b and n.

[0142] (3) Determine the superellipse constraint sampling parameters and superellipse position: the major axis 2a takes the length of the non-optimal feasible path detected by the first detection strategy, and the minor axis 2b takes the straight-line distance C from the starting position to the end position of the mobile robot arm. min, the center point of the ellipse (x0, y0) is the starting position (x start ,y start ) and the end position (x goal ,y goal ), the relevant data calculation formula is as follows:

[0143]

[0144] C best =C pioner ;

[0145]

[0146]

[0147]

[0148]

[0149]

[0150] In the formula, C pioner represents the length of the first detected path, C best Indicates the current optimal path length, according to C best The sampling area needs to be transformed to perform uniform sampling within the ellipse. First, sampling is performed within a unit circle, and then these points are mapped to the actual ellipse area. Specifically, the application of the x rotation angle and the rotation matrix is ​​involved. The rotation angle θ is:

[0151]

[0152] The rotation matrix R(θ) is:

[0153]

[0154] The final sampling point (x final ,y final )for:

[0155]

[0156] Where (x, y) is the point after the superellipse transformation. Repeat (2) to (3) until the sampling is completed.

[0157] It needs to be explained that the super-ellipse intelligent constraint sampling is mainly divided into two parts. One part is to first reconnaissance and search for a feasible path, and the other part is ellipse constraint. The super-ellipse constraint is used to continuously reduce the sampling area to improve the sampling quality. The length of the feasible path currently searched is used as a variable. Here is the path obtained by the first reconnaissance strategy. Through a series of mathematical changes, an ellipse domain is obtained. As the length of the search path shortens, the ellipse domain will become smaller and smaller. Ideally, when the path length tends to the straight-line distance between the starting point and the end point, the ellipse domain tends to a line segment from the starting point to the end point.

[0158] Step 4: Use bidirectional growth and bidirectional direct connection strategies to speed up path convergence. The specific steps are as follows:

[0159] (1) An empty search tree is constructed with the starting position of the mobile robot as the root point as the forward growth tree, and an empty search tree is constructed with the end position of the mobile robot as the root point as the reverse growth tree. The two trees generate a random sampling point q in the two-dimensional plane where the starting position and the end position of the mobile robot exist. zrand ,q frand .

[0160] (2) Find the same tree as q in the two search trees. zrand ,q frand The nearest node q znearest With q fnearest , from q znearest To q zrand and q fnearest To q frand The directions are extended by 1 meter using the Euclidean distance to obtain new nodes q znew With q fnew , the Euclidean distance calculation formula is as follows:

[0161]

[0162] Among them, x z and z are the coordinates of two points in the forward search tree, x f and f is the coordinates of two points in the reverse search tree, n is the number of dimensions, here n = 1, the detection from q znearest to q znew and q fnearest to q fnew Whether the path collides with the obstacle, if a collision occurs, the extension is discarded, if the path does not collide, the new state q znew and q fnew Add to the tree and connect q znearest and q znew and q fnearest and qfnew .

[0163] (3) Detect the new forward sampling point q znew and the reverse new sampling point q fnew Is there an obstacle between them? If there is no obstacle, a feasible path is generated and the path length is calculated. If there is an obstacle, (1) to (3) are repeated, and the path length is calculated using the Euclidean distance formula as follows:

[0164] The formula for calculating the path length of the forward search path is:

[0165]

[0166] The path length calculation formula for the reverse search path is:

[0167]

[0168] The formula for calculating the path length of a direct path is:

[0169]

[0170] Where L1 represents the path length of the forward search path, L2 represents the path length of the reverse search path, and L z Indicates the path length of the direct path, L b represents the total length of the search path, n represents the number of dimensions, d represents the Euclidean distance, P zi represents the forward node, P zi+1 represents the i+1th forward node, P fi Represents the reverse node, P fi+1 represents the i+1th reverse node, P znew Indicates the new node, P fnew Represents a new reverse node, and i represents the number of nodes.

[0171] It should be explained that the scope of path search is reduced to a certain extent by utilizing bidirectional growth and bidirectional direct connection strategies. The bidirectional growth strategy usually constructs two exploration and expansion trees, one that grows from the starting point, namely the forward exploration and expansion tree, and the other that grows from the end point, namely the reverse exploration and expansion tree. The two trees expand toward each other at the same time. In this process, the newly generated random sampling points of the two trees are directly connected. If there is no obstacle between the two sampling points, a feasible path is generated and the length of the path is calculated. Otherwise, the search continues. Specifically, the path search is divided into three segments, namely the forward growth path, the reverse growth path and the direct connection path. The final path length is the sum of the three segments.

[0172] Step 5: Optimize the existing path through traversal optimization strategy to achieve the path length tending to the most solvable, specifically:

[0173] Determine the traversal optimization parameters, L y is the length of the path currently being searched, L yLast is the path length of the last search. The parameters are compared. When L y <L yLast When y ; When L y >L yLast When , the optimal path is still L yLast , and finally achieve the optimal path. Repeat the selection until the maximum number of iterations is reached and then output the optimal path. Use traversal optimization to continuously find a better path. By shortening the path length, the sampling range is reduced and the sampling efficiency is improved.

[0174] At the same time, the searched optimal path data is transmitted to the mobile robotic arm, the mobile robotic arm moves according to the searched optimal path, and the overhead crane follows the mobile robotic arm to move. By reducing the time cost required for the path planning of the mobile robotic arm, the coordinated control of the overhead crane and the mobile robotic arm is realized.

[0175] Step 6: Introduce dynamic environment monitoring and real-time adjustment mechanism, specifically:

[0176] (1) While the mobile robot arm is moving along the optimal path, it monitors environmental changes in real time. Various types of sensors are deployed in the factory environment, and lidar and camera sensors are used for real-time monitoring to achieve real-time monitoring of obstacles and changes in the environment. These sensors continuously collect environmental data, including information such as the location, shape, and movement speed of obstacles, and transmit the data to the central processor through the wireless network. The central processor processes the data in real time and updates the environmental model to ensure safety and efficiency.

[0177] (2) When a new obstacle is detected or an existing obstacle is removed, the current path execution is immediately suspended. The central processor analyzes the sensor data in real time to determine whether a new obstacle has appeared or an existing obstacle has been removed. Once an environmental change is detected, a decision is made immediately to suspend the current path execution and send a pause command to the mobile robot arm through the wireless communication mechanism to ensure the safety and accuracy of the operation.

[0178] (3) Re-run the path planning algorithm to generate a new obstacle avoidance path and update the optimal path data. After receiving the pause command, the CPU restarts the IBTO-RRT path planning algorithm, taking into account the latest environmental information, including newly appeared or removed obstacles. Based on the updated environmental data, the system generates a new obstacle avoidance path and optimizes it to ensure that the path is the optimal solution. The new optimal path data is stored for subsequent use.

[0179] (4) The new optimal path data is transmitted to the mobile robot arm to ensure that it can still complete the task efficiently in a dynamic environment. The new optimal path data is transmitted to the mobile robot arm via wireless communication. After receiving the data, the robot arm starts to move along the new path. At the same time, the coordinated control between the overhead crane and the mobile robot arm is ensured so that the two can move synchronously. A feedback mechanism is established to monitor the execution of the mobile robot arm in real time to ensure that it moves smoothly on the new path.

[0180] By deploying sensors such as lidar and cameras in the factory environment, dynamic environmental monitoring and real-time data transmission are achieved. The central processor can intelligently analyze sensor data and quickly respond to environmental changes, suspend the current path and re-plan the optimal obstacle avoidance path. The new path data is transmitted to the mobile robotic arm through wireless communication to ensure its efficient task execution. At the same time, it cooperates with the overhead crane to establish a feedback mechanism to monitor the execution status, thereby achieving safe and flexible operation.

[0181] Based on this embodiment, simulation experiments are carried out on various path planning algorithms. Figures 9 to 13 As shown in the figure, black solid circles of different sizes represent obstacles. The starting point is (0, 0) and the end point is (15, 12). The simulation data is shown in Table 1. Then, the simulation was performed again in a complex path planning environment. The environment is shown in Table 1. Figures 14 to 18 As shown, Fig.14 It shows the non-optimal path of the RRT algorithm. Fig.15 and Fig.16 The experimental results of RRT* and Informed-RRT* algorithms are shown in Figure 2. Both algorithms have the advantage of asymptotic optimization. The Informed-RRT* algorithm further optimizes the path by establishing a superellipse sampling constraint. Fig.17 It is shown that the BITO-RRT* algorithm also exhibits asymptotic optimality; Fig.18 The IBTO-RRT* algorithm proposed in this embodiment first finds a feasible path through RRT, then establishes elliptical constraints, and performs bidirectional growth and direct connection detection. It shows the ability to quickly establish super-elliptical sampling constraints and reduce invalid sampling points, thereby reducing the time cost. In 30 simulation experiments, the five algorithms of RRT, RRT*, Informed-RRT*, BITO-RRT* and IBTO-RRT* were compared in terms of path length, time cost and comprehensive performance.

[0182] Qualitative results analysis: RRT algorithm performs poorly in path length, with an average length significantly higher than other algorithms and poor convergence, while RRT* and BITO-RRT* have higher path lengths, but their convergence has improved. Informed-RRT* shows poor convergence in complex environments, while IBTO-RRT algorithm is significantly better than the other three algorithms in path length. In terms of time cost, RRT algorithm has a greater advantage but is prone to outliers. IBTO-RRT algorithm has lower time average and median than Informed-RRT* and BITO-RRT*, and slightly lower than RRT. Overall, BITO-RRT* and IBTO-RRT algorithms have high success rates. IBTO-RRT* shows certain advantages in average path length, path length standard deviation, average time and its standard deviation, especially in length standard deviation.

[0183] like Figure 19 to Figure 20 It can be seen that based on the qualitative results, the path length of the RRT algorithm is relatively scattered, and the average length is significantly higher than that of other algorithms; although the partial path lengths of RRT* and BITO-RRT* are higher than the average, their convergence has improved; Informed-RRT* shows poor convergence in complex environments, while the IBTO-RRT algorithm is significantly better than the other three algorithms in path length. Fig.19 The results of time cost are shown. The RRT algorithm has a greater advantage in time cost but is prone to outliers. The IBTO-RRT* algorithm is lower than Informed-RRT* and BITO-RRT* in time average and median, and slightly lower than RRT*. Fig. 20 The overall performance is summarized. The BITO-RRT* and IBTO-RRT* algorithms have high success rates. IBTO-RRT* has certain advantages in average path length, path length standard deviation, average time and its standard deviation. Especially in terms of length standard deviation, the path length standard deviations of the five algorithms are 4.084, 2.233, 0.615, 0.185 and 0.041, respectively, which can significantly reflect the advantages of IBTO-RRT*. The specific data are shown in Table 2.

[0184] Table 1 Simulation data in simple case

[0185]

[0186] Table 2 Simulation data table in complex cases

[0187]

[0188]

[0189] To sum up, with the help of the above-mentioned technical scheme of the present invention, the present invention optimizes the path planning algorithm of the mobile robotic arm to achieve efficient path search, which can effectively improve the following ability of the overhead crane for the mobile robotic arm, ensuring the coordinated operation between the robotic arm and the overhead crane during the transportation of heavy objects, and avoiding the situation where the movement speed of the overhead crane exceeds the following ability of the robotic arm due to insufficient path planning speed, thereby reducing transportation time and improving overall operation efficiency.

[0190] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A path planning method for coordinated control of a mobile robot arm of an overhead crane, characterized in that: The path planning method includes: S1. Obtain the position information of the mobile robot arm and the overhead crane based on the two-dimensional plan of the factory, combine the position information with the path planning algorithm to generate a feasible path, and calculate the path length of the feasible path; S2. Establish a super ellipse intelligent constraint sampling condition with the path length of the feasible path as the major axis, and obtain sampling points based on the super ellipse intelligent constraint sampling condition to generate an ellipse constraint; S3, combining the ellipse constraint with the bidirectional growth direct connection strategy to generate a direct connection path, and calculating the path length of the direct connection path, and obtaining the optimal planning path according to the path length of the direct connection path and the optimization strategy; S4. Generate driving instructions based on the optimal planned path to control the moving paths of the mobile robot arm and the overhead crane, and introduce a dynamic environment monitoring mechanism to monitor and adjust the moving status of the mobile robot arm and the overhead crane in real time.

2. A path planning method for coordinated control of a mobile robot arm of an overhead travelling crane according to claim 1, characterized in that: The method of obtaining the position information of the mobile robot arm and the overhead crane based on the two-dimensional plan of the factory, combining the position information with the path planning algorithm to generate a feasible path, and calculating the path length of the feasible path includes: S11, extracting a two-dimensional plan view of the factory, obtaining the initial position and target position of the mobile robot arm and the overhead crane, and the position information of obstacles inside the factory; S12, setting the initial position of the mobile robot arm as the starting position, the target position as the end position, and using the path planning algorithm to generate an initialization search tree, defining the starting position as the root point of the search tree; S13, generating a random sampling point according to the starting position and the end position, searching for a node whose distance from the random sampling point is a threshold value in the search tree, and extending a preset length in the direction from the node to the random sampling point to generate a new node; S14, judging whether there is a collision between the new node and the path between the nodes according to the obstacle position information, if a collision occurs, the path is discarded, if no collision occurs, the new node is added to the search tree, and the random sampling point is connected to the new node; S15. Determine the distance relationship between the new node and the end point, generate a feasible path based on the distance result, and record the path length of the feasible path.

3. A path planning method for coordinated control of a mobile robot arm of an overhead traveling crane according to claim 1, characterized in that: The method of establishing a super ellipse intelligent constraint sampling condition with the path length of the feasible path as the major axis, and obtaining sampling points based on the super ellipse intelligent constraint sampling condition to generate an ellipse constraint includes: S21, defining a superellipse based on superellipse intelligent constraint sampling, performing random sampling in the unit circle according to the sampling rules to obtain random points, and converting the random points into the superellipse; S22, determining the center point of the superellipse according to the starting position and the end position of the mobile mechanical arm on the horizontal axis and the vertical axis, and defining the radius information of the superellipse by using the path length of the feasible path and the straight-line distance from the starting position to the end position of the mobile mechanical arm, and determining the position of the superellipse based on the radius information and the center; S23. Determine super ellipse constraint sampling parameters using the super ellipse position, and determine sampling points within the super ellipse according to the super ellipse constraint sampling parameters.

4. A path planning method for coordinated control of a mobile robot arm of an overhead traveling crane according to claim 1, characterized in that: The step of combining the ellipse constraint with the bidirectional growth direct connection strategy to generate a direct connection path, calculating the path length of the direct connection path, and obtaining the optimal planning path according to the path length of the direct connection path and the optimization strategy includes: S31, constructing a forward growth tree and a reverse growth tree with the starting position and the end position of the mobile robot arm as the root points respectively, and generating forward random sampling points and reverse random sampling points by using the forward growth tree and the reverse growth tree based on the ellipse constraint; S32, searching for a forward node and a reverse node in the forward growth tree and the reverse growth tree respectively, the distance between the forward random sampling point and the reverse random sampling point being a threshold value; S33, extending a preset length from the forward node to the forward random sampling point, and extending a preset length from the reverse node to the reverse random sampling point, and generating a forward new node and a reverse new node according to the extension result; S34, generating a direct path based on the forward new node and the reverse new node, calculating the path length of the direct path after determining the existence of obstacles, and combining the optimization strategy to obtain the optimal planning path.

5. A path planning method for coordinated control of a mobile robot arm of an overhead travelling crane according to claim 4, characterized in that: The generating of a direct path based on the forward new node and the reverse new node, calculating the path length of the direct path after determining the existence of obstacles, and obtaining the optimal planning path in combination with the optimization strategy includes: S341, connecting the forward new node and the forward random sampling point to obtain a forward search path, and connecting the reverse new node and the reverse random sampling point to obtain a reverse search path, and respectively determining the existence of obstacles on the forward search path and the reverse search path; S342, if there is an obstacle, the corresponding path is abandoned; if there is no obstacle, the forward new node and the reverse new node are added to the forward growth tree and the reverse growth tree respectively, and the forward new node and the reverse new node are connected to obtain a direct path; S343, respectively calculating the path lengths of the forward search path, the reverse search path and the direct path, and determining the traversal optimization parameters based on the optimization strategy, and obtaining the optimal planning path according to the traversal optimization parameters and the path length.

6. A path planning method for coordinated control of a mobile robot arm of an overhead travelling crane according to claim 5, characterized in that: The path length calculation formula of the forward search path is: The path length calculation formula of the reverse search path is: The calculation formula of the path length of the direct path is: L z =d(P znew ,P fnew ); Where L1 represents the path length of the forward search path, L2 represents the path length of the reverse search path, and L z represents the path length of the direct path, n represents the number of dimensions, d represents the Euclidean distance, and P zi represents the forward node, P zi+1 represents the i+1th forward node, P fi Represents the reverse node, P fi+1 represents the i+1th reverse node, P znew Indicates the new node, P fnew Represents a new reverse node, and i represents the number of nodes.

7. The path planning method for coordinated control of a mobile robot arm of an overhead traveling crane according to claim 1, characterized in that: The generating of driving instructions based on the optimal planning path, controlling the moving paths of the mobile robot arm and the overhead crane, and introducing a dynamic environment monitoring mechanism to monitor and adjust the moving states of the mobile robot arm and the overhead crane in real time include: S41, generating a driving instruction based on the optimal planned path, and sending the driving instruction to the mobile robot arm driving controller, so that the mobile robot arm and the overhead travelling crane move according to the optimal planned path; S42, using sensors and radars in the factory to monitor the movement process of the mobile robot arm and the environmental information and obstacle information in the factory, and transmit the monitoring results to the central processor; S43, the central processing unit receives the monitoring result to identify the abnormal state, generates a decision instruction to control the mobile robot arm according to the abnormal state, and regenerates the planned path to control the moving state of the mobile robot arm.

8. A path planning method for coordinated control of a mobile robot arm of an overhead travelling crane according to claim 7, characterized in that: The generating of the driving instruction based on the optimal planned path and sending the driving instruction to the mobile mechanical arm driving controller, wherein the mobile mechanical arm and the overhead travelling crane move according to the optimal planned path, comprises: S411, generating a path point based on the optimal planned path, analyzing the joint angles of each joint of the mobile robot arm when executing the path point, and determining the target speed according to the joint angles and the initial joint angles of the mobile robot arm; S412, combining the joint angle and the target speed as a driving instruction for the mobile robotic arm, and sending the driving instruction to the mobile robotic arm driving controller using the communication interface; S413: The mobile robot arm driving controller receives the driving instruction to control the movement of the mobile robot arm, and the overhead crane follows the movement of the mobile robot arm during the movement.

9. A path planning method for coordinated control of a mobile robot arm of an overhead travelling crane according to claim 8, characterized in that: The generating of the path points based on the optimal planned path, analyzing the joint angles of the joints of the mobile robot arm when executing the path points, and determining the target speed according to the joint angles and the initial joint angles of the mobile robot arm include: S4111, generating a grid map based on the two-dimensional plan of the factory, and generating path Voronoi points on the grid map according to the optimal planning path and the Voronoi diagram generation rule; S4112, traversing the Voronoi points of the path to select intersection points, and connecting adjacent intersections, extracting guide points based on the relationship between the connecting lines of the intersection points and obstacles, and using the guide points as path points; S4113. Establish a finite element model of the mobile robot arm using computer simulation technology, and simulate the target joint angles of the mobile robot arm when it moves along the path points according to inverse kinematics in the finite element model; S4114. Obtain an initial joint angle of the mobile robotic arm, and determine a joint angular velocity of the mobile robotic arm based on the initial joint angle and the target joint angle.

10. A path planning method for coordinated control of a mobile robot arm of an overhead travelling crane according to claim 9, characterized in that: The calculation formula of the joint angular velocity of the mobile robot arm is: T=k p (T target -T cur ); Where T represents the joint angular velocity of the mobile robot, k p represents the proportional gain, T target represents the target joint angle, T cur Represents the initial joint angle.

Images