2.1 Data acquisition

For each detection event, new laser measurements ri,θi, i=1,…, nt are captured from the robot, where r_i and θi∈[-π/2,π/2] are respectively the distance and the orientation relatives to the robot from which a human is observed, and n_t is the number of measurements of humans for each detection event (see Fig. 1).

These measures are organized in a vector of n_t-distances dnew=r1,…, rntT and its corresponding ray-angles θnew=θ1,…, θntT. Due to computational costs, a mechanism for eliminating non-informative data must be incorporated.

Figure 1.  Robotic Cognition System. 

In this manner, new observations of humans, compactly expressed by Dnew=[dnew,θnew] must be incorporated or not into the points-cloud D=[d,θ] acquired so far. The procedure is based on storage and update policies, which are shown in the next subsections (see flowchart in Fig. 2).

Figure 2.  Flowchart of the cognition system. 

2.2 Storage and update algorithms

Let N=size(D) be the size of the points-cloud, which is limited by the need of reducing computational costs, outliers observations, and redundant data. These objectives are reached by using a subsampling procedure, which is the random process of reducing the sample size from K to k<K (subsampling performed in one step), where each observation remains in the subsample with probability k=K. If a storage quota of C observations is also considered, then it is required to perform a subsampling every time that the number of stored observations N exceeds the quota C, in which each observation has a probability C=N of remaining in the subsample (recursive subsampling).

This task is reached by using Algorithm 1, in which new observations are properly incorporated in D so that all observations (regardless of when they are acquired) have the same probability p of belonging to the points-cloud.

The procedure consist of initializing a empty points-cloud (N=0) and an acceptance probability of new observations p=1. The algorithm incorporates all the observations acquired to the pointscloud until N+n_t>C. When it happens for the first time, C observations must be subsampled and the remaining observations must be deleted.

Then, probability of belonging to the points-cloud is updated to p=C/N, and N is set to C. From this moment, all new observations must undergo an acceptance process in order to match their probability with the probability p of the observations acquired previously (steps 6-10 of the Algorithm 1). Later, if the quota is still exceeded a new subsampling is performed and the acceptance probability is decreased (steps 11-13 of the Algorithm 1). In this way, all observations (originals and news) have the same probability p of belonging to the points-cloud.

Algorithm 1 (D,p)=subsampling(D,D_new,p)

1: N=size(D)

2: n_t=size(D_new)

3: if N+n_t≤C then if the storage quota is not exceeded

4: D=[D;D_new]

5: else. if the storage quota is exceeded

6: for i=1:n_t do for each new observation

7: u=rand(0,1) uniform random number

8: if u>p then acceptance-rejection procedure

9: i-th new observation is deleted

10: n_t=n_t-1

11: if N+n_t>C then if the quota is still exceeded

12: subsample C obs. without replacement.

13: p=pC/N update the acceptance probability

14: N=C

15: else

16: D=[D;D_new]

Subsequently, a process is incorporated to avoid the agglomeration of observations in specific angular direction from the robot framework, in order to unify the angular distribution of the pointscloud. For them, the number of measures per ray is also limited by considering only the ˜C nearest measures to the robot, and measures in excess are discarded (see Algorithm 2).

Algorithm 2 D=RayMeasures(D)

1: for i=1:180 do for each angle

2: #θ_i=size(θ= θ_i) number of obs. in each angle θ_i.

3: if #θ_i>C~ then agglomeration test

4: Leave only the C~ nearest obs. and delete the others.

2.3 Identification of the elliptical social zone

The identification procedure consists of estimating the parameters a and b that minimize the following functional

Ja,bD=∑i=1Nwicos2θa2+sin2θb2-1r22,

where ri,θi is the i-th observation, and wi are weight factors that allow to incorporate influence levels of the observations in the estimation. This optimization is equivalent to find the weighted least squares solution of the linear system Y = XΘ, where

Y=r1-2r2-2⋮rN-2,X=cos2θ1cos2θ2⋮cos2θNsin2θ1sin2θ2⋮sin2θN,Θ=a-2b-2

and the weight matrix is the NxN diagonal matrix

W=diag({w_i}).

In this case, the weight factors are defined as

wi=δ+sin2(θi)δ+1exp⁡-ri-rmin,iμ2,

where δ,μ>0 are design constants and r_min,i is the minimum distance observed in the direction θi. This definition gives higher priority to points near to the robot (to obtain an internal ellipse to the points-cloud), as well as, to points in the front-side (to compensate that these observations are less frequent and distant from each other).

Then, the solution of the optimization problem is given by

Algorithm 3 performs this estimation.

Algorithm 3 [a;b]=ElipseWLS(D)

1: r = D(:,1), θ=D(:,2)

2: for i = 1:N do for each observation

3: r_min,i=min(r(θ = θ_i))

4: Y(i,:)=[cos2(θi),sin2(θi)]

5: X(i)=1/r_i²

6: W(i,i)=w_i

7: Θ = (XTWX)-1XTWY.

8: a=Θ(1)-1/2,b=Θ(2)-1/2

2.4 Learning indicator

Infer a social space requires to define a learning indicator during the cognition stage, which determines a condition where the ellipse dimensions can be well defined, and in consequence a social zone for human collision avoidance algorithms could be estimated.

Let γi be a dispersion factor for the laser-angle θi with i=1,...,180, defined as,

where r-i is the mean of the distances in the direction θi, R is a radius-range used as design parameter and where it is expected to fit the ellipse (represents a radius of social interaction), and r_min,i is the minimum distance observed in the direction θi.

In this manner, let γ be the learning indicator defined by,

In this manner, the points-cloud can be only used for estimating a social zone when γ is less than a threshold.

4.1 Principal task: Collision avoidance with humans

The meddling avoidance of the robot into human social zones is defined as the principal task. Thus, if V is considered as the task variable, then from (8), the minimal norm solution for this task is expressed through the control action x˙r(1) as,

where the desired potential is V_d=0, V~≔Vd-V, and K2=diag(k2,k2), with k₂>0 a design parameter. The incorporation of g improves the performance of the evasion of humans compared to other approaches for common dynamic obstacles. This factor compensates the linear motion of the obstacle, but also it is related to the angular motion of this elliptical shape.

4.2 Secondary task: Trajectory tracking control

For the secondary task, consider a desired trajectory x_t=[x_t,y_t]^T , which must be tracked by the robot. In this way, the control objective is based on controlling the robot position x_r=[x_r,y_r]^T. For this, consider an error-proportional solution for the secondary task expressed through the Cartesian robot velocities as

where x~=[xt-xr,yt-yr]T, and K₂=diag(k₂,k₂) with k₂>0 a design parameter for the secondary task.

4.3 Solution for a differential drive mobile robot

Consider the non-holonomic kinematic model of the robot expressed as,

where x_r=[x_r,y_r]^T is the robot position located on the symmetry axis between the wheels at a distance of r from the wheel axis, φr is the robot orientation, J_r is the Jacobian, and u_r=[V_r,w_r]^T is a control input vector with the linear and angular velocity respectively.

Thereby, consider an inverse kinematic solution given by

where X˙rc=[x˙rc,y˙rc]T is the control action in Cartesian coordinates, which must be defined to develop both tasks respecting the priority order. With this purpose, it is defined a null-space based control defined by (^{Antonelli, Arrichiello, y Chiaverini, 2009}; ^{Chiaverini, 1997}):

where I-J1+J1 is the projection over the null-space of J₁.

5.1 Experimental setup

The experimentation is developed in a structured scenario, which is scanned by a ceiling camera. The algorithm consists in capturing color images in each sample time, where the posture of each experimental individual, i.e. the robot and the human obstacle is characterized through the centroid positions of two colors, easily distinguishable (Colors are neither in the scenario nor in the experimental individuals.).

Additionally, a Pioneer 3AT robot is equipped with a 180-degrees LIDAR sensor (one degree of resolution), and the measures and commands to the robot are sent through a client/server WiFi connection. In this case, a sample time of t_s = 0.1[s] is considered to control the robot during the experiment. The selected parameters for the experiments are shown in Table 1.

5.2 Cognition system results

In Fig. 4, it is presented the trajectory followed by the robot when captures the information. Note that, at first, the robot takes measurements, which are not representative to infer a social zone, but they are discarded in an iterative way during the experiment, and only the points in the nearest to the robot are stored and updated according to the previous defined policies.

Figure 4.  Photo sequence during the experiment. 

At the beginning of the experiment, the robot tracks the trajectory without avoiding the human obstacles, i.e. while r > 1 the humans are in charge of avoiding the robot, because the robot is not able to infer a social zone yet (see video clip in https://youtu.be/acpYu0I3XDI).

Along time, the inferred major and minor-axis length achieves a bounded and practically constant condition, even when the learning factor continues decreasing (see results before dashedred-line in Fig. 5).

Once that the spread of the points-cloud is well-defined around the social zone of the robot, i.e. when r < 1, the robotic system starts using this learning to avoid the human obstacles while tracking the trajectory as intended. This time depends of each experiment and the degree of interaction with humans, and in this case, it happens at t = 133.3[s]. Note that the cognitive system continues getting new data and improving its learning factor, but the variation of the inferred major and minor-axis length remains practically bounded (see results after dashed-red-line in Fig. 5).

Figure 5.  Learning indicator and inferred ellipse dimensions along the time. Dashed red lines refers the point when r = 1. 

5.3 Motion control results

The control errors and the trajectories generated during a timelapse of the experiment are shown in Fig. 6. Marked with green boxes, some collision avoidance events are shown, where the controller is able to guarantee the convergence of the trajectories after that the collision is avoided. Note that the control objectives are fulfilled in a priority order as expected (see video clip in https://youtu.be/acpYu0I3XDI).

Figure 6.  Control errors of the robot.