Search

CN-121989235-A - Mechanical arm motion planning method and system

CN121989235ACN 121989235 ACN121989235 ACN 121989235ACN-121989235-A

Abstract

The invention discloses a mechanical arm motion planning method and system, the method comprises the steps of obtaining a target and obstacle point cloud through a 3D camera and registering to obtain pose information, carrying out URDF modeling and MoveIt configuration on a mechanical arm, improving an RRT-connect algorithm, introducing a dynamic target bias, directional heuristic sampling and elastic band smoothing algorithm mechanism, improving planning efficiency and path smoothness, carrying out inverse kinematics solving on path points to obtain a joint track, carrying out three-time or five-time polynomial interpolation according to the complexity of an interpolation section by adopting an adaptive polynomial interpolation optimization track, and finally converting the track into a control instruction through a Modbus TCP protocol to drive the mechanical arm to safely move to a target position. The invention obviously improves the planning speed, track quality and motion stability of the mechanical arm in a complex environment.

Inventors

  • CHEN JING
  • YU KAI
  • YANG HAO

Assignees

  • 南京邮电大学

Dates

Publication Date
20260508
Application Date
20260123

Claims (7)

  1. 1. The mechanical arm motion planning method is characterized by comprising the following steps of: Step S1, processing and registering three-dimensional point clouds acquired by a 3D camera to obtain the pose of a target workpiece and an obstacle; Step S2, modeling the used mechanical arm URDF, and configuring a mechanical arm model through MoveIt; S3, performing path planning in an obstacle scene by utilizing an improved RRT-connection algorithm; S4, performing inverse kinematics solution on the track points of the mechanical arm by using a formula method to obtain the motion track of the mechanical arm in the joint space; S5, performing polynomial interpolation optimization on the joint track of the mechanical arm by utilizing an improved polynomial interpolation algorithm so as to improve the smoothness of the motion track and the service life of the mechanical arm; and S6, converting the final track into a standard instruction format, and performing track control on the physical mechanical arm through a Modbus TCP communication protocol to enable the physical mechanical arm to avoid obstacles, move to the target workpiece position and wait for grabbing.
  2. 2. The method for planning motion of a mechanical arm according to claim 1, wherein the step S3 specifically includes: s3.1, issuing the pose of the obstacle through the topic, and representing the obstacle through the common sphere, cuboid and cylinder enclosure, so as to provide reference for obstacle avoidance path planning; S3.2, setting the tail end of the current mechanical arm as a starting planning point P start , setting the position of a target workpiece as a target planning point P goal , and starting to expand a starting random tree and a target random tree by taking the target workpiece as root nodes respectively; S3.3, applying dynamic target bias, updating target bias probability according to barrier density near an expansion point, directly taking a target point as a direction to expand by using the bias probability alpha when starting tree expansion, otherwise, randomly expanding nodes in the direction; Step S3.4, applying directionality heuristic, calculating an included angle theta between a vector P nearest P rand and a vector P nearest P goal , comparing the included angle theta with a threshold angle maxangle, and judging whether to expand to a sampling point, wherein P nearest is the nearest point in the neighborhood, and P rand is a random sampling point; Step S3.5, judging whether the new expansion node P new collides with the obstacle, if so, judging that the new expansion node P new is invalid, and not expanding the iteration at the time, if not, setting P nearest as a P new father node; s3.6, the target tree uses the bias probability alpha to expand P new as an expansion direction; Step S3.7, after the two trees meet, backtracking to the root node according to the father node to obtain a complete path; Step S3.8, carrying out elastic band smoothing on the path obtained by planning, updating a node P i new according to an elastic force item F elastic,i and a contraction force F contract,i , wherein a related formula is as follows (wherein mu is a smoothness control parameter and xi is a shortening strength control parameter): F elastic,i ＝μ(P i-1 +P i+1- 2P i ) P i new ＝P i +F elastic +F contract 。
  3. 3. the method according to claim 2, wherein the step S3.3 specifically comprises: S3.3.1 setting a range of obstacle density measurement, namely setting a circle with a radius of 1.5lambda (lambda is an expansion step length) by taking P new as a center in a two-dimensional environment, and setting a sphere with a radius of 1.5lambda by taking P new as a center in a three-dimensional environment; S3.3.2 calculating barrier density ρ, dividing the perception range into 16 directions in a two-dimensional environment, carrying out barrier detection on P new in 16 directions, setting that N directions have barriers, and dividing the perception range into 5 planes in a three-dimensional scene, wherein each plane is 16 directions, adding the directions right above and right below, carrying out barrier detection on P new in 82 directions, and setting that the barrier density ρ=N/82 S3.3.3 updating the bias probability alpha according to the obstacle density, wherein alpha is 1 when the obstacle density is 0 and approaches 0 when the density is more than or equal to 0.5, so the calculation formula is as follows: α=e -ρΦ Where Φ is a bias coefficient, and represents the influence of the barrier density on the bias probability.
  4. 4. The method according to claim 2, wherein the step S3.4 specifically comprises: S3.4.1 calculating the included angle theta between the vector P nearest P rand and the vector P nearest P goal , wherein the calculation formula is as follows: S3.4.2, selecting a judgment threshold angle, and calculating the preferable range of maxangle to be 60-120 degrees by considering the minimum safe angle and the end angular velocity constraint, so that maxangle =90 degrees are adopted by the algorithm; S3.4.3 if θ is greater than maxangle, the point is regarded as an invalid point, sampling selection is carried out again, if θ is smaller than or equal to P new is expanded in the direction, and if resampling is carried out for more than 10 times, random expansion is adopted as an alternative strategy.
  5. 5. The method for planning motion of a mechanical arm according to claim 1, wherein the step S5 specifically includes: s5.1, reading an original mechanical arm joint track file, wherein the track file comprises a time stamp and data of each joint angle, and storing the time and the joint angle of each track point into a data structure; s5.2, carrying out sectional analysis on the original joint track, and calculating a complexity evaluation index of each track section based on the time length of each section, the joint angle change rate and the comparison characteristics of adjacent sections; step S5.3, adaptively selecting an interpolation polynomial order according to the complexity evaluation index, interpolating a track segment with low complexity by adopting a cubic polynomial, and interpolating a track segment with high complexity by adopting a penta polynomial; s5.4, respectively solving polynomial coefficients of track segments of each joint, wherein a third-order polynomial meets the continuous conditions of position and speed, and a fifth-order polynomial meets the continuous conditions of position, speed and acceleration; s5.5, generating interpolation points in each section of track according to a set time step based on the solved polynomial coefficients, and ensuring smooth transition of each joint movement track; and S5.6, storing the optimized smooth joint track to a file for the mechanical arm control system to execute.
  6. 6. The method for planning motion of a mechanical arm according to claim 1, wherein the step S6 specifically includes: s6.1, packaging an optimized joint track in an instruction format, and converting the time, joint angle and joint speed of each track point into an instruction data frame which can be identified by a mechanical arm controller, wherein the instruction data frame comprises a joint position control instruction, a speed feedforward instruction and a timestamp synchronous identification; s6.2, establishing Modbus TCP communication connection between the upper computer and the mechanical arm controller, configuring a slave station address, a function code and a register mapping table, mapping joint instruction data according to register address segments, and ensuring real-time and reliable transmission of the instruction data; and S6.3, transmitting joint control instructions one by one according to a track time sequence, reading actual position feedback of a mechanical arm joint in each control period, and finally enabling the end effector to be aligned with the workpiece grabbing pose to finish motion control.
  7. 7. A robotic arm motion planning system comprising the following modules: The vision processing module is used for obtaining point clouds of the obstacle and the target workpiece by using scanning equipment, and preprocessing and registering to obtain pose information of the target and the obstacle; The mechanical arm motion planning module performs obstacle avoidance path planning in an obstacle scene by utilizing an improved RRT-connect algorithm, performs inverse kinematics solution on the obtained tail end track to obtain a joint space track, and performs interpolation optimization on the planning track by utilizing an improved polynomial interpolation algorithm; And the communication control module is used for converting the final track into a standard instruction format, and carrying out track control on the physical mechanical arm through a Modbus TCP communication protocol so as to enable the physical mechanical arm to avoid obstacles, move to the target workpiece position and wait for grabbing.

Description

Mechanical arm motion planning method and system Technical Field The invention relates to the technical field of robot motion planning, in particular to a mechanical arm motion planning method and system based on an improved RRT-connect algorithm. Background The mechanical arm is used as a core execution unit in an industrial automation process and is widely applied to complex production scenes such as assembly, carrying, welding, sorting, spraying and the like. The motion planning system needs to generate an efficient, smooth and safe motion track in real time in a dynamic and multi-obstacle working environment so as to finish an accurate operation task. The traditional path planning algorithm based on graph search, such as an A-algorithm, a Dijkstra algorithm and the like, is excellent in low-dimensional grid map, but in a high-dimensional joint space of a mechanical arm, the calculation complexity of the path planning algorithm is exponentially increased, the severe requirement of industrial site on real-time performance is difficult to meet, and the motion constraint in a continuous configuration space cannot be directly processed. To address the problem of planning in high dimensional space, random sampling class algorithms have been developed, represented by random fast expansion trees (Rapidly-exploring Random Tree, RRT) and variants thereof. The algorithm explores the configuration space in a random sampling mode, and avoids complete modeling and searching in a high-dimensional space, so that the algorithm has good high-dimensional adaptability and calculation feasibility. The RRT-connect is used as an important improvement algorithm of the RRT, and a bidirectional expansion strategy is adopted, so that the method has higher searching efficiency compared with the traditional RRT. However, classical RRT-like algorithms (including RRT-connect) have inherent limitations, mainly in that, first, the random sampling mechanism causes the search process to lack target guidance, and second, the quality of the generated path is to be improved. In particular, in the aspect of track optimization, the conventional polynomial interpolation method generally adopts a fixed order (such as three or five times) to uniformly process the whole track, and cannot perform self-adaptive adjustment according to the actual complexity of the track segment. For smooth motion segments, higher order polynomials add unnecessary computational burden, while for complex steering segments, lower order polynomials have difficulty guaranteeing acceleration continuity, which can lead to shock and vibration when the robotic arm is running. Therefore, in the application scenario of mechanical arm motion planning, a novel motion planning method which can integrate visual perception information, has strong target guidance, supports real-time obstacle avoidance planning, and can generate smooth and dynamic feasible tracks is needed, so as to improve the operation intelligence, reliability and overall system efficiency of the mechanical arm in a complex unstructured environment. Disclosure of Invention In order to solve the problems, the invention aims to provide a method and a system for planning the movement of a mechanical arm, which realize real-time, efficient and safe obstacle avoidance grabbing of the mechanical arm. A mechanical arm movement planning method comprises the following steps: Step S1, processing and registering three-dimensional point clouds acquired by a 3D camera to obtain the pose of a target workpiece and an obstacle; Step S2, modeling the used mechanical arm URDF, and configuring a mechanical arm model through MoveIt; S3, performing path planning in an obstacle scene by utilizing an improved RRT-connection algorithm; S4, performing inverse kinematics solution on the track points of the mechanical arm by using a formula method to obtain the motion track of the mechanical arm in the joint space; S5, performing polynomial interpolation optimization on the joint track of the mechanical arm by utilizing an improved polynomial interpolation algorithm so as to improve the smoothness of the motion track and the service life of the mechanical arm; and S6, converting the final track into a standard instruction format, and performing track control on the physical mechanical arm through a Modbus TCP communication protocol to enable the physical mechanical arm to avoid obstacles, move to the target workpiece position and wait for grabbing. Further, the step S3 specifically includes: s3.1, issuing the pose of the obstacle through the topic, and representing the obstacle through the common sphere, cuboid and cylinder enclosure, so as to provide reference for obstacle avoidance path planning; S3.2, setting the tail end of the current mechanical arm as a starting planning point P star t, setting the position of a target workpiece as a target planning point P goal, and starting to expand a starting random tree and a target random tree b