Collision-Free Robot Pose Optimization Method Based on Improved Algorithms

Abstract

In modern shipbuilding, the structural complexity of ship components and the constrained workspace make robotic grinding prone to collisions. To improve safety and stability, this paper proposes a collision-free posture optimization method for ship-component operations. First, forward and inverse kinematic models are established, and postures along the path are organized into a directed graph. Feasible postures are then identified under joint-limit and singularity constraints. Directed bounding boxes and the GJK collision detection algorithm are applied to construct a collision-free posture set. An improved A* algorithm is then introduced. It incorporates a multi-source heuristic based on joint-space geometric distance and a safety-distance penalty to compute an optimal posture sequence with minimal joint deviation. This design promotes smooth transitions between consecutive postures. Simulation results show that the proposed method avoids robot–workpiece interference in constrained environments and improves obstacle avoidance and motion smoothness. Compared with the standard A* algorithm, the proposed approach reduces search time by 15.8% and increases the minimum safety distance by nearly fivefold. Compared with a non-optimized posture sequence, cumulative joint variation is reduced by up to 92.5%. The joint amplitude range decreases by an average of 41.2%, and the standard deviation of joint fluctuations decreases by 37.8%. The proposed method provides a generalizable solution for robotic measurement, assembly, and machining in complex and confined environments.

1. Introduction

Currently, grinding ship components is performed manually. This practice results in low production efficiency, dust-related health risks, and inconsistent surface quality. These issues cause material waste and delays in shipbuilding schedules [1,2]. In contrast, industrial robots can operate continuously in hazardous environments and provide precise control of polishing accuracy, which improves productivity. As a result, robotic grinding offers an alternative to manual polishing and satisfies the requirements for precision and efficiency in ship-component finishing [3,4]. However, grinding complex structures in constrained workspaces increases the risk of robot collisions [5]. Inadequate pose planning may also drive the robot toward kinematic singularities or joint-limit violations, resulting in unstable motion and possible polishing failure.

To ensure stable robot motion during polishing in constrained environments, extensive studies have investigated collision detection and avoidance methods. For collision-free path generation in complex scenarios, Wang et al. [6] combined three-dimensional grid modeling with ACO-based local optimization and PSO-based global sequencing to generate a short collision-free welding path. Zhao et al. [7] achieved collision-free path planning in both Cartesian and joint-spaces by integrating a repulsive-field model with an improved RRT* algorithm. Motion timing was further optimized using an enhanced whale optimization algorithm. Wen et al. [8] constructed orthogonal configurations using a sphere–cylinder composite model. They converted continuous optimal control into a nonlinear programming problem and generated collision-free trajectories with time–jerk optimization and constant end-effector velocity. Zhang et al. [9] proposed an improved RRT method to generate smooth and collision-free paths. However, this approach focuses on path-point planning and does not consider the effect of joint-angle continuity on overall trajectory smoothness. Luo et al. [10] combined an energy-optimization objective with artificial potential field sampling to generate obstacle-avoidance paths. Liu et al. [11] introduced a collision-free trajectory planning approach based on danger-field information among robots. Moreover, robot trajectory planning directly affects energy consumption, machining time, and motion smoothness in grinding tasks. Lu et al. [12] presented a minimum-jerk trajectory planning approach based on quintic polynomial interpolation for a translational 3-DOF parallel robot. Motion performance was improved through time-parameter optimization. Nevertheless, before time-based trajectory planning, a collision-free and geometrically continuous set of joint-space path points must be obtained. This process, referred to as pose optimization, constitutes the core focus of this study. A high-quality geometric pose sequence provides a reliable input for subsequent trajectory planning and contributes to improved grinding quality.

To further improve motion smoothness, researchers have explored pose optimization using the redundant degrees of freedom available in robotic systems [13,14,15]. Dai et al. [16] proposed a redundancy-based hierarchical framework. It generates initial collision-free trajectories through graph search and refines them using greedy jerk optimization and local adaptive filtering to improve motion smoothness. Erdős et al. [17] addressed joint motion planning for redundant industrial robots using multi-synchronized control-point modeling and configuration-space graph optimization. Their method exploits redundancy to reduce cycle time in remote laser welding. Gao et al. [18] developed a redundant motion optimization method for fiber-placement workstations. The positioner is treated as a redundancy variable, and two-stage dynamic programming combined with spline interpolation is used to ensure coordinated and smooth joint motion. Ju et al. [19] introduced a predictive obstacle-avoidance model based on triangular collision planes. Geometry-related cost functions were incorporated into null-space optimization to achieve continuous obstacle avoidance and smooth motion in dynamic environments. Huo et al. [20] proposed a weight-adaptive scheme to address joint-limit and singularity avoidance in redundant robots. Despite these advances, generating a pose sequence that satisfies both collision-free constraints and joint-motion smoothness remains a key challenge in robot motion planning.

The A* algorithm is widely used in robotic path search due to its completeness and optimality. With a well-designed heuristic function, A* can guide the search toward an optimal solution efficiently. However, in practical engineering applications, the standard A* algorithm has several limitations. Search efficiency depends heavily on the heuristic function, and poorly designed heuristics may cause excessive expansion of invalid nodes. For multi-source or multi-target problems, traditional single-source heuristics do not adapt well to complex topologies. Furthermore, standard A* primarily emphasizes path-length minimization and insufficiently accounts for safety-distance constraints or collision risks. To address these issues, various enhancements have been proposed. He et al. [21] developed a joint-space A* algorithm with a pre-planning strategy for 6-DOF manipulators. Full-link collision avoidance is achieved through dimensionality reduction and multi-cube collision detection. Zeng et al. [22] introduced a dynamic weight adjustment mechanism to balance search efficiency and path quality. Zhao et al. [23] combined A* with jump-point search to reduce unnecessary node expansions. Li et al. [24] proposed an index-based A* algorithm for weighted directed acyclic graphs with unknown vertex coordinates. Earliest-arrival and reverse-arrival indices are used for hybrid pruning. However, most existing improvements focus on two-dimensional or three-dimensional path planning. Research on multi-source node search with safety constraints in high-dimensional joint-space remains limited. Systematic methods that jointly consider search efficiency, motion smoothness, and obstacle-avoidance safety are still lacking. In addition to motion planning, contact force control and vibration suppression during grinding are also crucial to the final surface quality [25,26]. Although this paper focuses on geometric collision-free pose optimization, the resulting smooth joint motion can reduce structural vibrations caused by abrupt joint-acceleration changes. This provides a more stable motion basis for end-effector force control and vibration suppression.

In summary, existing research on robotic polishing systems gives limited attention to motion smoothness under complex environmental constraints. In addition, the standard A* algorithm is inefficient for multi-source node search in high-dimensional joint-space and does not fully account for safety margins. To address these challenges, this paper proposes a pose optimization method for robotic polishing based on an improved A* algorithm. The main contributions of this study are summarized as follows:

(1) A pose optimization framework is established for ship-component polishing tasks. Forward and inverse kinematics modeling, orientation flexibility analysis, and multi-constraint screening are used to construct a feasible pose set. This set incorporates joint-limit constraints, singularity avoidance, and collision detection. This provides a high-quality candidate configuration space for subsequent optimization.

(2) An improved A* search algorithm is developed to overcome efficiency bottlenecks and safety limitations in multi-source node exploration. A multi-source heuristic based on joint-space geometric distance is introduced, together with an exponentially decaying safety-distance penalty term, to improve search efficiency and obstacle-avoidance capability. This design improves computational performance and safety margins while preserving global optimality.

(3) A pose-sequence optimization model is formulated to minimize joint-angle variation. A global search is performed on a directed node graph using the improved A* algorithm to obtain an optimal pose sequence. The resulting sequence satisfies motion-smoothness requirements, constraint compliance, and collision avoidance. This formulation improves pose continuity and trajectory smoothness in constrained-space polishing operations.

The paper is organized as follows. Section 2 presents the forward and inverse kinematics of the robot. The kinematic model is derived using the modified D–H parameter method, and the robot’s orientation flexibility is analyzed. Section 3 constructs a robot motion-planning framework for constrained environments. Orientation adjustment poses are mapped to candidate node sets using directed graph modeling. Multi-level screening is performed by integrating joint-limit constraints, singularity constraints, and GJK-based collision detection. A joint-angle-variation minimization model is established. Two key enhancements to the A* algorithm, namely the multi-source heuristic and the safety-distance penalty, are introduced in detail. Section 4 presents simulation results to validate the proposed method. The improved A* algorithm is compared with the standard version in terms of search efficiency, safety distance, and other key performance indicators. Joint-angle motion characteristics are analyzed to evaluate changes in motion smoothness before and after pose optimization. Section 5 summarizes the main findings and contributions. It also discusses the limitations of the current work. Section 6 concludes the paper and outlines directions for future research and applications.

2. Robot Kinematics Analysis

This study focuses on the FANUC M-20iA industrial robot (Shanghai FANUC Robotics Co., Ltd., Shanghai, China), a six-degree-of-freedom serial manipulator. Its structural parameters and link dimensions are shown in Figure 1a.

Figure 1. (a) The FANUC M-20iA robot arm physical model; (b) Robot kinematic model.

In manipulator kinematic modeling, the D–H parameter method is a widely used approach [27,28]. The D-H parameters of each link of the robot used in this study are listed in Table 1. This method parameterizes the spatial relationships between joints and links and establishes the coordinate transformation model of the kinematic chain. Based on the modified D–H parameter method and the robot’s structural features and dimensions shown in Figure 1a, a kinematic model of the robot is established, as illustrated in Figure 1b. The model describes the geometric relationships between the robot’s joints and links and provides a basis for subsequent forward and inverse kinematics calculations and pose-optimization analysis.

Table 1. Modified D-H parameters of the robot.

Joint No.	${\mathit{\theta }}_{\mathit{i}}$ (deg)	${\mathit{d}}_{\mathit{i}}$ (m)	${\mathit{a}}_{\mathit{i}-1}$ (m)	${\mathit{\alpha }}_{\mathit{i}-1}$ (deg)
1	0	0	0	0
2	−90	0	0.150	−90
3	0	0	0.790	0
4	0	0.835	0.250	−90
5	0	0	0	90
6	0	0	0	−90
TCP	-	0.100	0	0

In Table 1, ${\theta }_i$ denotes the joint angle, $d_i$ denotes the joint offset, $a_i$ denotes the link length, and ${\alpha }_i$ denotes the link twist. Since all six joints of the M-20iA robot are revolute, only ${\theta }_i$ is a variable, while the other three parameters are constants.

2.1. Robot Kinematics Model

In ship-component polishing tasks, the end-effector path must be planned to ensure path continuity and polishing quality. The overall polishing trajectory is typically discretized into a series of path points $p_i(i=1,2,\dots ,\mathrm{n})$. The corresponding end-effector pose matrix ${\mathit{T}}_{\mathit{i}}$ is computed at each path point. Based on the D-H parameterization method, the homogeneous transformation matrix between adjacent robot link frames $\left\{i-1\right\}$ and $\left\{i\right\}$ is expressed as

$${}_{\mathit{i}}{}^{\mathit{i}-\mathbf{1}}\mathit{T}=\left[\begin{array}{cccc}\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_i& -\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_i& 0& a_{i-1}\\ \mathrm{s}\mathrm{i}\mathrm{n}{\theta }_i\mathrm{c}\mathrm{o}\mathrm{s}{\alpha }_{i-1}& \mathrm{c}\mathrm{o}\mathrm{s}{\theta }_i\mathrm{c}\mathrm{o}\mathrm{s}{\alpha }_{i-1}& -\mathrm{s}\mathrm{i}\mathrm{n}{\alpha }_{i-1}& -d_i\mathrm{s}\mathrm{i}\mathrm{n}{\alpha }_{i-1}\\ \mathrm{s}\mathrm{i}\mathrm{n}{\theta }_i\mathrm{s}\mathrm{i}\mathrm{n}{\alpha }_{i-1}& \mathrm{c}\mathrm{o}\mathrm{s}{\theta }_i\mathrm{s}\mathrm{i}\mathrm{n}{\alpha }_{i-1}& \mathrm{c}\mathrm{o}\mathrm{s}{\alpha }_{i-1}& d_i\mathrm{c}\mathrm{o}\mathrm{s}{\alpha }_{i-1}\\ 0& 0& 0& 1\end{array}\right]$$

(1)

The end-effector transformation matrix of the robot is expressed as

$${}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}={}_{\mathbf{1}}{}^{\mathbf{0}}\mathit{T}{}_{\mathbf{2}}{}^{\mathbf{1}}\mathit{T}{}_{\mathbf{3}}{}^{\mathbf{2}}\mathit{T}{}_{\mathbf{4}}{}^{\mathbf{3}}\mathit{T}{}_{\mathbf{5}}{}^{\mathbf{4}}\mathit{T}{}_{\mathbf{6}}{}^{\mathbf{5}}\mathit{T}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& w_x\\ n_y& o_y& a_y& w_y\\ n_z& o_z& a_z& w_z\\ 0& 0& 0& 1\end{array}\right]$$

(2)

The end-effector pose matrix of the robot in the base coordinate frame is given as

$${\mathit{T}}_{\mathit{i}}={}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}\left({\theta }^i\right){}_{\mathit{E}}{}^{\mathbf{6}}\mathit{T},$$

(3)

where ${}_{\mathit{E}}{}^{\mathbf{6}}\mathit{T}$ is the homogeneous transformation matrix from the robot end flange to the tool coordinate frame; $\left\{{\theta }^i\left|i=1,2,\dots ,n\right.\right\}$ is the set of joint angles at path point $p_i$.

Inverse kinematics involves determining the robot’s joint angles through backward derivation from the known end-effector pose and structural link parameters, with the joint angles expressed as

$${\theta }^i=f^{-1}\left({}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}\right)$$

(4)

According to Pieper’s criterion, when the axes of the robot’s 4th, 5th, and 6th joints intersect at a single point, the inverse kinematics problem can be solved analytically [29,30]. The inverse kinematic solutions are given as follows (the detailed derivation is shown in the Appendix A):

$$\left\{\begin{array}{c}{\theta }_1=\pm \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2(w_y,w_x)\\ {\theta }_2=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2({\displaystyle \frac{K}{\rho }},\pm \sqrt{1-{\displaystyle \frac{K^2}{{\rho }^2}}})-\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{2}(k_3,k_1)\\ {\theta }_3=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(s_{23},c_{23}\right)-{\theta }_2\\ {\theta }_4=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(a_x{s}_1-a_y{c}_1,a_x{c}_1{c}_{23}+a_z{c}_{23}+a_y{s}_1{s}_{23}\right)\\ {\theta }_5=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(\pm \sqrt{1-c_5^2},c_5\right)\\ {\theta }_6=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(-o_x{c}_1{c}_{23}+o_z{s}_{23}-o_y{s}_1{c}_{23},n_x{c}_1{c}_{23}-n_z{s}_{23}+n_y{s}_1{c}_{23}\right)\end{array}\right.$$

(5)

2.2. Task-Space Orientation Flexibility Analysis

To ensure surface uniformity and polishing consistency, complex ship-component polishing tasks require the robot end-effector to follow a prescribed spatial path while maintaining a task-consistent contact posture. For a six-degree-of-freedom industrial robot performing a path-constrained task, the three translational degrees of freedom are fully determined by the tool center point trajectory. The remaining rotational degrees of freedom provide orientation flexibility.

However, the end-effector orientation is not entirely free. Rotation about the end-effector x-axis ($\alpha$) must be strictly constrained. Any deviation may cause the tool edge to gouge the workpiece surface and degrade machining quality. Rotation about the z-axis ($\gamma$) is constrained by the path tangent direction to ensure stable material removal rates. In contrast, rotation about the y-axis ($\beta$) admits a certain degree of adjustability within the allowable process range. In the polishing of deep-groove components such as H-beams, adjusting the pitch angle alters the spatial position of the robot wrist and links relative to the workpiece flanges. As a result, this degree of freedom is particularly relevant for collision avoidance.

Therefore, to balance collision avoidance, polishing quality, and computational efficiency, the rotation about the y-axis, denoted by $\beta$, is selected as the only orientation adjustment variable. This strategy reduces the complex full-orientation search space to an optimization problem focused on the key collision-avoidance degree of freedom.

The robot end-effector pose matrix is denoted as

$${}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}=\left[\begin{array}{cc}R\left({\varphi }_j\right)& \mathit{p}\\ 0& 1\end{array}\right],$$

(6)

where $R({\varphi }_j):{\varphi }_j\to SO(3)$ denotes the end-effector orientation rotation matrix parameterized by the orientation adjustment angle ${\varphi }_j$, and $\mathit{p}\in {\mathbb{R}}^3$ represents the end-effector position.

When the orientation adjustment angle is set to ${\varphi }_j=0$, the robot reaches a reference pose at each path point, denoted by $R_n$ with corresponding joint angles ${\theta }^{i,0}$. By varying ${\varphi }_j$ about the end-effector y-axis, alternative feasible end-effector orientations are generated, leading to corresponding joint configurations ${\theta }^{i,j}$. The resulting orientation matrix is expressed as

$$\mathrm{R}\left({\varphi }_j\right)=R_n\cdot R_y\left({\varphi }_j\right),$$

(7)

where $R_n$ denotes the reference end-effector orientation and $R_y\left({\varphi }_j\right)$ represents the rotation matrix about the end-effector y-axis with angle ${\varphi }_j$.

At each path point, variations in the orientation adjustment angle ${\varphi }_j$ do not alter the spatial position of the end-effector but induce corresponding changes in the robot joint angles. Consequently, for each path point $p_i$, multiple feasible joint configurations can be obtained within the allowable range of ${\varphi }_j$, which are computed via the inverse kinematics function

$${\theta }^{i,j}=f^{-1}\left(R\left({\varphi }_j\right)\right)$$

(8)

where $f^{-1}(\cdot ),$ denotes the inverse kinematics function of the robot.

3. Robot Motion Planning in Constrained Spaces

These calculations yield several joint-angle solutions ${\theta }^{i,j}$ for each path point $p_i$. A directed node network is then created to represent the set of poses at each path point, with nodes screened based on kinematic and collision constraints. Following filtering, multiple possible positions are kept at each path point. The A* algorithm is then used on the directed node graph to determine the ideal pose sequence that minimizes joint-angle deviations, resulting in smooth joint transitions and improved overall motion smoothness.

3.1. Directed Graph Construction and Feasible Solution Filtering

The polishing path is discretized into $n$ path points $p_i(i=1,2,\dots ,n)$ when building the robot’s directed node graph G*. The candidate directed node graph G* is created by further assigning $m$ orientation adjustment angles $\left\{{\varphi }_j\left|j=0,1,\dots ,m\right.\right\}$ at each discrete path point (see Figure 2). Each of the graph’s $n\times m$ nodes represents a collection of joint-angle solutions ${\theta }^{i,j}$.

Figure 2. Candidate directed graph G* (horizontal axis: path points, vertical axis: orientation adjustment angles).

The computational complexity is primarily determined by the node generation and edge construction. Let $N$ be the number of path points and $M$ be the number of discretized orientation adjustment angles. The node generation process involves inverse kinematics and collision detection, scaling linearly as $O(N·M)$. Since the graph is constructed by fully connecting feasible nodes between adjacent layers, the number of edges scales as $O(N·M^2)$.

The feasible robot poses in the graph G* are filtered according to three types of constraints:

1.

Joint-Limit Constraint

To avoid over-travel, mechanical interference, actuator overload, or pose-solving failure, the robot’s six revolute joints must meet their respective joint-angle constraints during operation. This constraint can be stated as follows:

$${\theta }_{l,\mathrm{m}\mathrm{i}\mathrm{n}}\le {\theta }_l^{i,j}\le {\theta }_{l,\mathrm{m}\mathrm{a}\mathrm{x}}$$

(9)

2.

Pose Singularity Constraint

When a robot enters singular or near-singular poses, it may lose motion control, affecting trajectory accuracy and system stability. To prevent singular configurations, this study applies Yoshikawa for pose singularity constraints. Yoshikawa [31] is defined as

$$\omega =\sqrt{\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{J}{\mathit{J}}^{\mathbf{T}}\right)}=\sqrt{{\lambda }_1{\lambda }_2\dots {\lambda }_r}={\sigma }_1{\sigma }_2\dots {\sigma }_r$$

(10)

where $\mathit{J}$ denotes the robot’s Jacobian matrix; $\sqrt{\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{J}{\mathit{J}}^{\mathbf{T}}\right)}$ is the determinant of matrix $\mathit{J}{\mathit{J}}^{\mathbf{T}}$; ${\lambda }_1,{\lambda }_2,\dots ,{\lambda }_r$ denotes the eigenvalues of matrix $\mathit{J}{\mathit{J}}^{\mathbf{T}}$; ${\sigma }_1{\sigma }_2\dots {\sigma }_r$ denotes the singular values of the Jacobian matrix.

To ensure safe operation, the manipulability of the Jacobian matrix $\mathit{J}\left(\theta \right)$ must remain below a specified threshold $\eta$. By substituting the joint angles ${\theta }^{i,j}$ into $\mathit{J}\left(\theta \right)$, the manipulability is computed and constrained not to exceed the prescribed upper bound. The constraint can be expressed as

$$\omega =\sqrt{\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{J}{\mathit{J}}^{\mathbf{T}}\right)}>\eta$$

(11)

3.

Collision-free constraint

To avoid collisions between the robot and the grinding workpiece, collision detection must be performed at all robot postures. Collision-free robot configurations are then selected. Since the robot’s joint links resemble cylinders, cylindrical bounding boxes are applied to the robot base and two joint links to simplify computation and improve collision-checking efficiency [32,33], as shown in Figure 3a. The cuboid bounding box model of the grinding workpiece (H-beam) is shown in Figure 3b. Thus, the complex robot and workpiece models are converted into a collision detection between cylinders and cuboids, enabling efficient collision checking using the GJK algorithm [34].

Figure 3. (a) Robot link cylindrical bounding model; (b) H-beam cuboid bounding model.

In summary, the selection of robot grinding postures is divided into two stages: kinematic constraint verification and collision detection. First, all candidate joint angle solutions ${\theta }^{i,j}$ are evaluated under the constraints. The posture is kept if it meets the joint-limit condition ${\theta }_{l,\mathrm{m}\mathrm{i}\mathrm{n}}\le {\theta }_l^{i,j}\le {\theta }_{l,\mathrm{m}\mathrm{a}\mathrm{x}}$ and the manipulability $\omega =\sqrt{\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathit{J}{\mathit{J}}^{\mathbf{T}}\right)}>\eta$; otherwise, it is removed. After the initial posture filtering, the cylindrical bounding boxes of all robot links are updated in real time, and collision detection is performed with the GJK algorithm. The algorithm evaluates the Minkowski difference between the robot and obstacles, turning collision detection into checking whether the origin lies inside the difference set. It iteratively searches for support points of the shapes in specific directions and updates the point set; if the set contains the origin, a collision is detected, otherwise the posture is considered safe. Finally, a feasible posture set is obtained that satisfies joint limits, singularity constraints, and collision-free conditions.

The robot postures that do not satisfy the constraint conditions are removed from the candidate directed graph G*, resulting in a multi-constraint directed graph G containing only feasible postures, as shown in Figure 4.

Figure 4. Directed graph G after filtering feasible solutions.

In this directed graph, red nodes indicate robot poses that violate constraints, and blue nodes represent feasible polishing poses satisfying multiple constraints. Connections between blue nodes correspond to collision-free transitions. This graph clearly illustrates the topological connections among feasible poses and offers the search space for subsequent A*-based pose optimization and trajectory planning.

3.2. Construction of the Optimal Joint Motion Smoothness Model

Following constraint screening, each path point still has multiple possible polishing positions that satisfy kinematic and geometric constraints. Directly using these poses in path planning could result in excessive joint-angle changes between adjacent points, diminishing motion continuity and stability. Thus, a metric quantifying pose-transition smoothness is required, and the pose sequence is optimized based on it.

To achieve this, a cost function $q$ for joint motion smoothness is defined mathematically as follows:

$$q=\mathrm{m}\mathrm{i}\mathrm{n}\sum _{i=1}^n\sum _{j=0}^m\sum _{k=0}^m\sum _{l=1}^6{\displaystyle \frac{\left|{\theta }_l^{i,j}-{\theta }_l^{i-1,k}\right|}{{\theta }_{l,max}-{\theta }_{l,min}}}.$$

(12)

Specifically, the joint smoothness cost is defined as the accumulated relative joint variation between adjacent path points. For the $j$-th candidate posture at path point $i$ and the $k$-th candidate posture at path point $i-1$, the variation of each joint is computed as the absolute angular difference. To account for the heterogeneous motion capabilities of industrial robot joints, the angular variation of each joint is normalized by its allowable motion range $∆{\theta }_l={\theta }_{l,max}-{\theta }_{l,min}$, yielding a dimensionless relative variation. The normalized variations of all joints are summed and accumulated along the entire path to form the global cost function $q$. By minimizing this cost function, the resulting posture sequence achieves balanced joint variations across all joints, thereby ensuring smooth, continuous, and stable robot motion throughout the grinding process.

3.3. Polishing Pose Optimization Based on the Improved A* Algorithm

The A* algorithm is an efficient direct search method for finding the optimal path, combining the Greedy strategy with Dijkstra’s algorithm. It uses a heuristic function to guide the search direction while ensuring the optimal path [35]. A* determines the search direction using an evaluation function, expanding nodes from the start, computing the cost of each neighboring node, and selecting the node with the lowest cost for expansion repeatedly until the goal is reached, producing the final path. Since each node on the path has minimal cost, the total path cost is guaranteed to be minimal. The evaluation function $f\left(n\right)$ of A* is defined as

$$f\left(n\right)=g\left(n\right)+h\left(n\right)$$

(13)

where $f\left(n\right)$ denotes the evaluation function from the start point through any node $n$ to the goal; $g\left(n\right)$ denotes the actual cost from the start to node $n$; $h\left(n\right)$ denotes the estimated cost from node $n$ to the goal.

When $g\left(n\right)=0$, the evaluation function $f\left(n\right)$ reduces to estimating the cost $h\left(n\right)$ from node n to the goal, and A* behaves as a greedy best-first search, which is fast but not necessarily optimal; when $h\left(n\right)=0$, it reduces to computing the actual cost $g\left(n\right)$ from the start to node n, transforming A* into Dijkstra’s algorithm, which requires many node computations and is inefficient. During the search, A* computes both $g\left(n\right)$ and $h\left(n\right)$, assuring the best path while keeping the search efficient.

1.

Improvements to the A* Algorithm

This section introduces two optimization strategies designed to enhance both the efficiency and robustness of the A* algorithm.

• Optimization of heuristic function

For graph search problems with multiple start and multiple goal states at the task level, the conventional single-source heuristic distance cannot be directly applied. Moreover, joint angle variations and spatial safety distances have different physical dimensions, and their direct combination leads to ambiguity in the cost function. To address this issue, all heterogeneous quantities are explicitly nondimensionalized in this work. The joint-space distance is normalized using the motion range of each joint. The normalized joint configuration is defined as ${\tilde{\theta }}_l$

$${\tilde{\theta }}_l={\displaystyle \frac{{\theta }_l-{\theta }_{l,min}}{{\theta }_{l,max}-{\theta }_{l,min}}},i=1,\dots ,6$$

(14)

The normalized joint vector is expressed as ${\tilde{Q}}_n={\left[{\tilde{\theta }}_1,{\tilde{\theta }}_2,\dots ,{\tilde{\theta }}_6\right]}^{\mathrm{T}}.$ Based on this formulation, a dimensionless heuristic function suitable for multi-target search is constructed, where the geometric distance in joint-space between the current node and the goal node set is used to estimate the minimum required cost to reach a target

$$h\prime \left(n\right)=\lambda \times \underset{m\in S_{\mathit{last}}}{\min }{‖\tilde{Q}\left(n\right)-\tilde{Q}\left(m\right)‖}_2$$

(15)

where $S_{\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}$ denotes the set of all feasible nodes at the final layer, and $\lambda$ is a scaling coefficient meeting $0<\lambda <1$ to guarantee the admissibility of the heuristic function.

At this point, the overall evaluation function of the A* algorithm is modified as

$$f_1\left(n\right)=g(n)+h\prime \left(n\right)=g\left(n\right)+\lambda \times \underset{m\in S_{\mathit{last}}}{\min }{‖\tilde{Q}\left(n\right)-\tilde{Q}\left(m\right)‖}_2$$

(16)

• Add a safety-distance penalty term

To ensure safe robot operation in complex environments, the safety distance to obstacles must be considered during pose optimization. Trajectories generated by the conventional A* algorithm may lie very close to obstacles; therefore, a distance-based penalty term is introduced into the standard A* cost function to balance path optimality and robot safety. To ensure dimensional consistency in the cost function, the minimum robot–obstacle distance $d\left(n\right)$ is normalized using the safety threshold distance $d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}$

$$\tilde{d}\left(n\right)={\displaystyle \frac{d\left(n\right)}{d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}}}$$

(17)

The revised actual cost function is formulated using the normalized distance $\tilde{d}\left(n\right)$

$$g^{\prime }\left(n\right)=g\left(n\right)+\varphi \left(\tilde{d}\left(n\right)\right)$$

(18)

where $\varphi \left(\tilde{d}\left(n\right)\right)$ denotes the distance-penalty function, which measures the risk associated with a node being near obstacles.

The penalty function takes the form of an exponential decay, as follows:

$$\varphi \left(\tilde{d}\left(n\right)\right)=\left\{\begin{array}{c}\mathrm{e}\mathrm{x}\mathrm{p}\left(1-\tilde{d}\left(n\right)\right)\\ 0\end{array},\begin{array}{c}\tilde{d}\left(n\right)<1\\ \tilde{d}\left(n\right)\ge 1\end{array}\right.$$

(19)

Accordingly, the overall evaluation function of the improved A* algorithm is modified as

$$f_2\left(n\right)=g^{\prime }\left(n\right)+h\prime \left(n\right)=g\left(n\right)+\varphi \left(\tilde{d}\left(n\right)\right)+\lambda \times \underset{m\in S_{\mathit{last}}}{\min }{‖\tilde{Q}\left(n\right)-\tilde{Q}\left(m\right)‖}_2$$

(20)

In this formulation, all terms are nondimensional. $g\left(n\right)$ represents the actual path cost calculated using the normalized joint angles. This expression maintains mathematical consistency while balancing path optimality and safety.

Notably, even with the inclusion of the obstacle-distance penalty $\varphi \left(\tilde{d}\left(n\right)\right)$, the improved A* algorithm remains admissible. The reason is that the heuristic $h\prime \left(n\right)$ accounts only for the remaining geometric path length, while the true optimal remaining cost also incorporates a non-negative penalty term. Consequently, $h\prime \left(n\right)\le h^{\mathrm{*}}\left(n\right)$ always holds, ensuring that the algorithm finds a globally optimal solution under the comprehensive evaluation metric balancing path length and safety.

2.

Algorihm process

The flowchart of the improved A* algorithm for searching the optimal grinding posture sequence is shown in Figure 5.

Figure 5. Flowchart of searching the optimal polishing pose sequence using the improved A* algorithm.

The improved A* algorithm controls nodes in the graph using two lists: the open list and the closed list. The pseudocode is shown in Table 2.

Table 2. Pseudocode of the improved A* algorithm.

Pseudocode of the improved A* algorithm

$1.\mathrm{Add}\mathrm{a}\mathrm{virtual}\mathrm{start}\mathrm{node}s$$\mathrm{and}\mathrm{a}\mathrm{virtual}\mathrm{end}\mathrm{node}t$ $\mathrm{Connect}s$ to feasible nodes in the first layer (cost = 0) $\mathrm{Connect}t$ to feasible nodes in the last layer (cost = 0)

$2.\mathrm{Add}\mathrm{the}\mathrm{start}\mathrm{node}s$ to the open list

3. while (open list is not empty) do

$\mathrm{a}.\mathrm{Select}\mathrm{the}\mathrm{node}\mathrm{with}\mathrm{the}\mathrm{smallest}f$ from the open list as current node                 b. Move current node from open list to closed list                 c. for each neighboring node of current node do

i. if (neighbor is inaccessible) OR (neighbor is in closed list) then                              skip this neighbor                         end if

ii. if (neighbor is not in open list) then                              Add neighbor to open list                              Set neighbor’s parent to current node                              $\mathrm{Store}\mathrm{neighbor}’\mathrm{s}f$$,g$$,h$ metrics                         end if

iii. if (neighbor is in open list) then                              $\mathrm{Calculate}\mathrm{the}\mathrm{new}g$ of neighbor via current node                              $\mathbf{if}(\mathrm{new}g$$<\mathrm{original}g$) then                                  Update neighbor’s parent to current node                                  $\mathrm{Update}\mathrm{neighbor}’\mathrm{s}\mathrm{g}\mathrm{and}f$                              end if                         end if

end for

$\mathrm{d}.\mathbf{if}(\mathrm{target}\mathrm{node}t$ is in open list) then                              Break the loop (path found)                       end if

end while

$4.\mathrm{Trace}\mathrm{back}\mathrm{from}\mathrm{target}\mathrm{node}\mathrm{t}\mathrm{to}\mathrm{start}\mathrm{node}s$ via parent nodes to get the optimal pose sequence return Optimal pose sequence

4. Simulation and Analysis

This paper tested the robot measurement pose optimization algorithm in a simulated environment to ensure its practicality and effectiveness. The simulation was carried out with a FANUC M-20iA six-degree-of-freedom industrial robot and an H-beam steel ship component as the object.

4.1. Parameter Selection

To simplify optimization calculations and increase search efficiency, the orientation adjustment parameter variables at each discrete measurement point along the measurement path are discretized. The robot is initialized to an initial pose rotated $135°$ about the y-axis, with a $1°$ discretization step. The orientation adjustment angle parameter $\beta$ accepts values between $-25°$ and $25°$, yielding 51 orientation adjustment angles. The polishing path has 50 discrete path points, resulting in $50\times 51=2550$ candidate nodes.

In singularity detection, the threshold for determining singular joint configurations is set as $\eta =0.01$. When the detection value is below this threshold, it is considered at singular risk and removed.

In the heuristic function improvement of the A* algorithm, to quantitatively determine the reasonable range of the scaling factor $\lambda$, this paper uses a statistical sampling method to estimate the ratio of the actual graph distance to the Euclidean distance in joint-space. The specific steps are:

1. Randomly sample 50 pairs of nodes $\left(n_i,n_j\right)$ from the feasible node set;

2. Use the Dijkstra algorithm to calculate the actual graph distance $d_G\left(n_i,n_j\right)$ between each pair of nodes;

3. Calculate the corresponding Euclidean distance $d_E\left(n_i,n_j\right)={‖q_i-q_j‖}_2$;

4. Calculate the ratio ${\rho }_{ij}=d_G\left(n_i,n_j\right)/d_E\left(n_i,n_j\right)$.

According to statistics, ${\rho }_{ij}$ is distributed in the range $\left[1.00,1.24\right]$. To ensure the admissibility condition, the scaling factor should satisfy the following

$$\lambda \le \min \left({\rho }_{ij}\right)=1.00.$$

(21)

However, considering numerical errors in inverse kinematics calculation, collision detection, and other processes in practical applications, a safety margin is required. Therefore, based on the theoretical upper bound, a $10\mathrm{\%}$ conservative factor is introduced as follows:

$$\lambda =0.9\times \min \left({\rho }_{ij}\right)=0.9.$$

(22)

Considering both efficiency and robustness, the scaling factor was finally chosen as $\lambda =0.9$.

The safety-distance penalty term accounts for positioning and trajectory-tracking errors, as well as random vibration-induced deviations. Simplified modeling of robot links and workpieces with bounding boxes also introduces geometric approximation errors. Thus, a reasonable safety-distance threshold is needed to offset these errors and maintain safety margins. In ship-component grinding, a threshold that is too small increases collision risk; one that is too large reduces the solution space and limits optimization. Considering this, $d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}=0.05$ is set for this paper.

4.2. Comparative Analysis of the Improved A* Algorithm

To verify the effectiveness of the proposed improved A* algorithm, three algorithm schemes were designed for comparison experiments on a standard HW 300 × 300 workpiece in a grinding scenario: (1) Objective function $f$: determine the starting node of the optimal path using Dijkstra’s algorithm, then plan the path with the standard A* algorithm; (2) Objective function $f_1$: apply only the multi-source heuristic improvement proposed in this study; (3) Objective function $f_2$: the complete improved scheme with both multi-source heuristic and safety distance penalty. The three schemes optimize the robot polishing pose sequence under the same experimental conditions, and their performance indicators are compared in Table 3.

Table 3. Performance comparison of three algorithm schemes on HW 300 × 300.

Objective Function	$\mathit{f}$	${\mathit{f}}_1$	${\mathit{f}}_2$
Search time (s)	0.57	0.58	0.48
Expanded Nodes	415	453	298
Joint Cost	21.85	21.85	24.32
Minimum safe distance (mm)	8.4	8.4	50.2
Average safe distance (mm)	44.8	44.8	68.2

Table 3 shows that the three algorithm approaches differ in terms of performance indicators. The standard A* algorithm $f$ picks a single ideal beginning node using Dijkstra’s algorithm. While it can swiftly generate a posture sequence at a low joint cost, it is still a single-source search with a minimum safety distance of only 8.4 mm, meaning a larger collision risk. The $f_1$ scheme, which only uses the multi-source heuristic improvement, extends the algorithm to a multi-source search, allowing optimization for multiple start and goal nodes at the same time, better reflecting the scenario in which multiple candidate poses exist at each path point in robot pose optimization. Although $f_1$ maintains the same joint cost and safety distance as standard A*, the search time increases to 0.58 s and the extended node count reaches 453. This efficiency reduction is mostly caused by the multi-source heuristic calculating the minimal distance from the current node to all target nodes, which increases per-step computation and expands the number of candidate nodes without effective pruning.

In comparison, the fully enhanced scheme $f_2$ described in this study offers significant benefits. In terms of search efficiency, $f_2$ reduces the search time by 15.8% and the number of expanded nodes by 28.2% compared with the standard A* algorithm. Compared to $f_1$, the search time is reduced by 17.2%, while the number of expanded nodes decreases by 31.4%. This improvement is due to the implementation of the safety distance penalty, which effectively limits the expansion of nodes near obstacles during the search process, thereby reducing unnecessary exploration. Second, in terms of obstacle avoidance safety, $f_2$ achieves a minimum safety distance of 50.2 mm, representing nearly a fivefold increase compared with the 8.4 mm obtained by the standard A* algorithm. Meanwhile, the average safety distance increases from 44.8 mm to 68.2 mm, corresponding to an improvement of 52.2%. Although the joint cost of $f_2$ increases slightly, this small cost increase results in a substantial enhancement in safety margin, greatly improving operational stability and reliability for robots in complicated situations.

Figure 6 shows the comparison of search paths in the directed node graph between the standard A* algorithm and the improved A* algorithm. In terms of search space exploration, the standard A* algorithm exhibits a relatively scattered expansion pattern, with the algorithm making broad attempts across nodes at each layer to find the globally optimal joint cost solution. In contrast, the improved A* algorithm has a more concentrated search path. Constrained by the safety distance penalty, high-risk nodes are excluded early, focusing the search within safe and feasible regions. This difference in search behavior reflects the trade-off between two optimization objectives: the former emphasizes joint motion optimality, while the latter balances it with operational safety.

Figure 6. Comparison of polishing paths before and after A* improvement.

Figure 7a,b show simulated robot polishing positions in 3D space prepared by the standard A* algorithm and the improved A* algorithm, respectively. Figure 7a shows that the minimal safety distance between the robot pose planned by the standard A* and the H-beam workpiece is just 8.4 mm. The robot’s arm is nearly in contact with the workpiece surface, leaving little safety buffer and a considerable danger of collision during operation. Figure 7b shows that the revised A* algorithm achieves a minimum safety distance of 50.2 mm, with the robot attitude clearly giving avoidance space relative to the workpiece, thereby significantly lowering collision risk. The comparison clearly shows that the revised algorithm successfully directs the robot to a safer operational configuration by including the safety distance constraint in the cost function.

Figure 7. Simulation at the minimum distance between the robotic arm and the workpiece. (a) Before the improved A* optimization; (b) After the improved A* optimization.

In conclusion, the improved A* algorithm proposed in this study, which integrates multi-source heuristics and a safety distance penalty, demonstrates clear improvements in both search efficiency and obstacle avoidance safety. Compared with the standard A* algorithm, the proposed method reduces search time by 15.8% and decreases the number of expanded nodes by 28.2%. In addition, the minimum safety distance is increased by nearly fivefold, while the average safety distance is improved by 52.2%, confirming the effectiveness and practicality of the proposed approach for robot pose optimization in complex constrained environments.

4.3. Robustness Analysis

To further analyze the robustness of the proposed improved A* algorithm under different task geometrical conditions, six H-beam models with distinct specifications were selected as experimental objects. These H-beams exhibit significant variations in geometric dimensions, such as flange width and web thickness, which lead to substantial changes in obstacle distribution and feasible configuration space constraints during the robotic grinding process. Therefore, they can be regarded as multiple planning environments with pronounced structural differences.

For each H-beam model, comparative experiments were conducted between the standard A* algorithm and the proposed improved algorithm under identical initial conditions and algorithmic parameter settings, ensuring the fairness and comparability of the experimental results.

Table 4 summarizes the experimental results of the proposed improved A* algorithm in terms of search efficiency and safety-related metrics across the six H-beam grinding scenarios, including search time, minimum safety distance, and average safety distance. The values in parentheses indicate the improvement ratios relative to the standard A* algorithm (performance of the standard A* is shown in Appendix B).

Table 4. Performance of the proposed method across different H-beam models.

H-Beam	Search Time(s)	Min. Safety Dist. (mm)	Avg. Safety Dist. (mm)
HW 125 × 125	1.00 (33.3%)	51.0 (27.8%)	59.0 (11.1%)
HW 150 × 150	0.56 (28.2%)	50.8 (61.8%)	56.0 (22.5%)
HW 175 × 175	0.87 (6.5%)	50.8 (16.5%)	64.7 (6.8%)
HW 200 × 200	0.72 (4.0%)	50.5 (43.9%)	61.6 (15.6%)
HW 250 × 250	1.16 (17.1%)	51.3 (109.4%)	67.4 (30.6%)
HW 350 × 350	0.40 (2.4%)	51.3 (235.3%)	73.9 (37.4%)

As shown in Table 4, although different H-beam models exhibit geometric variations, the improved algorithm demonstrates a consistent optimization trend across all test cases: the average search time is reduced by 15.3%, the average safety distance increases by 20.7%, and the minimum safety distance shows the largest average improvement of 82.5%. Notably, in complex constraint scenarios involving large-scale workpieces (e.g., HW 350 × 350), the improved algorithm achieves particularly significant improvements in safety-related metrics (exceeding 200%), demonstrating strong robustness and adaptability in narrow workspace operations.

4.4. Comparison of Joint Angle Motion Characteristics Before and After Pose Optimization

The improved A* algorithm is employed to perform a global search over the candidate pose sequence derived from the optimization model described above. Within the constraint of ensuring collision-free motion in a restricted workspace, the initial polishing pose sequence is denoted as A, while the optimized measurement pose sequence generated by the proposed method—characterized by minimal joint-angle variation—is denoted as A*. The comparative variations in robot joint angles between these two pose sequences are illustrated in Figure 8, providing a visual assessment of the optimization results.

Figure 8. Robot joint angle variations before and after optimization: (a–f) represent the angle variations of robot joint 1 to 6.

As shown in Figure 8, the original sequence A exhibits pronounced fluctuations in both joint angles and step transitions, indicating frequent and abrupt posture changes. In contrast, the optimized sequence A* demonstrates a notably smoother overall profile, with significantly reduced oscillations in joint movements. This comparison highlights the algorithm’s strong capability in trajectory smoothing under joint-variation constraints. By effectively suppressing sudden changes in robot joint configurations, the proposed method minimizes motion discontinuities and enhances the stability and continuity of robot operations.

To further evaluate the effect of the optimization algorithm, we conducted quantitative analysis of the robot joint angle variations. This included cumulative change, fluctuation amplitude, and standard deviation. The cumulative joint angle changes are shown in Figure 9. As observed, after pose optimization, the joint angle curves generally become smoother. Peak values are significantly reduced, especially for joint 4 (reduced by 92.5%) and joint 6 (reduced by 79.1%). These findings indicate the optimization algorithm’s clear advantage in suppressing large joint swings.

Figure 9. Cumulative changes of robot joint angles before and after optimization.

Meanwhile, the fluctuation amplitude and standard deviation are statistically evaluated. As shown in Table 5, their averages decrease by 41.2% and 37.8%, respectively, indicating significant improvements in joint-motion stability and trajectory smoothness.

Table 5. Comparison of robot joints before and after optimization.

Joint No.	Reduction (%)	Range of Joint Motion		STD
		A	A*	A	A*
1	3.4	75.17	72.63	29.79	28.48
2	6.7	30.85	28.79	11.39	10.45
3	16.2	55.24	46.28	20.11	16.80
4	86.5	239.66	32.35	64.73	12.80
5	68.0	61.19	19.57	18.52	6.83
6	66.2	251.99	85.28	75.13	34.35

In summary, the proposed method not only ensures collision-free motion of the robot along the measurement path within constrained spaces but also significantly improves its dynamic coordination and motion smoothness during polishing operations, providing a reliable posture-optimization strategy for precision polishing tasks under complex conditions.

5. Discussion

5.1. Summary of Contributions

The main research work of this paper includes the following three aspects:

(1) A robot posture-optimization framework was established for ship-component polishing tasks. An enhanced D–H parameter method was used to derive the forward and inverse kinematics of the FANUC M-20iA, forming an analytical relationship between end-pose and joint variables. Based on the robot’s orientation adjustment characteristics, rotation around the end-effector y-axis was selected as the orientation adjustment variable, allowing multiple joint solutions per path point and supplying ample solution space for later optimization. Furthermore, a directed-node graph was constructed, mapping the orientation adjustment postures at discrete path points into a candidate-node set. Multiple filters—including joint-limit constraints, manipulability-based singularity constraints, and GJK-based collision checking—were applied to obtain feasible posture solutions that satisfy kinematic and geometric-safety requirements, forming the foundation for global optimization.

(2) An improved A* search algorithm was proposed to systematically address multi-source search efficiency and obstacle-avoidance safety. To overcome the efficiency limitations of standard A* in multi-source and multi-target searches within high-dimensional joint-space, a multi-source heuristic based on geometric distance in joint-space was introduced. It computes the minimum Euclidean distance from the current node to the target-node set as the heuristic value, expanding A* from a single-source/single-target scheme to a multi-source/multi-target search, which aligns with the fact that each path point in posture optimization corresponds to multiple candidate postures. To compensate for the insufficient consideration of safety margins in standard A*, an exponential-decay-based safety-distance penalty was introduced. By embedding the minimum robot–obstacle distance into the cost function, the algorithm proactively avoids high-risk regions while pursuing optimal paths. Experimental results show that, compared with standard A*, the improved algorithm reduces the search time by 15.8%, increases the minimum safety distance by nearly five times (up to 50.2 mm), and improves the average safety distance by 52.2%, achieving simultaneous gains in search efficiency and obstacle-avoidance safety while maintaining global optimality.

(3) A posture-sequence optimization model was formulated to minimize joint-angle changes, markedly enhancing the smoothness of robot motion. To address the severe joint-angle variations that may occur when directly using multiple feasible postures, a mathematical model was formulated that minimizes the cumulative joint-angle deviation between adjacent path points. The improved A* algorithm performs global search on the directed-node graph to obtain an optimal posture sequence that balances motion smoothness, constraint satisfaction, and collision avoidance. Compared with the unoptimized posture sequence, the optimized sequence reduces the maximum cumulative joint-angle variation by up to 92.5%, narrows the variation range of the six joints by an average of 41.2%, and decreases the fluctuation standard deviation by an average of 37.8%. These improvements effectively eliminate joint-angle jumps and drastic oscillations, greatly enhancing continuity, stability, and trajectory smoothness during polishing.

5.2. Limitations

Although the proposed method performs well in simulation experiments, several limitations remain and require further improvements in future work:

(1) The selection of rotation about the end-effector y-axis as the sole orientation adjustment variable in this study is based on a comprehensive consideration of grinding process constraints and collision avoidance effectiveness. Although this strategy theoretically reduces the feasible solution space compared with a full orientation space search $(R_x,R_y,R_z)$, it eliminates a large number of invalid solutions that either violate process requirements (e.g., excessive side tilting) or contribute little to collision avoidance (e.g., in-place spinning). As a result, the A* algorithm is able to identify a globally optimal sequence within a limited computational time. Future work may explore multi-degree-of-freedom coordinated optimization methods for machining scenarios that require higher flexibility in tool orientation.

(2) The proposed algorithm performs offline path planning based on a static environment, assuming that the positions of the workpiece and obstacles remain fixed. However, in practical ship-component polishing, dynamic factors such as workpiece positioning errors, fixture interference, or the presence of other robots or operators in the workspace may occur, making offline planned trajectories potentially invalid. Future work could integrate the method with dynamic obstacle avoidance algorithms, introducing local path re-planning or real-time adjustments based on sensor feedback to enhance adaptability to dynamic environments.

6. Conclusions

The grinding environment of ship components is complex and spatially constrained. Robots face multiple challenges during operation, including posture singularities, motion discontinuities, and collision risks, which significantly hinder the adoption of automated grinding. To address these issues, this paper proposes a robot polishing pose optimization method based on an improved A* algorithm. Using a systematic posture-planning framework and efficient search strategies, the method achieves smooth, collision-free robot motion in constrained spaces.

Simulation results show that the proposed method significantly improves robot operational safety and motion smoothness while preserving global optimality. Compared with conventional methods, the improved algorithm reduces the search time by 15.8%, decreases the number of expanded nodes by 28.2%, and increases the minimum safety distance by nearly five times (up to 50.2 mm). In addition, the optimized joint angle sequence eliminates step-like discontinuities, resulting in a substantial improvement in motion smoothness. This study provides effective theoretical foundations and technical support for automated grinding of large and complex components.

Future work will focus on (1) incorporating dynamic obstacle avoidance for real-time applications; (2) developing time-optimal trajectory planning methods that consider robot dynamic constraints, further optimizing velocity, acceleration, and jerk profiles based on pose optimization to improve overall operational efficiency; and (3) conducting physical grinding experiments to further validate the surface quality.

Appendix A.1. Robot Forward Kinematics

The elements of the matrix ${}_6{}^0\mathit{T}$ can be determined as follows:

$$\left\{\begin{array}{c}{n}_x=c_6\left[s_5(c_1{c}_2{c}_3-c_1{s}_2{s}_3)+c_5(c_1{c}_2{s}_3{c}_4+c_1{s}_2{c}_3{c}_4+s_1{s}_4)\right]-s_6\left[s_4(c_1{c}_2{s}_3+c_1{s}_2{c}_3)-s_1{c}_4\right]\\ n_y=c_6\left[s_5(s_1{c}_2{c}_3-s_1{s}_2{s}_3)+c_5(s_1{c}_2{s}_3{c}_4+s_1{s}_2{c}_3{c}_4-c_1{s}_4)\right]-s_6\left[s_4(s_1{c}_2{s}_3+s_1{s}_2{c}_3)+c_1{c}_4\right]\\ n_z=-c_6(s_{23}{s}_5-c_{23}{c}_4{c}_5)-c_{23}{s}_4{s}_6\\ o_x=-s_6\left[s_5(c_1{c}_2{c}_3-c_1{s}_2{s}_3)+c_5(c_1{c}_2{s}_3{c}_4+c_1{s}_2{c}_3{c}_4+s_1{s}_4)\right]-c_6\left[s_4(c_1{c}_2{s}_3+c_1{s}_2{c}_3)-s_1{c}_4\right]\\ o_y=-s_6\left[s_5(s_1{c}_2{c}_3-s_1{s}_2{s}_3)+c_5(s_1{c}_2{s}_3{c}_4+s_1{s}_2{c}_3{c}_4-c_1{s}_4)\right]-c_6\left[s_4(s_1{c}_2{s}_3+s_1{s}_2{c}_3)+c_1{c}_4\right]\\ o_z=s_6(s_{23}{s}_5-c_{23}{c}_4{c}_5)-c_{23}{s}_4{c}_6\\ a_x=c_5(c_1{c}_2{c}_3-c_1{s}_2{s}_3)-s_5\left[c_4(c_1{c}_2{s}_3+c_1{s}_2{c}_3)+s_1{s}_4\right]\\ a_y=c_5(s_1{c}_2{c}_3-s_1{s}_2{s}_3)-s_5\left[c_4(s_1{c}_2{s}_3+s_1{s}_2{c}_3)-c_1{s}_4\right]\\ a_z=-s_{23}{c}_5-c_{23}{c}_4{s}_5\\ w_x=c_1(a_1+d_4{c}_{23}+a_3{s}_{23}+a_2{s}_2)\\ w_y=s_1(a_1+d_4{c}_{23}+a_3{s}_{23}+a_2{s}_2)\\ w_z=a_3{c}_{23}-d_4{s}_{23}+a_2{c}_2\end{array}\right..$$

(A1)

For conciseness, $\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_i$ and $\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_i$ are denoted as $s_i$ and $c_i$, while $\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_i+{\theta }_j\right)$ and $\mathrm{s}\mathrm{i}\mathrm{n}\left({\theta }_i+{\theta }_j\right)$ are denoted as $c_{ij}$ and $s_{ij}$.

Appendix A.2. Robot Inverse Kinematics

1.

Solve for ${\theta }_1$

Equation (A2) is derived from Equation (A1)

$${\theta }_1=\pm \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2(w_y,w_x),$$

(A2)

where ${\theta }_1$ has two possible solutions, corresponding to the positive and negative signs.

2.

Solve for ${\theta }_2$

From Equation (A1), it follows that

$${\displaystyle \frac{w_x}{c_1}}-a_1=d_4{c}_{23}+a_3{s}_{23}+a_2{s}_2=k_1\Rightarrow k_1-a_2{s}_2=a_3{s}_{23}+d_4{c}_{23},$$

(A3)

$$w_z=a_3{c}_{23}-d_4{s}_{23}+a_2{c}_2=k_3\Rightarrow k_3-a_2{c}_2=a_3{c}_{23}-d_4{s}_{23}$$

(A4)

Squaring Equations (A3) and (A4) and then summing them yields

$$k_1{s}_2+k_3{c}_2={\displaystyle \frac{k_1^2+k_3^2+a_2^2-a_3^2-d_4^2}{2a_2}}=K$$

(A5)

This paper defines $k_1=\rho \mathrm{c}\mathrm{o}\mathrm{s}\varphi ;k_3=\rho \mathrm{s}\mathrm{i}\mathrm{n}\varphi$ using trigonometric substitution

$$\begin{array}{c}\mathrm{s}\mathrm{i}\mathrm{n}({\theta }_2+\varphi )={\displaystyle \frac{K}{\rho }};\mathrm{c}\mathrm{o}\mathrm{s}({\theta }_2+\varphi )=\pm \sqrt{1-{\displaystyle \frac{K^2}{{\rho }^2}}}\\ {\theta }_2+\varphi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2({\displaystyle \frac{K}{\rho }},\pm \sqrt{1-{\displaystyle \frac{K^2}{{\rho }^2}}}),\\ {\theta }_2=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2({\displaystyle \frac{K}{\rho }},\pm \sqrt{1-{\displaystyle \frac{K^2}{{\rho }^2}}})-\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2(k_3,k_1)\end{array}$$

(A6)

where $\rho =\sqrt{k_1^2+k_3^2};\varphi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(k_3,k_1\right)$.

3.

Solve for ${\theta }_3$

From Equations (A3) and (A4), we obtain

$$\begin{array}{c}\left\{\begin{array}{c}{s}_{23}={\displaystyle \frac{a_3{l}_1-d_4{l}_3}{a_3^2+d_4^2}}\\ c_{23}={\displaystyle \frac{a_3{l}_3+d_4{l}_1}{a_3^2+d_4^2}},\end{array}\right.\\ \Rightarrow {\theta }_3=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(s_{23},c_{23}\right)-{\theta }_2\end{array}$$

(A7)

where $l_1=k_1-a_2{s}_2=a_3{s}_{23}+d_4{c}_{23};l_3=k_3-a_2{c}_2=a_3{c}_{23}-d_4{s}_{23}$

4.

Solve for ${\theta }_5$

By left-multiplying both sides of Equation (2) with the inverse transformation ${{}_{\mathbf{3}}{}^{\mathbf{0}}\mathrm{T}}^{-\mathbf{1}}$, we obtain

$${{}_{\mathbf{3}}{}^{\mathbf{0}}\mathit{T}}^{-1}{}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}={}_{\mathbf{4}}{}^{\mathbf{3}}\mathit{T}{}_{\mathbf{5}}{}^{\mathbf{4}}\mathit{T}{}_{\mathbf{6}}{}^{\mathbf{5}}\mathit{T}$$

(A8)

In Equation (A8), the (2, 3) elements of both matrices are equal, which gives

$$\begin{array}{c}{c}_5=a_x{c}_1{c}_{23}-a_z{s}_{23}+a_y{s}_1{c}_{23}\\ \Rightarrow {\theta }_5=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(\pm \sqrt{1-c_5^2},c_5\right)\end{array}$$

(A9)

5.

Solve for ${\theta }_4$

In Equation (A8), the (1, 3) and (3, 3) elements of both matrices are equal, which gives

$$\begin{array}{c}\left\{\begin{array}{c}-c_4{s}_5=a_x{c}_1{s}_{23}+a_z{c}_{23}+a_y{s}_1{s}_{23}\\ s_4{s}_5=a_y{c}_1-a_x{s}_1\end{array}\right.\\ \Rightarrow \tan \left({\theta }_4\right)={\displaystyle \frac{a_x{s}_1-a_y{c}_1}{a_x{c}_1{s}_{23}+a_z{c}_{23}+a_y{s}_1{s}_{23}}}\\ \Rightarrow {\theta }_4=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(a_x{s}_1-a_y{c}_1,a_x{c}_1{c}_{23}+a_z{c}_{23}+a_y{s}_1{s}_{23}\right)\end{array}$$

(A10)

6.

Solve for ${\theta }_6$

In Equation (A8), the (2, 1) and (2, 2) elements of both matrices are equal, which gives

$$\begin{array}{l}\left\{\begin{array}{c}{c}_6{s}_5=n_x{c}_1{c}_{23}-n_z{s}_{23}+n_y{s}_1{c}_{23}\\ -s_5{s}_6=o_x{c}_1{c}_{23}-o_z{s}_{23}+o_y{s}_1{c}_{23}\end{array}\right.\\ \Rightarrow \tan \left({\theta }_6\right)={\displaystyle \frac{-o_x{c}_1{c}_{23}+o_z{s}_{23}-o_y{s}_1{c}_{23}}{n_x{c}_1{c}_{23}-n_z{s}_{23}+n_y{s}_1{c}_{23}}}\\ \Rightarrow {\theta }_6=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(-o_x{c}_1{c}_{23}+o_z{s}_{23}-o_y{s}_1{c}_{23},n_x{c}_1{c}_{23}-n_z{s}_{23}+n_y{s}_1{c}_{23}\right)\end{array}$$

(A11)

When the robot end-effector transformation matrix ${}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}$ is known, the corresponding multiple sets of joint angles can be obtained by the above formulas.