Guidance Point Generation-Based Cooperative UGV Teleoperation in Unstructured Environment

sensors

Article Guidance Point Generation-Based Cooperative UGV Teleoperation in Unstructured Environment

Sen Zhu 1,2, Guangming Xiong 1,*, Huiyan Chen 1 and Jianwei Gong 1

1 Intelligent Vehicle Research Center, Beijing Institute of Technology, Beijing 100081, China; [email protected] (S.Z.); [email protected] (H.C.); [email protected] (J.G.) 2 China North Vehicle Research Institute, Beijing 100072, China * Correspondence: [email protected]

Abstract: Teleoperation is widely used for unmanned ground vehicle (UGV) navigation in military and civilian fields. However, the human operator has to limit speed to ensure the handling stability because of the low resolution of video, limited field of view and time delay in the control loop. In this paper, we propose a novel guidance point generation method that is well suited for human–machine cooperative UGV teleoperation in unstructured environments without a predefined goal position. The key novelty of this method is that the guidance points used for navigation can be generated with only the local perception information of the UGV. Firstly, the locally occupied grid map (OGM) was generated utilizing a probabilistic grid state description method, and converted into binary image to constructed the convex hull of obstacle area. Secondly, we proposed an improved thinning algorithm to extract skeletons of navigable regions from binary images, and find out the target skeleton related to the position of the UGV utilizing the k-nearest neighbor (kNN) algorithm. The target skeleton was reconstructed at the midline position of the navigable region using the decreasing gradient

algorithm in order to obtain the appropriate skeleton end points for use as candidate guidance points. For visually presenting the driving trend of the UGV and convenient touch screen operation, we

Citation: Zhu, S.; Xiong, G.; Chen, transformed guidance point selection into trajectory selection by generating the predicted trajectory H.; Gong, J. Guidance Point correlative to candidate guidance points based on the differential equation of motion. Experimental Generation-Based Cooperative UGV results show that the proposed method signiﬁcantly increases the speed of teleoperated UGV. Teleoperation in Unstructured Environment. Sensors 2021, 21, 2323. Keywords: unmanned ground vehicle; human-machine cooperative control; unstructured environ- https://doi.org/10.3390/s21072323 ment; skeleton extraction; guidance point

Academic Editor: Charlie Yang

Received: 3 February 2021 1. Introduction Accepted: 22 March 2021 Published: 26 March 2021 Unmanned ground vehicles (UGV) have been very useful in a number of civilian and military ﬁelds, including rescue, reconnaissance and patrol mission. With the rapid

Publisher’s Note: MDPI stays neutral development of sensor, communication and intelligent control, humans are able to increase with regard to jurisdictional claims in their distance from dangerous tasks. Control of a UGV over a distance is referred to as published maps and institutional affil- teleoperation and has become a very popular control mode. While adding distance between iations. the human operator and UGV can be advantageous to the safety of the human, it also introduces negative side effects. In particular, with this arrangement human operators often have to control the vehicle using 2D video feeds with low resolution and limited field of view [1]. To make matters more difficult, the video feed and control command are delayed hundreds of milliseconds because of data processing and transmission over Copyright: © 2021 by the authors. Licensee MDPI, Basel, Switzerland. a wireless network. Previous work has shown that time delays decrease the speed and This article is an open access article accuracy with which human operators can complete a teleoperation task [2,3]. distributed under the terms and Many researchers have been working on methods to assist humans overcome these conditions of the Creative Commons challenges inherent to teleoperation. For example, researchers have developed algorithms Attribution (CC BY) license (https:// to combine camera and lidar data to generate 3D exocentric views for more intuitive creativecommons.org/licenses/by/ teleoperation [4]. For well-defined tasks in structured environments, researchers could 4.0/). design integrated multi-sensor systems and developed algorithms to make the UGV more

Sensors 2021, 21, 2323. https://doi.org/10.3390/s21072323 https://www.mdpi.com/journal/sensors Sensors 2021, 21, 2323 2 of 28

autonomous [5,6]. However, many UGVs carry out tasks in unstructured environments in which the current autonomous navigation technology is difficult to ensure the safety of vehicles, and still require human knowledge or expertise. Cosenzo and Barnes claim most military robotic systems will continue to require active human control or at least supervision with the ability to take over if necessary [7]. Some studies have suggested that human–machine cooperative control (versus manual control) of a UGV can improve task performance in high speed navigation [8]. Macharet and Florencio developed a method that uses artificial potential fields and vector field histograms for navigation with a human operator in the control loop [9]. Researchers in the automotive domain have conducted studies that show model predictive control (MPC) can be an effective tool in cooperative control. The MPC based methods augment driver inputs based on factors such as predicted vehicle stability [10] and predicted vehicle positions [11,12]. However, there are still gaps in the research and methods that attempt to best combine human operator and navigation control system of a UGV. More specifically, many of the previous works assumed that navigation control system knows the key guidance information of task. In many cases this is a reasonable assumption, e.g., an autonomous navigation task based on road network, it may be reasonable to assume that global path points have been determined in advance. In unstructured environments, e.g., off-road areas, often it is not reasonable to assume the navigation control system knows the human’s goal position. This paper presents a novel guidance point generation method that is well suited for human–machine cooperative UGV teleoperation in unstructured environments without a predefined goal position. The key novelty of this method is that the guidance points used for navigation can be generated with only the local perception information of the UGV. Firstly, the locally occupied grid map (OGM) was generated utilizing a probabilistic grid state description method, and converted into binary image to constructed the convex hull of obstacle area. Secondly, we proposed an improved Zhang–Suen algorithm to extract skeletons of navigable regions from image, and find out the target skeleton related to the position of UGV utilizing the kNN algorithm. The target skeleton was reconstructed at the midline position of the navigable region using the decreasing gradient algorithm in order to obtain the appropriate skeleton end points for use as candidate guidance points. For convenient touch screen operation, we transformed guidance point selection into trajectory selection by generating the predicted trajectory correlative to candidate guidance points based on the differential equation of motion. The remainder of this paper is organized as follows. Section2 contains a review of work in the literature related to mainstream cooperative control techniques for UGV teleoperation. Section3 introduces the proposed generating algorithm of candidate guidance points. Section4 describes the human–machine cooperative control method base on guidance points selection. Section5 introduces the hardware platform and the experimental setting, and analyses the experimental results in detail. Section6 provides conclusions and suggestions for future research.

2. Related Works With the continuous improvement of autonomy of UGVs, the relationship between the UGV and human operator has changed from the traditional master-slaver control to a more ﬂexible cooperative relationship. In this paper the human–machine cooperative control refers in particular to the operator and the navigation control system being in the control loop at the same time, and sharing the task information, environmental information and UGV status to complete the driving task [13]. According to the differences of cooperative modes, the cooperative control can be divided into three categories: input correction cooperative control, haptic interaction cooperative control and guidance interaction cooperative control. Sensors 2021, 21, x FOR PEER REVIEW 3 of 29

Sensors 2021, 21, 2323 3 of 28 correction cooperative control, haptic interaction cooperative control and guidance interaction cooperative control.

2.1.2.1. InputInput CorrectionCorrection CooperativeCooperative Control Control TheThe navigation navigation control control system system uses uses the the control control command command generated generated by the by autonomous the auton- navigationomous navigation algorithm algorithm to modify to the modify control th commande control inputcommand by the input operator by throughthe operator the human–machinethrough the human–machine interface, and interface, transmits and theresults transmits to the the chassis results controller, to the chassis so as controller, to realize cooperativeso as to realize control. cooperative Figure1 shows control. an Figure overview 1 shows of the inputan overview correction of cooperativethe input correction control system.cooperative control system.

FigureFigure 1. 1.System System overviewoverview of of input input correction correction cooperative cooperative control. control.

EarlyEarly researchresearch workwork waswas basedbased onon thethe assumption assumption ofoffixed fixed control control ratio ratio between between operatoroperator andand navigationnavigation controlcontrol systemsystem toto designdesign thethe cooperativecooperative controlcontrol methodmethod[ [14,15].14,15]. ReferenceReference [ 16[16]] points points out out that that the the distribution distribution of controlof control right right between between human human and machineand ma- shouldchine should be adaptively be adaptively adjusted adjusted according according to the operator’sto the operator’s intention. intention. Reference Reference [17] uses [17] theuses human the human factor modelfactor model to predict to predict the operator’s the operator’s control control behavior, be andhavior, designs and adesigns nonlin- a earnonlinear model predictionmodel prediction controller controller to assist to the assist operator the operator with minimal with minimal intervention. intervention. Some researchersSome researchers believe believe that the that operator’s the operator’s freedom freedom of control of control should should be guaranteed be guaranteed in the in processthe process of cooperative of cooperative control. control. Reference Reference [18] proposed [18] proposed a human-machine a human-machine input mixing input methodmixing basedmethod on based path on homotropy path homotropy concept, co andncept, introduced and introduced autonomous autonomous control actioncontrol onlyaction when only necessary when necessary to avoid to collision, avoid collisio so as ton, achieveso as to a achieve balance a between balance humanbetween freedom human offreedom operation of operation and driving and safety. driving safety. 2.2. Haptic Interaction Cooperative Control 2.2. Haptic Interaction Cooperative Control A haptic interaction cooperative control mode has two advantages over input correc- A haptic interaction cooperative control mode has two advantages over input cor- tion cooperative control method: (1) the operator continuously interacts with the UGV throughrection cooperative haptic interface, control and method: thus integrates (1) the operator into the continuously control loop interacts deeply; (2)with this the mode UGV givesthrough the haptic operator interface, higher and control thus authority, integrates and into the the operator control loop can overcomedeeply; (2) the this torque mode feedbackgives the from operator the navigation higher control control authority, system to takeand overthe operator the UGV can completely overcome [11 ].theFigure torque2 showsfeedback an overviewfrom the navigation of the haptic control interaction system cooperative to take over control the UGV system. completely [11]. Fig- ure 2In shows reference an overview [19], a haptic of the guidancehaptic interaction with predictive cooperative capability control wassystem. added to the cooperative control, which increases the completion rate of the remote driving task and the operator’s workload were improved to a certain extent. Reference [20] has proved that the performance of operators under cooperative control can be improved in both lane-keeping and vehicle-following tasks. The key factor of haptic interaction cooperative control is to determine the desired auxiliary torque that conforms to the characteristics of human muscular motion. Refer- ence [21] proves that the appropriate relationship model between muscular movement and expected torque is a key factor to improve the interaction performance in the process of cooperative control. Reference [22] studied haptic interaction cooperative control under different degrees of haptic feedback force, and found that the performance of the system improved more signiﬁcantly when the feedback force was small. Reference [23] designed a dynamic cooperative steering control system by modeling the biomechanical impedance of

Sensors 2021, 21, x FOR PEER REVIEW 4 of 29

Sensors 2021, 21, 2323 4 of 28

Figure 2. System overview of haptic interaction cooperative control. Sensors 2021, 21, x FOR PEER REVIEW 4 of 29 the human arm, and realized dynamic torque feedback to the operator by adjusting the cooperativeIn reference impedance. [19], a haptic guidance with predictive capability was added to the cooperative control, which increases the completion rate of the remote driving task and the operator’s workload were improved to a certain extent. Reference [20] has proved that the performance of operators under cooperative control can be improved in both lane-keeping and vehicle-following tasks. The key factor of haptic interaction cooperative control is to determine the desired auxiliary torque that conforms to the characteristics of human muscular motion. Refer- ence [21] proves that the appropriate relationship model between muscular movement and expected torque is a key factor to improve the interaction performance in the process of cooperative control. Reference [22] studied haptic interaction cooperative control under different degrees of haptic feedback force, and found that the performance of the system improved more significantly when the feedback force was small. Reference [23] designed a dynamic cooperative steering control system by modeling the biomechanical impedance of the human arm, and realized dynamic torque feedback to the operator by adjusting the cooperative impedance. FigureFigure 2.2. SystemSystem overviewoverview ofof haptichaptic interactioninteraction cooperativecooperative control.control.

2.3.2.3. GuidanceGuidanceIn reference InteractionInteraction [19], aCooperative Cooperativehaptic guidance ControlControl with predictive capability was added to the co- operativeUnderUnder control, thethe guidanceguidance which increases interactiveinteractive the cooperative cooperativcompletione rate controlcontrol of the mode,mode, remote therethere driving isis aa hierarchical taskhierarchical and the cooperativeoperator’scooperative workload relationship relationship were between between improved the the operator to operator a cert andain and theextent. the navigation navigationReference control [20]control systemhas systemproved [24]. The[24].that operatortheThe performanceoperator makes makes a controlof aoperators control decision decision under according accocooper tording theative statuesto controlthe statues and can environmental andbe environmentalimproved perception in bothper- informationlane-keepingception information from and the vehicle-following UGV,from andthe UGV, input tasks. correspondingand input corresponding global or local global guidance or local information guidance toinformation navigateThe key the to factor UGVnavigate of maneuvering haptic the UGV interaction maneuvering toward cooperative target toward position. control target Figure isposition. to3 shows determine Figure an overview the3 shows desired of an theauxiliaryoverview guidance torqueof the interactive guidancethat conforms cooperative intera toctive the control cooperative characteristics system. control of human system. muscular motion. Refer- ence [21] proves that the appropriate relationship model between muscular movement and expected torque is a key factor to improve the interaction performance in the process of cooperative control. Reference [22] studied haptic interaction cooperative control under different degrees of haptic feedback force, and found that the performance of the system improved more significantly when the feedback force was small. Reference [23] designed a dynamic cooperative steering control system by modeling the biomechanical impedance of the human arm, and realized dynamic torque feedback to the operator by adjusting the cooperative impedance.

2.3. Guidance Interaction Cooperative Control Under the guidance interactive cooperative control mode, there is a hierarchical cooperative relationship between the operator and the navigation control system [24]. The operator makes a control decision according to the statues and environmental perception information from the UGV, and input corresponding global or local guidance FigureinformationFigure 3. 3.System System to navigate overviewoverview oftheof guidance guidance UGV maneuvering interaction interaction cooperative cooperative toward target control. control. position. Figure 3 shows an overview of the guidance interactive cooperative control system. Guidance control based on global path is a cooperative control method for wide range navigation. The operator plans the path based on the global map, and UGV takes the path as the guide to complete the maneuvering by relying on its own autonomous navigation ability [25]. In the process of autonomous maneuver, the operator can modify the global path at any time so as to realize the dynamic update of global guidance information [26]. In the local guidance control mode, the operator plans the local path based on the local environmental map to guide the UGV maneuvering in a limited area [27]. The key points guidance mode is also known as the “point-to-go” mode [28], which can be regarded as a special form of local guidance control. In this mode the operator selects a key point from the local map or the video image feedback from the UGV as a maneuvering target point. For navigation control in an unstructured environment, the above guidance control methods have the following limitations: 1. Not all regions have high precision maps for global path guidance. Even for an area with a global map, due to the lack of timely update of map data, the seemingly Figure 3. System overview of guidance interaction cooperative control.

Sensors 2021, 21, 2323 5 of 28

passable area on the map is actually impassable when the natural environment changes, which leads to the infeasibility of the planned global path. 2. Due to the limitation of autonomous capability of a UGV, it is difficult to complete the task only by its own autonomous navigation system in the complex unstructured environment. 3. The operator needs to actively determine the position of the guidance point in the absence of decision support, which results in a large workload for the operator. On the other hand, if the guidance point is not entered in time, it will bring serious fluctuation of speed. In view of these problems, the paper proposes a novel guidance point generation method that is well suited for human–machine cooperative UGV teleoperation in unstructured environments without a predefined goal position. The candidate points can be generated automatically with only the local perception information of the UGV. The operator selects a candidate point as the guidance points through the human-machine interface according to the driving intention. Subsequently, the navigation control system of the UGV generates feasible trajectory line according to its own motion state and environmental perception information, which can be tracked by the actuator.

3. Guidance Point Generation Method The proposed generation method of candidate guidance points mainly includes the following three parts of work: (1) the generation of OGM; (2) obstacle description based on mathematical morphology; (3) generation of candidate guidance points based on skeleton extraction. The details will be covered in this section.

3.1. Generation of Occupied Grid Map (OGM) In the ﬁeld of environment modeling for UGV, the common environmental representation methods include geometric feature map, point cloud map and occupied grid map. The geometric feature map describes the object mainly using simple geometrical elements, e.g., points, lines and planes. This representation method can describe the scene well in the structured environment, but it is difﬁcult to describe complex terrain accurately in the unstructured environment. As an intuitive representation of lidar measurement data, point cloud map contains all the information of the real world, but due to the large amount of data, it is not easy to store and manage. OGM divides the environment into grids, as Sensors 2021, 21, x FOR PEER REVIEWshown in Figure4. Each grid is given a probability of being occupied by an obstacle, which6 of 29 can simply represent the distribution of obstacles in the environment.

FigureFigure 4. 4.Occupied Occupied grid grid map. map.

3.1.1.3.1.1. Assumption Assumption ThisThis paper paper uses uses lidar lidar to collectto collect environmental environmental information information in order in toorder avoid to the avoid impact the ofimpact changes of inchanges lighting inand lighting weather and conditions weather conditions on environmental on environmental features, and features, describes and describes the surrounding environment of the UGV by OGM. Because the application scenario of the UGV is an unstructured outdoor environment, we can make the following assumptions: • Assumption 1: The trajectory of the UGV is located on a two-dimensional plane to ensure that the measured data obtained at different times are in the same plane, so that the environmental information obtained by light detection and ranging (lidar) at close times contains the same environmental objects. • Assumption 2: It is assumed that the environment is static, that is, the positions of all or most of the objects or features in the environment do not change over time. This assumption, also known as the Markov hypothesis, states that if the current state is known, the future state is independent of the past states. • Assumption 3: Based on the assumption of two-dimensional plane motion of UGV, the 6 degrees of freedom (DOF) spatial motion of the UGV can be simplified into 3-DOF plane motion. The motion state of the UGV can be determined by its heading and 2-DOF position information. • Assumption 4: It is assumed that the UGV is a rigid body during motion, and the influence of suspension changes, tire deformation and other factors on the vehicle body and the pitch and roll angle of the lidar is ignored, so as to improve the real-time performance of the map generation algorithm and meet the requirements of high-speed driving.

3.1.2. OGM Update We used a 325 × 150 OGM with a side length of 0.2 m, which corresponded to a range of 65 m × 30 m in the real environment. In the traditional binary OGM representation method, each grid only contains two states of “occupied” or “not occupied”, which are represented by 0 and 1 respectively. However, the obstacle detection algorithm based on lidar point cloud data faces the problem of missed detection and misdetection, which could lead to the incorrect state of some grids. In order to avoid this problem, a probabilistic grid state description method is adopted in this paper, and the probability range of grid occupied is [0, 1]. The occupancy state of a grid m can be expressed by a number 𝑝(𝑚) ∈ 0, 1. The 𝑝(𝑚) is defined as: > 0.5 𝑝(𝑚)= 0.5 , (1) < 0.5 where 𝑝(𝑚) > 0.5 indicates that the grid is occupied by obstacles; 𝑝(𝑚) <0.5 indicates that there is no obstacle in the grid; 𝑝(𝑚) =0.5 indicates that the area where the grid is

Sensors 2021, 21, 2323 6 of 28

the surrounding environment of the UGV by OGM. Because the application scenario of the UGV is an unstructured outdoor environment, we can make the following assumptions: • Assumption 1: The trajectory of the UGV is located on a two-dimensional plane to ensure that the measured data obtained at different times are in the same plane, so that the environmental information obtained by light detection and ranging (lidar) at close times contains the same environmental objects. • Assumption 2: It is assumed that the environment is static, that is, the positions of all or most of the objects or features in the environment do not change over time. This assumption, also known as the Markov hypothesis, states that if the current state is known, the future state is independent of the past states. • Assumption 3: Based on the assumption of two-dimensional plane motion of UGV, the 6 degrees of freedom (DOF) spatial motion of the UGV can be simpliﬁed into 3-DOF plane motion. The motion state of the UGV can be determined by its heading and 2-DOF position information. • Assumption 4: It is assumed that the UGV is a rigid body during motion, and the inﬂuence of suspension changes, tire deformation and other factors on the vehicle body and the pitch and roll angle of the lidar is ignored, so as to improve the real- time performance of the map generation algorithm and meet the requirements of high-speed driving.

3.1.2. OGM Update We used a 325 × 150 OGM with a side length of 0.2 m, which corresponded to a range of 65 m × 30 m in the real environment. In the traditional binary OGM representation method, each grid only contains two states of “occupied” or “not occupied”, which are represented by 0 and 1 respectively. However, the obstacle detection algorithm based on lidar point cloud data faces the problem of missed detection and misdetection, which could lead to the incorrect state of some grids. In order to avoid this problem, a probabilistic grid state description method is adopted in this paper, and the probability range of grid occupied is [0, 1]. The occupancy state of a grid m can be expressed by a number p(m) ∈ [0, 1]. The p(m) is deﬁned as:  >  0.5 p(m) = 0.5 , (1)  < 0.5 where p(m) > 0.5 indicates that the grid is occupied by obstacles; p(m) < 0.5 indicates that there is no obstacle in the grid; p(m) = 0.5 indicates that the area where the grid is located has not been explored. At the initial moment of OGM creation, the states of all the grids in the map can be initialized to p(m) = 0.5 since no valid observations of the environment have been made. Considering that the grids in the map are independent of each other, we only need to update the state of each grid to complete the update of the entire map. The probability expression of the grid occupied state is p(mt|x1:t, z1:t). According to Bayes’ theorem, this expression can be further transformed into Equation (2).

p(mt|x1:t, z1:t−1) · p(zt|x1:t, z1:t−1, mt) p(mt|x1:t, z1:t) = , (2) p(zt|x1:t, z1:t−1)

where t represents the time of data collection; x1:t and z1:t represent the position sequence of UGV and data series collected by lidar from the beginning of OGM creation (t = 1) to the current time, respectively. Since the grid state mt at time t is only related to the data series z1:t, and the data zt have not been obtained at time t−1, the following equation can be obtained:

p(mt|x1:t, z1:t−1) = p(mt−1|x1:t−1, z1:t−1). (3) Sensors 2021, 21, 2323 7 of 28

Since environmental observation results zt at time t is only related to the position of UGV at time t and the grid state mt−1 at time t − 1, the following equation can be obtained:

p(zt|x1:t, z1:t−1, mt) = p(zt|xt, mt−1), (4)

By substituting Equations (3) and (4) into Equation (2), the simpliﬁed probability expression of a grid is obtained by using the following equation:

p(mt−1|x1:t−1, z1:t−1) · p(zt|xt, mt−1) p(mt|x1:t, z1:t) = . (5) p(zt|x1:t, z1:t−1)

According to Bayes’ theorem and the conditional independence principle, p(zt|xt, mt−1) can be transformed as follows:

p(zt|xt) · p(mt−1|xt, zt) p(zt|xt, mt−1) = . (6) p(mt−1)

By substituting Equation (6) into Equation (5), the formula for calculating the gird occupancy probability is deﬁned as follows:

Correspondingly, the formula for calculating the non-occupancy probability can also be obtained as follows:

The left and right sides of Equation (7) divided by the left and right sides of Equation (8) gives us Equation (9).

The sum of probabilities of occupancy and non-occupancy of each grid is equal to 1 under any conditions, so we can transform Equation (9) to Equation (10):

where p(mt−1|x1:t−1, z1:t−1) represents the grid state at time t − 1, and p(mt−1) is the prior value of the gird state, which is only related to the initial state of the grid. Based on the deﬁnition of the grid initial state p(mt−1) = 0.5, we simplify Equation (10) as follows:

then the relation between p(mt|x1:t, z1:t) and p(mt−1|x1:t−1, z1:t−1) is obtained by using the following equations: S p(m |x , z ) = , (12) t 1:t 1:t 1 + S

p(m |x , z ) p(m − |x − , z − ) S = t t t · t 1 1:t 1 1:t 1 . (13) 1 − p(mt|xt, zt) 1 − p(mt−1|x1:t−1, z1:t−1)

In Equation (13), p(mt|xt, zt) represent estimated grid state based on xt and zt, which also known as the inverse sensor model [29]. This model describes how to estimate the environment’s state based on the sensor observation position xt and the sensor measurement value zt. Sensors 2021, 21, 2323 8 of 28

The light source of the lidar used in this paper is 905 nm infrared light, which has the advantages of good directivity and no penetration to most materials. Therefore, it can be considered that obstacles exist only at the location of the laser measurement point. However, due to the limitation of the accuracy of the lidar system, the reflection characteristics of the measured object and the difference of the transmission medium of the beam, the measurement data of the lidar may produce errors. For the above reasons, the possibility of inaccurate measurement is expressed in probability form based on the inverse sensor model.  >  pemp i f d zt p(mt|xt, zt) = pocc i f d = zt , (14)  punknown i f d < zt where: • d is the distance between the center position of the grid and the lidar; • pemp represent the occupancy probability of grid which locate in the area that the laser beam passes through, and pemp 6= 0; • pocc is the occupancy probability of grid where the measurement points are located, and pocc 6= 0; • punknown is the occupancy probability of grid outside the measurement range, and punknown = 0.5; In the course of research it was found that objects with smaller diameters, such as tree trunks and shrubs, are observed less often than they are not. Theoretically we should set pocc + pemp = 1, but if we follow this definition, the occupancy probability of grid where objects with smaller diameters are located will be less than 0.5 in most cases. This means that the locations of these obstacles will not be marked on the OGM. In order to facilitate the detection of fine objects in the environment, we set pocc + pemp > 1 in practical application, where pocc = 0.8, pemp = 0.4.

3.1.3. Binary Image Representation of OGM

Sensors 2021, 21, x FOR PEER REVIEW To achieve skeleton extraction based on mathematical morphology, OGM needs9 of to 29 be transformed into binary images. Let each grid of OGM correspond to a pixel in the image, then the binary image is deﬁned as:

255 i f P(x, y) > T D(x, y) = 255 𝑖𝑓 𝑃(𝑥,) 𝑦 > 𝑇, (15) 𝐷(𝑥,) 𝑦 = 0 otherwise , 0 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒 15) wherewhere 𝐷D((𝑥,x, y) 𝑦 ) represents pixelpixel graygrayvalue, value, and and threshold thresholdT =𝑇=0.50.5. Figure. Figure5 shows 5 shows a binary a bi- naryimage image corresponding corresponding to a realto a environment, real environment, where where the black the pixelblack represents pixel represents the obstacle the obstaclearea, the area, white the pixel white represents pixel represents the passable the passable area, the area, red square the red indicates square theindicates position the of positionthe UGV of inthe the UGV diagram. in the diagram.

(a) Image of real environment (b) Binary image of OGM

FigureFigure 5. 5. AA binary binary image image corresponding corresponding to to a areal real environment. environment.

3.2. Obstacle Description Based on Mathematical Morphology 3.2.1. Basic Operations of Mathematical Morphology Mathematical morphology is a tool for extracting image components that are useful for representation and description [30]. The basic morphological operators are erosion, dilation, opening and closing. Let 𝐸 be a Euclidean space or an integer grid, and 𝐴 a binary image in 𝐸. The erosion of the binary image 𝐴 by the structuring element 𝐵 is defined by:

𝐴 ⊝𝐵=𝑧∈𝐸 | 𝐵 ⊆ 𝐴, (16)

where 𝐵 is the translation of 𝐵 by the vector 𝑧, i.e., 𝐵 = 𝑏+𝑧 | 𝑏∈𝐵,∀∈𝐸. The dilation of A by the structuring element B is defined by: 𝐴 ⊕𝐵=𝑧∈𝐸|(𝐵 ) ∩ 𝐴 ≠𝜙, (17) where 𝐵 denotes the symmetric of 𝐵, that is, 𝐵 = 𝑥∈𝐸 |−𝑥∈𝐵. The opening of 𝐴 by 𝐵 is obtained by the erosion of 𝐴 by 𝐵, followed by dilation of the resulting image by 𝐵: 𝐴 ∘𝐵=(𝐴 ⊝𝐵)⊕𝐵. (18) The closing of 𝐴 by 𝐵 is obtained by the dilation of 𝐴 by 𝐵, followed by erosion of the resulting image by 𝐵: 𝐴 ∙𝐵=(𝐴 ⊕𝐵)⊝𝐵. (19)

Sensors 2021, 21, 2323 9 of 28

3.2. Obstacle Description Based on Mathematical Morphology 3.2.1. Basic Operations of Mathematical Morphology Mathematical morphology is a tool for extracting image components that are useful for representation and description [30]. The basic morphological operators are erosion, dilation, opening and closing. Let E be a Euclidean space or an integer grid, and A a binary image in E. The erosion of the binary image A by the structuring element B is deﬁned by:

A B = {z ∈ E | Bz ⊆ A}, (16)

where Bz is the translation of B by the vector z, i.e., Bz = {b + z | b ∈ B}, ∀z ∈ E. The dilation of A by the structuring element B is deﬁned by:

s A ⊕ B = {z ∈ E|(B )z ∩ A 6= φ}, (17)

where Bs denotes the symmetric of B, that is, Bs = {x ∈ E |−x ∈ B}. The opening of A by B is obtained by the erosion of A by B, followed by dilation of the resulting image by B: A ◦ B = (A B) ⊕ B. (18) The closing of A by B is obtained by the dilation of A by B, followed by erosion of the resulting image by B: A·B = (A ⊕ B) B. (19)

3.2.2. Expanding of Obstacles In order to treat the UGV as a particle in path planning, obstacles in binary image need to be expanded. Considering the irregular and discontinuous shape of obstacles in the unstructured environment, as well as the isolated distribution of some small obstacles in space, a disc-shaped structural element is selected for the expanding of the original image, and the radius of structural element is deﬁned by the 1/2 width of the UGV. In this paper, the widths of the UGV and grid of OGM are 1.97 m and 0.2 m. Thus, the disc-shaped structural element has a radius of 5 pixels. The result of the obstacle expansion is shown below. As can be seen from the Figure6a, there are narrow gaps between some obstacle Sensors 2021, 21, x FOR PEER REVIEW 10 of 29 areas. When we use the skeleton extraction method to generate the local path, it will produce some undesired paths which pass through narrow gaps. Although theoretically the UGV can safely pass through these narrow gaps, considering the positioning accuracy tioningand control accuracy accuracy, and therecontrol is stillaccuracy, a risk there of the is UGV still collidinga risk of withthe UGV an obstacle. colliding Therefore, with an obstacle.we use the Therefore, closing operatorwe use th toe ﬁllclosing the narrowoperator gaps to fill in the narrow expanded gaps binary in the image. expanded The binaryresult isimage. shown The in Figureresult 6isb. shown in Figure 6b.

(a) Obstacle expansion (b) Narrow gaps filling

FigureFigure 6. 6. ObstacleObstacle description description in in binary binary image. image.

3.3.3.3. Generating Generating Algorithm Algorithm of of Candidate Candidate Guidance Guidance Points Points AA skeleton skeleton line line is is one one of of the the important important feat featuresures to todescribe describe the the plane plane region. region. It re- It flectsreﬂects the the geometric geometric characteristics characteristics and and topologi topologicalcal structure structure of of the the plane plane region. region. Skeleton Skeleton extractionextraction algorithms cancan bebe divided divided into into three three categories. categories. The The ﬁrst first is theis the topology topology thinning thin- ningalgorithms algorithms [31], the[31], basic the basic idea ofidea which of which is to continuouslyis to continuously strip thestrip boundary the boundary points points of the ofobject the inobject an iterative in an iterative way until way there until is nothere extra is boundaryno extra boundary to strip off, to andstrip what off, remainsand what is remains is the skeleton. This method can ensure the connectivity of the skeleton, but it is sensitive to boundary noise, and the end of the skeleton may deviate from the center of the object. The second class of method is based on distance transformation [32], which forms the skeleton by looking for gradient ridges of the distance field. The position of the skeleton obtained by this method is accurate, but it is difficult to guarantee the connectivity of the skeleton. The third class of method is based on the Voronoi diagram [33], which has high complexity and requires pruning of the generated skeleton. In order to generate the single-pixel skeleton with accurate connectivity in real time, this paper proposed an improved Zhang–Suen topology thinning algorithm to extract the skeleton [34]. In addition, we perform convex hull transformation on the obstacle area to solve the problem that the skeleton shape is sensitive to the object boundary noise. Fur- thermore, a solution based on the gradient descent method is given to improve the deviation problem of the skeleton end.

3.3.1. An Improved Zhang–Suen Topology Thinning Algorithm The algorithm principle of Zhang–Suen topology thinning algorithm is as follows: • Define the foreground pixel value of a binary image to be 1 and the background point pixel value to be 0. For a certain foreground pixel P1, the pixel set of its eight neighborhoods is shown in Figure 7, where P2~P9 are adjacent pixels of pixel P1;

Figure 7. The neighbor pixels set of pixel P1.

• 𝑀(𝑃) =𝑃 +𝑃 +𝑃 +𝑃 +𝑃 +𝑃 +𝑃 +𝑃; • 𝑁(𝑃) is defined as the number of times that the pixel value changes from 1 to 0 by traversing neighbor pixels around 𝑃clockwise from 𝑃; • The two steps are repeated until no pixels are deleted in either step, and the output is the thinning skeleton of the foreground in binary image.

Sensors 2021, 21, x FOR PEER REVIEW 10 of 29

tioning accuracy and control accuracy, there is still a risk of the UGV colliding with an obstacle. Therefore, we use the closing operator to fill the narrow gaps in the expanded binary image. The result is shown in Figure 6b.

(a) Obstacle expansion (b) Narrow gaps filling Sensors 2021, 21, 2323 Figure 6. Obstacle description in binary image. 10 of 28

3.3. Generating Algorithm of Candidate Guidance Points the skeleton.A skeleton This line method is one can of ensurethe important the connectivity features ofto the describe skeleton, the butplane it is region. sensitive It tore- boundaryflects the geometric noise, and characteristics the end of the skeletonand topologi maycal deviate structure from of the the center plane of region. the object. Skeleton The secondextraction class algorithms of method can is based be divided on distance into three transformation categories. [ 32The], whichfirst is forms the topology the skeleton thin- byning looking algorithms for gradient [31], the ridges basic of idea the of distance which is field. to continuously The position strip of the the skeleton boundary obtained points byof thisthe methodobject in is an accurate, iterative but way it isuntil difficult there to is guarantee no extra boundary the connectivity to strip of off, the and skeleton. what Theremains third is class the ofskeleton. method This is based method on the can Voronoi ensurediagram the connectivity [33], which of the has skeleton, high complexity but it is andsensitive requires to boundary pruning of noise, the generated and the end skeleton. of the skeleton may deviate from the center of the object.In order The to generatesecond class the single-pixelof method is skeleton based withon distance accurate transf connectivityormation in [32], real which time, thisforms paper the skeleton proposed by an looking improved for gradient Zhang–Suen ridges topology of the distance thinning field. algorithm The position to extract of the theskeleton skeleton obtained [34]. Inby addition,this method we performis accurate convex, but it hull is difficult transformation to guarantee on the the obstacle connec- areativity to of solve the theskeleton. problem The that third the class skeleton of me shapethod is is sensitive based on to the objectVoronoi boundary diagram noise. [33], Furthermore,which has high a solutioncomplexity based and on requires the gradient pruning descent of the methodgenerated is skeleton. given to improve the deviationIn order problem to generate of theskeleton the single-pixel end. skeleton with accurate connectivity in real time, this paper proposed an improved Zhang–Suen topology thinning algorithm to extract the 3.3.1.skeleton An Improved[34]. In addition, Zhang–Suen we perform Topology convex Thinning hull transformation Algorithm on the obstacle area to solveThe the algorithm problem principlethat the skeleton of Zhang–Suen shape is topology sensitive thinning to the object algorithm boundary is as follows:noise. Fur- •thermore,Define a thesolution foreground based on pixel the valuegradient of adescent binary method image tois begiven 1 and to improve the background the devi- ationpoint problem pixel of value the skeleton to be 0. Forend. a certain foreground pixel P1, the pixel set of its eight neighborhoods is shown in Figure7, where P2~P9 are adjacent pixels of pixel P1; •3.3.1.M An(P1 Improved) = P2 + P Zhang–Suen3 + P4 + P5 + TopologyP6 + P7 + ThinningP8 + P9; Algorithm • NThe(P1 algorithm) is defined principle as the number of Zhang–Suen of times topology that the pixel thinning value algorithm changes fromis as follows: 1 to 0 by traversing neighbor pixels around P clockwise from P ; • Define the foreground pixel value1 of a binary image2 to be 1 and the background • The two steps are repeated until no pixels are deleted in either step, and the output is point pixel value to be 0. For a certain foreground pixel P1, the pixel set of its eight the thinning skeleton of the foreground in binary image. neighborhoods is shown in Figure 7, where P2~P9 are adjacent pixels of pixel P1;

FigureFigure 7. 7.The The neighborneighbor pixelspixels setset ofof pixelpixel PP11.. • Step𝑀(𝑃 1):=𝑃 Iterate +𝑃 through +𝑃 +𝑃 all the+𝑃 pixels +𝑃 +𝑃 in the +𝑃 foreground,; deleting the pixels that meet • one of𝑁 the(𝑃) following is defined conditions. as the number of times that the pixel value changes from 1 to 0 by 𝑃 𝑃 (1) 2traversing≤ M(P1) neighbor≤ 6; pixels around clockwise from ; • (2) NThe(P 1two) = steps1; are repeated until no pixels are deleted in either step, and the output (3) Pis2 the· P4 thinning· P6 = 0; skeleton of the foreground in binary image. (4) P4 · P6 · P8· = 0. Step 2: Iterate through all the pixels in the foreground, deleting the pixels that meet one of the following conditions. (1) 2 ≤ M(P1) ≤ 6; (2) N(P1) = 1; (3) P2 · P4 · P8 = 0; (4) P2 · P6 · P8· = 0. Although the Zhang–Suen algorithm has good real-time performance and can guarantee the connectivity of the skeleton, non-single pixel skeleton will appear at the fork, corner and step of skeleton. As shown in Figure8, the yellow grid represents redundant pixels that need to be removed. Sensors 2021, 21, x FOR PEER REVIEW 11 of 29 Sensors 2021, 21, x FOR PEER REVIEW 11 of 29

Step 1: Iterate through all the pixels in the foreground, deleting the pixels that meet Step 1: Iterate through all the pixels in the foreground, deleting the pixels that meet one of the following conditions. one of the following conditions. (1) 2≤𝑀(𝑃) ≤6; (1) 2≤𝑀(𝑃) ≤6; (2) 𝑁(𝑃) =1; (2) 𝑁(𝑃) =1; (3) 𝑃 ⋅𝑃 ⋅𝑃 =0; (3) 𝑃 ⋅𝑃 ⋅𝑃 =0; (4) 𝑃 ⋅𝑃 ⋅𝑃 ⋅= 0. (4) 𝑃 ⋅𝑃 ⋅𝑃 ⋅= 0. StepStep 22: :Iterate Iterate through through all all the the pixels pixels in in the the foreground, foreground, deleting deleting the the pixels pixels that that meet meet oneone ofof the the following following conditions. conditions. (1) 2≤𝑀(𝑃) ≤6; (1) 2≤𝑀(𝑃) ≤6; (2) 𝑁(𝑃 ) =1; (2) 𝑁(𝑃) =1; (3) 𝑃 ⋅𝑃 ⋅𝑃 =0; (3) 𝑃⋅𝑃⋅𝑃=0; 𝑃 ⋅𝑃 ⋅𝑃 ⋅= 0 (4)(4) 𝑃⋅𝑃⋅𝑃⋅= 0. . AlthoughAlthough thethe Zhang–SuenZhang–Suen algorithmalgorithm has has good good real-time real-time performance performance and and can can Sensors 2021, 21, 2323 guaranteeguarantee thethe connectivityconnectivity of of the the skeleton, skeleton, non-single non-single pixel pixel skeleton skeleton will will appear appear at11 theat of the 28 fork,fork, cornercorner andand stepstep of of skeleton. skeleton. As As shown shown in in Figure Figure 8, 8,the the yellow yellow grid grid represents represents re- re- dundantdundant pixelspixels that that need need to to be be removed. removed.

FigureFigureFigure 8. 8.8. Non-single Non-single pixel pixel skeletons skeletons generated generated generated by by by Zhang–Suen Zhang–Suen Zhang–Suen algorithm. algorithm. algorithm.

ToToTo solve solvesolve thethe the above above problems, problems, this this this paper paper paper proposes proposes proposes five five five criteria criteria criteria to to toidentify identify identify redundant redundant redundant pixels:pixels:pixels: (1) (𝑃 ×𝑃 =1) ∧ (𝑃 +𝑃 +𝑃 +𝑃 =0); (1)(1) ((P𝑃2 ××𝑃P8 ==11) ∧ (𝑃P4 +𝑃+ P5+𝑃+ P6+𝑃+ P9=0= 0)); ; (2) (𝑃 ×𝑃× =1= ) ∧∧(𝑃 +𝑃+ +𝑃+ +𝑃+ =0=); (2)(2) ((P𝑃6 ×𝑃P8 =11) ∧ (𝑃P2 +𝑃P3 +𝑃P4+𝑃P7=00)); ; (3) (𝑃 ×𝑃 =1) ∧ (𝑃 +𝑃 +𝑃 +𝑃 =0); (3)(3) ((P𝑃2 ××𝑃P4 ==11) ∧ ((𝑃P3 +𝑃+ P6+𝑃+ P7+𝑃+ P8=0= 0)); ; (4) (𝑃 ×𝑃 =1) ∧ (𝑃 +𝑃 +𝑃 +𝑃 =0); (4)(4) ((P𝑃4 ××𝑃P6 ==11) ∧ ((𝑃P2 +𝑃+ P5+𝑃+ P8+𝑃+ P9=0= 0)); ; (5) (𝑃 +𝑃+ +𝑃+ +𝑃+ =0= ) )∧∧(𝑃( +𝑃+ +𝑃+ +𝑃+ =0=). ) (5)(5) (P𝑃3 +𝑃P5 +𝑃P7 +𝑃P9 =00) ∧ (𝑃P2+𝑃P4+𝑃P6+𝑃P8=00). . The first four criteria are used to identify redundant pixels at the fork and corner of TheThe firstfirst four four criteria criteria are are used used to to identify identify redundant redundant pixels pixels at at the the fork fork and and corner corner of of skeleton. The last criterion is used to identify redundant pixels at the step of the skeleton. skeleton.skeleton. TheThe lastlast criterioncriterion isis usedused toto identifyidentify redundantredundant pixelspixels atat thethe stepstep ofof thethe skeleton.skeleton. In order to facilitate binary image processing, we built five templates for eliminating InIn orderorder toto facilitatefacilitate binarybinary imageimage processing,processing, wewe builtbuilt fivefive templates templates for for eliminating eliminating redundant pixels based on the above criteria, as shown in Figure 9. redundantredundant pixels pixels based based on on the the above above criteria, criteria, as as shown shown in in Figure Figure9 .9.

(a) Template 1 (b) Template 2 (c) Template 3 (d) Template 4 (a) Template 1 (b) Template 2 (c) Template 3 (d) Template 4

Sensors 2021, 21, x FOR PEER REVIEW 12 of 29 (e) Template 5 (e) Template 5

FigureFigure 9.9.The The templatestemplates forfor removingremoving redundantredundant pixels. pixels.

FigureFigure 1010 showsshows extractedextracted single-pixelsingle-pixel skeletonsskeletons ofof navigablenavigable regionsregions byby usingusing anan

improvedimproved Zhang–Suen Zhang–Suen algorithm. algorithm.

FigureFigure 10.10.Skeletons Skeletons ofof navigablenavigable regions.regions.

3.3.2.3.3.2. SmoothSmooth SkeletonSkeleton GenerationGeneration BasedBased onon ConvexConvex HullHull TransformTransform AnotherAnother disadvantagedisadvantage of of the the Zhang–Suen Zhang–Suen algorithm algorithm is is that that it isit sensitiveis sensitive to boundaryto bound- noise,ary noise, which which means means that whenthat when the outlinethe outlin of navigablee of navigable region region is not is smoothnot smooth enough, enough, the the extracted skeleton is also not smooth and it is possible to generate spurious branches, as shown in red circle in Figure 11.

Figure 11. Spurious branches of skeleton.

In this research, the obstacle convex hull is constructed to obtain the navigable region with smooth outline. The flow chart in Figure 12 describes the detail of convex hull transform for the grid map.

Sensors 2021, 21, x FOR PEER REVIEW 12 of 29

Figure 9. The templates for removing redundant pixels.

Figure 10 shows extracted single-pixel skeletons of navigable regions by using an improved Zhang–Suen algorithm.

Figure 10. Skeletons of navigable regions.

Sensors 2021, 21, 2323 12 of 28 3.3.2. Smooth Skeleton Generation Based on Convex Hull Transform Another disadvantage of the Zhang–Suen algorithm is that it is sensitive to boundary noise, which means that when the outline of navigable region is not smooth enough, extractedthe extracted skeleton skeleton is also is also not not smooth smooth and and it is it possible is possible to generate to generate spurious spurious branches, branches, as shownas shown in redin red circle circle in Figurein Figure 11. 11.

FigureFigure 11.11. SpuriousSpurious branchesbranches ofof skeleton.skeleton.

InIn thisthis research,research, the the obstacle obstacle convex convex hull hull is constructedis constructed to obtainto obtain the the navigable navigable region re- Sensors 2021, 21, x FOR PEER REVIEWwithgion smoothwith smooth outline. outline. The The ﬂow flow chart chart in Figurein Figure 12 12describes describes the the detail detail of of convex convex13 hullofhull 29

transformtransform forfor thethe gridgrid map.map.

FigureFigure 12.12.Flow Flow chartchart ofof convexconvex hullhull transformtransform forfor gridgrid map.map.

ForFor obstaclesobstacles based based on on grid grid representation, representation, the the internal internal occupied occupied grid grid has nohas contribu- no con- tiontribution to generating to generating a convex a convex hull, so hull, it can so be it considered can be considered to eliminate to eliminate the internal the occupied internal gridsoccupied as much grids as as possible much toas improvepossible theto improve search speed the search of convex speed hull of vertices. convex Inhull this vertices. paper, aIn grid this elimination paper, a grid method elimination for irregular method regions for irregular is proposed. regions This is methodproposed. consists This method of two phases:consists of two phases: PhasePhase 1:1: (1)(1) The leftmostleftmost andand rightmostrightmost gridsgrids ofof eacheach rowrow inin thethe obstacleobstacle areaarea areare markedmarked asas unremovable; (2)(2) The gridsgrids atat thethe toptop andand bottombottom ofof eacheach columncolumn inin thethe obstacleobstacle areaarea areare markedmarked asas unremovable; (3) Eliminate the remaining unmarked obstacle grids in the obstacle area. From the result of the first phase elimination in Figure 13b we can know that there are still a lot of occupied grids inside the obstacle, which can be further removed in Phase 2.

(a) Original OGM (b) The result of the first phase elimination

Sensors 2021, 21, x FOR PEER REVIEW 13 of 29

Figure 12. Flow chart of convex hull transform for grid map.

For obstacles based on grid representation, the internal occupied grid has no contribution to generating a convex hull, so it can be considered to eliminate the internal occupied grids as much as possible to improve the search speed of convex hull vertices. In this paper, a grid elimination method for irregular regions is proposed. This method consists of two phases: Phase 1: (1) The leftmost and rightmost grids of each row in the obstacle area are marked as unremovable; Sensors 2021, 21, 2323 13 of 28 (2) The grids at the top and bottom of each column in the obstacle area are marked as unremovable; (3) Eliminate the remaining unmarked obstacle grids in the obstacle area. (3) EliminateFrom the theresult remaining of the first unmarked phase eliminat obstacleion grids in Figure in the 13b obstacle we can area. know that there are stillFrom a lot the of result occupied of the grids ﬁrst phaseinside eliminationthe obstacle, in which Figure can 13 bbe we further can know removed that therein Phase are still2. a lot of occupied grids inside the obstacle, which can be further removed in Phase 2.

Sensors 2021, 21, x FOR PEER REVIEW 14 of 29

(a) Original OGM (b) The result of the first phase elimination

FigureFigure 13. 13.An An exampleexample ofof convexconvex hullhull verticesvertices searchingsearching forfor irregularirregular region.region.

PhasePhase 2:2:

(1)(1) TraverseTraverse thethe leftmostleftmost grid L𝐿i of of eacheach row.row. If the leftmost grid 𝐿Li−1 and L𝐿i+1 of the upperupper andand lower two rows are both on the left of L𝐿i,, oror either is on the left of of L𝐿i,, and thethe otherother isis inin thethe samesame columncolumn asas L𝐿i;; (2)(2) TraverseTraverse thethe rightmostrightmost grid gridR 𝑅i of of each each row. row. If theIf the rightmost rightmost grid gridRi− 𝑅1and andRi+ 1𝑅ofthe of upperthe upper and and lower lower two two rows rows are are both both on theon the right right of Rofi, or𝑅, either or either is on is theon the right right of R ofi, and𝑅, and the the other other is in is the in the same same column column as R asi; 𝑅; (3)(3) TraverseTraverse thethe top grid T𝑇i of of eacheach column.column. If the top grid grid 𝑇Ti−1 and T𝑇i+1 of of thethe leftleft and rightright twotwo columnscolumns are are both both on on the the top top of ofTi, 𝑇 or, eitheror either is on is the on topthe oftopTi ,of and 𝑇, the and other the isother in the is in same the rowsame as rowTi; as 𝑇; (4)(4) TraverseTraverse thethe bottombottom gridgridB 𝐵i of of each each column. column. If theIf the bottom bottom grid gridBi− 𝐵1 and andBi+ 1𝐵of the of leftthe andleft and right right two two columns columns are bothare both on the on below the below of B iof, or 𝐵 either, or either is on theis on below the below of Ti, andof 𝑇 the, and other the isother in the is in same the rowsame as rowBi. as 𝐵. ComparedCompared with with the the result result of of the the ﬁrst first phase, phase, 14 14 occupied occupied grids grids such such as F2,as H2,F2, H2, D3, D3, H3, B4,H3, I5, B4, C7, I5, H7, C7, F8, H7, H8, F8, C9, H8, D9 C9, and D9 I9 and were I9 further were further removed removed in the secondin the second stage, as stage, shown as inshown Figure in 13Figurec. Subsequently, 13c. Subsequently, the Graham the Graham scanning scanning method method [35] was[35] usedwas used to search to search the convexthe convex hull hull vertices vertices of obstacles of obstacles in the in remainingthe remaining occupied occupied grids. grids. The The search search results results are representedare represented in green in green grids grids in Figure in Figure 13d. 13d. AfterAfter ﬁnishing finishing the the search search of of convex convex hull hull vertices, vertices, it isit necessaryis necessary to connectto connect the the vertices ver- withticesstraight with straight lines to lines form to an form envelope. an envelope. Since a Since binary a binary image isimage a discrete is a discrete representation represen- of two-dimensionaltation of two-dimensional space, we space, use Bresenham’s we use Bresenham’s line algorithm line algorithm [36] to search [36] to grids search that grids can approximatelythat can approximately represent represent a straight a line straight in binary-image. line in binary-image. The envelopes The envelopes of obstacles of areob- stacles are shown in Figure 14a. There are some gaps between the envelope and the boundary of the obstacle. If these gaps are not filled, the skeleton of these gaps will also be extracted in the subsequent skeleton extraction process, which reduces the efficiency of skeleton extraction for binary-image. Therefore, we use a hole-filling algorithm based on contours to fill these gaps [37]. Figure 14b shows the results of gap filling, which is also the end result of convex hull transform. Then we can obtain a smooth skeleton of navigable regions, as shown in Figure 14c.

(a) (b) (c)

Sensors 2021, 21, x FOR PEER REVIEW 14 of 29

(c) The result of the second phase elimination (d) The convex hull vertices Figure 13. An example of convex hull vertices searching for irregular region.

Phase 2:

(1) Traverse the leftmost grid 𝐿 of each row. If the leftmost grid 𝐿 and 𝐿 of the upper and lower two rows are both on the left of 𝐿, or either is on the left of 𝐿, and the other is in the same column as 𝐿; (2) Traverse the rightmost grid 𝑅 of each row. If the rightmost grid 𝑅 and 𝑅 of the upper and lower two rows are both on the right of 𝑅, or either is on the right of 𝑅, and the other is in the same column as 𝑅; (3) Traverse the top grid 𝑇 of each column. If the top grid 𝑇 and 𝑇 of the left and right two columns are both on the top of 𝑇, or either is on the top of 𝑇, and the other is in the same row as 𝑇; (4) Traverse the bottom grid 𝐵 of each column. If the bottom grid 𝐵 and 𝐵 of the left and right two columns are both on the below of 𝐵, or either is on the below of 𝑇, and the other is in the same row as 𝐵. Compared with the result of the first phase, 14 occupied grids such as F2, H2, D3, H3, B4, I5, C7, H7, F8, H8, C9, D9 and I9 were further removed in the second stage, as shown in Figure 13c. Subsequently, the Graham scanning method [35] was used to search the convex hull vertices of obstacles in the remaining occupied grids. The search results are represented in green grids in Figure 13d. Sensors 2021, 21, 2323 After finishing the search of convex hull vertices, it is necessary to connect the14 ver- of 28 tices with straight lines to form an envelope. Since a binary image is a discrete representation of two-dimensional space, we use Bresenham’s line algorithm [36] to search grids that can approximately represent a straight line in binary-image. The envelopes of ob- staclesshown are in Figure shown 14 ina. ThereFigure are 14a. some There gaps are between some thegaps envelope between and the the envelope boundary and of the boundaryobstacle. If of these the obstacle. gaps are notIf these filled, gaps the are skeleton not filled, of these the gapsskeleton will alsoof these be extracted gaps will in also the besubsequent extracted skeleton in the subsequent extraction process,skeleton which extraction reduces process, the efficiency which reduces of skeleton the extractionefficiency offor skeleton binary-image. extraction Therefore, for binary-image. we use a hole-filling Therefore, algorithm we use a based hole-filling on contours algorithm to fill based these ongaps contours [37]. Figure to fill 14 theseb shows gaps the [37]. results Figure of gap 14b filling, shows which the results is also theof gap end filling, result ofwhich convex is alsohull the transform. end result Then of weconvex can obtainhull transfor a smoothm. skeletonThen we ofcan navigable obtain a regions, smooth asskeleton shown of in navigableFigure 14c. regions, as shown in Figure 14c.

Sensors 2021, 21, x FOR PEER REVIEW 15 of 29

(a) (b) (c) Figure 14. Smooth skeleton extraction for binary-image. (a) The red closed curves represent the envelopes of the obsta- Figurecles; (b 14.) resultSmooth of convex skeleton hull extraction transform; for ( binary-image.c) extracted smooth (a) The skeletons red closed after curves convex represent hull transform the envelopes of obstacles. of the obstacles; (b) result of convex hull transform; (c) extracted smooth skeletons after convex hull transform of obstacles. There may be several unconnected navigable regions in OGM, among which only the skeletonThere may of the be severalnavigable unconnected region where navigable the UGV regions is currently in OGM, located among is whichthe desired only theskeleton. skeleton In order of the to navigable reduce the region computational where the load, UGV it is is currently necessary located to find isout the the desired skele- skeleton.ton corresponding In order to to reduce the current the computational position of the load, UGV. it is The necessary basic idea to find of this out theprocess skele- is tontransforming corresponding the skeleton to the currentsearch problem position into of the a data UGV. classification The basic idea problem of this by process using the is transformingk-nearest neighbor the skeleton (kNN) search algorithm problem [38]. into The a data position classification data of problemall skeleton by usingpoints the of k-nearesteach skeleton neighbor was regarded (kNN) algorithm as a sample [38 ].set, The and position the position data of of all the skeleton UGV was points regarded of each as skeletonthe data wasto be regarded classified. as The a sample correlation set, and between the position the data of to the be UGV classified was regarded and the sample as the datais judged to be classified.by calculating The correlationthe distance between between the them. data toEuclidean be classified distance and thewas sample used isto judgedmeasure by the calculating distance thebetween distance the betweentest data them. and all Euclidean the training distance samples, was usedwhich to is measure defined the distance between the test data and all the training samples, which is defined as: as: ! 1 m 2 D(x, y) = |x − y |2 , (20) 𝐷(𝑥,) 𝑦 =∑ |i 𝑥 −𝑦i | , i=1 (20) wherewhere D(𝐷x(,𝑥,y) represents) 𝑦 represents distance distance between between point x andpointy, andx mand= 2 y,in two-dimensionaland 𝑚=2 in space.two-dimensional The searching space. result The of searching the desired result skeleton of the is desired shown inskeleton Figure 15is .shown The red in squareFigure indicates15. The red the square position indicates of the UGVthe posi in thetion diagram. of the UGV in the diagram.

FigureFigure 15.15. TheThe searchingsearching resultresult ofof thethe desireddesired skeleton.skeleton.

3.3.3.3.3.3. OptimizationOptimization ofof SkeletonSkeleton ShapeShape TheThe Zhang–SuenZhang–Suen algorithmalgorithm isis aa thinningthinning methodmethod basedbased onon thethe grassﬁregrassfire transform.transform. ItIt cancan bebe describeddescribed asas “setting“setting ﬁre”fire” toto thethe bordersborders ofof anan imageimage regionregion toto yieldyield descriptorsdescriptors suchsuch asas thethe region’sregion’s skeletonskeleton oror medialmedial axis.axis. However,However, anyany pointpoint cancan onlyonly traveltravel inin aa maximummaximum ofof 88 directionsdirections inin aa discretediscrete two-dimensionaltwo-dimensional space,space, soso itit isis hardhard toto simulatesimulate arbitraryarbitrary burningburning direction. direction. Therefore, Therefore, when when the the skeleton skeleton is extracted is extracted from from the region the region with irregularwith irregular shape, shape, it is likely it is tolikely generate to generate a skeleton a skeleton branch thatbranch is biased that is towards biased thetowards obstacle. the obstacle. In this paper, a skeleton shape optimization method based on gradient descent is proposed to solve the above problem. First of all, the skeleton is divided into two parts: trunk and branch. The segment connecting two fork points in the skeleton is defined as the trunk. The segment connecting a fork point and an endpoint is defined as the branch. It can be seen that the precondition of skeleton decomposition is to find the fork points and endpoints accurately, and the corresponding discriminant conditions are as follows: • The definition of the foreground pixel 𝑃 and its eight neighborhoods in Section 3.3.1 is followed. • 𝑀(𝑃) =𝑃 +𝑃 +𝑃 +𝑃 +𝑃 +𝑃 +𝑃 +𝑃; • Iterate through all the pixels in the foreground; • Mark the pixel with 𝑀(𝑃) =1 as the endpoint; • Mark the pixel with 𝑀(𝑃) =3 or 𝑀(𝑃) =4 as fork point. The searching result of fork points and endpoints is shown in Figure 16a, and blue and red circles indicate fork points and endpoints respectively. For efficiency, we only

Sensors 2021, 21, 2323 15 of 28

In this paper, a skeleton shape optimization method based on gradient descent is proposed to solve the above problem. First of all, the skeleton is divided into two parts: trunk and branch. The segment connecting two fork points in the skeleton is defined as the trunk. The segment connecting a fork point and an endpoint is defined as the branch. It can be seen that the precondition of skeleton decomposition is to find the fork points and endpoints accurately, and the corresponding discriminant conditions are as follows:

• The deﬁnition of the foreground pixel P1 and its eight neighborhoods in Section 3.3.1 is followed. • M(P1) = P2 + P3 + P4 + P5 + P6 + P7 + P8 + P9; • Iterate through all the pixels in the foreground; • Mark the pixel with M(P1) = 1 as the endpoint; SensorsSensors 20212021,, 2121,, xx FORFOR PEERPEER REVIEWREVIEW• Mark the pixel with M(P1) = 3 or M(P1) = 4 as fork point. 1616 ofof 2929

The searching result of fork points and endpoints is shown in Figure 16a, and blue and red circles indicate fork points and endpoints respectively. For efﬁciency, we only deal withdealdeal with thewith skeleton thethe skeletonskeleton branch branchbranch ahead aheadahead of the ofof UGV. thethe TheUGUGV.V. result TheThe ofresultresult skeleton ofof skeletonskeleton decomposition decompositiondecomposition is shown isis showninshown Figure inin 16 FigureFigureb,c. 16b,c.16b,c.

((aa)) ((bb)) ((cc))

FigureFigure 16.16. TheThe process process of of skeletonskeleton decomposition.decomposition. decomposition. ((aa ()a) Fork)Fork Fork pointspoints points andand and endpointsendpoints endpoints ofof of thethe the skeleton;skeleton; skeleton; ((bb)) ( bthethe) the skeletonskeleton skeleton trunk;trunk; trunk; ((cc)) (thethec) the skeletonskeleton skeleton branchbranch branch aheadahead ahead ofof the ofthe the UGV.UGV. UGV.

InIn orderorder toto applyapply thethe gradientgradientgradient descentdescent algorithm,algoalgorithm,rithm, wewe constructedconstructed aa distancedistance fieldfieldfield ofofof thethe OGMOGM utilizingutilizing distancedistancedistance transformtransformtransform methodmethodmethod [[39].[39].39]. TheThe distancedistance transformationtransformation ofof thethethe OGMOGM is is toto calculatecalculate thethe minimumminimum distancedistance fromfrom eacheach non-occupiednon-occupied gridgrid toto thethe gridgrid ofof obstacleobstacle boundary.boundary.boundary. When WhenWhen there therethere are arearem 𝑚𝑚isolated isolatedisolated obstacles obstaclesobstacles in theinin thth OGM,ee OGM,OGM, the the distancethe distancedistance field fieldfield can canbecan expressed bebe expressedexpressed as follows: asas follows:follows: 𝐹𝐹 ((𝑘𝑘))== min𝑑 min𝑑((𝑘,𝑘, 𝑆 𝑆 𝑖𝑖)),, 𝑖 𝑖 = = 1,2,1,2, ⋯ ⋯ ,𝑚 ,𝑚 Fd(k) = min(d(k, S[i])), i = 1, 2, ··· , m,, (21) (21)(21) where F (k) is the value of the distance field; d(, ) is Euclidean distance calculation function; wherewhere 𝐹d𝐹((𝑘𝑘)) isis thethe valuevalue ofof thethe distancedistance field;field; 𝑑𝑑((,,)) 𝑖𝑠𝑖𝑠 EuclideanEuclidean distancedistance calculationcalculation function;kfunction;is an arbitrary 𝑘𝑘 is is anan non-occupied arbitraryarbitrary non-occupiednon-occupied grid; S is collection grid;grid; 𝑆𝑆 isof is collectioncollection obstacle boundary ofof obstacleobstacle grids. boundaryboundary An example grids.grids. AnofAn constructed exampleexample ofof distance constructedconstructed field distancedistance is shown fieldfield in Figure isis shownshown 17. inin FigureFigure 17.17.

FigureFigure 17.17. AnAn exampleexample ofof thethe occupiedoccupied grgridgridid mapmap (OGM)(OGM) distancedistance ﬁeld.field.field.

GradientGradient descent descent is is aa ﬁrst-orderfirst-orderfirst-order iterativeiterative optimizationoptimization algorithmalgorithm forfor ﬁndingfindingfinding aa locallocallocal minimumminimum of of a a differentiabledifferentiable function.function. The The idea idea is is to to taketake repeatedrepeated stepssteps inin thethethe oppositeoppositeopposite directiondirection ofof thethe gradientgradient (or(or approximateapproximate gradient)gradient) ofof thethe functionfunction atat thethe currentcurrent point,point, becausebecause thisthis isis thethe directiondirection ofof steepeststeepest descent.descent. ThroughThrough eacheach iteration,iteration, thethe functionfunction toto bebe optimizedoptimized isis graduallygradually reduced,reduced, soso asas toto solvesolve thethe minimumminimum valuevalue ofof thethe function.function. TheThe iterativeiterative translationtranslation processprocess ofof skeletonskeleton pointspoints basedbased onon gradientgradient descentdescent cancan bebe de-de- scribedscribed byby thethe formula:formula:

𝑥𝑥,, =𝑥=𝑥,,−𝑎−𝑎𝑔𝑥𝑔𝑥,, ,, (22)(22)

wherewhere 𝑥𝑥,, isis thethe positionposition ofof thethe i-thi-th skeletonskeleton pointpoint obtainedobtained afterafter thethe k-thk-th iteration;iteration; 𝑔𝑥𝑔𝑥,, isis thethe gradientgradient magnitudemagnitude ofof thethe i-thi-th skeletonskeleton pointpoint atat positionposition 𝑥𝑥,,;; 𝑎𝑎 isis thethe it-it- erationeration stepstep length.length. BecauseBecause thethe distancedistance fieldfield ofof OGMOGM isis aa didiscretescrete two-dimensionaltwo-dimensional plane,plane, wewe utilizeutilize horizontalhorizontal andand verticalvertical convolutionconvolution toto calculatecalculate thethe horizontalhorizontal andand verticalvertical gradientgradient val-val- uesues ofof eacheach positionposition respectively,respectively, andand thenthen thethe gradientgradient magnitudemagnitude cancan bebe obtainedobtained byby usingusing EquationEquation (23).(23).

Sensors 2021, 21, 2323 16 of 28

direction of the gradient (or approximate gradient) of the function at the current point, because this is the direction of steepest descent. Through each iteration, the function to be optimized is gradually reduced, so as to solve the minimum value of the function. The iterative translation process of skeleton points based on gradient descent can be described by the formula: xi,k+1 = xi,k − akg(xi,k), (22)

Sensors 2021,, 21,, xx FORFOR PEERPEER REVIEWREVIEWwhere xi,k is the position of the i-th skeleton point obtained after the k-th iteration;17g( xofi ,k29)

is the gradient magnitude of the i-th skeleton point at position xi,k; ak is the iteration step length. Because the distance ﬁeld of OGM is a discrete two-dimensional plane, we utilize horizontal and vertical convolution to calculate the horizontal and vertical gradient values 𝑔𝑥 =||𝑆 || +𝑆 ,, of each position respectively, and then,, the gradient magnitude can be obtained by using(23)(23) Equation (23). where horizontal gradient value 𝑆 andandq verticalvertical gradientgradient valuevalue 𝑆 areare calculatedcalculated byby 2 2 Equations (24) and (25) respectively.g(xi,k ) = |Sx| + Sy , (23) where horizontal𝑆 = gradient𝑓(𝑖+1,𝑗+1𝑖+1,𝑗+1 value)S+2x and𝑓(𝑖+1,𝑗𝑖+1,𝑗 vertical) + gradient𝑓(𝑖+1,𝑗−1𝑖+1,𝑗−1 value S) y−are calculated by Equa- (24)(24) tions (24) and (25) respectively.𝑓(𝑖−1,𝑗+1𝑖−1,𝑗+1) −2𝑓(𝑖−1,𝑗𝑖−1,𝑗) − 𝑓(𝑖−1,𝑗−1𝑖−1,𝑗−1) ,,

Sx = f (i + 1, j + 1) + 2 f (i + 1, j) + f (i + 1, j − 1)− f (i − 1, j + 1) − 2 f (i − 1, j) − f (i − 1, j − 1), (24) 𝑆 = 𝑓(𝑖+1,𝑗+1𝑖+1,𝑗+1) +2𝑓(𝑖,𝑖, 𝑗 𝑗) +1 +1 + 𝑓(𝑖−1,𝑗+1𝑖−1,𝑗+1) − (25)(25) Sy = f (i + 1, j + 1) + 2 f (i, j + 1) + f (i𝑓−(𝑖−1,𝑗−1𝑖−1,𝑗−11, j + 1)− f)(i−2− 1,𝑓(j𝑖,𝑖,− 1 𝑗 𝑗) −) −1 −1 2−f (𝑓i,(𝑖+1,𝑗−1j𝑖+1,𝑗−1− 1) − f (i)+ ,, 1, j − 1), (25) TheThe terminationtermination condition condition of of iteration iteration of of the the gradient gradient descent descent algorithm algorithm is deﬁned is defined as: ( ) ( ) Letas: gLetxi ,k𝑔𝑥and,,g andxi,k −𝑔𝑥1 be,, the gradientbebe thethe gradientgradient magnitude magnitudemagnitude of skeleton ofof point skeletonskeletonxi for pointpoint twoconsecutive 𝑥 forfor twotwo iterations,consecutive and iterations,ε be the difference and 𝜀 be threshold the difference of the gradient threshold magnitude, of the whengradient meet magnitude, condition consecutive iterations, and be the difference threshold of the gradient magnitude, g(x ) − g(x ) < ε, the iteration ends. As shown in Figure 18, the green points are the wheni,k meet conditioni,k−1 𝑔𝑥𝑔𝑥,,−𝑔𝑥,, < 𝜀,, thethe iterationiteration ends.ends. AsAs shownshown inin FigureFigure 18,18, originalthethe greengreen skeleton pointspoints areare branches, thethe originaloriginal and the skeletonskeleton blue points branches,branches, are theandand new thethe blue distributionblue pointspoints areare of thethethe skeletonnewnew dis-dis- pointstributiontribution after ofof the thethe translation. skeletonskeleton pointspoints afterafter thethe translation.translation.

FigureFigure 18.18. TheThe resultresult ofof skeletonskeleton shapeshape optimization.optimization.

3.3.4.3.3.4. CandidateCandidate GuidanceGuidance PointPoint GenerationGeneration AfterAfter thethe translationtranslation byby thethe gradientgradient descentdescent method,method, thethe topologicaltopological connectionconnection betweenbetween thethe skeletonskeleton pointspoints isis destroyed,destroyed, soso ititit isisis necessary necessarynecessary tototo construct constructconstruct aaa new newnew skeleton skeletonskeleton bendbend basedbased onon thethe currentcurrent spatialspatial distributiondistribution ofof the thethe skeleton skeletonskeletonpoints. points.points. In InIn this thisthis paper, paper,paper,we weweuse useuse cubiccubic polynomialpolynomial toto ﬁtfit thethe skeletonskeleton points.points. AsAs cancancan bebebe seenseenseen fromfromfrom FigureFigureFigure 19 19,19,, the thethe skeleton skeletonskeleton branchesbranches obtainedobtained byby ﬁttingfitting areare closeclose toto thethethemidline midlinemidline position positionposition of ofof the thethe navigable navigablenavigablearea. area.area.

FigureFigure 19.19. TheThe resultresult ofof skeletonskeleton shapeshape optimization.optimization.

Taking the end point of the new skeleton branch (red points in Figure 19) as the expected target position of UGV, and taking the tangent direction of the curve at this posi- tiontion asas thethe expectedexpected heading,heading, multiplemultiple candidatecandidate guidanceguidance pointspoints containingcontaining positionposition andand heading information can be obtained. The candidate guidance point can be defined as follows:follows: (26) 𝑥,, =𝑥,, ,, (26)

𝑦,, =𝑦,, ,, (27)(27)

Sensors 2021, 21, 2323 17 of 28

Taking the end point of the new skeleton branch (red points in Figure 19) as the expected target position of UGV, and taking the tangent direction of the curve at this Sensors 2021, 21, x FOR PEER REVIEWposition as the expected heading, multiple candidate guidance points containing position18 of 29 and heading information can be obtained. The candidate guidance point can be deﬁned as follows: xi,t = xi,n, (26) 𝜕𝑓 𝜃 = 𝑎𝑟𝑐𝑡𝑎𝑛y = 𝑥y ,,𝑦 , (28) (27) i,t𝜕𝑥 i,n, , ∂ fi θ = arctan (xi n, yi n) , (28) where 𝑥,,𝑦, and 𝜃 represent position∂ xand , heading, of candidate guidance point, 𝑥,,𝑦, is the end point of the skeleton branch, 𝑓(𝑥,) 𝑦 is the curve function of i-th where (x , y ) and θ represent position and heading of candidate guidance point, (x , y ) skeleton ibranch.,t i,t i,n i,n is the end point of the skeleton branch, fi(x, y) is the curve function of i-th skeleton branch.

4.4. Human-Machine Human-Machine Cooperation-Based Cooperation-Based UGV Teleoperation WhenWhen the candidatecandidate guidanceguidance point point is is generated generated by by the the environment environment perception perception sys- system,tem, the the operator operator needs needs to select to select the appropriate the appropriate guidance guidance point through point through the teleoperation the tele- operationsystem as system the tracking as the targettracking of UGVtarget autonomous of UGV autonomous maneuvering. maneuvering. This human–machine This human– machinecooperative cooperative control mode control based mode on based guidance on guidance information information interaction interaction transforms transforms the opera- thetor’s operator’s high-frequency high-frequency driving control driving behavior control into behavior low-frequency into low-frequency decision behavior, decision which behavior,further which decouples further the operatordecouples and the UGV, operator thus and reducing UGV, the thus sensitivity reducing of the the sensitivity control loop of theto delay. control This loop section to delay. will giveThis asection detailed will introduction give a detailed to the intr human–machineoduction to the interaction human– machineprocess in interaction collaborative process control. in collaborative control.

4.1.4.1. Human–Machine Human–Machine Interaction Interaction Mechanism Mechanism Design Human–machineHuman–machine cooperative cooperative control control based on guidance information interaction realizesrealizes a hierarchical cooperation control through the information interaction among operator,operator, teleoperationteleoperation systemsystem and and the the UGV. UGV. In In this this process, process, first first of all,of all, candidate candidate guidance guid- ancepoints points need toneed be superimposedto be superimposed in the UGV in the feedback UGV feedback video. In video. order toIn makeorder the to candidatemake the candidateguidance pointsguidance overlay points the overlay dynamic the image dynami alwaysc image on a always fixed position on a fixed in the position scene, firstin the of scene,all, we first need of to all, convert we need the relativeto convert position the re oflative the guidance position point of the in guidance the vehicle point coordinate in the vehiclesystem coordinate to the absolute system position to the absolute in the global position coordinate in the global system. coordinate Then, system. based on Then, the basedtransformation on the transformation relationship between relationship the global between coordinate the global system coordinate and the image system coordinate and the imagesystem, coordinate the candidate system, guidance the candidate points areguidan superimposedce points are to superimposed each frame of to the each video. frame In ofaddition, the video. for theIn addition, intuitive andfor the efficient intuitive human-machine and efficient interaction,human-machine this paper interaction, chooses this the papertouch chooses screen as the the touch human screen interface as the device human of in theterface teleoperation device of system.the teleoperation The basic system. process Theof human–machine basic process of interactionhuman–machine is shown interaction in Figure is 20 shown. in Figure 20.

Figure 20. Human–machine interaction for cooperative control.

TheThe interaction mechanism of human-machine system is as follows: 1. TheThe autonomous autonomous control control system system generates generates candidate guidance points periodically andand sends sends them them to to the the teleoperation teleoperation system. system. 2.2. TheThe teleoperation teleoperation system system overlays overlays the the ca candidatendidate guidance guidance points points with the video imagesimages and and displays displays them. them. 3. The operator selects a candidate point as the guidance point of the UGV according to the control intention. 4. The teleoperation system sends the selected guidance point back to the UGV, and the autonomous control system takes the guidance point as the input to generate the desired trajectory line based on the kinematics constraints of the UGV. 5. During trajectory tracking, if the operator selects a new guidance point, the UGV will immediately generate trajectory based on the new guidance point.

Sensors 2021, 21, 2323 18 of 28

3. The operator selects a candidate point as the guidance point of the UGV according to the control intention. 4. The teleoperation system sends the selected guidance point back to the UGV, and Sensors 2021, 21, x FOR PEER REVIEW 19 of 29 the autonomous control system takes the guidance point as the input to generate the desired trajectory line based on the kinematics constraints of the UGV. 5. During trajectory tracking, if the operator selects a new guidance point, the UGV will 6. immediatelyWhen the UGV generate is close trajectory to the current based on guidance the new point, guidance if the point. operator has not yet 6. Whenprovided the a UGV new isguidance close to point, the current the autono guidancemous point, control if thesystem operator will select has not a can- yet provideddidate guidance a new guidance point as point,the tracking the autonomous target point control by itself system according will select to the a candidate principle guidanceof minimum point trajectory as the trackingcurvature. target point by itself according to the principle of 7. minimumIf no new guidance trajectory point curvature. is available, the UGV will stop automatically. 7. If no new guidance point is available, the UGV will stop automatically. 4.2. Trajectory Generation Based On Kinematic Constraints 4.2. Trajectory Generation Based On Kinematic Constraints When the UGV is running at a high speed, the scene in the video changes rapidly, leadingWhen to the the position UGV is of running the guide at a point high on speed, the screen the scene moving in the rapidly, video changeswhich makes rapidly, the leadingoperator to fail the to position select ofthe the guide guide point. point In on ad thedition, screen it movingis not helpful rapidly, for which the operator makes the to operatorknow the fail future to select driving the guide trend point. of the In UGV addition, only itby is marking not helpful several for the candidate operator guidance to know thepoints future on drivingthe video trend image. of the Therefore, UGV only a bymore marking effective several way candidateto display guidance guidance points infor- onmation the video is to image.superimpose Therefore, several a more candidate effective trajectories way to display on the guidance video image, information as shown is toin superimposeFigure 21. several candidate trajectories on the video image, as shown in Figure 21.

FigureFigure 21.21. SuperimposedSuperimposed candidatecandidate trajectoriestrajectories onon thethe videovideo image.image.

TheThe advantageadvantage of of using using trajectory trajectory superposition superposition is that is the that pixel the region pixel correspondingregion corre- tosponding the entire to trajectorythe entire cantrajectory respond can to respond the operator’s to the clickoperator’s operation, click operation, thus enlarging thus theen- responselarging the region response of touch region operation of touch from operation a small from pixel a region small pixel to a banded region pixelto a banded region. pixel The operatorregion. The does operator not have does to carefully not have click to oncarefu a locationlly click on on the a screen, location which on the greatly screen, improves which thegreatly success improves rate of thethe operation.success rate Another of the beneﬁtoperation. is that, Another by observing benefit theis that, predicted by observing driving trajectory,the predicted the operatordriving trajectory, can intuitively the operator know the can locations intuitively in theknow scene the wherelocations the in UGV the willscene probably where the pass UGV in the will next probably period pass of time, in the which next isperiod conducive of time, to which the operator is conducive to make to forward-lookingthe operator to make and safeforward-looking decisions. and safe decisions. InIn orderorder to to make make the the superimposed superimposed trajectory trajectory line line conform conform to to the the actual actual driving driving state state of theof the UGV, UGV, a candidate a candidate trajectory trajectory line is generatedline is generated based on based the motion on the differential motion differential constraint of the UGV. constraint of the UGV. . x = v × cos(θ), (29) (29) 𝑥 .=𝑣×cos(𝜃) , y = v × sin(θ), (30) 𝑦 = 𝑣 × sin(𝜃) , (30) . tan(δ) θ = v ×tan(𝛿) , (31) 𝜃 =𝑣× L , (31) where (x, y) is the current position of the UGV,𝐿 θ is the heading angle, v is the speed, Lwhereis the ( wheelbase,𝑥,) 𝑦 is the andcurrentδ is position the front of wheel the UGV, angle. 𝜃 Accordingis the heading to the angle, reference 𝑣 is the [40 speed,], the curve𝐿 is the satisfying wheelbase, the above and 𝛿 differential is the front equations wheel angle. of motion According can be describedto the reference by polynomial [40], the curve satisfying the above differential equations of motion can be described by polynomial parametric equation of degree 𝑛( 𝑛≥3). Therefore, the position of the UGV can be expressed as follows: 𝑥=𝑥(𝑢),𝑢 ∈0,1 , (32) 𝑦=𝑦(𝑢),𝑢 ∈0,1 , (33)

Sensors 2021, 21, 2323 19 of 28

Sensors 2021, 21, x FOR PEER REVIEW 20 of 29 parametric equation of degree n (n ≥ 3). Therefore, the position of the UGV can be expressed as follows: x = x(u), u ∈ [0, 1], (32) where 𝑥(𝑢) and 𝑦(𝑢) represent polynomial parametric equations of 𝑥 and 𝑦 coor- y = y(u), u ∈ [0, 1], (33) dinates, 𝑢 is dimensionless parameter, 𝑥(0),𝑦(0) and 𝑥(1),𝑦(1) represent the wherecoordinatesx(u) and of they(u )startingrepresent and polynomial end position parametrics. The parametric equations ofequationx and y transformscoordinates, theu isproblem dimensionless of solving parameter, the trajectory(x(0) ,curvey(0)) andinto (thex(1 problem), y(1)) represent of solving the the coordinates coefficient ofof thethe startingparametric and equation. end positions. According The parametric to the current equation posture transforms (𝑥,𝑦 the,𝛿,𝜃 problem, ) and ofthe solving target the trajectory curve into the problem of solving the coefﬁcient of the parametric equation. posture (𝑥,𝑦,𝛿,𝜃, ), the following boundary conditions can be obtained: According to the current posture (x , y , δ , θ ,) and the target posture (x , y , δ , θ ,), S S S S E E E E(34) the following boundary conditions𝑥( can0) =𝑥 be obtained:,𝑥(1) =𝑥,

𝑦(0) =𝑦,𝑦(1) =𝑦, (35) x(0) = xS, x(1) = xE, (34) 𝛿(0) =𝛿,𝛿(1) =𝛿, (36) y(0) = y , y(1) = y , (35) 𝑥(0) S E ( ) = ( ) = δ 𝑦0(0) δS, δ 1 cosδE 𝜃, (36)(37) . = , 𝑥(x0()0+𝑦) (0) sin 𝜃 . ( ) y𝑥(1)0 cos θS p = , (37) x2( ) + y2( ) sin θS 0𝑦(1) 0 cos 𝜃 (38) . = . 𝑥(x1()1+𝑦) (1) sin 𝜃 . y(1) cos θE The above boundary conditionsp can determine= the .unique solution of the cubic (38) polynomial parametric equation.x2( 1In) +ordery2(1 )to makesin theθE trajectory curve have more de- greesThe of abovefreedom boundary and reduce conditions the impact can determinestrength on the the unique curve, solution the polynomial of the cubic paramet- poly- nomialric equations parametric of degree equation. 5 was In selected order to to make model the the trajectory trajectory curve curve, have which more are degrees given ofas freedomfollows: and reduce the impact strength on the curve, the polynomial parametric equations of degree 5 was selected to model the trajectory curve, which are given as follows: (39) 𝑥(𝑢) =𝑥 +𝑥𝑢+𝑥𝑢 +𝑥𝑢 +𝑥𝑢 +𝑥𝑢 , 2 3 4 5 𝑦(x𝑢()u)=𝑦= x0+𝑦+ x𝑢+𝑦1u + x2𝑢u +𝑦+ x3𝑢u +𝑦+ x4𝑢u ++𝑦x5u𝑢 , . (39)(40) To solve the coefficients of the indefinite2 equations,3 4 we add5 additional adjustable y(u) = y0 + y1u + y2u + y3u + y4u + y5u . (40) parameters (𝜖,𝜖,𝜖,𝜖) so as to convert Equations (39) and (40) to explicit equations of coefficientsTo solve versus the coefficients adjustable of parameters the indefinite [40], equations, and the optimal we add solution additional can adjustable be deter- ( ) parametersmined. e1, e2, e3, e4 so as to convert Equations (39) and (40) to explicit equations of coefficientsIn order versus to keep adjustable the length parameters of the curve [40], short and the while optimal the change solution rate can of be the determined. maximum curvatureIn order is toas keepsmall the as lengthpossible, of thethe curve objective short function while the of change the optimization rate of the maximumsolution is curvature is as small as possible, the objective function of the optimization solution is defined as: defined as: . minmin maxmax(| 𝜅κ| ) ×𝑘+𝑠× k + s , , (41)(41)

wherewhereκ 𝜅denotes denotes the the curvature curvature of the of curve,the curve,s is the 𝑠 curveis the length, curve and length,k is theand weighting 𝑘 is the coefﬁcient.weighting Figurecoefficient. 22 shows Figure the 22 trajectory shows the curves trajectory calculated curves according calculated to the according same starting to the positionsame starting and heading position and and different heading ending and different position ending and heading. position and heading.

(a) Scene 1

Figure 22. Cont.

Sensors 2021, 21, 2323 20 of 28 Sensors 2021, 21, x FOR PEER REVIEW 21 of 29

(b) Scene 2

(d) Scene 4

FigureFigure 22. 22.The The trajectorytrajectory curves curves satisfying satisfying the the differential differential constraint constraint of of motion motion in in different different scenes. scenes.

5.5. ExperimentsExperiments InIn this this section, section, the the performance performance of of the the suggested suggested human-machine human-machine cooperative cooperative control con- methodtrol method is investigated is investigated under under various various in unstructured in unstructured environments. environments. 5.1. Test System 5.1. Test System The test system is composed of wheeled UGV, teleoperation system and wireless The test system is composed of wheeled UGV, teleoperation system and wireless communication system. communication system. 5.1.1. Wheeled Unmanned Ground Vehicle (UGV) 5.1.1. Wheeled Unmanned Ground Vehicle (UGV) The UGV used in the experiment was modiﬁed from a wheel off-road vehicle with a lengthThe of 5.1UGV m andused a in width the experiment of 1.97 m. The was navigation modified controlfrom a systemwheel off-road of the UGV vehicle consisted with a oflength environment of 5.1 m perceptionand a width sensors of 1.97 and m. Th ane on-board navigation computer, control system as shown of the in FigureUGV con- 23. Environmentsisted of environment perception perception sensors included sensors a and Velodyne an on-board HDL-64E computer, lidar, a real-time as shown kinematic in Figure (RTK)23. Environment GPS system, perception and a ﬁber-optic sensors inertialincluded navigation a Velodyne system HDL-64E (INS). Thelidar, vertical a real-time and horizontalkinematic views(RTK) ofGPS the system, lidar were and 26.8a fiber-optic◦ and 360 ◦inertial. We could navigation obtain system 1.3 million (INS). points The perver- secondtical and with horizontal a range accuracyviews of ofthe +/ lidar−2cm. were The 26.8° INS and had 360°. the horizontal We could positionobtain 1.3 accuracy million ofpoints 0.02 m~0.05per second m and with the a altituderange accuracy position of accuracy +/-2 cm. of The 0.02 INS m~0.08 had the m, andhorizontal the horizontal position attitudeaccuracy accuracy of 0.02 m~0.05 was within m and 0.01 the◦. altitude The on-board position computer accuracy isof a0.02 ruggedized m~0.08 m, machine and the (Intelhorizontal Core i7-8550Uattitude 1.8accuracy GHz, Intelwas HDwithin Graphics 0.01°. 620,The 512Gon-board Solid computer State Drive is GeForcea ruggedized 8800 Ultramachine video, (Intel and Core 4 GB i7-8550U memory) 1.8 with GHz, the 64-bitIntel LinuxHD Graphics operating 620, system 512G (Ubuntu Solid State 18.04). Drive It implementedGeForce 8800 intelligent Ultra video, decision and 4 making GB memory) and control, with andthe ensured64-bit Linux real-time operating operation system of software(Ubuntusuch 18.04). as It local implemented path planning intelligent and tracking decision control. making and control, and ensured real-time operation of software such as local path planning and tracking control.

SensorsSensors2021 2021, ,21 21,, 2323 x FOR PEER REVIEW 2221 ofof 2829 Sensors 2021, 21, x FOR PEER REVIEW 22 of 29

Figure 23. Components of the navigation control system. FigureFigure 23. 23.Components Components of of the the navigation navigation control control system. system. 5.1.2.5.1.2. TeleoperationTeleoperation SystemSystem 5.1.2. Teleoperation System TheThe teleoperationteleoperation systemsystem waswas housedhoused inin aa mobilemobile controlcontrol sheltershelter whichwhich providedprovided The teleoperation system was housed in a mobile control shelter which provided sufficientsufficient space,space, power, power, and and environmental environmental control control for for the the operator, operator, as shownas shown in Figure in Figure 24. sufficient space, power, and environmental control for the operator, as shown in Figure The24. The operator operator received received video video and state and feedbackstate feed databack on data display on display terminal terminal and issued and driving issued 24. The operator received video and state feedback data on display terminal and issued commandsdriving commands through UGVthrough controller UGV andcontroller touch screens.and touch The screens. teleoperation The teleoperation computer had com- the driving commands through UGV controller and touch screens. The teleoperation com- sameputer hardwarehad the same configuration hardware as configuration the navigation as controlthe navigation computer. control A multi-screen computer. displayA mul- puter had the same hardware configuration as the navigation control computer. A mul- systemti-screen was display applied system to display was applied the wide-field-of-view to display the wide-field-of-view video and graphical video user and interface. graph- ti-screen display system was applied to display the wide-field-of-view video and graph- Theical Logitechuser interface. G27 driving The Logitech device G27 was driving used for device manual was driving. used for manual driving. ical user interface. The Logitech G27 driving device was used for manual driving.

Figure 24. Teleoperation system. FigureFigure 24. 24.Teleoperation Teleoperation system. system. 5.1.3.5.1.3. WirelessWireless CommunicationCommunication SystemSystem 5.1.3. Wireless Communication System TheThe wirelesswireless communicationcommunication systemsystem betweenbetween thethe teleoperationteleoperation systemsystem andand thethe UGVUGV The wireless communication system between the teleoperation system and the UGV waswas carriedcarried byby aa mobilemobile adad hochoc networknetwork (MANET)(MANET) [[41].41]. TheThe MANETMANET waswas aa collectioncollection ofof was carried by a mobile ad hoc network (MANET) [41]. The MANET was a collection of wirelesswireless mobilemobile nodesnodes dynamicallydynamically formingforming aa temporarytemporary networknetwork withoutwithout thethe useuse ofof anyany wireless mobile nodes dynamically forming a temporary network without the use of any existingexisting networknetwork infrastructureinfrastructure oror centralizedcentralized administration.administration. TheThe frequencyfrequency bandband ofof thethe existing network infrastructure or centralized administration. The frequency band of the communicationcommunication system was 560~678 MHz, MHz, which which could could provide provide the the data data bandwidth bandwidth of of 8 communication system was 560~678 MHz, which could provide the data bandwidth of 8 8Mbps, Mbps, and and the the link link transmission transmission delay delay was was less less than than 20 ms. In ﬂatflat terrainterrain environment,environment, Mbps, and the link transmission delay was less than 20 ms. In flat terrain environment, thethe maximummaximum communicationcommunication distancedistance couldcould reachreach 1010 km.km. ForFor easeease ofof implementation,implementation, the maximum communication distance could reach 10 km. For ease of implementation, teleoperationteleoperation commandscommands andand feedbackfeedback informationinformation travelledtravelled overover thethe samesame network. network. teleoperation commands and feedback information travelled over the same network. 5.2. Experimental Design 5.2. Experimental Design 5.2. WeExperimental conducted Design a human-in-the-loop ﬁeld experiment on a semi-desert region in north- We conducted a human-in-the-loop field experiment on a semi-desert region in west China.We conducted The experiment a human-in-the-loop was carried out field in the experiment daytime without on a semi-desert rain or snow. region Three in northwest China. The experiment was carried out in the daytime without rain or snow. closednorthwest experimental China. The routes experiment were designed was carried in an unstructuredout in the daytime environment, without each rain of or which snow. Three closed experimental routes were designed in an unstructured environment, each of wasThree about closed 4.96 experimental km, 2.4 km and routes 3.4 kmwere in designed length, including in an unstructured typical environmental environment, elements each of which was about 4.96 km, 2.4 km and 3.4 km in length, including typical environmental suchwhich as was straight, about curve, 4.96 km, pit 2.4 and km mound, and 3.4 and km the in length, starting including point and typical the end environmental point were elements such as straight, curve, pit and mound, and the starting point and the end point theelements same, such as shown as straight, in Figure curve, 25. pit There and weremound, signs and at the the starting fork and point endpoint and the to end inform point were the same, as shown in Figure 25. There were signs at the fork and endpoint to in- participantswere the same, where as shown to make in aFigure turn and25. There when were to stop. signs In at order the fork to ensure and endpoint the safety to ofin- form participants where to make a turn and when to stop. In order to ensure the safety of form participants where to make a turn and when to stop. In order to ensure the safety of

SensorsSensors 20212021,,21 21,, 2323x FOR PEER REVIEW 2322 of 2829

thethe experiment, the maximum speed speed of of the the UG UGVV was was limited limited to to 55 55 km/h, km/h, 55 55 km/h km/h and and 40 40km/h, km/h, respectively, respectively, depending depending on onthe the road road conditions conditions of the of the three three routes. routes.

(a) Test environment (b) Test route 1

Figure 25. Test environmentenvironment andand route.route.

Four operators were selected as thethe subjectssubjects toto carrycarry outout experimentsexperiments inin threethree testtest routes. Among the operators, there were threethree malesmales andand oneone female,female, agedaged betweenbetween 2323 and 30, andand allall ofof themthem hadhad UGVUGV remoteremote controlcontrol drivingdriving experience.experience. TheyThey werewere askedasked toto driving the UGVUGV insideinside thethe roadroad asas farfar asas possible.possible. BeforeBefore thethe experiment,experiment, thethe operatoroperator waswas allowedallowed toto practicepractice freelyfreely inin thethe experimentalexperimental sitesite forfor aa whilewhile toto fullyfully adaptadapt toto thethe testtest system. It also allowed the operator to previewpreview the full course of thethe testtest routeroute usingusing aa digital map. The operator had no direct eye contactcontact withwith thethe UGVUGV inin thethe ﬁeld.field. ForFor eacheach experiment route, the operatoroperator used thethe remoteremote controlcontrol modemode (manual(manual driving)driving) andand thethe cooperative control mode to controlcontrol thethe UGVUGV runningrunning counterclockwisecounterclockwise alongalong thethe testtest route respectively,respectively, andand recordedrecorded thethe motionmotion informationinformation ofof eacheach lap.lap. SubjectsSubjects drewdrew lotslots toto determine thethe experimentalexperimental order. order. While While one on subjecte subject was was experimenting, experimenting, the the other other subjects sub- werejects were not allowed not allowed to look. to look. A 5-min A 5-min break brea wask was given given between between the the modes. modes. If thereIf there was was a seriousa serious accident accident such such as theas UGVthe UGV falling falling into ainto pit ora pit crashing or crashing into a mound, into a mound, the experiment the ex- wouldperiment be would judged be a failure.judged Ita failure. would beIt would judged be according judged according to its actual to mileageits actual to mileage evaluate to theevaluate completion the completion rate of this rate task. of this task. 5.3. Experimental Result 5.3. Experimental Result We evaluated the performance of the proposed cooperative control method from two We evaluated the performance of the proposed cooperative control method from aspects: maneuvering task performance and handling stability. two aspects: maneuvering task performance and handling stability. 5.3.1. Maneuvering Task Performance 5.3.1. Maneuvering Task Performance The task completion rate and average speed are usually used to evaluate the maneu- veringThe task task performance completion of rate a UGV and [28average,42]. The speed task are completion usually used rate reﬂectsto evaluate the extent the ma- to neuvering task performance of a UGV [28,42]. The task completion rate reflects the extent

Sensors 2021, 21, 2323 23 of 28 Sensors 2021, 21, x FOR PEER REVIEW 24 of 29

whichto which the UGVthe UGV completes completes a particulara particular task task under under thethe operator’soperator’s control, control, which which can can bebe deﬁneddefined as as follows: follows: 𝐿𝑒𝑛𝑔𝑡ℎ 𝑜𝑓 𝐷𝑟𝑖𝑣𝑒𝑛 𝑅𝑜𝑢𝑡𝑒 Length of Driven Route (42) 𝑇𝑎𝑠𝑘Task Completion 𝐶𝑜𝑚𝑝𝑙𝑒𝑡𝑖𝑜𝑛 Rate = 𝑅𝑎𝑡𝑒= ×× 100%100%. . (42) 𝑇𝑎𝑠𝑘Task Route 𝑅𝑜𝑢𝑡𝑒 Length 𝐿𝑒𝑛𝑔𝑡ℎ During the experiment, four operators used two teleoperation modes to complete all During the experiment, four operators used two teleoperation modes to complete all three routes safely, and the completion rate of the task reached 100%. three routesThe average safely, andspeed the represents completion the ratecompletion of the task efficiency reached of 100%. the maneuver task. The higherThe the average average speed speed, represents the greater the the completion maneuverability efﬁciency of the of theUGV maneuver in the task task. envi- The higherronment, the averageand the speed,shorter the the greater time to the complete maneuverability the task. ofFigure the UGV 26 shows in the the task driving environment,speed and and the acceleration shorter the in time different to complete test routes the based task. Figureon remote 26 shows control the mode driving and human– speed and accelerationmachine cooperative in different control test routes mode based from onone remote of the controlsubjects. mode As can and be human–machine seen from the cooperativechart, both controlmodes can mode achieve from a one similar of the ma subjects.ximum speed, As can but be the seen speed from control the chart, stability both modesof the can former achieve is better a similar than that maximum of the latter. speed, but the speed control stability of the former is better than that of the latter.

(a) Route 1

(b) Route 2

FigureFigure 26. Driving26. Driving speed speed and accelerationand acceleration variation variation in three in testthree routes test basedroutes on based remote on control remote mode control and mode human-machine and human-machine cooperative control mode from one of the subjects. cooperative control mode from one of the subjects.

Table1 shows the average driving speed of each subject in remote control and human– machine cooperative control mode in three routes. By comparing the mean values of average speed between these two modes in each test route, we can see that human–machine

Sensors 2021, 21, 2323 24 of 28

cooperative control mode has advantages for UGV teleoperation in an unstructured environment.

Table 1. Average driving speed of each subject in remote control and human–machine cooperative control mode in three routes.

Average Speed of Route 1 Average Speed of Route 2 Average Speed of Route 3 Subject [m/s] [m/s] [m/s] R. C. H. C. C. R. C. H. C. C. R. C. H. C. C. 1 5.70 6.96 7.98 8.34 8.66 9.17 2 6.38 5.94 5.67 6.83 7.02 7.89 3 5.93 6.33 7.58 6.17 9.61 10.36 4 5.24 5.78 8.86 9.84 7.37 7.61 Mean Value 5.81 6.25 7.52 7.80 8.17 8.76 R. C., Remote Control; H. M. C., Human–machine Cooperative Control.

5.3.2. Handling Stability In the ﬁeld of vehicle engineering, yaw rate and sideslip angle are important indicators to measure the handling stability of a vehicle [43]. The yaw rate γ, heading ϕ, velocity components vx and vy in global coordinates can be measured by the onboard INS. Based on the Ackerman bicycle model, the sideslip angle β is calculated by Equation (43) [44]:

β = θ − ϕ, (43)

where θ is the direction angle of velocity, which is deﬁned as:

vy θ = tan−1 , (44) vx In this experiment, we used the mean absolute deviation (MAD) of yaw rate and sideslip angle to evaluate the handling stability of UGV. The MAD of a dataset is the average distance between each data point and the mean. It gives us an idea about the variability in a dataset. The MAD of a dataset {x1, x2, ··· , xn} is deﬁned as:

1 n D = ∑|xi − m(X)|, (45) n i=1

where m(X) is the mean of dataset X. Tables2 and3 show the MAD of yaw rate and sideslip angle respectively.

Table 2. Mean absolute deviation (MAD) of yaw rate of each subject in remote control and human–machine cooperative control mode in three routes.

MAD of Yaw Rate in Route 1 MAD of Yaw Rate in Route 2 MAD of Yaw Rate in Route 3 ◦ ◦ ◦ Subject [ /s] [ /s] [ /s] R. C. H. C. C. R. C. H. C. C. R. C. H. C. C. 1 0.97 0.79 2.68 2.78 1.52 0.96 2 1.10 0.90 2.30 3.48 1.84 1.21 3 1.13 0.69 2.38 2.59 1.62 0.91 4 0.95 0.67 3.61 2.92 1.38 1.00 Mean Value 1.04 0.76 2.74 2.94 1.59 1.02 Sensors 2021, 21, 2323 25 of 28

Table 3. MAD of sideslip angle of each subject in remote control and human-machine cooperative control mode in three routes.

MAD of Sideslip Angle MAD of Sideslip Angle MAD of Sideslip Angle ◦ ◦ ◦ Subject in Route 1 [ ] in Route 2 [ ] in Route 3 [ ] R. C. H. C. C. R. C. H. C. C. R. C. H. C. C. 1 0.25 0.16 0.45 0.37 0.33 0.21 2 0.24 0.21 0.51 0.40 0.41 0.19 3 0.20 0.14 0.54 0.36 0.29 0.26 4 0.18 0.15 0.33 0.43 0.37 0.24 Mean Value 0.22 0.17 0.46 0.42 0.35 0.23

5.4. Experiment Analysis The purpose of the experiment was to verify the usability of the proposed human– machine cooperative control method in multiple unstructured environments. The four operators all completed the maneuvering tasks of the three test routes safely by using the cooperative control method. Through the comprehensive analysis of the speed data and task routes, it can be seen that both modes can achieve a similar maximum speed on the straight section of each route. But due to the delay in the control loop, the handling stability of the remote control mode decreases when the UGV is driving at high speed, which makes it difficult for the operator to maintain the high-speed driving state for a long time. To ensure driving safety, the operator will take the initiative to reduce the driving speed even under good road conditions. Moreover, due to the limited visual field of the video image, the operator will also slow down in the remote control mode during sharp turns or continuous curves to ensure the UGV can pass safely. For task routes with more curves, this will lead to longer task completion time. It can also be seen from Figure 26 that the fluctuation of the acceleration curve in the remote control mode is significantly greater than that in the cooperative control mode. In the human–machine cooperative control mode, the control command generated by the teleoperation system is the guidance point that is spatially ahead of the current UGV position. The desired path is generated by the navigation control system according to the guidance point, then the UGV can drive along the path at the optimal speed and course under precise control. Compared with the operator’s direct control, the control accuracy and frequency of the autonomous control system to the UGV chassis are higher, so it can continuously maintain a higher speed, and pass the curve section more smoothly.

6. Conclusions This paper focuses on guidance point generation-based cooperative UGV teleoperation in an unstructured environment. The key novelty of this method is that the guidance points used for navigation can be generated with only the local perception information of UGV. In order to extract smooth skeletons of navigable regions, the OGM was generated utilizing a probabilistic grid state description method, and converted into binary images to construct the convex hull of the obstacle area. An improved thinning algorithm is designed to extract single-pixel skeleton, and ﬁnd out the target skeleton related to the position of UGV utilizing the kNN algorithm. By using the decreasing gradient algorithm the target skeleton is modiﬁed in order to obtain the appropriate candidate guidance points. For visually presenting the driving trend of the UGV and convenient touch screen operation, we transformed guidance point selection into trajectory selection by generating the predicted trajectory correlative to candidate guidance points based on the differential equation of motion. Experimental results show that the cooperative control mode is suitable for UGV teleoperation in off-road environments. The advantage of the proposed method is that the operator only needs to select the guiding point consistent with its control intention as the target point of UGV trajectory track- Sensors 2021, 21, 2323 26 of 28

ing to realize the teleoperation. Compared with the traditional remote-control method, this human–machine cooperative control mode transforms the operator’s high-frequency control behavior (e.g., steering, accelerating and braking actions) into low-frequency decision- making behavior, which further decouples the operator from the UGV, thus reducing the workload of the operator and the sensitivity of the control loop to delay. In the remote control mode, all motion control commands are input by the operator. This means that the operator needs to train for a while to adapt to the handling characteristics and delay effects of the UGV. When the controlled vehicle changes, the operator also needs to be retrained. In comparison, the human–machine cooperative control method is weakly correlative with the vehicle type. The operator is not concerned with the motion control of the vehicle. By using this method, the operator can obtain acceptable teleoperation ability after a few elements of training. Therefore, we believe that this method should have broad application prospects. The limitation of the method proposed in this paper lies in the fact that the grid occupied state is not considered when generating the trajectory line, and there is a risk of collision with obstacles in extreme cases. Therefore, the trajectory cluster-generating method will be studied in following research. The safe trajectory will be selected from the trajectory cluster for UGV tracking. Since the onboard camera of the UGV does not have night vision ability, images taken in the night environment cannot be used for the operator to make decisions, so current research is only based on the daytime environment. In following research work, we will choose a camera with low illumination imaging ability to carry out the research on human–machine cooperative teleoperation in the night environment.

Author Contributions: Conceptualization, S.Z., G.X. and H.C.; Methodology, S.Z., G.X. and J.G.; Software, S.Z.; Writing-original draft, S.Z.; Supervision, H.C. All authors have read and agreed to the published version of the manuscript. Funding: This research was funded by National Natural Science Foundation (NNSF) of China, grant number 61703041. Institutional Review Board Statement: Not applicable. Informed Consent Statement: Not applicable. Data Availability Statement: Not applicable. Acknowledgments: The authors are grateful to Zhao Xijun for the technical support of software development, and Zhou Changyi for the contribution of the experiments. Conﬂicts of Interest: The authors declare no conﬂict of interest.

References 1. Chen, J.Y.; Haas, E.C.; Barnes, M.J. Human performance issues and user interface design for teleoperated robots. IEEE Trans. Syst. Man Cyber. Part C Appl. Rev. 2007, 37, 1231–1245. [CrossRef] 2. Luck, J.P.; McDermott, P.L.; Allender, L.; Russell, D.C. An investigation of real world control of robotic assets under communication latency. In Proceedings of the 1st ACM SIGCHI/SIGART Conference on Human-Robot Interaction, Salt Lake City, UT, USA, 2–3 March 2006; pp. 202–209. 3. Storms, J.; Tilbury, D. Equating user performance among communication latency distributions and simulation ﬁdelities for a teleoperated mobile robot. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Seattle, WA, USA, 26–30 May 2015; pp. 4440–4445. 4. Ferland, F.; Pomerleau, F.; Le Dinh, C.T.; Michaud, F. Egocentric and exocentric teleoperation interface using real-time, 3d video projection. In Proceedings of the 4th ACM/IEEE International Conference on Human-Robot Interaction (HRI), La Jolla, CA, USA, 9–13 March 2009; pp. 37–44. 5. Badue, C.; Guidolini, R.; Carneiro, R.V.; Azevedo, P.; Cardoso, V.B.; Forechi, A.; Jesus, L.; Berriel, R.; Paixão, T.M.; Mutz, F.; et al. Self-driving cars: A survey. Expert. Syst. Appl. 2020, 2020, 113816. 6. Yurtsever, E.; Lambert, J.; Carballo, A.; Takeda, K. A survey of autonomous driving: Common practices and emerging technologies. IEEE Access 2020, 8, 58443–58469. [CrossRef] Sensors 2021, 21, 2323 27 of 28

7. Cosenzo, K.A.; Barnes, M.J. Who needs an operator when the robot is autonomous? The challenges and advantages of robots as team members. In Designing Soldier Systems: Current Issues in Human Factors; Martin, J., Allender, L., Savage-Knepshield, P., Lockett, J., Eds.; CRC Press: Boca Raton, FL, USA, 2018; pp. 35–51. 8. Kelly, A.; Chan, N.; Herman, H.; Warner, R. Experimental validation of operator aids for high speed vehicle teleoperation. In Experimental Robotics; Desai, J.P., Dudek, G., Khatib, O., Kumar, V., Eds.; Springer: Berlin/Heidelberg, Germany, 2013; pp. 951–962. 9. Macharet, D.G.; Florencio, D. A collaborative control system for telepresence robots. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Algarve, Portugal, 7–12 October 2012; pp. 5105–5111. 10. Anderson, S.J.; Walker, J.M.; Iagnemma, K. Experimental performance analysis of a homotopy-based shared autonomy framework. IEEE Trans. Humman-Mach. Syst. 2014, 44, 190–199. [CrossRef] 11. Erlien, S.M.; Funke, J.; Gerdes, J.C. Incorporating non-linear tire dynamics into a convex approach to shared steering control. In Proceedings of the IEEE American Control Conference (ACC), Portland, OR, USA, 4–6 June 2014; pp. 3468–3473. 12. Shia, V.; Gao, Y.; Vasudevan, R.; Campbell, K.D.; Lin, T.; Borrelli, F.; Bajcsy, R. Semiautonomous vehicular control using driver modeling. IEEE Trans. Intell. Transp. Syst. 2014, 15, 2696–2709. [CrossRef] 13. Bicker, R.; Ow, S.M. Shared control in bilateral telerobotic systems. In Proceedings of the SPIE—The International Society for Optical Engineering, San Diego, CA, USA, 9–11 July 1995; Volume 2351, pp. 200–206. 14. Brandt, T.; Sattel, T.; Bohm, M. Combining haptic human-machine interaction with predictive path planning for lane-keeping and collision avoidance systems. In Proceedings of the IEEE Intelligent Vehicles Symposium, Istanbul, Turkey, 13–15 June 2007; pp. 582–587. 15. Mulder, M.; Abbink, D.A.; Boer, E.R. Sharing control with haptics: Seamless driver support from manual to automatic control. Hum. Factors 2012, 54, 786–798. [CrossRef][PubMed] 16. Flemisch, F.; Heesen, M.; Hesse, T.; Kelsch, J.; Schieben, A.; Beller, J. Towards a dynamic balance between humans and automation: Authority, ability, responsibility and control in shared and cooperative control situations. Cogn. Technol. Work 2012, 14, 3–18. [CrossRef] 17. Gray, A.; Ali, M.; Gao, Y.; Hedrick, J.; Borrelli, F. Semi-autonomous vehicle control for road departure and obstacle avoidance. In Proceedings of the IFAC Symposium on Control in Transportation Systems, Soﬁa, Bulgaria, 12–14 September 2012; pp. 1–6. 18. Petermeijer, S.M.; Abbink, D.A.; de Winter, J.C. Should drivers be operating within an automation-free bandwidth? Evaluating haptic steering support systems with different levels of authority. Hum. Factors 2015, 57, 5–20. [CrossRef][PubMed] 19. Forsyth, B.A.; Maclean, K.E. Predictive haptic guidance: Intelligent user assistance for the control of dynamic tasks. IEEE Trans. Visual. Comput. Graph. 2006, 12, 103–113. [CrossRef] 20. Mulder, M.; Abbink, D.A.; Boer, E.R. The effect of haptic guidance on curve negotiation behavior of young, experienced drivers. In Proceedings of the IEEE International Conference on Systems, Man and Cybernetics, Singapore, 12–15 October 2008; pp. 804–809. 21. Abbink, D.A.; Cleij, D.; Mulder, M.; Van Paassen, M.M. The importance of including knowledge of neuromuscular behaviour in haptic shared control. In Proceedings of the IEEE International Conference on Systems, Man, and Cybernetics, Seoul, Korea, 14–17 October 2012; pp. 3350–3355. 22. Mars, F.; Deroo, M.; Hoc, J.M. Analysis of human-machine cooperation when driving with different degrees of haptic shared control. IEEE Trans. Haptics 2014, 7, 324–333. [CrossRef][PubMed] 23. Boehm, P.; Ghasemi, A.H.; O’Modhrain, S.; Jayakumar, P.; Gillespie, R.B. Architectures for shared control of vehicle steering. IFAC-PapersOnLine 2016, 49, 639–644. [CrossRef] 24. Witus, G.; Hunt, S.; Janicki, P. Methods for UGV teleoperation with high latency communications. In Unmanned Systems Technology XIII; International Society for Optics and Photonics: Bellingham, WA, USA, 2011; Volume 8045, p. 80450N. 25. Silver, D.; Sofman, B.; Vandapel, N.; Bagnell, J.A.; Stentz, A. Experimental analysis of overhead data processing to support long range navigation. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 9–15 October 2006; pp. 2443–2450. 26. Zych, N.; Silver, D.; Stager, D.; Green, C.; Pilarski, T.; Fischer, J.; Kuntz, N.; Anderson, D.; Costa, A.; Gannon, J.; et al. Achieving integrated convoys: Cargo unmanned ground vehicle development and experimentation. In Unmanned Systems Technology XV; International Society for Optics and Photonics: Bellingham, WA, USA, 2013; Volume 8741, p. 87410Y. 27. Liu, D. Research on Human-machine Intelligent Integration Based Path Planning for Mobile Robots. Master’s Thesis, National University of Defense Technology, Changsha, China, 2011. 28. Suzuki, T.; Amano, Y.; Hashizume, T.; Kubo, N. Vehicle teleoperation using 3D maps and GPS time synchronization. IEEE Comput. Graph. Appl. 2013, 33, 82–88. [CrossRef] 29. Kaufman, E.; Lee, T.; Ai, Z.; Moskowitz, I.S. Bayesian occupancy grid mapping via an exact inverse sensor model. In Proceedings of the IEEE American Control Conference (ACC), Boston, MA, USA, 6–8 July 2016; pp. 5709–5715. 30. Dougherty, E. Mathematical Morphology in Image Processing; CRC Press: Boca Raton, FL, USA, 2018. 31. Xie, W.; Thompson, R.P.; Perucchio, R. A topology-preserving parallel 3D thinning algorithm for extracting the curve skeleton. Pattern Recognit. 2003, 36, 1529–1544. [CrossRef] 32. Ding, Y.; Liu, W.Y.; Zheng, Y.H. Hierarchical connected skeletonization algorithm based on distance transform. J. Infrared Millim. Waves 2005, 24, 281–285. 33. Ogniewicz, R.L. Hierarchic Voronoi skeletons. Pattern Recognit. 1995, 28, 343–359. [CrossRef] Sensors 2021, 21, 2323 28 of 28

34. Zhang, T.Y.; Suen, C.Y. A fast parallel algorithm for thinning digital patterns. Commun. ACM 1984, 27, 236–239. [CrossRef] 35. Graham, R.L. An efficient algorithm for determining the convex hull of a finite planar set. Info. Pro. Lett. 1972, 1, 132–133. [CrossRef] 36. Bresenham, J.E. Algorithms for computer control of a digital plotter. IBM Syst. J. 1965, 4, 25–30. [CrossRef] 37. Zhang, D.C.; Zhou, C.G.; Zhou, Q.; Chi, S.Z.; Wang, S.J. Hole-filling algorithm based on contour. J. Jilin Uni. 2011, 49, 82–86. 38. Rajaguru, H.; Prabhakar, S.K. KNN Classifier and K-Means Clustering for Robust Classification of Epilepsy from EEG Signals; A Detailed Analysis; Anchor Academic Publishing: Hamburg, Germany, 2017; pp. 31–36. 39. Saito, T.; Toriwaki, J.I. New algorithms for euclidean distance transformation of an n-dimensional digitized picture with applications. Pattern Recognit. 1994, 27, 1551–1565. [CrossRef] 40. Broggi, A.; Bertozzi, M.; Fasciolia, A.; Guarino, C.; Lo Bianco, C.G.; Piazzi, A. The ARGO autonomous vehicles vision and control systems. Int. J. Intell. Cont. Syst. 1999, 3, 409–441. 41. Vegda, H.; Modi, N. Review paper on mobile ad-hoc networks. Int. J. Comput. Appl. 2018, 179, 33–35. [CrossRef] 42. Zheng, Y.; Brudnak, M.J.; Jayakumar, P.; Stein, J.L.; Ersal, T. An experimental evaluation of a model-free predictor framework in teleoperated vehicles. IFAC-PapersOnLine 2016, 49, 157–164. [CrossRef] 43. Abe, M. Vehicle Handling Dynamics: Theory and Application; Butterworth-Heinemann: Oxford, UK, 2015. 44. Emmanuel, L.A.; Christian, C.; Mbaocha, C.C.; Olubiwe, M. Design of Two-Degree-Of-Freedom (2DOF) Steering Control for Automated Guided Vehicle. Int. J. Scienti. Eng. Sci. 2019, 3, 57–64.