Automatic Sensor Placement For Complex Three-dimensional Inspection andExploration

Francois-Michel De Rainville, Christian Gagne, and Denis Laurendeau

Laboratoire de vision et systemes numeriquesDepartement de genie electrique et de genie informatique

Universite Laval, Quebec, Quebec, Canada G1V 0A6francois-michel.de-rainville.1@ulaval.ca,

{christian.gagne, denis.laurendeau}@gel.ulaval.ca

Abstract

This paper describes an end-to-end system for posi-tioning sensors to observe desired parts of arbitrary ini-tially unknown three-dimensional environments. The sys-tem finds the appropriate number of sensors and their con-figuration using a derivative-free cooperative optimizationalgorithm relying on a visibility estimation cost function.The system is tested of scenarios with different types ofsensors and observation tasks. The first scenario impliessix degrees of freedom floating robots to inspect a brokenpart of the international space station, while the secondscenario involves three degrees of freedom rovers gather-ing information on a stuck vehicle.

1 Introduction

Advances in mobile computing and robotic minia-turization recently enabled researchers to design roboticsystems made of multiple relatively simple independentrobots to solve tasks a single sophisticated robot can-not [5]. The individuals in a group of cooperative robotsoften have the advantage of being physically simpler,which lead to more affordable, scalable, and robust sys-tems. These characteristics make collectives of robotswell suited for inspection and surveillance of hazardous orunattainable environments, common for operations suchas in space module inspection and spatial exploration.

We propose an automated deployment system for mo-bile sensors to capture complex three-dimensional envi-ronments. We envision telepresence applications where auser investigates a scene that is inaccessible to humans.We define the virtual sensor sv as the object manipulatedby the operator to control the desired view of the environ-ment. As shown in figure 1, the virtual sensor sv definesthe area for which real sensors si (the group of mobilerobots) need to gather information to reconstruct the vir-tual view. In general, the intrinsic parameters of the vir-tual sensor, such as its resolution and field of view, willbe greater than the ones of the real sensors. Thus, numer-

Figure 1. : Sensor placement problem to observe a non-planar wall. The region of interest, in light grey, is inducedby the virtual sensor sv field of view (blue pyramid). Thefields of view of real sensors s1 and s2 are shown in darkgrey.

ous well-positioned sensors have to combine their sensingcapacity to observe the user-selected region with the re-quested accuracy.

This problem is related to the well-known art galleryproblem [13], where one wants to guard an art gallery us-ing a minimal number of guards. Our problem divergesfrom the original art gallery problem on a few points.First, we want to work in arbitrary and initially unknownthree-dimensional environments. Second, we considerguards with limited sensing capabilities with respect tothe field of view and resolution. Third, we require thatthe resulting resolution matches a predetermined accuracyin every point of the observed environment. These dif-ferences make our problem applicable to a wider rangeof real world applications where robots have to automati-cally deploy themselves to gather specific information innew or unknown environments.

Different approaches have been explored to solve theart gallery problem. A first class of algorithms, calledNext Best View algorithms [4, 14], is most commonlyused for inspecting manufactured objects but which wereapplied recently to indoor scene acquisition. The goalof the algorithm is to acquire automatically and withthe fewest views possible all the surfaces of a three-dimensional object enclosed inside a defined volume. Af-

RobotRobot

Robot

Representation

Localisation

SensorsFinal images

Optimization

3D Data

Position

PointcloudImages

VisibilityEstimation

+

CommandPath Planningand Deployment

Figure 2. : Proposed system. Algorithms in the “robot”box run locally on each robot, while the other algorithmsare centralized on an external controller.

ter each acquisition, the algorithm computes the nextbest position where each sensor should be placed in or-der to maximize the coverage of the unknown volume.While Connoly [4] and Pito [14] proposed a determinis-tic solution to this problem, Nuchter et al. [12], and Tar-box and Gottschlich [18] used a stochastic algorithm toincrease the efficiency and decrease the number of views.The common ground between their work and our ap-proach is the simulation of the view a sensor would havefrom different poses in the environment. Based on thesesimulations, they selected the best one and move the sen-sor to this position. However, their approach rely on plan-ning sensor placement one step at a time, which dividesthe observed space in small non-connected parts and gen-erates a greater number of views than required.

Other multisensor solutions use gradient methods tooptimize an observability function [17]. The observabil-ity function combines the visibility of each sensor undera single function and performs the optimization globally,making every sensor aware of what the other sensors areobserving. This strategy allows for a better control of agroup of sensors as a whole. Unfortunately, the visibilityfunction has to be differentiable, a condition that is rarelymet in general three-dimensional space due to disconti-nuities and occlusions. In this paper we propose an end-to-end system capable of optimizing sensor placement inarbitrary and unknown three-dimensional environments.

2 Rationale

This section briefly describes the end-to-end systemused to deploy a group of mobile sensors in a full three-dimensional environment. As shown in figure 2, the sys-tem contains two major phases, optimization and deploy-ment, while the sensors never stop their sensing of the en-vironment.

The optimization phase is responsible to find the ap-propriate number of sensors and their configuration giventhe geometry of the environment and the desired virtualview. It uses a visibility estimation procedure as cost func-tion to optimize this configuration. The optimization isrun on a centralized unit for all robots. After a giventime interval, the optimization algorithm is paused and thebest configuration is sent to the robots for the deploymentphase. The cost function used for the optimization is de-scribed in section 3, while the optimization algorithm isdetailed in section 4.

The deployment of the sensors is made at constantintervals between optimization phases. The robots plantheir displacements locally, using a standard path planningalgorithm and localization system. Once all sensors reachtheir position, the optimization algorithm resumes from itsprevious state. The deployment phase is not a subject ofinnovation in the current work and will not be describedhere.

In the proposed system, the robots continuously sensetheir environment. The collected information is stored in acentralized representation accessible to the optimizationalgorithm. An appropriate three-dimensional representa-tion, containing all the information required by the costfunction, is described in section 3.2.

3 Visibility Cost Function

As defined previously, the problem faced is to placean undetermined number of sensors to observe entirelyand with a defined accuracy a region of the space delim-ited by the field of view of a virtual sensor. The presentsection will outline the mathematical details of the costfunction used in the optimization as well as the informa-tion retrieval procedures from the selected representation.

3.1 Cost Function

Let E ⊂ R3 be the bounded environment to be ob-served with a given precision. E is delimited by the virtualsensor sv field of view. The precision of a camera sensor si

at any point e ∈ E is defined by its sensing density D, thatis the number of pixel per area unit covering this point.

D(si, e) =number of pixels

area(1)

Sensor si is defined to be in P = Rδ, a δ degrees of free-dom space. The sensing density D(S, e) of a group ofsensors S = {s1, . . . , sn} is defined as the union of the den-sities among S.

The problem is to match the virtual sensor sens-ing density D(sv, e) with the real sensors sensing densityD(S, e) for the whole environment E. This is accom-

plished by minimizing their difference

S∗ = arg minS∈Pn, n

∫E

max(

D(sv, e j) − D(S, e j)D(sv, e j)

, 0)

de, (2)

where the max operation between the normalized differ-ence and 0 is to saturate the quality measure when sensingthe target space with a density higher than requested. Notethat equation 2 also minimizes the number of sensors in S.

For general three-dimensional environments, the ex-act geometry of E is unknown. Consequently, the inte-gral of equation 2 cannot be solved analytically. However,with a good representation of the environment geometry,the sensing density at any point in the environment can beapproximated reasonably well. In fact, the sensing den-sity of traditional line-of-sight sensors depends on theirintrinsic parameters, the distance to the point, and the ori-entation of the surface at the observed point. The intrinsicparameters are constant, while the distance and the normalcan be obtained from the geometric representation of theenvironment. The numerical integration of the integrandin equation 2 can be obtained by estimating the sensingdensity at N points e j in the virtual sensor field of view,such that it can be approximated by

S∗ = arg minS∈Pn, n

1N

N∑j=1

max(

D(sv, e j) − D(S, e j)D(sv, e j)

, 0). (3)

3.2 Information RetrievalThe chosen representation for the three-dimensional

data is an occupancy grid [6] encoded as an octree struc-ture [19]. This representation contains the necessary infor-mation required to compute equation 3, i.e. the distanceand the orientation of the closest surface to the sensor.

Octrees are well known for their fast ray-tracing op-eration [1], which can return the first occupied voxel hitby the ray. The distance is simply the distance betweenthe two voxel centres, or more precisely between the sen-sor position and the centre of the occupied voxel hit. Thenormal at this point can be found by computing one iter-ation of the marching cube algorithm [11] centred on thehit voxel and its 26 direct neighbours. From all the nor-mals, we choose the one that has minimal angle with thevirtual sensor sv optical axis if it is a line of sight sensorand the normalized sum of all normals otherwise.

4 Sensor Placement Optimization

The summation in equation 3 qualifies a sensor place-ment for a given environment and virtual sensor. Thederivative of this cost function cannot be calculated dueto the specificity of arbitrary three-dimensional environ-ments. Thus, we must rely on less restrictive global op-timization algorithms. Derivative-free optimization algo-

rithms, such as evolution strategies [2, 9], manage opti-mization problems where the only information that can beretrieved from a process f ([x1 · · · xn]) is its function qual-ity [y1 · · · ym], where f : Rn 7→ Rm. However, these al-gorithms do not handle naturally functions with a variablenumber of inputs n, e.g. a varying number of sensors. Thissection proposes a two-level derivative-free optimizationalgorithm designed to minimize the number of sensors atthe first level and to optimize their placement at the secondlevel.

4.1 Cooperative OptimizationCooperative optimization has been proposed by Pot-

ter and De Jong [15] as a method to coadapt subsolutionsto solve a problem by decomposing it automatically intosubproblems. The framework uses the explicit notion ofmodularity to assemble multiple subcomponents (i.e. thesensors), to solve a global problem (i.e. multisensor place-ment).

initialize P← {Si, i = 1, . . . , n};1

choose R← {select random(Si), i = 1, . . . , n};2

while ¬stop do3

foreach species Si ∈ P do4

Si ← generate solutions(Si);5

evaluate(Si,R\ri);6

Si ← update(Si);7

R← {select best(Si), i = 1, . . . , n};8

if improvement < Ti then9

J = { j | contrib(S j) < Tc, j = 1, . . . , n};10

P← P\⋃

j∈J S j;11

R← R\⋃

j∈J R j;12

add a new species P← P ∪ {S′};13

R← R ∪ {select random(S′)};14

Algorithm 1: Cooperative co-evolution.

The complete cooperative optimization algorithmproposed by Potter and De Jong [15] is presented in al-gorithm 1. First, a population P of n species Si is initial-ized randomly, each species optimizes one sensor. Onerepresentative ri of each species is copied in a represen-tative set R. Then, the optimization loop begins at line 3where, in turn, each species is given a chance to optimizethe position of the sensor it represents. At lines 5 to 7, agenerate-update optimization scheme [3] is used to posi-tion the sensors, although any type of algorithm could beused. More details on the selected procedure is given inthe next section.

Once an iteration of the species level optimizationis completed for each species, their current best solutionforms the new representative set R. Then, the improve-ment of the evolution is verified at line 9. If the globalsolution quality, given by the representatives set evaluated

with equation 3, has not improved by a given threshold Tiover the last Tl generations, the system is considered asstagnating. In this case, unproductive species (i.e. thosecontributing less than a threshold Tc) are removed fromthe population along with their representative fromR. Thecontribution of any species Si ∈ P is given by the qualityratio of R with and without Si. Finally, a new species S′

is added to the population and the cooperative optimiza-tion continues until the termination criterion is reached.

The process of removing and adding species whenstagnation occurs produces a pressure on the system tofind useful placement for the sensors enabling the searchfor both the number of sensors and their optimal position.

4.2 Covariance Matrix Adaptation EvolutionStrategy

The algorithm chosen to optimize each sensor posi-tion (lines 5 to 7 of algorithm 1) is the (1 + λ) variantof the Covariance Matrix Adaptation Evolution Strategy(CMA-ES) [9]. CMA-ES is a derivative-free optimizationtechnique based on the self-adaptation of a multivariatenormal search distribution. For details on CMA-ES, theinterested reader is referred to [9].

Few modifications have been made to the original al-gorithm to fit the sensor placement problem and coop-erative optimization scheme. First, every time a speciesis added, the step size of all CMA-ES strategies is dou-bled to favour coadaptation to new species and increasethe chances of escaping local minimas. Second, CMA-ES is known for its log-linear convergence on optimumsdue to the adaptation of the step size and covariance ma-trix. However, in situations where coadaptation is neces-sary, contraction of the search space below a certain levelis undesirable. In cooperative optimization algorithm, weprevented the search space from contracting if its smalleraxis falls below 10 cm.

5 Experiments

We conducted two different simulation experimentswith different environments and type of sensors. The firstset of experiments took place in space around the Inter-national Space Station (ISS), while the second set of ex-periments took place on the ground. All experiments wereperformed using Gazebo a multirobot simulator [10] com-bined with ROS the robot operating system [16].

The experiments were run with the occupancy grid ofthe environment loaded initially. For each set of experi-ments, the occupancy grid was built offline with a robotsimilar to the one used in the experiments. The robot wascontrolled manually and was commanded movement pat-terns analogous to those the optimization algorithm andpath planning would produce. This allowed to offload the

(a) Complete ISS three-dimensional model used for the simulations.

(b) View of the photovoltaic array to be observed by the sensors.

(c) Three-dimensional octree built by scanning the ISS 3D model withinthe simulator. Each blue cube is an occupied voxel in the occupancyoctree.

Figure 3. : International Space Station inspection envi-ronment

central computer that had also to run the complete multi-robot simulator and to accelerate the experiments.

5.1 International Space Station InspectionThe first simulation experiments took place around

the ISS, where a user wanted to inspect one of the pho-tovoltaic arrays. The user placed a virtual camera so thatthe desired part of the array was entirely seen by the vir-tual sensor, as shown in figure 3(b).

The virtual camera intrinsic parameters were set tof = 1597.71 px, u = v = 1 px and the horizontal andvertical field of view were respectively 130◦ and 100◦.The sensors participating in the optimization process hadsix degrees of freedom: their position [x, y, z], orientation[φ, θ] (pan and tilt) and a zoom factor ζ. Their intrinsic pa-rameters were the same as the virtual camera for f , u, andv, while their field of view at a zoom factor of 1 was 62◦

0.0 0.2 0.4 0.6 0.8 1.0

Function Evaluations 1e5

0.0

0.2

0.4

0.6

0.8

1.0

Avera

ge D

ensi

ty E

rror

CoopCMA-ES

0

2

4

6

8

10

Num

ber

of

Senso

rs

Figure 4. : Average results for the two optimization al-gorithms for the experiment around the ISS. The circlemarked lines report the average density error with the 5%-95% confidence interval range presented by the error bars,while the unmarked lines show the average number of sen-sors.

horizontally and 48.2◦ vertically. The three-dimensionalsensor used on these robots is a rotating laser range findersimilar to a Hokuyo URG-04LX.

Experiment were run with the presented cooperativealgorithm (Coop) and a global optimizer for comparison.The parameters for the cooperative algorithm were:

• an improvement length Tl = 5n iterations, where n isthe current number of sensors;

• an improvement threshold Ti = 10%;• a contribution threshold Tc = 0.25.

The Coop algorithm had a pool of 10 sensors available andstarted the optimization with 2. The global optimizer wasthe well-known CMA-ES [9] described earlier. The de-fault parameters for CMA-ES were selected, except forthe number of offspring which was set to 200. SinceCMA-ES cannot tune by itself the number of sensors, nwas set to 9, the highest number of sensors found in anyrun of the cooperative algorithm. Both algorithms werestopped after 100 000 evaluations or when the error waslower than 0.01. The optimization algorithm implemen-tations that were used come from DEAP, an evolutionaryalgorithm framework [8].

Figure 4 shows the average results of 11 repetitions ofthe ISS experiment. The figure exposes how the coopera-tive algorithm outperforms a classical global optimizationalgorithm even if it has to find the number of sensors inaddition to their placement. The error bars for the twoalgorithm almost stop overlapping at half of the allowedoptimization time. The average final errors are 0.02 and0.24 with standard deviation of 0.020 and 0.125 respec-tively for Coop and CMA-ES. The average final numberof sensors for Coop is 7.18 with a standard deviation of1.11.

Figure 6. : Ground environment with stuck vehicle high-lighted. The four other robots are at their initial position.

Figure 5 presents the median configuration for bothoptimization algorithms. We observe, on the one hand,that the cooperative algorithm is able to observe almostentirely the desired surface with the desired density. Wealso note that all seven sensors contribute to the final solu-tion, as they are all oriented directly towards the surface.On the other hand, the poor performance of the global op-timization algorithm is explained by the fact that not allnine sensors contribute to the final solution. In fact, itseems that the global optimizer is unable to find whichsensors do not contribute to observe the target. Thus, wecan state that the internal mechanism quantifying the con-tribution of each sensors helps the cooperative algorithmto find good solutions even in complex observation sce-narios.

Finally, defining a constrained region around the tar-get would probably help the global optimization algo-rithm to find better solutions. However, defining such con-straints is often hard and time consuming even for simpleenvironments. Thus, the advantage of the cooperative al-gorithm to be able to work without these constraints is amajor advantage.

5.2 Ground Inspection of a Stuck vehicleThe second set of experiments involved sensors

mounted on wheeled robots that were requested to observea stuck vehicle as shown in figure 6. In these experiments,the user had to set a bounding box around the entire vehi-cle so that the sensors had to observe it from all angles.

The target was delimited by a box centred on thestuck vehicle, the desired sensing density D was set to5 × 105 px/m2 for every surface inside the boundingbox. The real sensors are mounted on Clearpath RoboticsHusky1 like vehicles. The camera sensors had the sameresolution and field of view as in the ISS experiment, butwithout a zooming factor. Thus the sensors had three de-grees of freedom: position [x, y] and orientation θ (pan).

1http://www.clearpathrobotics.com/husky/

(a) Cooperative optimization algorithm (average error of 0.0147) (b) CMA-ES optimization algorithm (average error of 0.1773)

Figure 5. : Median visibility results for the ISS inspection environment. The green cubes are the voxels in the virtualsensor field of view observed by the real sensors S with at least the required density d, the yellow cubes are the voxelsobserved but with a density lower than required, and the red cubes are the voxels not seen by any sensor. The violet cubes(with arrows) are the position and orientation of the sensors, while the dark orange cube is the position of the virtualsensor and the blue cubes are the rest of the environment.

0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

Function Evaluations 1e4

0.55

0.60

0.65

0.70

0.75

0.80

0.85

0.90

0.95

1.00

Avera

ge D

ensi

ty E

rror

Coop0.0

0.5

1.0

1.5

2.0

2.5

3.0

3.5

4.0

4.5

Num

ber

of

Senso

rs

Figure 7. : Average results for the cooperative optimiza-tion algorithms in the ground experiment. The circlemarked line reports the average density error with themaximum and minimum range presented by the error bars,while the unmarked line shows the average number of sen-sors.

The Huskies also carried a rotating Hokuyo URG-04LXas three-dimensional sensor.

Figure 7 presents the average error over 11 indepen-dent runs of the ground experiment. We observe thatthe average final error is 0.60 with standard deviation of0.015. This high final error is mainly caused by the fixedheight of the sensors. As a matter of fact, their height doesnot allow them to see the top and the interior of the stuck

Figure 8. : Median visibility results for the ground exper-iments. The colour code is the same as in figure 5.

vehicle, preventing them to sense a greater number of sur-faces in the desired bounding box. Figure 8 presents thevisibility data of the median results. We observe that mostof the side voxels are perfectly covered (green voxels),while the interior of the vehicle is responsible for most ofthe error. The final images taken from the four sensorsof the median result are shown in figure 9, while the cor-responding placement is shown in figure 10. This showsthat the cooperative algorithm is able to cope with sce-nario where the available sensors cannot cover the entiretarget.

Figure 9. : Views acquired by the four Huskies of the me-dian result in the stuck vehicle experiments. The Huskiesare ordered left-right and top-down.

6 Conclusion

This paper presented a framework to position mobilerobotic sensors to observe desired parts of arbitrary three-dimensional environments. The framework has beenproved useful in scenarios borrowed from the space do-main and different type of sensors, where it outperformsclassical global optimization algorithm and finds an ap-propriate number of sensors to observe the target. Futurework will focus on the following:

• computing the pixel density in overlapping regionsusing super-resolution algorithms [7],

• using multiview virtual sensor to enable stereo on thetarget,

• and integrating the displacement cost in the costfunction to reduce displacement and energy loss.

Acknowledgement

This work has been supported by grants from theFRQ-NT (Quebec) and NSERC (Canada).

References

[1] J. Amanatides and A. Woo. A fast voxel traversalalgorithm for ray tracing. In Proc. of Eurographics,1987.

[2] H.-G. Beyer and H.-G. Schwefel. Evolution strate-gies: A comprehensive introduction. Natural Com-puting, (1):3–52, 2002.

Figure 10. : Huskies final position in the median result ofthe stuck vehicle experiments. Numbers correspond to theviews shown in figure 9.

[3] Y. Collette, N. Hansen, G. Pujol, D. Salazar Aponte,and R. Le Riche. On object-oriented programmingof optimizers – Examples in Scilab. In P. Bre-itkopf and R. F. Coelho, editors, MultidisciplinaryDesign Optimization in Computational Mechanics,chapter 14, pages 527–565. Wiley, 2010.

[4] C. Connoly. The determination of next best views.In Proc. of the International Conference on Roboticsand Automation, pages 432–435, 1985.

[5] G. Dudek, M. R. M. Jenkin, E. Milios, andD. Wilkes. A taxonomy for multi-agent robotics. Au-tonomous Robots, 3(4):375–397, 1996.

[6] A. Elfes. Using occupancy grids for mobile robotperception and navigation. Computer, 22(6):46–57,1989.

[7] S. Farsiu, R. L. Robinson, M. Elad, and Milan-far. Fast and robust multi-frame super-resolution.IEEE Transaction on Image Processing, 13(10):1327–1344, 2004.

[8] F.-A. Fortin, F.-M. De Rainville, M.-A. Gardner,M. Parizeau, and C. Gagne. DEAP: Evolutionaryalgorithms made easy. Journal of Machine LearningResearch, Machine Learning Open Source Software,2012.

[9] N. Hansen and A. Ostermeier. Completely deran-domized self-adaptation in evolution strategies. Evo-lutionary Computation, 9(2):159–195, 2001.

[10] N. Koenig and A. Howard. Design and useparadigms for Gazebo, an open-source multi-robotsimulator. In Proc. of the International Conferenceon Intelligent Robots and Systems, 2004.

[11] W. E. Lorensen and H. E. Cline. Marching Cubes:A high resolution 3D surface construction algorithm.Computer Graphics, 21(4):163–169, 1987.

[12] A. Nuchter, H. Surmann, and J. Hertzberg. Planningrobot motion for 3D digitalization of indoor environ-ments. In Proc. of the International Conference onAdvanced Robotics, pages 222–227, 2003.

[13] J. O’Rourke. Art Gallery Theorems and Algorithms.Oxford University Press, 1987.

[14] R. Pito. A solution to the next best view problem forautomated surface acquisition. IEEE Transaction onPattern Analysis and Machine Intelligence, 21(10):1016–1030, 1999.

[15] M. A. Potter and K. A. De Jong. Cooperative co-evolution : An architecture for evolving co-adaptedsubcomponents. Evolutionary Computation, 8(1):1–29, 2001.

[16] M. Quigley, K. Conley, B. P. Gerkey, J. Faust,T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng. ROS:An open-source Robot Operating System. In ICRAWorkshop on Open Source Software, 2009.

[17] M. Schwager, D. Rus, and J. J. Slotine. Unifying ge-ometric, probabilistic, and potential field approachsto multi-robot deployment. International Journal ofRobotics Research, 30(3):371–383, 2011.

[18] G. H. Tarbox and Gottschlich. Planning for completesensor coverage in inspection. Computer Vision andImage Understanding, 61(1):84–111, 1995.

[19] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stach-niss, and W. Burgard. OctoMap: A probabilis-tic, flexible, and compact 3D map representation forrobotic systems. In Proc. of the ICRA Workshopon Best Practice in 3D Perception and Modeling forMobile Manipulation, 2010.