Obstacle avoidance planning and cross-platform control system for five-axis robot based on A* and APF algorithm

Abstract

The application discloses a five-axis mechanical arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms and relates to the technical field of mechanical arm motion control. In view of the problems that the existing A* algorithm is easy to generate a virtual path exceeding the limit, motion step is stuck, and the traditional potential field method does not cover the anti-collision of the connecting rod and is easy to fall into local optimization, the application builds an improved D-H kinematics model and a standardized simulation environment, proposes an improved A* global planning algorithm with joint physical constraints and quintic polynomial smoothing, and ensures the continuous and smooth trajectory. Meanwhile, the application proposes an improved APF local obstacle avoidance algorithm by introducing a velocity Jacobian matrix to map the repulsive force of the whole arm connecting rod. The system adopts cross-platform communication between the upper computer and the ROS2 bottom layer node, and the smooth trajectory is losslessly issued to the entity mechanical arm for execution. The application takes into account the path feasibility and safety, greatly reduces the hardware impact, and provides a quantitative algorithm comparison and verification mechanism.

Description

Technical Field

[0001] This invention relates to the technical field of robotic arm motion control, specifically to a five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms. Background Technology

[0002] With the popularization of industrial automation and robotics education, desktop five-DOF robotic arms, represented by Jet Arm, are widely used in university research, teaching experiments, and small-scale automated operation scenarios due to their flexible structure, moderate cost, and native support for ROS (Robot Operating System). Path planning, as the core of robotic arm motion control, directly determines the feasibility of robotic arm movement, work efficiency, and equipment safety.

[0003] Currently, the mainstream solutions for path planning of such five-degree-of-freedom robotic arms are the A* algorithm and the Artificial Potential Field (APF) method, which are also the existing technologies closest to this invention. However, in practical engineering implementation, these two technologies have the following core technical defects: Firstly, regarding the A* path planning algorithm: While the existing A* algorithm boasts strong global path search capabilities and good path optimality, when applied to finished robotic arms like Jet Arm, it often performs blind path searches in Cartesian space, failing to adequately match the physical rotation limits of the joints as specified by the robotic arm manufacturer. This easily leads to the algorithm generating false paths that exceed the joint motion limits and are physically impossible for the robotic arm to execute. Furthermore, the original paths generated in the mesh space correspond to a sawtooth-like stepped trajectory in the joint space. If this step displacement command is directly sent to the underlying layer, it will cause severe jerking and mechanical shocks in the robotic arm's actual movement, completely failing to meet the smooth motion requirements of physical servos, resulting in a serious disconnect between the algorithm simulation results and actual hardware control.

[0004] Secondly, regarding the artificial potential field method: While existing artificial potential field methods offer extremely fast local obstacle avoidance response and simple underlying control logic, their physical field construction often only sets up attraction and repulsion forces for the end effector of the robotic arm, completely failing to cover the motion state of the entire long linkage of the robotic arm, such as the upper arm and lower arm. This means that when the mechanism moves through narrow spaces, it is impossible to avoid the risk of collisions between the middle section of the linkage and environmental obstacles, posing a serious safety hazard. Furthermore, traditional potential field functions, limited by the characteristics of the physical field, are prone to falling into the trap of local optima in force equilibrium, causing the robotic arm to come to a complete standstill and fail to reach the target work point.

[0005] Finally, existing technologies generally lack a unified algorithm performance comparison and verification mechanism: due to the lack of a simulation test environment with standardized boundary conditions, researchers cannot quantitatively compare the core performance indicators of different path planning algorithms in the same scenario, such as total path length, search calculation efficiency, and motion smoothness. This is extremely detrimental to the accurate algorithm selection and engineering optimization of robotic arms in different dynamic or static operation scenarios.

[0006] In response to the numerous fatal flaws of the existing technologies, the industry urgently needs a five-degree-of-freedom robotic arm path planning and control scheme that can take into account the absolute feasibility of physical paths, the safety of full-arm motion, the smoothness of high-order motion, and has the ability to compare and verify standardized algorithms and seamlessly integrate software and hardware. Summary of the Invention

[0007] The present invention addresses the problem of overly simplistic solutions in existing technologies by providing a significantly different approach. It primarily offers a five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms, thereby resolving the technical problems mentioned in the background section.

[0008] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows: A five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms, the control method of the system includes the following steps: Step 1: Establish the kinematic model of the five-degree-of-freedom robotic arm: The five rotary joints of the five-degree-of-freedom robotic arm are modeled using the improved DH parameter method, and the forward and inverse kinematics solutions are completed. Step 2: Build a standardized path planning simulation and verification environment: Construct a 3D simulation model of the robotic arm and define spherical obstacles in Cartesian space, and set uniform collision safety distance and joint space discrete resolution boundary conditions; Step 3: Implement an improved A* path planning algorithm with physical constraints and path smoothing for the robotic arm: In the discretized joint space, invalid path points that exceed the calibrated rotation angle limit are eliminated. The global original path is generated by combining full-arm link-level collision detection and smoothing is performed using fifth-order polynomial interpolation to generate an executable joint angle sequence. Step 4: Implement the improved artificial potential field method for collision detection with linkages: Construct gravitational and repulsive fields in the joint space, map the Cartesian space repulsive force calculated based on the shortest distance between the entire arm linkage and the obstacle to the joint space through the Jacobian matrix, and configure a local optimal solution detection mechanism. Step 5, Algorithm Standardization, Comparison, Verification and Visualization: In the standardized environment, the core performance indicators of the improved A* algorithm and the improved artificial potential field method are quantitatively compared in terms of total path length, search time and number of expanded nodes, and the trajectory is verified in three dimensions through a multi-window independent visualization module; Step 6: Establish cross-platform communication and physical robotic arm control: The host computer node converts the planned joint angle path into a standard message format and sends it to the embedded master control node through the ROS2 communication domain. The underlying controller drives the physical robotic arm to perform precise movements and transmits motion status data back in a closed loop.

[0009] Preferably, the improved DH parameters used in step one include the link length. Linkage torsion angle Joint offset and joint angle Its inverse kinematics model employs the Newton-Raphson numerical method, with the solution accuracy error condition set as the end-effector pose error. .

[0010] Preferably, the cost function of the improved A* path planning algorithm in step three is: ;in, This represents the total change in joint angles from the starting point to the current node, calculated by summing the Euclidean norms of the joint angle vectors of adjacent nodes. Let be the Euclidean norm of the angle vector from the current node to the target node in the joint space.

[0011] Preferably, the full-arm link-level collision detection in step three specifically involves: at each step of the path search, calculating the spatial coordinates of each link corresponding to the current joint angle using a forward kinematics model, traversing all links and calculating the shortest distance between them and obstacles, and determining a collision if the distance is less than the collision safety distance and removing the path node.

[0012] Preferably, in step four, the gravitational term of the improved artificial potential field method is directly defined in joint space as... Its repulsion term formula is: ,in This is the velocity Jacobian matrix corresponding to the current joint angle of the robotic arm, used to implement the repulsive force vector in Cartesian space. Mapping of forces to joint space.

[0013] Preferably, the local optimal solution detection mechanism in step four is as follows: when the robotic arm iterates more than a set number of times and the change in joint angles in multiple consecutive steps is less than a set position change threshold, it is determined that it has fallen into a local minimum and an early warning is issued.

[0014] Preferably, in step six, the host computer establishes a publisher by configuring global variables and binding the subscription topic to the ROS2 driver. When the underlying driver is initialized, system commands are executed to release the hardware serial port and servo motor control permissions, and high-performance communication middleware is configured to reduce instruction transmission latency.

[0015] A five-axis robotic arm obstacle avoidance planning and cross-platform control system based on obstacle avoidance algorithms, comprising: The upper-level computer layer is equipped with scientific computing software and a robotics toolkit for simulation environment construction, algorithm planning, and visualization. The embedded main control layer includes an edge computing development board running the ROS2 operating system, which is used to receive instructions from the host computer and manage communication nodes. The underlying execution layer, which includes a microcontroller and the robotic arm body, is connected to the embedded main control layer via serial communication to directly drive the robotic arm joint bus servos and transmits joint angles and torques back at a frequency of 50Hz. The power supply layer adopts a dual-power independent isolated power supply scheme, which independently supplies power to the embedded main control layer and the underlying execution layer.

[0016] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. This invention uses an improved A* path planning algorithm with joint physical constraints and polynomial interpolation smoothing to generate a continuous and smooth trajectory that fully conforms to the joint motion constraints of a five-degree-of-freedom robotic arm. This solves the motion stuttering and mechanism jitter problems caused by direct issuance of traditional discrete paths. Actual measurements show that it reduces the joint jitter amplitude by 30% during robotic arm movement, significantly improves motion stability, and avoids the risk of triggering hardware joint limits.

[0017] 2. This invention introduces an artificial potential field method that maps the repulsive force of the entire link of the robotic arm to the velocity Jacobian matrix, thereby realizing real-time obstacle avoidance planning across the entire multi-link system of the robotic arm. This avoids the shortcomings of traditional methods that only avoid obstacles at the end and ignore collisions in the middle links, effectively adapts to complex working conditions with multiple obstacles, and significantly improves the safety redundancy and environmental adaptability of the robotic arm's motion operation.

[0018] 3. This invention achieves direct, unmodified porting of MATLAB simulation algorithms to physical robotic arm hardware by designing a lightweight ROS2 cross-platform relay node and establishing a bidirectional data communication link. This eliminates the barriers between simulation and physical implementation, eliminates the need to rewrite the underlying hardware control code, shortens the project development cycle by more than 60%, and achieves 100% reuse of the underlying code.

[0019] 4. By building a standardized simulation pre-verification environment with unified boundary conditions, this invention enables the quantitative verification and optimization of all algorithm parameters and trajectory safety before driving the physical robotic arm. This avoids the risks of physical link collisions and hardware damage during the traditional algorithm parameter tuning and trial-and-error process, achieving zero hardware loss and improving overall debugging efficiency by more than 70%.

[0020] 5. This invention achieves adaptation to different models of robotic arms by constructing a lightweight control architecture based on the dynamic mapping of improved DH kinematic parameters and communication topics. It eliminates the need for preliminary work such as secondary development of complex URDF models and configuration of kinematic plugins. Only the DH parameters of the corresponding robotic arm need to be modified to quickly migrate to other five-degree-of-freedom serial robotic arms, which greatly reduces the engineering implementation threshold and learning and development costs of the system.

[0021] 6. This invention achieves secondary scalability of the system by adopting a two-level control architecture that highly decouples the upper-level path calculation and the lower-level motion execution. It reserves standardized extension interfaces for multi-algorithm fusion and dynamic obstacle avoidance. Without reconstructing the core control physical link, it can be directly extended to complex operation scenarios such as dynamic obstacle avoidance and multi-machine cooperative motion control. Attached Figure Description

[0022] Figure 1 This is an overall flowchart of a five-degree-of-freedom robotic arm obstacle avoidance trajectory planning and cross-platform control method according to the present invention.

[0023] Figure 2 This is a block diagram of the hardware architecture and signal flow of the five-degree-of-freedom robotic arm path planning and control system of the present invention.

[0024] Figure 3 This is a block diagram illustrating the principle of the improved A* algorithm with joint physical constraints and path smoothing in this invention.

[0025] Figure 4 This is a schematic diagram of the improved DH parameter modeling and link coordinate system of the five-DOF robotic arm of this invention.

[0026] Figure 5 This is a comparison curve of joint trajectories before and after smoothing using the improved A* algorithm of this invention.

[0027] Figure 6 This is a schematic diagram comparing the principle of the improved artificial potential field method for collision detection with linkage in this invention.

[0028] Figure 7 This is a schematic diagram comparing the obstacle avoidance trajectory effects of different path planning algorithms of this invention.

[0029] Figure 8 This is a bar chart comparing the core performance indicators of different path planning algorithms of this invention.

[0030] Figure 9 This invention relates to the MATLAB-ROS2 cross-platform communication node architecture and message flow diagram.

[0031] Figure 10 This is a schematic diagram of the actual measured obstacle avoidance trajectory of the physical robotic arm of the present invention; Figure 11It is an official calibrated and improved DH core parameter and joint physical movement limit table; Figure 12 This is a quantitative comparison table of the core performance indicators of different path planning algorithms under a standardized environment.

[0032] Figure 13 This is a flowchart of the safety verification process for the hierarchical execution of physical robotic arms. Detailed Implementation

[0033] To facilitate understanding of the present invention, a more comprehensive description of the present invention will be given below with reference to the accompanying drawings, which illustrate several embodiments of the present invention. However, the present invention can be implemented in different forms and is not limited to the embodiments described in the text. Rather, these embodiments are provided to make the disclosure of the present invention more thorough and complete.

[0034] It should be noted that when an element is referred to as being "fixed to" another element, it can be directly on the other element or there may be an intermediate element present. When an element is referred to as being "connected to" another element, it can be directly connected to the other element or there may be an intermediate element present. The terms "vertical," "horizontal," "left," "right," and similar expressions used in this article are for illustrative purposes only.

[0035] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly associated with one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to limit the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items. Example

[0036] Technical limitations regarding the underlying physical hardware architecture and communication network system, such as Figure 2 As shown, the system adopts a two-level control decoupling architecture of upper-level path planning and lower-level motion execution. From the physical layer to the logic layer, it is specifically divided into the host computer layer, the embedded main control layer, the lower-level execution layer, and the dual-power isolated power supply layer.

[0037] At the host computer level, this invention uses a PC running Windows 11 and MATLAB R2023b software, which is equipped with the Robotics Toolbox as the algorithm calculation engine, virtual simulation sandbox and data visualization screen for the entire intelligent obstacle avoidance system.

[0038] In the embedded main control layer, such as Figure 2As shown, the core is the NVIDIA Jetson Orin Nano 8GB edge AI development board, which comes pre-installed with the Ubuntu 22.04 LTS operating system and the ROS2 Humble robot operating system native environment. This layer acts as a bridge connecting abstract algorithms and hardware electrical signals, responsible for receiving instructions from the host computer and managing the underlying communication links.

[0039] At the underlying execution layer, the core consists of the Jet Arm five-DOF serial teaching robotic arm and the STM32F407VET6 Mini controller. The execution end includes five sets of high-precision serial bus servos, achieving a single-joint angle control accuracy of [insert accuracy here]. It supports real-time status feedback. The specific hardware configuration is as follows: Joint 1 uses the HTS-35H model with a rated torque of 25 kg·cm; Joints 2, 3, and 4 use the HTD-35H model, also featuring high torque; Joint 5 uses the HX-12H model; and the end effector uses the HTS-21H model. In addition, the main control board is connected to an Orbbec Astra Mini monocular depth camera via a USB 3.0 high-speed interface for high-frequency acquisition of 3D point cloud data of obstacles in the workspace.

[0040] In the power supply layer, such as Figure 2 As shown, the system employs a dual-power independent isolated power supply scheme. A 19V / 2.37A DC adapter serves as an independent power source, supplying logic power to the Jetson Orin Nano main control board; while another high-power 12V / 5A DC adapter is dedicated to independently powering the STM32 controller and five heavy-duty bus servo systems. This hardware routing isolation design physically shields the transient voltage drops and reverse current surges generated by the servo mechanisms during emergency obstacle avoidance start / stop operations from interfering with the stable operation of the main control chip.

[0041] like Figure 9 As shown, the host PC and the main control board are connected to the same network segment via the local area network TCP / IP protocol. To ensure the real-time performance of high-frequency control, the ROS_DOMAIN_ID=5 identifier is uniformly set in the communication link, and rmw_fastrtps_cpp is configured as a high-performance data distribution service middleware.

[0042] The signal flow exhibits a closed-loop characteristic: the command issuance link originates from the native ROS2 joint angle command issuance node on the PC, and is transmitted to the main control board via the topic / jetarm / joint_trajectory or the message type sensor_msgs / msg / JointState. It is then transmitted to the STM32 controller via the serial port to drive the servo motor, with a baud rate of 115200. The status feedback link is transmitted back to the host computer via the underlying status node at an absolute hardware sampling frequency of 50Hz, transmitting real-time angle and torque to the topic / jetarm / joint_states for monitoring.

[0043] like Figure 4 As shown, the system uses a modified version of the DH parameter method to complete the underlying forward kinematic mathematical modeling. At the center of each physical rotation node of the robotic arm, the algorithm establishes a model as follows: Figure 4 The five local Cartesian reference coordinate systems shown are from the base coordinate system. To the end coordinate system The modeling process extracted four core physical characteristic variables between adjacent coordinate systems: link length. Its physical meaning is the effective length of the preceding metal connecting rod; the connecting rod torsion angle. This represents the assembly torsion angle; joint offset. That is, the vertical distance between adjacent joints along the X-axis; and the joint rotation angle. , which is the real-time rotation angle.

[0044] To ensure seamless mapping from simulation to physical implementation, this invention retains the original absolute physical link parameters calibrated by the official manufacturer of the robotic arm, and imports the improved DH parameter matrix from the official calibration, as shown below. Figure 11 As shown.

[0045] The system traverses Figure 11 The geometric parameters are used to calculate the homogeneous transformation high-order matrix between each set of adjacent local coordinate systems using trigonometric algebra. After multiplying these five fourth-order matrices sequentially, the absolute three-dimensional spatial coordinates and pose Euler angles of the end effector relative to the ground base are calculated under the forward kinematics model. Simultaneously, to deduce the target angles of each servo motor from the issued three-dimensional target points, this invention employs the Newton-Raphson numerical method to solve the inverse kinematics equations. The target pose in Cartesian space is converted into the rotation angle values ​​of each joint, completing the mapping from path points to joint motion commands. The solution accuracy is ≤0.01mm, and it is fully compatible with the kinematic solution logic of the official ROS2 driver.

[0046] After the model matrix is ​​loaded into memory, the host computer uses the SerialLink core function in MATLAB's Robotics Toolbox, inputting the above parameters, to construct a one-to-one replica of the real machine's dimensions. Figure 11 A three-dimensional digital twin simulation ontology was created. To perform obstacle avoidance experiments, three typical spherical obstacle models were artificially defined within a set three-dimensional Cartesian virtual space. The center coordinates and radius characteristics of these three mathematical obstacle spheres were determined. In this embodiment, the distances are set to: [0, -0.1, 0.2, 0.03] meters, [-0.05, -0.05, 0.25, 0.02] meters, and [0, -0.1, 0.3, 0.03] meters. To provide a standardized measurement environment, the system globally sets a uniform physical safety distance threshold of 0.015m for the anti-collision space, and performs multi-dimensional spatial arithmetic step-size discretization according to a gridded discrete resolution of 12.

[0047] In the standardized virtual test sandbox, the system defines the joint angles corresponding to the initial pose of the robotic arm as [0, 0, 0, 0, 0] rad as the origin, and sets the joint angles of the target pose as... rad is the endpoint. For example... Figure 3 As shown, when the global improved A* search is triggered, the execution flow strictly follows the following closed-loop defense: At each step of the discrete space node roaming generation, the algorithm engine forcibly extracts the five-dimensional joint angle combination corresponding to the candidate node and inputs it one by one. Figure 11 The system records and compares the extreme values ​​of physical rotation angles. Any illegal pose nodes that guide the robotic arm to exceed the physical limits of the metal components, such as "through-the-mold" poses, will be completely removed from the system's search queue immediately to ensure the absolute mechanical feasibility of subsequent analysis paths.

[0048] The core logic lies in the algebraic reconstruction of the spatially heuristic cost optimization function. When the system selects the optimal next-hop position among a massive number of grid nodes, the core optimization control mathematical expression it relies on is: Among them, the cost variable The actual physical motion cost consumed by the representative mechanism is physically represented as the total angular displacement that all five sets of motor shafts must output when the robotic arm moves from the starting point to the current node position. During calculation, the system extracts the multidimensional joint angle vectors of adjacent nodes in the path, calculates their mathematical Euclidean norm, and performs sequential accumulation. Heuristic guiding variables... It is the estimated cost of the shortest motion angle predicted by the system. Its physical meaning is set as the Euclidean linear distance norm of the multidimensional angle difference vector in joint space, which is the distance between the current node coordinates and the final target node coordinates.

[0049] This method is a collision geometry traversal pre-detection mechanism for a full-arm linkage-level 3D solid space. Traditional algorithms often only check whether the outermost gripper is colliding, while this invention... Figure 3In the illustrated loop, each node that passes the rigidity check must again invoke the forward kinematics high-dimensional matrix model. The system calculates in real time the absolute coordinates of the fuselage's metal links in the three-dimensional Cartesian world coordinate system under the assumed attitude, and establishes the algebraic analytical geometric equation of the spatial bounded line segments of the links. Subsequently, the algorithm initiates intersection operations, traversing and calculating the absolute shortest spatial perpendicular distance from each metal link entity line segment to the center of all known spherical obstacles in the environment. Only when it is confirmed that the physical distance between the five links of the entire arm and all environmental obstacles is greater than the set warning red line of 0.015m is the multi-dimensional attitude node allowed to be adopted into the global path sequence.

[0050] The original discrete sawtooth path output is reconstructed using advanced mathematical smoothing and denoising with a fifth-order polynomial. For example... Figure 7 As shown, the unmodified "original version of the A* algorithm" produces an extremely abrupt 3D trajectory at inflection points; for example... Figure 5 As shown, its angular signal in the time domain is a step-like abrupt change signal. To protect the underlying reduction gearbox from current and torque impacts, the system incorporates a fifth-order polynomial mathematical spline flexible interpolation rendering engine with an interpolation step size of 50 into its back-end processing stream. The system sets four differential boundary conditions at the beginning and end of each action segment—initial angular velocity, termination angular velocity, initial angular acceleration, and termination angular acceleration—which must be absolutely zero, thus establishing a fifth-order high-order equation system for inverse differential coefficient solution. For example... Figure 5 As shown, after this mathematical reconstruction, the originally rough step-shaped broken line was forcibly subdivided and merged into a smooth, continuously differentiable, and gentle curved surface. Ultimately, as... Figure 7 As shown, the system outputs an executable control matrix that is not only collision-free but also has extremely consistent motion curvature, which is an "improved version of the A* algorithm".

[0051] Given that global planning is time-consuming and difficult to adapt to highly dynamic collaborative environments with moving interference sources, the system sets up an improved artificial potential field method local processing module with microsecond-level computing power in parallel on the same set of underlying parameters.

[0052] Its core underlying computing idea is to directly construct an abstract model of the interaction between physical field vectors of gravity and spatial repulsion that are superimposed within the operating space of the robotic arm itself.

[0053] The first step is to establish the mathematical function of joint gravity that drives convergence. To save floating-point computing power, the gravity model is directly built in a high-dimensional joint space:

[0054] In this gravity-driven expression, the system presets a gravity gain coefficient. = 1.0. Variable The variable represents the current joint angle vector state of the fuselage transmitted back in real time by the sensors. This represents the limit state of the joint angle vector of the desired target pose. The physical meaning of this formula is the force that drives the robotic arm to approach the target joint angle, which is directly defined in joint space without the need for Cartesian coordinate transformation.

[0055] The second step is the mathematical mapping of the repulsive force across the entire arm of the linkage in space. For example... Figure 6 As shown, traditional APFs only generate repulsive forces at the ends, while in this system, the obstacle ball generates a spatial absolute normal repulsive force on the nearest connecting rods, such as the upper and lower arms. This is achieved by setting a repulsive force gain coefficient. = 100.0 and the influence distance threshold is 0.1m. When the system detects that a link entity in the three-dimensional Cartesian world is about to touch the obstacle warning red line, the Cartesian space repulsion vector... Generation. The push-pull physical force is converted into the motor's rotation angle signal. The system extracts the partial derivative operator corresponding to the robot arm's posture at that instant, namely the velocity Jacobian multidimensional matrix J. v The repulsive defense function is reconstructed as:

[0056] By using the physical transpose of the Jacobian matrix With space vector thrust The algorithm performs underlying multiplication operations, achieving a physical energy conversion of the system within milliseconds. It maps the external three-dimensional pushing force into repulsive angular torque components acting on the five sets of inner servo control axes. The sign of the direction after the algebraic superposition of the internal and external torques determines the robotic arm's emergency torsional escape maneuver in the next microsecond.

[0057] To address the issues of local optima, motion stagnation, and target unreachability that arise in traditional Artificial Potential Field (APF) methods during robotic arm path planning due to the mutual cancellation of obstacle repulsion and target attraction, this paper proposes an improved APF based on active local optimum detection, adaptive escape force guidance, and a smooth regression mechanism. This improved method maintains the original APF target point, obstacle environment, attraction model, and repulsion model unchanged, introducing a temporary auxiliary escape mechanism only when the robotic arm is detected to be trapped in a local optimum. This ensures fairness in comparing the algorithms before and after the improvement and maintains consistency of control variables.

[0058] The overall force model of the improved APF algorithm can be expressed as:

[0059] Among them, F att F still represents the gravitational force at the end of the robotic arm pointing towards the real target point. rep F represents the repulsive force generated by an obstacle. escapeThis is an escape assist force temporarily added when the local optimum is reached. It is important to emphasize that the improved algorithm does not change the final target point of the robotic arm. The escape force only takes effect briefly when a local optimum occurs. Its role is to help the robotic arm escape the local potential well. After escaping, it will still return to the artificial potential field planning process guided by the real target point.

[0060] 1. Active detection of local optimal states To accurately determine whether a robotic arm is trapped in a local optimum, this paper introduces a multi-dimensional state detection method based on a sliding window. The algorithm monitors the changes in the robotic arm's end-effector position, joint spatial motion amplitude, resultant force modulus, and target distance in real time over several iterations. The robotic arm is determined to have entered a local optimum or stagnated when the following conditions are met: First, if the magnitude of the resultant force remains below the preset threshold for several consecutive iterations, it indicates that the attraction and repulsion forces may cancel each other out. Second, the change in the position of the robotic arm's end effector is small and the path advancement is not obvious, indicating that although the robotic arm is still iterating, its actual movement is close to stagnation. Third, the distance between the current end point and the target point does not decrease significantly within the detection window, indicating that the robotic arm has not effectively converged towards the target point; Fourth, the distance between the end effector of the robotic arm and the obstacle is within the range of repulsive force, indicating that the stagnation may be caused by the repulsive force of the obstacle.

[0061] By integrating multiple indicators such as resultant force, displacement, target distance, and obstacle distance, misjudgments can be avoided by relying solely on a single resultant force threshold. For example, when the robotic arm is close to the target point, the decrease in resultant force is a normal convergence phenomenon and should not be misjudged as a local optimum; only when the robotic arm is still far from the target point, and its position change is small while being affected by the repulsive force of obstacles, should it be judged as a local optimum.

[0062] 2. Adaptive Escape Mechanism Design When the algorithm detects that the robotic arm is trapped in a local optimum, it immediately initiates an adaptive escape mechanism. This mechanism does not change the true target point, but instead temporarily adds an auxiliary escape force to the existing attractive and repulsive forces, enabling the robotic arm to escape the local potential well.

[0063] First, multiple candidate escape directions are generated in the current joint space or end-effector motion space. Candidate directions are preferentially selected from those approximately orthogonal to the current resultant force direction to avoid oscillating along the original force direction within a localized minima. Simultaneously, candidate directions must have a positive component pointing towards the target point to ensure that the escape process is not an ineffective motion away from the target point.

[0064] Secondly, the candidate directions are evaluated. Evaluation indicators mainly include: whether the direction increases the safe distance between the robotic arm and obstacles, whether it maintains the trend of approaching the target point, whether it causes the path to cross obstacles, and whether it causes excessive joint changes. After comprehensive evaluation, the direction with the lowest cost and highest feasibility is selected as the current escape direction.

[0065] Then, a temporary auxiliary escape force F is applied in the selected escape direction. escape This force only takes effect in locally optimal states and works in conjunction with the original APF's attractive and repulsive forces, rather than replacing the original target attraction. This maintains the consistency of the algorithm's objective while enhancing the robotic arm's ability to escape locally optimal states.

[0066] During the escape phase, the algorithm can appropriately increase the step size, enabling the robotic arm to quickly escape the local potential well; at the same time, it sets the escape duration steps to prevent the escape force from persisting for a long time and disrupting the original path planning goal. After the escape is completed, the auxiliary escape force gradually decreases, and the robotic arm returns to the motion state determined by the combined attraction of the real target point and the repulsive force of the obstacle.

[0067] 3. Smooth Regression and Stability Guarantee To avoid abrupt path changes or motion oscillations in the robotic arm after escape, this paper introduces a smooth regression mechanism. After escaping a local optimum, the algorithm does not immediately and completely remove the escape force, but instead uses a gradual decay method to reduce F... escape The value is gradually reduced to zero over several iterations. This ensures a smooth transition in the robotic arm's movement direction, reduces path breaks and joint abrupt changes, and improves the continuity and executability of the robotic arm's movements.

[0068] Meanwhile, to prevent the robotic arm from being immediately identified as a new local optimum after just escaping one, the algorithm includes a cooling-off period. During this period, the sensitivity for detecting local optima is appropriately reduced, and the stagnation criteria are relaxed, allowing the robotic arm sufficient time to continue moving towards the target point along a new path. After the cooling-off period, the algorithm resumes normal detection.

[0069] Furthermore, the improved algorithm can dynamically adjust the range of the repulsive field based on the distance between the robotic arm and obstacles. When the robotic arm is close to the obstacle, the repulsive force is appropriately increased to ensure obstacle avoidance safety; when the robotic arm is far from the obstacle, the repulsive force is reduced to avoid excessive repulsive force causing path detours or oscillations. Through dynamic repulsive force adjustment, the smoothness of the path and the stability of the planning can be further improved.

[0070] 4. Improved algorithm features In summary, the improved APF algorithm proposed in this paper mainly includes active detection of local optima, adaptive escape direction generation, temporary escape force assistance, adaptive step size adjustment, smooth decay of escape force, escape cooling period, and dynamic repulsion force adjustment. Compared with the traditional APF, the improved algorithm enhances the robotic arm's ability to escape local optima and improves the success rate and stability of path planning without changing the real target point and the basic potential field model.

[0071] This method retains the advantages of traditional artificial potential field method, such as simple structure, strong real-time performance and low computational cost, and makes targeted improvements to address local optima and target unreachability problems. It is suitable for path planning tasks of robotic arms in complex obstacle environments.

[0072] In addition, the module incorporates a deadlock prevention mechanism, namely a local optimum detection and escape mechanism. Within a maximum set iteration cycle of 10,000 times, when the deadlock detector detects an indicator that the computation has entered a loop and stalls, the logic layer immediately determines that a serious force balance deadlock trap has occurred and decisively triggers an audible and visual interruption warning. This measure completely repairs the fatal flaw of the traditional potential field method.

[0073] After the algorithm development was completed, the system entered the algorithm standardization, quantification, and stress testing phase. In a unified simulation sandbox environment where all variables were absolutely locked, that is, using the same starting posture, endpoint target, completely identical 3D obstacle arrangement, and the same set of simulation collision boundary extreme conditions, the system deployed and executed 10 sets of long-term, high-intensity, repeated combat experiments.

[0074] The system's underlying core performance metrics extreme value quantification comparison matrix, such as... Figure 8 bar chart and Figure 12 As shown, the conclusion is as follows: 1. In the dimension of evaluating the optimality of physical motion loss, the total change of path joint space of the high-frequency improved artificial potential field method is fixed at 1.7000 rad, which is significantly lower than the total physical displacement cost of 3.4325 rad of the global algorithm, indicating that it has less joint motion loss and a shorter path; while the path of the improved A algorithm is the global optimal solution, without local optima problem, and can guarantee the global safety and optimality of the path in a static known environment.

[0075] 2. In terms of core computing load and real-time response evaluation, the improved artificial potential field method, with a time of 0.0502 s and 34 micro-step iterations, completely outperforms the global algorithm, which requires 12.7 s and 1355 large memory nodes. This demonstrates that it has a small computational load, fast response speed, and significant real-time advantages, making it more suitable for local obstacle avoidance scenarios in dynamic obstacle environments.

[0076] 3. In terms of motion continuity, the smooth version of the improved A* algorithm achieves smooth transition of joint trajectories without changing the total path length, resulting in better motion continuity. It can effectively reduce the impact and vibration during the movement of the robotic arm and meet the stable movement requirements of physical robotic arms.

[0077] The system terminal activates six independent high-speed rendering canvases for a full-dimensional visual tour. For example... Figure 7 As shown, it provides a panoramic view of the robotic arm's initial controlled baseline, the original rough and abrupt end-effector transitions of the A* curve, the optimized trajectory after polynomial smoothing, and the APF's compact, edge-hugging avoidance gliding arc. And... Figure 6 and Figure 7 In the video, the digital twin simulator visually demonstrates to developers the spectacular transformation of mathematical matrices into three-dimensional motion forms.

[0078] At the command-line level of the main control embedded development board, the operator initiates a privilege escalation command to forcibly terminate the native default background application daemon process that boots with the system, and completely clears the serial port channel and bus channel for exclusive use. The system then writes ROS_DOMAIN_ID=5 and middleware configuration identifiers to the system variables to establish a private high-speed data distribution highway network in the chaos. After the main control activates the native SDK node file, the registers of each underlying controller are reset and dedicated relay subscription channels such as / jetarm / joint_trajectory are successfully started.

[0079] On the MATLAB host computer with gigabit computing power, the system engine executes configuration pull commands to generate standard ROS2 native software nodes and broadcast publishers based on specific message timing. For the aforementioned sandbox array of safe and smooth trajectory data points that has been verified to be 100% compliant, the system calls a message conversion compiler to compress high-dimensional floating-point numbers to a data bit width supported by the underlying controller. After completing strict legality verification and out-of-bounds interception checks, the data is assembled into standardized data message packets with precise motion time stamps.

[0080] During final execution, MATLAB injects the bitstream into the local area network topic bus via a dedicated release command. The ROS2 low-level node quickly intercepts and parses this high-density instruction, bridging it to the STM32 controller via a high-speed serial port at 15200 baud. This allows for duty cycle pulse allocation within microseconds, precisely driving five metal mechanisms weighing several kilograms each, completing the necessary communication with... Figure 10 The system exhibits consistent and smooth spatial motion. During this process, it simultaneously transmits status data such as joint angles and torques at a frequency of 50Hz. If a joint exceeds its limits or there is a risk of collision, it immediately triggers emergency braking to ensure hardware safety.

[0081] The present invention has been described by way of example in conjunction with the accompanying drawings. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any non-substantial improvement made by adopting the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution of the present invention to other occasions without modification, shall be within the protection scope of the present invention.

Claims

1. A five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms, characterized in that, The control method of the system includes the following steps: Step 1: Establish a kinematic model of a five-degree-of-freedom robotic arm. Use the improved DH parameter method to model the five rotary joints of the five-degree-of-freedom robotic arm and complete the forward and inverse kinematics solutions. Step 2: Build a standardized path planning simulation and verification environment, construct a 3D simulation model of the robotic arm and define spherical obstacles in Cartesian space, and set uniform simulation boundary conditions; Step 3: Implement an improved A* path planning algorithm with physical constraints and path smoothing for the robotic arm. In the discretized joint space, invalid path points that exceed the calibrated rotation angle limit are eliminated. The global original path is generated by combining full-arm link-level collision detection and smoothing is performed using fifth-order polynomial interpolation to generate an executable joint angle sequence. Step 4: Implement the improved artificial potential field method for collision detection with linkages. Construct gravitational and repulsive fields in the joint space. Map the Cartesian repulsive force calculated based on the shortest distance between the entire arm linkage and the obstacle to the joint space through the Jacobian matrix. At the same time, configure a local optimal solution detection mechanism. Step 5: Algorithm standardization, comparison, verification, and visualization. In the standardized environment, the core performance indicators of the improved A* algorithm and the improved artificial potential field method are quantitatively compared in terms of total path length, search time, and number of expanded nodes. The trajectory is verified in three dimensions through a multi-window independent visualization module. Step six: Establish cross-platform communication and physical robotic arm control. The host computer node converts the planned joint angle path into a standard message format and sends it to the embedded master control node through the ROS2 communication domain. The underlying controller drives the physical robotic arm to perform precise movements and transmits motion status data back in a closed loop.

2. The five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms according to claim 1, characterized in that: The improved DH parameters used in step one include the link length. Linkage torsion angle Joint offset and joint angle The forward and inverse kinematics solutions are obtained using the Newton-Raphson numerical method, and the accuracy error condition is set as the end-effector pose error. .

3. The five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms according to claim 2, characterized in that: The cost function for optimizing the improved A* path planning algorithm in step three is: ； in, This represents the total change in joint angles from the starting point to the current node, calculated by summing the Euclidean norms of the joint angle vectors of adjacent nodes. Let be the Euclidean norm of the angle vector from the current node to the target node in the joint space.

4. The five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms according to claim 3, characterized in that: The full-arm link-level collision detection in step three is as follows: at each step of the path search, the spatial coordinates of each link corresponding to the current joint angle are calculated using the forward kinematics model. The shortest distance between all links and obstacles is calculated. If the distance is less than the collision safety distance, a collision is determined and the path node is removed.

5. The five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms according to claim 4, characterized in that: In step four, the gravitational term of the improved artificial potential field method is directly defined in joint space as: ； The formula for the repulsion term is: ， in, This is the velocity Jacobian matrix corresponding to the current joint angle of the robotic arm, used to implement the repulsive force vector in Cartesian space. F cart Mapping of forces to joint space.

6. The five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms according to claim 5, characterized in that: The local optimum detection mechanism in step four is as follows: when the robotic arm iterates more than a set number of times and the joint angle change is less than a set position change threshold for multiple consecutive steps, it is determined to be trapped in a local minimum and an early warning is issued.

7. The five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms according to claim 6, characterized in that: In step six, the host computer establishes a publisher by configuring global variables and binding the subscription topic with the ROS2 driver. When the underlying driver is initialized, system commands are executed to release the hardware serial port and servo motor control permissions, and high-performance communication middleware is configured to reduce instruction transmission latency.

8. A five-axis robotic arm obstacle avoidance planning and cross-platform control system based on obstacle avoidance algorithms, comprising the five-axis robotic arm obstacle avoidance planning and cross-platform control system based on A* and APF algorithms as described in any one of claims 1 to 7, characterized in that, include: The upper-level computer layer is equipped with scientific computing software and a robotics toolkit for simulation environment construction, algorithm planning, and visualization. The embedded main control layer includes an edge computing development board running the ROS2 operating system, which is used to receive instructions from the host computer and manage communication nodes. The underlying execution layer, which includes a microcontroller and the robotic arm body, is connected to the embedded main control layer via serial communication to directly drive the robotic arm joint bus servos and transmits joint angles and torques back at a frequency of 50Hz. The power supply layer adopts a dual-power independent isolated power supply scheme, which independently supplies power to the embedded main control layer and the underlying execution layer.

Images