Singlestep collisionfree trajectory planning of biped climbing robots in spatial trusses
 Haifei Zhu^{1},
 Yisheng Guan^{1}Email author,
 Shengjun Chen^{1},
 Manjia Su^{1} and
 Hong Zhang^{1, 2}
Received: 20 April 2015
Accepted: 10 September 2015
Published: 24 February 2016
Abstract
For a biped climbing robot with dual grippers to climb poles, trusses or trees, feasible collisionfree climbing motion is inevitable and essential. In this paper, we utilize the samplingbased algorithm, BiRRT, to plan singlestep collisionfree motion for biped climbing robots in spatial trusses. To deal with the orientation limit of a 5DoF biped climbing robot, a new state representation along with corresponding operations including sampling, metric calculation and interpolation is presented. A simple but effective model of a biped climbing robot in trusses is proposed, through which the motion planning of one climbing cycle is transformed to that of a manipulator. In addition, the pre and postprocesses are introduced to expedite the convergence of the BiRRT algorithm and to ensure the safe motion of the climbing robot near poles as well. The piecewise linear paths are smoothed by utilizing cubic Bspline curve fitting. The effectiveness and efficiency of the presented BiRRT algorithm for climbing motion planning are verified by simulations.
Keywords
Background
To release workers from tedious and dangerous highrise tasks in trusstype environments, such as inspecting or spraypainting the frame of gymnasiums, airports and large bridges, and so on, robots able to autonomously climb poles are ideal solutions with a lot of benefits. Motivated by this, a variety of poleclimbing robots including UTPCR [1], CPR [2], Shady3D [3], Climbot [4] and 3DCLIMBER [5] have been developed. Among them, biped poleclimbing robots (BiPCRs), whose main bodies are usually serial arms with multiple degrees of freedom (DoFs) and both ends are mounted with attaching devices, are considered to be outstanding, thanks to their high mobility in terms of multiple climbing gaits, strong ability to transit between poles and to overcome obstacles.
The ultimate goal of developing BiPCRs is to autonomously carry out highrise tasks in place of humans. To this end, autonomous climbing is a fundamental and essential functionality of a BiPCR. In some sense, a BiPCR can be regarded as a mobile manipulator, whose base may be changed and fixed in turn. During climbing, the robot fixes and supports itself with one of the two grippers served as the base, and moves the other one (the swinging gripper) to the target position, interchanging the roles of the two grippers in each climbing cycle. Hence to completely describe how a BiPCR climb in a spatial truss, we have to provide a series of discrete footholds and the continuous trajectories between adjacent footholds of the same swinging grippers. While the former define the gripping configurations of the BiPCR from the initial position to the destination, the latter determine the climbing motion of the robot in each climbing step. How to plan the footholds refers to climbing path planning or grasp pose planning of a BiPCR, which is out of the scope of this paper. Rather, given the footholds of the two grippers, how to plan the smooth and collisionfree motion of the swinging gripper in one climbing step for a BiPCR in complex spatial trusses is an open and challenging issue and is the focus of this paper.
Climbing path planning of BiPCRs in spacial trusses has been investigated to some extent in the literature. The problem was converted into the classical traveling salesman problem considering the energy consuming during each climbing cycle in [6] and [7]. In [3], the trusses were discretized into a series of nodes and the sequence of clamping points from a given initial node to the destination one was planned by calculating the Dijkstra shortest distance and motion complexity as criteria. The above work on climbing path planning actually belongs to the category of foothold planning. However, to the best of our knowledge, singlestep collisionfree trajectory planning of a BiPCR climbing in complex spatial trusses has not been explored.
Climbing motion planning of a BiPCR in one climbing step is similar to that of an manipulator, since the robot is fixed and supported on a pole by the base gripper at a specific foothold, and the swinging gripper moves from its initial foothold (configuration) to the target one. Therefore, traditional algorithms for collisionfree motion planning of manipulators, such as artificial potential field (APF) [8], probabilistic road map (PRM) [9], rapidlyexploring random tree (RRT) [10], and almost all the intelligent algorithms like genetic algorithm, particle swarm optimization (PSO) [11] can be utilized to generate the climbing trajectories. However, the motion planning of a BiPCR has its own features compared with that of an industrial robot. First, when the base of a BiPCR is changed and switched between the two grippers during climbing, those algorithms suitable for fixed base, such as PRM, will exhibit low efficiency. Second, some part(s) of the target pole is/are the graspable region(s) and other parts should be treated as obstacles, traditional algorithms like APF will encounter difficulties. Third, the role of a pole (target or obstacle) may interchange in different climbing cycles.
Considering the RRT algorithm has wide adaptation and good robustness to multiple degrees of freedom and dynamic environments, we address the problem of collisionfree motion planning in one climbing step for BiPCRs in the spatial trusses, utilizing the BiRRT algorithm. The main contributions of this paper are as follows. On the one hand, the framework for climbing motion planning of BiPCRs with different DoFs is first built based on BiRRT. The proposed planning algorithm is adaptive to BiPCRs with different numbers of DoFs including five and six. For a 5DoF BiPCR like Climbot5D (hereafter we use Climbot5D and Climbot6D to represent the Climbot with five and six degrees of freedom, respectively) whose orientation is limited due to its special configuration, a simple but effective state expression method is presented to deal with the sampling, interpolation and metric processes, which also adapts to Climbot6D. On the other hand, preprocess and postprocess methods are proposed in this paper to guide the swinging gripper to move away from the starting foothold and to the target foothold. In addition, cubic Bspline curves are utilized to smooth the climbing trajectories.
Theoretical analysis
Description of a BiPCR in a truss
Problem statement
In a single climbing step, collisionfree motion planning involves three adjacent footholds, one of which determines the grasping configuration of the base gripper and the other two are the initial and the target configurations of the swinging gripper.^{1} A feasible and collisionfree trajectory is to be found between the two footholds for the swinging gripper. The problem can be described as follows.
Pre and postprocess for easy trajectory planning
The pre and postprocesses bring several benefits to the motion planning of a BiPCR including (1) expediting the convergence of the searching procedure with samplingbased algorithms, (2) simplifying collision check, no need to distinguish the grasped poles and the obstacle poles and (3) easy integration of collisionfree trajectory planning and autonomous alignment of the gripper.
Utilization of the reachable workspace
In this paper, the reachable workspace (as shown in Fig. 3) of a BiPCR is considered to simplify the planning problem. It is clear that only those poles inside the reachable workspace, rather than the whole truss, should be considered as the target or obstacle poles in the planning. Therefore, the reachable workspace contributes to filter the poles in order to accelerate the collision detection. Moreover, it is also utilized to define the sampling area.
Collision detection
 Step 1::

describing the poles and the BiPCR in the world frame \(\{ W\}\). Collision check can be conducted only under the condition that the robot and the obstacles are expressed in the same coordination frame. Without loss of generality, supposing “A” is an arbitrary point of the robot, its position can be calculated by the forward kinematics with respect to frame \(\{ B\}\) as \({^B{\varvec{p}_A}}\), then transformed to \(\{ W\}\) by \(^W{\varvec{p}_A} = {}_B^W{\varvec{T}}^B{\varvec{p}_A}\).
 Step 2::

finding the pole segments within the simplified reachable workspace of the robot. The algorithm to calculate the line segment inside a sphere can be found in [12]. This intersecting segment can be described by two points with parameters \({{t_1}}\) and \({{t_2}}\) respectively, having the form aswhere \({{{\varvec{p}}_0}}\) and \({\varvec{d}}\) represent the reference point and the unit direction vector of a line segment.$$\begin{aligned} {{{\varvec{p}}_i}} = {{{\varvec{p}}_0}} + {t_i} {\varvec{d}} \end{aligned}$$(12)
 Step 3::

computing the minimum distances between the remaining poles and links of the robot, and comparing with the threshold (the sum of radii of the pole and the robotic link). Collision is reported when the computed distance is less than the threshold; otherwise, there is no collision between the poles and the robot. The pseudocodes of the algorithm are listed in Algorithm 1. In the algorithm, Seg2SegDist means the function calculating the Euclidean distance between two spatial line segments.
The motion planning algorithm
BiRRT algorithm
Constraints on grasping orientation
It is well known that a manipulator with six DoFs may reach arbitrary configuration in its workspace. The configuration of a 6DoF BiPCR can be described by a 3D position and a 4D unit quaternion, similar to that of an industrial robot.
Unfortunately, a 5DoF BiPCR is unable to reach arbitrary orientation. As a result, if the Euler angles or quaternions are utilized to describe its orientation and to interpolate, the reachability of a desired configuration cannot be guaranteed. In other words, the computed configuration of the robot may not be accurate when the inverse kinematics presented in [4] is used directly.
Random sampling
In order to guarantee the uniform distribution of the sampling, and to take into account the multiple gaits of a BiPCR, we sample the configuration of a BiPCR in the workspace with respect to the base frame.
On the other hand, since we use vectors with different dimensions to describe the orientation of the swinging grippers of 5DoF and 6DoF BiPCRs, two methods are utilized to sample the orientation component, respectively. For a 6DoF BiPCR, a simple sampling algorithm in SO(3) performs well in sampling unit quaternion [14]. For a 5DoF BiPCR, we need to sample the direction of a virtual pole (the \({\varvec{n}}\) component) and then calculate the grasping orientation by Eq. (13). To this end, the HEALPix algorithm [15] is employed to generate two angular parameters (\(\theta , \varphi\)) in spherical coordinates, which is then transformed to a 3D directional vector by \({\varvec{n}}={[\cos \theta \;\;\sin \theta \cos \varphi \;\;\sin \theta \sin \varphi ]^T}\).
So far, through sampling we have achieved a 6D random state (a 3D position and a 3D direction vector) for a 5DoF BiPCR and a 7D random state (a 3D position and a 4D unit quaternion) for a 6DoF BiPCR, respectively.
Distance metric
Configuration interpolation
When interpolating between two configurations, it is usually separated into two parts corresponding to the position and orientation components. For the position component, a simple linear interpolation is suitable. As for the orientation component, it depends on the inner product of the two unit quaternions or 3D vectors. If the orientations are close enough (their inner product is bigger than the predefined threshold), the linear interpolation algorithm is applied. Otherwise, the spherical linear interpolation algorithm is carried out, which is able to ensure the smooth interpolation between two configurations along geodesics.
Motion smoothing
Samplingbased planning may sometimes generate jerky and unnatural trajectories whose first derivatives are not continuous [16], which results in nonsmooth motion or vibration of the robot. Therefore, motion smoothing is necessary. We utilize cubic Bspline fitting in this paper considering its sufficient flexibility and highorder smoothness.
Note that we set double coincidence points at the two ends of the piecewise linear path to ensure the cubic Bspline curve passing them exactly. We set also weights for each control point to adjust the shape of the cubic Bspline curve. The larger the weight is, the closer the curve gets to the control point. As discussed above, the path is composed of a series of configurations of the robot. While the position portions of the configurations are fitted with a cubic Bspline in 3D space, the orientation portions are calculated by interpolating between every two adjacent configurations at the vertices of the shortcut path. In configuration check along of the path, the position portion may be changed by adjusting the weights of the control points of the cubic Bspline to satisfy the inverse kinematics and ensure collision avoidance.
Simulations and results
To verify the effectiveness of the theoretical analysis and the presented algorithms above, simulations are conducted in this section. The trusses are composed of cylindrical and squared poles with a diameter of 60 mm in arbitrary orientation in 3D space. Both Climbot5D and Climbot6D are employed to test the proposed algorithm.
A comparison between the simulations
BiPCR  Time (s)  Iteration times  Tree nodes  Path length (m) 

Climbot5D  4.46  130  104  2.93 
Climbot6D  0.593  32  38  1.21 
Conclusions and future work
Autonomous climbing is an essential function to carry out highrise tasks with BiPCRs. Collisionfree motion planning of BiPCRs in spatial trusses is an open problem, which has been addressed in this paper as a fundamental step to autonomous planning of climbing motion. A samplingbased algorithm, BiRRT, has been ultilized for singlestep collisionfree trajectory planning for BiPCRs. With appropriate description of a BiPCR in a truss, climbing motion planning has been conducted in a manner similar to that of a manipulator. The constraint on grasping orientation and the basic operations such as sampling, configuration distance calculation and interpolation have been discussed to facilitate the application of RRT. To expedite the convergence of the BiRRT algorithm, preprocess and postprocess have been presented to deal with leaving from the starting point (the initial grasp configuration) and approaching the goal point (the final grasping configuration) of the swinging gripper. A method to smooth the piecewise linear jerky trajectory generated by the BiRRT algorithm has been proposed by utilizing cubic Bspline curve fitting. Simulations have verified the effectiveness of the theoretical analysis and the presented algorithm. The algorithm is general and universal for motion planning of other robots including manipulators and biped wallclimbing robots.
In the future, the algorithm will be integrated into the robot’s multilayered planner for online climbing path and motion planning. And the dynamic constraints like the limit of joint velocity, acceleration and torque will be taken into account.
The foothold of the base gripper may be at the end or in the middle of the three footholds, depending on the climbing gait—if the inchworm gait is used in the climbing step, the foothold of the base gripper is at the end, since the (front and rear) order of the two grippers are not changed; otherwise, if the turningaround gait or the flippingover gait is employed, it is in the middle of the three footholds, since the order of the two grippers interchange [4].
Declarations
Acknowledgements
The work in this paper has been supported by the NSFCGuangdong Joint Fund (Grant No. U1401240), the Natural Science Foundation of Guangdong Province (Grant Nos. S2013020012797, 2015A030308011), the State International Science and Technology Cooperation Special Items (Grant No. 2015DFA11700), the Frontier and Key Technology Innovation Special Funds of Guangdong Province (Grant Nos. 2014B090919002, 2015B010917003).
Competing interests
The authors declare that they have no competing interests.
Open AccessThis article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
Authors’ Affiliations
References
 Baghani A, Ahmadabadi MN, Harati A. Kinematics modeling of a wheelbased pole climbing robot (utpcr). In: Proceedings of the IEEE international conference on robotics and automation. 2005. p. 2111–16.Google Scholar
 Chung WK, Xu Y. A hybrid pole climbing and manipulating robot with minimum dofs for construction and service applications. Ind Robot: Int J. 2005;32(2):171–8.View ArticleGoogle Scholar
 Yoon Y, Rus D. A robot that climbs 3d trusses. In: Proceedings of the IEEE international conference on robotics and automation. 2007. p. 4071–76.Google Scholar
 Guan Y, Jiang L, Zhu H, et al. Climbot: a modular bioinspired biped climbing robot. In: Proceedings of the IEEE international conference on intelligent robots and systems. 2011. p. 1473–78.Google Scholar
 Tavakoli M, Marjovi A, Marques L, et al. 3dclimber: a climbing robot for inspection of 3d human made structures. In: IEEE/RSJ international conference on intelligent robots and systems. 2008. p. 4130–35.Google Scholar
 Balaguer C, Gimenez A, Pastor JM, et al. A climbing autonomous robot for inspection applications in 3d complex environments. Ind Robot. 2000;18(3):287–97.View ArticleGoogle Scholar
 Chung WK, Xu Y. Minimum energy demand locomotion on space station. J Robot. 2013;2013(1):1–15.View ArticleMathSciNetGoogle Scholar
 Khosla P, Volpe R. Superquadric artificial potential for obstacle avoidance and approach. In: Proceedings of the IEEE international conference on mechatronics and automation. 1988. p. 1778–84.Google Scholar
 Kayraki LE, Svestka P, Latombe JC, et al. Probabilistic roadmaps for path planning in highdimensional configurations space. Proc IEEE Trans Robot Autom. 1996;12(4):566–80.View ArticleGoogle Scholar
 Lavalle SM, Kuffner JJ. Rapidlyexploring random trees: progress and prospects. In: Proceedings of algorithmic and computational robotics: new directions. 2001. p. 293–308.Google Scholar
 Lovbjerg M, Rasmussen TK, Krink T. Hybrid particle swarm optimiser with breeding and subpopulations. In: Proceedings of genetic and evolutionary computation conference. 2001. p. 469–76.Google Scholar
 Ericson C. Real time collision detection. Oxford: Morgan Kaufmann Publishers; 2004.Google Scholar
 LaValle SM, Kuffner JJ Jr. Randomized kinodynamic planning. In: Proceedings of international conference on robotics and automation. 1999. p. 473–79.Google Scholar
 LaValle SM. Planning algorithm. Cambridge: Cambridge University Press, Cambridge University; 2006.View ArticleGoogle Scholar
 Go’rski KM, Hivon E, Banday AJ, et al. Healpix: a framework for highresolution discretization and fast analysis of data distributed on the sphere. Astrophys J. 2005;622(2):759–771View ArticleGoogle Scholar
 Hauser K, NgThowHing V. Fast smoothing of manipulator trajectories using optimal boundedacceleration shortcuts. In: IEEE international conference on robotics and automation. 2010. p. 2493–98.Google Scholar