A distributed variable density path search and simplification method for industrial manipulators with end‐effector’s attitude constraints＊#

Jin WANG ,Shengjie LI ,Haiyun ZHANG ,Guodong LU ,Yichang FENG ,Peng WANG ,Jituo LI

1State Key Laboratory of Fluid Power and Mechatronic Systems,School of Mechanical Engineering,Zhejiang University,Hangzhou 310027,China

2Engineering Research Center for Design Engineering and Digital Twin of Zhejiang Province,School of Mechanical Engineering,Zhejiang University,Hangzhou 310027,China

3School of Robotics,Ningbo University of Technology,Ningbo 315211,China

Abstract: In many robot operation scenarios,the end-effector’s attitude constraints of movement are indispensable for the task process,such as robotic welding,spraying,handling,and stacking.Meanwhile,the inverse kinematics,collision detection,and space search are involved in the path planning procedure under attitude constraints,making it difficult to achieve satisfactory efficiency and effectiveness in practice.To address these problems,we propose a distributed variable density path planning method with attitude constraints (DVDP-AC) for industrial robots.First,a position–attitude constraints reconstruction (PACR) approach is proposed in the inverse kinematic solution.Then,the distributed signed-distance-field (DSDF) model with single-step safety sphere (SSS) is designed to improve the efficiency of collision detection.Based on this,the variable density path search method is adopted in the Cartesian space.Furthermore,a novel forward sequential path simplification (FSPS) approach is proposed to adaptively eliminate redundant path points considering path accessibility.Finally,experimental results verify the performance and effectiveness of the proposed DVDP-AC method under end-effector’s attitude constraints,and its characteristics and advantages are demonstrated by comparison with current mainstream path planning methods.

Key words: Path planning;Industrial robots;Distributed signed-distance-field;Attitude constraints;Path simplification

1 Introduction

Autonomous path planning of industrial robots is of great significance for improving their ease of use and making them meet the requirements of dif‐ferent tasks.When considering task requirements and constraints,the path planning of industrial robots in real applications becomes more complicated and chal‐lenging.First,the path planning of mobile robots is generally carried out in the two-dimensional space,but the search space of six-degree-of-freedom (6-DOF) industrial robots is three-dimensional or higher.Sec‐ond,the constraint relationship between the links must be considered,and the whole robot cannot be regarded as a point,so collision detection is much more complex.In addition,the inverse kinematic solution of the robot will cause additional computation costs.This study will focus on efficient path planning in the Cartesian space.

Collision-free path planning has been widely used in automatic driving,unmanned aerial vehicle (UAV) navigation,game development,and other fields,and some classical theories and algorithms have been pro‐posed,e.g.,A* (Hart et al.,1968),rapidly-exploring random tree (RRT) (LaValle,1998),and Dijkstra’s algorithm (Dijkstra,1959).On this basis,some new algorithms have been derived.Koenig and Likhachev (2005) proposed the D* lite algorithm based on the lifelong planning A* (LPA*) (Koenig et al.,2004) al‐gorithm.Its main feature is incremental search,and the current position is regarded as the starting posi‐tion after the environment changes,so as to ensure that the path is the shortest.On this basis,Sun et al.(2010) proposed an improved D* lite algorithm for moving targets,which makes up for the deficiency of the D* algorithm in moving target search.Grid-based search in the Cartesian space is a feasible method,but the traditional grid-based search limits the search direction of each step.Ferguson and Stentz (2006) therefore proposed the field D* algorithm,which ad‐justs the search direction based on linear interpola‐tion and solves the problem of an excessive number of grids with multi-resolution grids.Inspired by the RRT algorithm,Persson and Sharf (2014) combined the classic A* algorithm with sampling and proposed sampling-based A* (SBA*),which improves the plan‐ning success rate.However,this method still requires many samples to ensure the planning success.Collisionfree path planning based on artificial potential field methods is also an important direction (Adeli et al.,2011;Baziyad et al.,2021).

Based on the basic algorithm research and con‐sidering the characteristics of the industrial robot path planning problem,some related studies have been carried out.Ademovic and Lacevic (2014) proposed an evolutionary algorithm based on trees of bubbles of free C-space.This algorithm improves the reliabil‐ity,but the redundant path is long.Fu et al.(2018) improved pre-and post-processing based on SBA*.It could improve the planning efficiency and solve the problem of redundant path points,but the plan‐ning efficiency may be reduced when post-processing does not work.Xie et al.(2020) found the defect of the distorted configuration space (DCS) method and pre‐sented an optimized IDCS method to shorten the gen‐erated path,but this method is not available in some cases.Qureshi and Ayaz (2016) combined RRT* with an artificial potential field to reduce memory utiliza‐tion and improve the convergence rate.In view of the impact of collision detection on planning effi‐ciency,Han et al.(2018) proposed an adaptive dis‐crete collision detection method,which adjusts the collision model in the Cartesian space according to the distance between robots and obstacles to improve efficiency and accuracy.Hernández et al.(2014) pro‐posed an improved adaptive A* (AA*) algorithm,named multipath adaptive A* (MPAA*),which can reuse paths found by previous A* searches.The path is proved to be optimal,and the search is carried out in the Cartesian space.Abele et al.(2016) defined mul‐tiple working points in the Cartesian space,solved the traveling salesman problem based on the A* algo‐rithm,and realized time-optimal path point sequence planning.

To sum up,there are three main factors that af‐fect planning efficiency: inverse kinematics,colli‐sion detection,and planning algorithm design.The current path planning research for industrial robots confronts the following problems in three aspects:

End-effector’s attitude control: Most of the cur‐rent mainstream motion planning algorithms,such as covariant Hamiltonian optimization for motion plan‐ning (CHOMP) (Zucker et al.,2013),RRT-connect (Kuffner and LaValle,2000),and stochastic trajectory optimization for motion planning (STOMP) (Kal‐akrishnan et al.,2011),are solved in the configura‐tion space,which makes it difficult to directly control the attitude of the end-effector.To cope with endeffector’s attitude constraints in the Cartesian space,the conventional inverse kinematic analytical method is generally complicated and will generate multiple solu‐tions,and the selection of solutions is time-consuming.

Excessive collision detection: Collision detection of industrial robots has a high time complexity.Besides,to ensure that the continuous trajectory is reachable,it will be repeated in the planning process,resulting in a large computational load.

Unnecessary twists: The grid-based path search will generate unnecessary twists in the path,and a significant number of inverse kinematic solutions and collision detections are needed to perform path simplification to remove redundant path points,which has a great impact on efficiency.

In this paper,a variable density path-planning and simplification method based on a distributed signeddistance-field (DSDF) is proposed.The major work and contributions of this study are as follows:

Inverse kinematics: In tasks that require attitude constraints,accurate positioning is needed only at the starting and ending points,but not in the process of motion.Therefore,a position–attitude constraints re‐construction (PACR) inverse kinematic solution ap‐proach is proposed,which can satisfy the attitude con‐straints in the intermediate process and improve the efficiency of solving inverse kinematics.

Collision detection: For efficient collision detec‐tion,the models of DSDF and single-step safety sphere (SSS) are proposed.DSDF solves the problem of low reconstruction efficiency of the original SDF,and SSS can help reduce the number of collision detections during single-step search and path simplification.

Path simplification: Aiming at the inherent de‐fect of grid-based search,a novel forward sequential path simplification approach (FSPS) is proposed to eliminate the redundant path points and speed up the process of path simplification.

2 Problem statement

We consider the handling task of a 6-DOF in‐dustrial robot with attitude constraints,which is very common in robot applications,such as industrial sce‐narios and scientific research,as shown in Fig.1.

Fig.1 Robot handling scenarios with attitude constraints: (a) material handling;(b) scientific research

In a material handling task,the robot motion planning needs to meet the following requirements:

1.The robot needs to be precisely positioned at the starting and ending points,so as to grasp and release materials with the appropriate position and attitude (PA).

2.Obstacle avoidance constraints and joint limita‐tions should be considered in the intermediate move‐ment process,which is the premise of the feasible path.

3.Since the objects being moved are not always in a closed state,as shown in Fig.2,it is necessary to ensure that the material will not overturn or fall dur‐ing the movement,so attitude constraints for the endeffector are essential.

Fig.2 Material handling without form closure

4.Unlike processing tasks,the intermediate pro‐cess of handling tasks does not require precise posi‐tioning,which provides more possibilities for improv‐ing the efficiency of the planning algorithm.

For the above requirements,the attitude of the end-effector can be directly controlled based on inverse kinematics,which requires the construction and analy‐sis of the kinematic model of the robot.According to the Denavit–Hartenberg (DH) parameters shown in Table 1,the robot kinematic model is defined (Fig.3).

Fig.3 Kinematic model of industrial robots

Table 1 Denavit–Hartenberg (DH) parameters of 6‐DOF industrial robots

The path planning in the Cartesian space can be regarded as the planning of the PA sequence of the manipulator’s end-effector.The PA of the manipula‐tor can be described by a homogeneous matrix:

where the rotation matrix in the left dashed box de‐fines the attitude of the end-effector,and the position vector in the right dashed box defines the position of the end-effector.According to the requirements of the transportation task analyzed above,two assump‐tions are proposed:

1.Precise control of PA is necessary at the start‐ing and ending points of the trajectory,while attitude control accuracy is more important than position control accuracy in the process of motion to avoid collision.

2.All the obstacles in the scene are rigid bodies.

Based on the above assumptions,the algorithm framework and details of this study are proposed and detailed in the following sections.

3 Methodology

3.1 Overview of DVDP‐AC

The framework of the distributed variable density path planning method with attitude constraints (DVDP-AC) is inspired by D* lite.D* lite is a path planning algorithm with incremental search.It can use the information obtained from previous search steps to improve the efficiency of the subsequent planning (Koenig and Likhachev,2005).

However,due to the limitations of grid-based search,the original D* lite suffers from two draw‐backs.First,planning time increases dramatically as the number of grid cells increases.Second,the paths often have unnecessary twists,which makes the paths longer than desired.In addition,although great achieve‐ments have been made in the research of collision de‐tection,the conventional collision-free path planning methods still spend most of their calculation time on collision detection.

Considering the above problems and characteris‐tics of industrial robots,the framework of DVDP-AC is constructed,and the kinematic solution,collision detection,search method,and path simplification method are all improved.The steps are as follows:

1.Initialize the orthogonal variable density search space and collision models.

2.The improved kinematic model,collision mod‐els,and search space are used for path planning in the initial state.

3.If the robot does not reach the goal cell,search for the node with the lowest cost,then update the robot position,and the node information is updated according to the situation of obstacles.

4.If the robot reaches the goal cell,post-process the obtained path (path simplification) based on FSPS.

In Fig.4,the yellow box represents the improve‐ments adopted,which will be elaborated in the follow‐ing subsections.

Fig.4 Framework of DVDP‐AC (References to color refer to the online version of this figure)

3.2 PACR inverse kinematics

At present,most industrial robots satisfy the Pieper criterion (Liu HS et al.,2015;Liu YY et al.,2021),which enables the inverse kinematic analyti‐cal solution of the robot.However,solving the in‐verse kinematics of a 6-DOF robot is still a complex problem,and since there are multiple solutions,it is necessary to consider how to choose the optimal one.In the planning process,inverse kinematics needs to be solved many times,so it is helpful to improve the efficiency.For a robot satisfying the Pieper criterion,it is generally believed that the end position is con‐trolled mainly by the first three joints,and the end at‐titude is controlled mainly by the last three joints.Based on this characteristic,the PACR inverse kine‐matic solution method is proposed.

3.2.1 Deconstruction and reconstruction of the homogeneous position and attitude matrix

In general,path planning in the Cartesian space involves planning the PAs of the end-effector,includ‐ing solving the robot configuration based on inverse kinematics and carrying out collision detection.Solv‐ing the inverse kinematics of a 6-DOF robot requires computing multiple solutions and selecting the opti‐mal one.To improve the planning efficiency of inter‐mediate path points,it is necessary to consider how inverse kinematics is applied in path planning.

As mentioned above,the end-effector homoge‐neous matrix contains attitude information and posi‐tion information,and the terminal PA sequence of the intermediate path points describes the movement pro‐cess of the manipulator.However,according to the assumption described in Section 2,the end position in the intermediate process does not require high pre‐cision,and only the PAs of the starting point and the ending point need to be highly accurate.Therefore,for the starting and ending points of the path,the in‐verse kinematics needs to be solved by the traditional method,while for the intermediate path points,the homogeneous matrix can be reconstructed,and the reconstructed matrix can be used to describe the movement process of the manipulator:

whereθ4,θ5,andθ6are the last three joint angles.

For the above PA constraints,a novel PACR in‐verse kinematic solution method is proposed.Specifi‐cally,the robot applicable to the PACR method should conform to the Pieper criterion,and its structure and DH parameters are shown in Fig.3 and Table 1,re‐spectively.The principles of PACR are explained in detail below.

3.2.2 PACR: solving the first three joint angles based on the wrist position constraint

Since the wrist coordinates (x,y,z) are known during the search,the solution process of the first three joints can be greatly simplified and complex matrix operations can be saved.In other words,it can be solved with a geometric method.Note that there are two sets of the first three joint angles that allow the wrist to reach the same position,as shown in Fig.5.However,considering the practical situation,the con‐figuration of Fig.5b is generally not adopted.There‐fore,only the calculation method for the configura‐tion of Fig.5a is given.

Fig.5 Two configurations corresponding to the same position: (a) common situation;(b) special situation

The first three joint angles are calculated as follows:

In Eqs.(8) and (9),AtoEare defined as shown in Fig.5a,andExandEyare components ofEalongxandyaxes,respectively.The calculation formulae ofAtoEare shown in Table 2.In addition,considering the initial value of the second joint,π/2 should be added whenθ2is involved in subsequent steps.

Table 2 Brief introduction of each group

3.2.3 PACR: solving the last three joint angles based on the attitude constraint of the end-effector

According to the attitude of the end-effector of the starting and ending points,the attitude interpola‐tion can be carried out in the form of a quaternion,and then the quaternion can be converted into a ma‐trix form to solve the angles of the last three joints.

Since the angles of the first three joints are known,the homogeneous matrix of these joints can be calculated according to

Similar to the second joint,-π/2 should be added whenθ5is involved in subsequent steps.

Fig.6 Transformation matrices of the joints

Since the motion process is required to be con‐tinuous,it is feasible and necessary to select the solution in advance when calculatingθ1andθ5.The multi-solution problem can be solved by using a pre-selection solution and posing a constraint separa‐tion,because other solutions will cause an abrupt atti‐tude change of the end-effector,resulting in the mate‐rial falling in the process of transportation.The pro‐posed PACR method not only meets the practical re‐quirement,but also reduces the calculation needed to find solutions and select the optimal one.

3.3 Collision detection acceleration based on DSDF and SSS

Collision detection takes up much time in the planning process.In recent years,collision detection methods based on triangular mesh have been continu‐ously improved.For example,collision detection based on bounding volume hierarchy (BVH) mesh screening is the current mainstream (Li et al.,2018;Tan et al.,2020).In addition,for path planning,there is a more efficient collision detection approach,which is based on SDF.Generally,the sphere-chain collision model is built for robots and SDF is built for the surrounding environment:

1.The original model of the robot is abstracted into a collision model formed by a sphere chain.The initial sphere chain is generated based on an oriented bounding box (Gottschalk et al.,1996),and the radii and center coordinates of all balls are adjusted based on the genetic algorithm (Harik et al.,1999) to mini‐mize the total volume of all bounding spheres.The value ofCccan be defined according to the radius of the smallest sphere of the sphere chain.

2.Collision models of obstacles are constructed based on SDF.SDF consists of a series of sampling point clouds,and each point of SDF has a signed number,whose absolute value is the shortest distance from the obstacles.If the sampling point is outside the obstacle,the signed number is positive;otherwise,it is negative.In this way,collision detection can be carried out quickly by comparing the value of the sampling point with the radius of the collision sphere.

An obvious problem with traditional SDF is that it needs to be rebuilt when the surrounding obstacles change,and the reconstruction process is often time-consuming,making SDF-based collision detection difficult to apply in a dynamic scenario.To solve this problem,the concept of DSDF is proposed,based on the second assumption in Section 2.The principle is that DSDF is constructed in advance for the obstacle,and when the obstacle moves or rotates,its DSDF moves and rotates accordingly.Because the obstacles are rigid bodies,complex reconstruction is avoided.

The range of DSDF is enlarged bydeon the basis of axis-aligned bounding boxes (AABB) (Xing et al.,2010).deis the radius of the maximum sphere of the moving parts,as shown in Fig.7.The collision models of robot and obstacle are shown in Fig.8.Some papers have proposed the method of construct‐ing SDF (Huo et al.,2014;Klingensmith et al.,2015),so the construction process will not be repeated here.

Fig.7 Scope of DSDF for a single obstacle

Fig.8 The robot collision model based on sphere chain (a) and obstacle collision model based on DSDF (b)

In this paper,the collision detection of a robot is carried out based on its collision spheres.First,it is determined whether the center of the collision sphere is within the sampling range of DSDF.If not,the robot is deemed to be collision-free;if so,the coordi‐nates of the nearest point from the center of the ball can be found with

where (xc,yc,zc) is the center of the bounding ball,(xs,ys,zs) is the sampling point closest to the center of the ball,(xmin,ymin,zmin) is the point with the small‐est coordinate values in all directions among the sam‐pling points of DSDF,anddiis the sampling interval.If the signed number of the sampling point is greater than the radius of the bounding box,it is deemed to be a collision.

In addition,to improve the efficiency of singlestep search and reduce the computation of continuous collision detection,a safety zone model named SSS is proposed,as shown in Fig.9.

Fig.9 Diagram of safe state determination

In Fig.9,A is the collision sphere before move‐ment,B is the collision sphere after movement,dis the displacement,RSNis the signed value of the sam‐pling point closest to the center of the collision sphere,and it is also the radius of the SSS.The prin‐ciple of continuous collision detection acceleration is to directly judge whether the single-step movement is collision-free,by evaluating whether the sweep area of the collision sphere is covered by SSS.If the single-step sweep area of the collision ball cannot be covered by SSS,continuous collision detection is required.

To ensure accurate judgment,there are two spe‐cial cases that need to be considered,as shown in Fig.10.Fig.10a presents the situation in which the collision sphere breaks through the SSS at the end of the single step,and Fig.10b presents the situation in which the collision sphere breaks through the SSS during the intermediate stage of the single step.In Fig.10,the green line is the path of the end flange,and the blue curve is the moving track of the center of the elbow collision sphere.

Fig.10 The unsafe state at the end (a) and intermediate (b) segments of single‐step movement (References to color refer to the online version of this figure)

For the first case,it can be inferred from inequal‐ity (21) whether the motion trend of the sphere at the end segment is safe:

wheredn-1represents the distance between the penul‐timate interpolation point of the single-step trajectory and the center of the sphere in state A.If the above formula does not hold,continuous interpolation is required.

For the second case,Algorithm 1 can be used to deduce whether the intermediate stage is safe.

With the exception of the above two cases,most cases are shown in Fig.11,in which the single-step movement of wrist is linear and the step size is small.Then the single-step safety state can be determined by

Fig.11 The safe state of single‐step movement (a) and corresponding motion process in general (b)

If inequality (22) is true,the single-step search is safe.

Specifically,for DSDF,the collision sphere is safe under the following conditions:

Algorithm 1Safety state discrimination based on DSDF-SSS

1.The sphere is outside DSDF and the distance from DSDF is greater thanR.

2.Before the single-step motion,the sphere is in DSDF and the signed number of the sampling point closest to its center is greater than the safe distance (d+R).

3.4 Variable density path‐planning

In robot working scenarios,the distribution of obstacles is usually uneven.That is to say,there are densely distributed areas and sparsely distributed areas.To ensure security,it is necessary to increase the mesh density of the search space.However,for the area where obstacles are sparsely distributed,an overdense grid will reduce the search efficiency.Therefore,in this study we adopt a search space generation method that adaptively adjusts the mesh density according to the distribution of obstacles.Its principle is intro‐duced below.

The octree is generated based on the robot work‐space and the search space is initially established.When the manipulator is close to the obstacles,more precise planning is needed to ensure that collision is avoided.Therefore,the collision clearanceCcand the minimum scale in the planning process should first be defined.Then the AABB of the obstacles is con‐structed which can delineate the area of an obstacle at a low computational cost.The grid cells intersecting with AABB should be further refined until the size of the leaf node is not greater than the collision clear‐anceCc,as shown in Fig.12.

Fig.12 Variable density grid based on octree

The subdivided area is called the intensive area,and the grid cells without collision are defined as safe area cells.Thus,the construction of the variable density search space is preliminarily completed.In the subsequent planning process,if a collision occurs in the unrefined areas,the robot will return to the state of the previous step and search after the mesh is refined.Due to the presence of variable density areas,it is infeasible to search at a constant step size,so the search is carried out in a segmented way.Ac‐cording to the grid density,the search space can be divided into a safe area and an intensive area.No mat‐ter whether the next step is in the safe area or the inten‐sive area,Eq.(23) can be used to quickly calculate the location of the center point:

where (xm,ym,zm) are the coordinates after moving one step,(x'min,y'min,z'min) is the point with the small‐est coordinate values in all directions among the sampling points in the area in which (xm,ym,zm) falls,anddsis the search step size.Note that using vari‐able density search sometimes results in longer paths because large grids may lengthen local paths,as shown in Fig.13.

Fig.13 The difference of paths caused by uniform density search (a) and variable density search (b)

To solve this problem,according to the positions of the child nodes,additional path points are added to the local paths that pass through only the grid nodes of the safe area,and then the path is further simplified.In the next subsection,a novel approach will be introduced.

3.5 Forward sequential path simplification

Since the path planning in this study is based on grid-based search,there are redundant bends in the path.To achieve optimal planning results,it is neces‐sary to remove the redundant path points.Existing methods tend to start with the farthest nodes in terms of connection relationships,such as the method of Fu et al.(2018):

1.Define nodepiat the starting point.

2.Establish the local paths betweenpiand sub‐sequent non-adjacent path pointspj.

3.Find the collision-free local path which con‐tains nodepjwith the maximum subscriptj,usepi?pjto replace the corresponding segment of the initially planned path,and then assignjtoi.

4.If there is no collision-free local path,i=i+1.

5.Repeat steps 2?4 untilpjreaches the last point.

It can be seen that the simplification order in Fu et al.(2018) is from back to front,and the simplifica‐tion method proposed in this study is improved in two aspects:

First,the method of FSPS is proposed.The de‐tails are as shown in Algorithm 2.

Algorithm 2Forward sequential path simplification(FSPS)

The above process is shown in Fig.14.The red dashed lines are the local paths with collision,the green dashed lines are the local paths without colli‐sion,and the solid green lines are the local paths adopted.The novel simplification method can effectively reduce the number of redundant path points.More‐over,the time complexity isO(n) in the worst case,which is lower than that of the existing method,O(n2).In addition,this method can reduce collision detection of long local paths,such as linear paths from the starting point to the goal point.The longer the distance,the more times collision detection is per‐formed,and avoiding collision detection of long paths can further improve efficiency.Therefore,the pro‐posed method has smaller time fluctuation in different situations and can improve efficiency.In addition,the idea of variable density can be used for collision de‐tection and reachability detection,that is,to reduce the number of interpolation points of the local path in the safe area.

Fig.14 The process of forward sequential path simplifica‐tion: (a) original path;(b) finding the second path point;(c) finding the third path point;(d) finding the fourth path point;(e) finding the fifth path point;(f) completing the simplification (References to color refer to the online version of this figure)

Second,some redundant points can be elimi‐nated by checking whether they are collinear,which takes little time to reduce the number of collision de‐tections,as shown in Fig.14d.In addition,to solve the problem shown in Fig.13,a condition needs to be added: if only the collinear redundant path points are eliminated in a certain step,and the path is not short‐ened,this step of path simplification should be rejected,and the process should continue from the next point.

4 Validation and discussion

4.1 Ablation study

For a preliminary analysis of the effect of the proposed DVDP-AC method,a series of simulation cases were set up for testing.The computer used for the simulation was equipped with an AMD Ryzan 3600 CPU with 16 GB memory,whose base frequency was 3.6 GHz,and a GTX1070 GPU was used to ac‐celerate the construction of DSDF.The simulation environment was built based on C++and OpenGL in Windows 10.Six groups were set for each case to analyze the contribution of different measures,as shown in Table 3.

Table 3 Brief introduction of each group

MSVM uses the DVDP-AC method proposed in this study,and OBUO uses the original D* lite algo‐rithm (Koenig and Likhachev,2005).The simulation cases consist of a robot,a conveyor belt,and two shelves,simulating the processes of taking out the workpiece from the shelf and placing it on the con‐veyor belt,as illustrated in Fig.15.Based on this,the starting point,ending point,and positions of obsta‐cles are changed to form 10 different scenes.The cal‐culation time and the length of the cases are shown in Table 4.

Fig.15 Motion process of the simulation cases

Table 4 Performance comparison in the simulation cases

To verify the effectiveness of the proposed algo‐rithm in the real environment,tests were carried out in a static scenario (case 1) and a dynamic scenario (case 2).The setups of these two cases are shown in Fig.16.The path planning was repeated 10 times in each case to reduce the error.In case 1,there is a robot,a computer numerical control (CNC) milling machine,a couple of aluminum brackets (obstacles),and a table.The robot was ABB IRB120,required to avoid obstacles and move to the CNC milling machine (Fig.16a).In case 2,the robot was required to com‐plete loading and unloading actions.The robot was xArm 6,which does not satisfy the Pieper criterion,so the simplified kinematic model was not adopted.In this case,five groups were reset for each path,as shown in Table 5,where the meanings of the letters are the same as those in Table 3.While waiting for processing,an obstacle was added so that the manipu‐lator cannot return along the original path (Fig.16b).According to the framework of DVDP-AC,if the ob‐stacles were found to be changed,the search space will be rebuilt and the planning will be carried out again.The calculation time and path length are shown in Tables 6 and 7.

Fig.16 The setups of case 1 (a) and case 2 (b)

Table 5 Brief introduction of each group in case 2

Table 6 Performance comparison of case 1

Table 7 Performance comparison of case 2

Since four paths need to be planned in case 2,the path length in Table 7 is the sum of the four paths,and the average time,maximum time,and minimum time are all calculated based on the total planning time.In Tables 6 and 7,there is a big difference in plan‐ning time,which is related mainly to the path length,obstacle distribution,robot structure,and other factors.Among them,the path length will affect the number of search steps,the obstacle distribution will affect the time required for collision detection,and the robot structure will affect the efficiency of an inverse kine‐matic solution.In addition,as mentioned before,Table 7 shows the total planning time of the four paths,and thus the gap is further increased.

4.2 Baseline comparison

CHOMP (Zucker et al.,2013),RRT-connect (Kuffner and LaValle,2000),and Bi-Fast-Marching-Tree algorithm (BFMT*) (Starek et al.,2015) were chosen as the benchmarks to measure the performance of the proposed DVDP-AC.CHOMP and RRT-connect are both mainstream algorithms in the field of manipu‐lator motion planning.CHOMP can efficiently plan a reachable path in a configuration space based on covariant Hamiltonian optimization.RRT-connect is known for its bidirectional search,which can effec‐tively improve the efficiency of an RRT algorithm.RRT-connect can solve the feasible path not only with a high success rate in a narrow search space,but also with a high efficiency in a wide search space.BFMT* is a bidirectional version of FMT*,which can quickly solve complex motion planning problems in high-dimensional configuration spaces (Janson et al.,2015).In this study,BFMT* needs only to find variable paths,so additional optimization will be re‐duced to further improve its efficiency.The hardware conditions of this test were the same as the previous ones.The simulation environment was built based on ROS Noetic,which ran on an Ubuntu 20.04 operat‐ing system.

To demonstrate the performance of the above algorithms in terms of planning efficiency and endattitude constraints,a 6-DOF manipulator was adopted,which operated in a scene that included some obsta‐cles such as cups and tables,as shown in Fig.17.In case 3,the robot was instructed to deliver four cups to points 1 to 4 in sequence.Since cups usually contain liquid,attitude control is necessary.For the above al‐gorithms,the parameters are taken as follows.

Fig.17 The setup of case 3: (a) simulation environment;(b) experimental environment

4.2.1 DVDP-AC

The parameters of DVDP-AC and their intro‐duction are shown in Table 8.

Table 8 Brief introduction of the parameters of DVDP‐AC

4.2.2 CHOMP

The parameters of CHOMP and their introduc‐tion are shown in Table 9.

4.2.3 RRT-connect

The parameters of RRT-connect and their intro‐duction are shown in Table 10.The larger thesr,the higher the exploration efficiency.However,whensris too large,the trajectory quality and success rate will also be affected.The smaller thesr,the finer the trajectory,but the lower the exploration efficiency.Since the obstacles of case 3 are sparse,a largersrcan be adopted to improve efficiency.

Table 10 Brief introduction of the parameter of RRT‐connect

4.2.4 BFMT*

The parameters of BFMT* and their introduc‐tion are shown in Table 11.Kois used to determine the conditions for the end of the algorithm.WhenKois true,the algorithm ends when the best path is found;otherwise,the algorithm ends when the first viable path is found.

Table 11 Brief introduction of the parameters of BFMT＊

To enhance the reliability of the conclusions,the planning of each path was repeated 10 times,and the experimental data were recorded and processed,as shown in Table 12.According to Table 12,the DVDP-AC method showed high efficiency under vari‐able and known obstacle distribution,and performed better in both planning efficiency and end-attitude controllability.In the process of planning the above four trajectories,the DVDP-AC algorithm reduced the average time by more than 39% compared with CHOMP.Specifically,in the scenario of the first path,DVDP-AC saved more time because of the sparse dis‐tribution of obstacles,finding the feasible path within 40 ms,while CHOMP took more than 100 ms.Com‐pared with RRT-connect and BFMT*,DVDP-AC can maintain a significant efficiency advantage,reducing the average planning time by 8% to 38%.What is more,by observing the motion process,it can be found that the algorithms of the three control groups failed to maintain the proper attitude of the end-effector,as shown in Fig.18.

Table 12 Comparison of planning time

Fig.18 Effect comparison among four methods of the attitude constraint: (a) DVDP‐AC;(b) CHOMP;(c) RRT‐connect;(d) BFMT＊

4.3 Discussion

According to the above experimental results,in all test scenarios,the above improvement measures (DSDF,PACR,and FSPS) can improve the planning efficiency.The effects of the measures are discussed in detail below.

4.3.1 DSDF-based collision detection

Based on DSDF and SSS,DVDP-AC can quickly determine whether the local path is safe,which greatly reduces the number of collision detections,thus effectively improving the overall efficiency.

According to Tables 4,6,and 7,in test cases,the average calculation time was reduced by 14.63% to 27.20% by adopting DSDF and SSS.The analysis re‐sults of the ablation study are shown in Fig.19.In ad‐dition,by using SSS to reduce the number of colli‐sion detections,DVDP-AC achieved an efficiency im‐provement in baseline comparison.

Fig.19 Effect of DSDF‐based detection

4.3.2 PACR inverse kinematics

As a basic improvement,PACR inverse kine‐matics improved efficiency in all scenarios.Although the original analytical solution is already efficient,the inverse kinematic solution needs to be carried out many times in path planning,so simplified inverse ki‐nematics can effectively improve the efficiency.In the test cases,the average calculation time was reduced by 7.79% to 14.68% by adopting the PACR method.The analysis results are shown in Fig.20.Moreover,in baseline comparison,although DVDP-AC requires an additional inverse kinematic solution,which af‐fects the computational efficiency,the PACR method can effectively reduce this impact.

Fig.20 Effect of PACR inverse kinematics

4.3.3 FSPS

The effects of FSPS are as follows: first,it re‐duces the computational complexity when there are few redundant path points;second,it directly elimi‐nates the collinear path points;third,it reduces the number of feasibility detections of the safety zone.Therefore,the effect is clear when there are sparse obstacles,few redundant path points,or many collin‐ear path points.In the test cases,the average calcula‐tion time can be reduced by 12.58% to 23.64% due to FSPS,as shown in Fig.21.

Fig.21 Effect of FSPS

5 Conclusions

In this paper,a distributed variable density path planning method named DVDP-AC is proposed.In our proposed method,the inverse kinematic solution,collision detection,and post-processing are modi‐fied.First,collision detection based on distributed signed-distance-field (DSDF) and a single-step safety sphere (SSS) generally improves efficiency,especially in the scenarios where obstacles are densely distributed.In the ablation study cases,the computation time is reduced by 14.63% to 27.20% due to DSDF-based collision detection.Second,the position–attitude con‐straints reconstruction (PACR) inverse kinematics can further improve efficiency while meeting the re‐quirements of the handling task.In the ablation study cases,the computational time is reduced by 7.79% to 14.68% due to PACR.Third,the adoptation of for‐ward sequential path simplification (FSPS) further improves the quality and efficiency of path simplifi‐cation.In the ablation study cases,FSPS reduces the computational time by 12.58% to 23.64%.In base‐line comparison,DVDP-AC shows advantages in effi‐ciency and attitude constraints compared with exist‐ing mainstream algorithms.

The most important contribution of this study is to realize efficient path planning with terminal attitude constraints,which have important application value in industrial robot handling,stacking,spraying,and other tasks.The proposed method can be compiled into a planning tool and combined with three-dimensional modeling techniques to become an important part of offline programming software for industrial robots.We hope to implement these in future work.

Contributors

Jin WANG designed the research.Shengjie LI,Yichang FENG,and Peng WANG processed the data.Shengjie LI drafted the paper.Jin WANG and Guodong LU helped organize the paper.Shengjie LI,Haiyun ZHANG,and Jituo LI revised and finalized the paper.

Compliance with ethics guidelines

Jin WANG,Shengjie LI,Haiyun ZHANG,Guodong LU,Yichang FENG,Peng WANG,and Jituo LI declare that they have no conflict of interest.

Data availability

The data that support the findings of this study are available from the corresponding author upon reasonable request.

List of supplementary materials

1 Collision detection based on SDF

2 Existing path simplification approach

3 Setup of simulation cases

4 Motion process in ablation study

5 Motion process in baseline comparison

Fig.S1 Schematic of collision detection based on SDF

Fig.S2 Simplification method of Fu et al.(2018)

Fig.S3 Some simulation cases and their setup

Fig.S4 Motion process of case 1

Fig.S5 Motion process of case 2

Fig.S6 Motion process comparison of case 3