Constrained VPH+: a local path planning algorithm for a bio-inspired crawling robot with customized ultrasonic scanning sensor

This paper aims to develop a local path planning algorithm for a bio-inspired, reconfigurable crawling robot. A detailed description of the robotic platform is first provided, and the suitability for deployment of each of the current state-of-the-art local path planners is analyzed after an extensive literature review. The Enhanced Vector Polar Histogram algorithm is described and reformulated to better fit the requirements of the platform. The algorithm is deployed on the robotic platform in crawling configuration and favorably compared with other state-of-the-art local path planning algorithms.


Background
Robotic systems with the ability to reconfigure their morphologies in response to the application scenario display great potential with their versatility, fault tolerance, and efficiency for a variety of rugged missions in real world. A few works on reconfigurable robotics are [1][2][3][4][5].
Many reconfigurable robotic platforms are based on bio-inspired or biomimetic designs, which are based on naturally evolving mechanisms. A focus of reconfigurable robotic research has been the development of a bio-inspired platform which displays both rolling and crawling abilities, leading to the robotic platform known as BiLBIQ [6]. However, these efforts had been focused completely on mechanism design with almost no effort associated with perception or autonomous functionality.
While remote-controlled robot mechanisms have sufficed for most applications encountered thus far, emerging applications in the fields of surveillance and security necessitate the development of robots possessing a measure of autonomy and intelligence, including basic functionality such as mapping and local path planning.
Unfortunately, integrating complex reconfigurable design mechanisms with perception introduces multiple new research challenges.
Recently, a family of reconfigurable robotic platforms (i.e., Scorpio) capable of crawling and rolling locomotion has been developed. These robots mimic the morphology of a huntsman spider that can transform between crawling and rolling by reconfiguring their legs.
The Scorpio is in development for applications pertaining to surveillance and security; thus, the primary focus has been the reduction in size of the platform, as well as improvement of power efficiency of the overall system. Furthermore, the changing requirements of the platform have resulted in a rapid evolution of five stages of the robotic platform in 2 years. In order to adhere to both these conditions, the Arduino Mini Pro 328 has been chosen as the onboard processing unit allowing for rapid prototyping of applications.
A customized ultrasonic sensor is designed for the system due to the relative computational inexpensiveness in processing the data, as well as to reduce power consumption. The customized sensor allows greater control of the sensor to restrict environmental observations to data that is relevant to the successful completion of the mission.
Previous works based on the Scorpio focus on the development of an efficient rolling controller [7,8] and Open Access *Correspondence: akshay_rao@sutd.edu.sg 1 Temasek Laboratory, Singapore University of Technology and Design, 08 Somapah Road, Singapore 487372, Singapore Full list of author information is available at the end of the article the formulation and implementation of an intelligent vision-based terrain perception module [9]. This paper attempts to add to the suite of autonomous functions for the Scorpio by developing an efficient obstacle avoidance algorithm in crawling mode.
One of the desired applications for the Scorpio platform has been the rapid traversal of unknown, unstructured terrain in a fixed period of time. A wall-following algorithm [10] was first implemented as a proof-of-concept obstacle avoidance algorithm, using the hardware described above. Despite its ability to traverse a room and find an exit, it was an inefficient algorithm, leading to oscillations in certain scenarios.
Conventional local path planning algorithms [11][12][13][14][15][16][17][18][19][20][21][22] assume the use of a robotic platform with Ackermann steering model, as well as highly accurate, rapidly scanning range sensors producing highly dense data. Since the Scorpio robot is unable to satisfy the assumptions for both the proprioceptive and the exteroceptive models, design of a local path planning algorithm that better fits the robot is necessitated.
In this paper, we present the design and development of a local path planning algorithm based on the Enhanced Vector Field Histogram (VPH+) and inspired by the inherent constraint due to the choice of sensor and platform. The remainder of the paper is organized as follows: The "Design of the robot" section describes the design specification of the robotic platform. Thereafter, in "Methods" section, we explain the design of the robot, sensor, and algorithm. The "Formulation" section derives the Constrained VPH+ algorithm from the VPH+ algorithm, while "Implementation" section provides a pseudocode for the algorithm, along with a discussion of some issues faced in implementing the algorithm on the Scorpio. The results are described in "Results" section. Finally, "Conclusions and future work" section concludes the paper and suggests avenues of future research.

Huntsman spider
The Scorpio robot is inspired by the Cebrennus rechenbergi, a species of huntsman spider. This species is able to travel using both crawling and rolling motion, as shown in Fig. 1. The rolling locomotion of the huntsman spider was discovered by Ingo Rechenberg from TU Berlin [6]. The habitat of Cebrennus rechenbergi is the sand dunes of the Erg Chebbi desert in Southern Morocco on the boundary of the Sahara Desert.
While the spider normally crawls on eight legs similar to other species of spiders, when provoked or threatened by an external stimulus, it can escape by doubling its normal crawling speed using forward or backward flips with the use of its eight legs simultaneously, similar to acrobatic flic-flac movements used by gymnasts.
Most notably, the spider turns somersaults to move independent of surrounding conditions. As a result, it does not need a slope to initiate the rolling process using the gravitational force. It also does not need to perform a run-up or a start-up gesture to trigger the rolling locomotion.
The main aim of the huntsman spider in deploying rolling mechanism as observed so far seems to maximize the displacement of the spider to escape from threatening circumstances, or when meeting a conspecific. So far, the spider has not been observed to perform any other functions in rolling mode, such as changing positions and hunting for prey.
An environment representation technique was arrived at in [12] known as the occupancy grid map, which is still used as a map representation technique in state-of-theart algorithms. Elfes [13] first formulated the use of occupancy grids for navigation and mobile robot perception.
Borenstein and Koren [14] developed a local path planning algorithm known as Virtual Force Field (VFF), using occupancy grids for obstacle representation and potential fields [23] for navigation. The VFF algorithm employed the use of repelling force fields around obstacles and an attracting force in the direction of the target. The simplicity of the formulation made it an attractive path planning algorithm in the robotics research community.
Mathematical and practical drawbacks of the VFF were discovered and analyzed in [15]. While the occupancy grid method was a computationally inexpensive way to generate an approximate representation of the map, it was unable to compensate for the contradiction between the complexity and roughness of the grids, rendering it unsuitable for use with a low precision sensor like an ultrasonic range sensor. A tendency of the algorithm to get trapped in local minima was also discovered in scenarios where the goal was behind an obstacle.
In response to this problem, the Vector Field Histogram (VFH) was first introduced in [15] and further expanded and analyzed in [16,17]. The VFH uses a twodimensional Cartesian grid as a representative world model, while local environmental data are represented as a one-dimensional polar histogram around the robot. Each section of the polar histogram represents the obstacle density in that direction. The algorithm then chooses a direction which contains the best trade-off between the obstacle density and the goal direction. A subsequent step provided appropriate steering commands to generate motion in the desired direction.
Extensive testing and implementation of the VFH resulted in the discovery that the VFH algorithm does not take into account the vehicle kinematics, resulting in unrealistic and impractical inputs provided to the robot. In response, the Dynamic Window Planning approach was postulated in [18] which was specifically designed to account for the limited velocities and accelerations inherent in wheeled mobile robots.
The VFH was also improved, and its drawbacks addressed in the VFH+ algorithm [19]. The VFH+ algorithm added two extra stages to the VFH algorithm. The VFH algorithm also displayed the tendency of creating oscillations in environments with multiple narrow openings due to the sharp thresholding near the entrances. The first additional stage in the VFH+ algorithm was used to provide a hysteresis function between two threshold values, to reduce the oscillations and produce a smoother trajectory.
The next stage added to the VFH algorithm in the VFH+ algorithm was to mask the histogram representing obstacle density to consider only directions feasible for the robot to travel in. The robot was assumed to be a wheeled robot with Ackermann steering mechanism, and hence, the robot trajectory was assumed to be based on series of circular arcs.
The VFH* algorithm developed in [20] used the A* search algorithm to add look-ahead capability to the VFH+ algorithm to reduce problems arising from purely local obstacle avoidance.
An and Wang [21] developed the Vector Polar Histogram (VPH) algorithm by combining the VFH+ algorithm and the Potential Field Method. The VPH algorithm relied on the newly available laser range scanners with comparably higher accuracy to accurately represent the local obstacle map.
Gong et al. [22] extended the VPH algorithm to the VPH+ algorithm by grouping individual obstacles to obstacle blocks and determining concave blocks ahead of time, in order to increase the efficiency of the robot in traversing the environment. The VPH+ algorithm also extended the VPH cost function to take the robot heading and speed into account.

Design of the robot
The section presents the mechanical design and system architecture of the Scorpio robot.

Mechanical design
The design of the Scorpio is based on the huntsman spider introduced in the previous section, to enable it to perform crawling and rolling locomotions. While the huntsman spider has eight legs, the Scorpio robot is designed with four legs which are adequate to perform crawling and rolling locomotions. Figure 2 contains a part-by-part view of Scorpio robot showing the assemblies. It is observed that the Scorpio robot consists of four legs (tibia), four servo covers and joints (femur), four main joints (coxa), and a body. The processor, controller, and sensors are placed inside the body which is made from PLA plastic. Twelve servo motors are used in this Scorpio robot to generate locomotion. Each leg (model shown in Fig. 3) is mounted with three servos, so it has 3 degrees of freedom. These legs are able to rotate and transform from crawling to rolling gaits. The specifications of the Scorpio robot are listed in Table 1.
For crawling motion, the Scorpio robot opens up its four legs as shown in Fig. 4a. The crawling involves 2 degrees of freedom. Transformation from crawling pose to cylindrical exoskeleton for rolling requires a motion of 3 degrees of freedom. The Scorpio robot uses its legs to push from the ground and shift the center of gravity to achieve the rolling motion with 1 degree of freedom. The rolling speed of the Scorpio robot doubles the rate of crawling speed.

Sensor design
The team aimed to create a customized range sensor with a greater degree of control. To this end, an ultrasonic sensor (model number SRF01) was mounted on a HS-35HD Ultra Nano Servo Motor. The ultrasonic sensor has a beam width of 12°. Further details of the sensor are tabulated in Table 2. A picture of the sensor mounted on the Scorpio can be found in Fig. 5.
The servo motor is actuated to the desired angle, after which the ultrasonic sensor is triggered. The time taken for the sensor to rotate to the desired angle, transmit, and receive the data is roughly 0.15 s per point. Due to the slow speed of sensing, a comprehensive scan of the entire environment at each instance is not recommended. Figure 6 displays a sample scan taken with the ultrasonic sensor.

Mode of locomotion
The Scorpio platform demonstrates different modes of locomotion in the two morphologies, each with their respective strengths and weaknesses.  • The rolling mode allows for greater speed of locomotion with reduced environmental perception. • The crawling mode conversely trades better environmental perception for reduced locomotion.
This paper makes use of only the crawling mode of locomotion, to improve perception of environmental obstacles. While the limited field of view in rolling mode limits the potential for integration into the robot motion planner, future improvements in terrain traversal efficiency necessitate the use of rolling mode to be taken into account.

Algorithm design
The local path planning algorithm design is informed by the following constraints inherent in the Scorpio robotic platform: 1. Kinematic constraints: Since the mode of locomotion in the Scorpio is crawling, standard wheeled robot kinematics are not valid. Consequently, all recent standard local path planning algorithms which assume Ackermann steering model cannot be used [14][15][16][17][18][19][20][21][22]. 2. Sensor inaccuracy constraints: The customized ultrasonic sensor has a beam width of 15°, making it more inaccurate as the range increases. Thus, any obstacle in the sonar beam at the same range (as seen in Fig. 7) will produce the same reading. Hence, algorithms which rely on the accuracy of environmental information provided by the exteroceptive sensor [21,22] will perform poorly on the Scorpio. 3. Sensor speed constraints: Slow scanning speed of the sensor implies performing scans over the full angular range frequently will negatively impact the efficiency of the algorithm. As a result, all recent local path planning algorithms which utilize the full sensor range [15][16][17][18][19][20][21][22] cannot be implemented. 4. Complex kinematic model: The Scorpio platform consists of twelve joint motors, resulting in a complex kinematic motion model. Computation of the entire kinematic model for locomotion on the Arduino processor used would greatly decrease speed and efficiency of the robot, rendering invalid all recent algorithms requiring computation of exact vehicle kinematics [18][19][20][21][22]. 5. Processor constraints: The Arduino Mini Pro processor is single-threaded; thus, each separate action has to be performed serially. As a consequence, the platform is unable to scan the environment and move simultaneously resulting in the invalidation of all algorithms in which robot velocity is an important determinant of direction of motion [17][18][19][20][21][22].
Thus, to summarize, the optimal local path planning algorithm must be able to perform inferences on the local obstacle distribution using sparse, inaccurate data obtained at a low frequency. Furthermore, due to inefficiency of the mechanism while turning, it is desirable to ensure the robot motion is as straight as possible, with minimum turning. The goal of the algorithm is to ensure the robot traverses the maximum distance from the starting point in a fixed period of time.
The most recent state-of-the-art local path planning algorithms in the robotics research community are the Enhanced Vector Polar Histogram (VPH+) [22]. The  VPH+ algorithm enhances the VPH algorithm formulation with obstacle grouping and classification, along with inclusion of the robot velocity and heading into the cost function. Since the sensors used by the Scorpio are too inaccurate to group obstacles accurately, and the singlethreaded onboard processor ensures the platform will not be in motion when the algorithm is being computed, the VPH+ algorithm will be modified with suitable constraints to better fit the Scorpio platform. Motion primitives will be used to approximate the Scorpio kinematic model, to ensure acceptable performance on the Arduino Mini Pro processor.

Formulation
The VPH+ algorithm represents the local environment as a polar histogram. It generates a cost function for each sensor angle using inputs from both the range sensor and the robot kinematic model. It leverages on the accuracy and the range of the laser range sensor to determine the boundaries of obstacle blocks, as well as to classify the obstacle as concave or convex. Concave obstacles are avoided outright, with the histogram value at the corresponding angles being set to zero. Figure 8 depicts the diagram used to determine the reachable distance in each direction. The VPH+ algorithm modifies each range reading with the radius of the robot. Thus, in the diagram, the maximum traversable distance by the robot in direction O i due to obstacle O j is d ′ ij , the length of P r M given by:  where y ij is the angle between points i and j, s ij = d j sin y ij , R is the robot radius.

VPH+ algorithm
The overall maximum traversable distance in direction O i is given by where n is the total number of beams per scan given by π/α BW for a sensor with beam width α BW .
The observed points are then grouped as obstacles if the distance between them is lesser than a user-defined safe threshold d thr , which is larger than the robot radius, plus a buffer distance. The distance between two adjacent points is given by: The VPH+ classifies obstacles as concave or convex, depending on which the following symbol function is constructed: The kinematics of the robot are then taken into consideration with the current robot velocity, minimum turning radius, and maximum robot velocity being used to generate a value for the safe distance d safe . A threshold function H(i) similar to B(i) is then created such that An angular cost function is constructed considering the angles shown in Fig. 9 given by: where h g is the angle between the goal and the current sensor angle, h 0 is the angle between the current direction of the robot and the current sensor angle, k 1 , k 2 , and k 3 are user-defined constants, k 1 h g is the cost associated with deviating from the direction to the goal, k 2 h 0 is the cost associated with deviating from the current direction of motion, and k 3 is a nonzero constant used to ensure the denominator is nonzero. Thus, a robot with high k 1 and low k 2 will prioritize goal following at the cost of maintaining a smooth trajectory. The final cost function is calculated with the individual cost functions with the expression: The desired direction is the direction with the maximum cost function.

Effect of sensor inaccuracy
The design of the Constrained Enhanced Vector Polar Histogram (CVPH+) is informed by the constraints from the inaccuracy of the sonar sensor, the small size of the platform, as well as the low processing power of the 7 Inaccuracy due to beam width. All three obstacles at the same range will produce the same reading due to the large beam width Fig. 8 Grouping of obstacle blocks in VPH+. Adjacent points closer than a pre-defined threshold are classified as a single obstacle onboard processing unit. The field of view of the ultrasonic sensor can be divided into n sectors, as shown in Fig. 10, where n is given by: where α FOV is the total field of view of the sensor and α BW is the beam width of the sensor, as defined earlier.
Inaccuracies arising due to the beam width of the sonar are of the order of the robot size. For example, consider the scenario in Fig. 11, in which acceptable distance d acc is the range at which the lateral beam width becomes equal to the diameter of the robot (d R , say). The value for d acc can be found from the expression given by: Consequently, obstacle pings with ranges greater than d acc cannot be used directly in VPH+ for obstacle grouping and to obtain obstacle boundaries. When the range reading from obstacles is less than d acc (implying that the distance between two obstacles d GAP is less than the size of the robot d R ), further accuracy regarding obstacle spacing becomes unnecessary; hence, the ranges can be used directly to obtain further inferences regarding the obstacle field.
In the case of [22], the range sensor used was the LMS200, which has an angular beam width of 4.4 millirad [25] and a maximum range of 80 m. Thus, the diameter of the robot must be greater than 0.352 m, according to Eq. 10. The Pioneer P3AT robot used by them has a width of 0.381 m, which fulfills this criterion.

Cost function reformulation
In the cost function for the VPH+ algorithm, the B(i) and H(i) terms are indicator terms having values of only 1 or 0. Consider the modified cost function Assuming the same values for B(i) and H(i) in the VPH+ formulation, in this case, the desired direction at the end of the inference cycle will be given by: Let the absolute angle for the goal as seen in Fig. 9 be α G and the absolute angle for the point i be α i . Then, from Fig. 9, the following relations are true: The cost function then becomes: where K α is a constant for the current inference cycle given by: and K k is a constant for the algorithm obtained from the following expression: Another angle, α j , will be chosen if its cost function C ′ (j) will be lesser than C ′ (i). However, the maximum range of the sensor r max implies that an upper limit to the angle can be determined and is given by the expression: By observation, it can be seen that the numerator of the cost function is minimum for scan angles that directly point toward the goal or toward the robot heading, depending on the values of k 1 and k 2 . Hence, the maximum scanning angle for the current scan needs to take both into account, along with the maximum possible angle reachable by the sensor α sensor,max .
The resulting number of beams required is then obtained by: A simple example would be when the robot is pointed toward the goal (i.e., α G = h r , and there is no obstacle in front). In this case, it is expected that the cost function will be minimum in the direction of the goal; hence, no other angle can have a cost function lower than the goal direction. Sure enough, on inputting the values in the above equation, we receive Since α G is the first angle sector, the relation simplifies to α j,max being equated to α G , the goal angle. Figure 12 shows the numbering convention used for the sonar data. The sonar scan is divided into an odd number of scans n. The center beam is numbered 0, the beams on the left are numbered negative, while the beams on the right are numbered positive.

Implementation
Gait primitives for the Scorpio robot were first obtained. The motion of the Scorpio robot in three directions was characterized: left, right, and forward. Histograms of the gaits were obtained, as shown in Figs. 13, 14 and 15. Structural inaccuracies introduced due to the inexact nature of the 3D printing process resulted in different values for left and right gait primitives. The forward motion gait was also discovered to display a drift, as shown in Fig. 16, which was compensated for in the implementation of the algorithm.
The values of k 1 and k 2 were obtained from by optimizing the implementation of the VPH+ algorithm for the best result, as tabulated in Table 3. The pseudocode of the Constrained VPH+ algorithm is displayed below.

Pseudocode
loop While robot path is not completed Scan and obtain ranges d g and d 0 in direction towards goal, and towards robot heading. Calculate number of beams n s Calculate constants K k and K α for i = 1 to n s do Append d i to unprocessed ranges array R end if end for Calculate maximum reachable distance in each direction for n R unprocessed ranges in array R  The algorithm and its performance were compared against the VPH+ algorithm on two obstacle courses shown in Figs. 17 and 18. First, a simple obstacle course was created consisting of a single obstacle, on which the algorithms were evaluated. A second, more complicated obstacle course was created with a higher obstacle density.
The height of the ultrasonic sensor is around 17 cm from the ground; hence, it was attempted to ensure obstacles of a similar minimum height. The cluttered office environment is used to ensure effectiveness of the algorithm in realistic settings.
To ensure minimal errors due to pose inconsistency, the goal was set to be simply 1.20 m from the start point.
The trials were conducted 6 times for each algorithm at each obstacle course. Figure 19 shows the comparison of the paths taken by the robot for the simple obstacle course, consisting of a single obstacle, shown in Fig. 17. As seen in the figure, the paths taken by the robot due to both the algorithms compare very closely. However, the VPH+ algorithm causes the robot to double back due to erroneous grouping of obstacle caused by the wide beam width of the sonar.

Results
The VPH+ algorithm took an average of 249.7254 s, while the CVPH+ algorithm clocked an average time of 224.3952 s. Figure 20 shows the comparison between the paths taken in the more complicated obstacle course shown in Fig. 18. The closeness of the two obstacles in comparison with sonar beam width causes the VPH+ algorithm to erroneously group them together, resulting in the robot doubling back halfway to look for another path. The CVPH+ algorithm, however, closely follows the contours of two obstacles, resulting in a longer, more conservative path. The cause of this path is the choice of constants k 1 and k 2 , which were optimized for the first obstacle course. Further optimization of the constants can lead to more performance gains for the CVPH+ algorithm.   The VPH+ algorithm took an average of 336.5960 s, while the CVPH+ algorithm took 329.4032 s.

Conclusions and future work
The CVPH+ algorithm is found to outperform the VPH+ algorithm in both empty and clutter-filled environments. The improvements in the algorithm were obtained from formalizing implicit assumptions made in the VPH+ algorithm regarding the sensor and the robot model, both of which were invalid for the Scorpio robot.
Further insight was obtained from examining the original VPH+ cost function further and reformulating it to improve the performance of the CVPH+ algorithm.
Pose inaccuracy due to the noisy gait primitive estimates, as well as the inherent inaccuracies due to the 3D printing process, has been seen to reduce the performance of both the VPH+ and the CVPH+ algorithms. A more comprehensive formulation taking into account pose and map estimates will result in better performance of the algorithms.
A further avenue of future research includes the use of the rolling configuration of the robot to improve its speed in traversing the environment. The challenges caused by the displacement of the ultrasonic sensors in the rolling mode of the robot will also be an interesting aspect to be explored.