Música parte esencial de la capoeira

In this section, we extend the ORCA algorithm (Van den Berg et al., 2011) to elliptical agents based on the linear approximation. We refer to the new algorithm as EORCA.

2.4.2.1 Computing Neighboring Agent Constraints

To compute the velocity obstacle for an elliptical agent, we first compute a tangent from the origin of the velocity space to the boundaries of the Minkowski Sum scaled by the inverse ofτ. For a Minkowski Sum withmsamples, we can find tangents inO(lg(m)) using binary search on the vertices. The forward face of the truncated cone lies between the tangent points, and the nearest point can be computed by another binary search. Figure 2.2(A, B) illustrates the construction of the velocity obstacle of the elliptical agents using the Minkowski Sum and the tangents.

Next, we use the velocity obstacle to compute the set of permitted velocities for an agent. Given the velocity obstacle, V O_Aτ_|B, we construct the set of permitted velocities forA for reciprocal collision avoidance (Van den Berg et al., 2011), which is denoted as EORCAτ_A|B. Consider a vector~ufrom the relative velocity~vA−~vBto the nearest point on the truncated velocity obstacle. Also, let~nbe the outward normal of

the boundary ofV Oτ

A|Bat point(~vA−~vB) +~u. We assume that all agents use the same collision-avoidance

strategy. Therefore, agentAis responsible for adapting its velocity by 1₂~uassuming thatB will do the same. In this manner, the setEORCAτ_A|Bof permitted velocities forAis defined by the half-plane pointing in the direction of~ncentered at the point~vA+ 1₂~u(see Figure 2.2(C)). Hence, one half-plane constraint is

VB VA PA PB VA VB PA -PB A + B _____ τ _____ τ Forw ard Face VA VB VA-VB u u _ 2 EORCA τ A|B| (A) (B) (C) VA PA PO VA P_____O - PA τ τ A|M τ τ τ τ VAnew EORCA AA|O EORCA A|Z EORCA A|P EORCA A|M EORCA |W AA (D) (E) (F)

Figure 2.2: Computing Collision-free Velocity(A)Two agents are moving toward one another in space. The approximating polygons are shown. (B)The velocity obstacle forAinduced byBtakes the shape of a truncated cone. The apex of the cone is located at the origin in velocity space. The forward arc of the cone is the forward face of the Minkowski Sum of−AandB scaled byτ and centered at~pB−p~A/τ.(C)The

set of permitted velocities for agentAw.r.t.Brepresented by the half-plane constraintEORCAτ_A|B.(D)

An agent moves toward a line segment obstacle. (E)To computeV O_A|O, we rotate the frame of reference

such thatOis parallel to the positive x-axis. In this case, the forward arc of the cone is the forward face of the Minkowski Sum of−AandOscaled byτ and centered at~pO−~pA/τ. We also show the EORCA

constraint as a half-plane perpendicular to the vector connectingvAandV OA|O and passing through the

nearest point onV O_A|O. After the constraint is computed, it is rotated back to the axis-aligned reference

frame. (F)After all the constraints have been computed, we can determine a feasible velocity inside the union of all the EORCA sets. A new velocity is chosen from the region of feasible velocities.

2.4.2.2 Computing Neighboring Obstacle Constraints

Without loss of generality, we can assume that all obstacles in the scene are triangulated and their projections on the 2D plane are given as a collection of line segments. To compute the velocity obstacle

V Oτ_A|Ofor agentAinduced by a line segmentO, we implicitly compute the Minkowski Sum of−AandO

scaled by the inverse ofτ. An agentAwill collide with obstacleOwithin the timeτ if its velocity~vAis

insideV O_Aτ_|O. Geometrically, the Minkowski Sum is equivalent to sliding the reflectedAalong the scaled line segment (each scaled byτ). Because discs are unaffected by orientation issues, the computations related to determining the shape of the velocity obstacle, finding tangents, and determining the closest point on the obstacle can be implemented by using a small number of floating point operations. With elliptical agents, the shape of the velocity obstacle, the tangents, and the nearest point operations are governed by the orientation of the agent as well as of the obstacle.

Let −θ ∈ [0,−2π] represent the angle between O and the positive x-axis. We can simplify the computation by rotating the coordinate system byθ, i.e., we rotateO and agentAbyθand compute the appropriate constraints in the transformed space. Let~orot_A denote the orientation of the ellipse after the rotation transformation. For the remainder of this section, we can assume thatOis parallel to the positive x-axis. Therefore, the shape of the velocity obstacle depends only on the orientation~orot_A of the agent. We can also accelerate the computation of tangents and the closest points (Section 2.5).

Once rotated, we determine whether the agent’s velocity projects onto the left tangent, right tangent, left face, right face, or line segment. Next, we compute the pointF_A|O on the velocity obstacle closest to

~vA. The half plane defined using the tangent to the velocity obstacle atFA|Oyields the set EORCAτA|Oof

permitted velocities forAwith respect toO(Figure 2.2(E)). We rotate the final computed constraint by−θ

to transform back into the original space (Figure 2.2(F)). 2.4.2.3 Choosing a Collision-free Velocity

At every simulation step, we construct the half-plane constraints for each neighboring agent and obstacle. The set of neighboring agents and obstacles can be computed efficiently by using a spatial data structure, such as a kD-tree. The set of permitted velocities for agentAis simply the convex region – EORCAτ_A– given by the intersection of the half-planes of the permitted velocities that are induced by all the neighboring agents and obstacle (Figure 2.2(F)).

EORCAτ_A= \

B6=A

EORCAτ_A|B (2.16)

The agent is responsible for selecting a new velocityvnew_A from EORCAτ_Athat minimizes the deviation from its preferred velocity~v_A0.

~

v_Anew= arg min

~

v∈EORCAτA

k~v−~v_A0k (2.17)

Equation (2.16) and Equation (2.17) can be solved efficiently with an expected runtime ofO(n)using linear programming, where n is the total number of constraints.

2.4.2.4 Collision-free Guarantees

If the linear programming algorithm can compute a feasible solution for each agent, we can guar- antee that the resulting trajectories will be collision-free. This follows from our conservative, bounding

Px

S

O

P

S

S

O

P

S

Figure 2.3:Bounding Disc vs Ellipse.Disc (Blue) and ellipse (Brown) for (Left) a human of average body width and depth; (Right) HRP-4 robot.

linear approximation (Section 2.4.1) for each ellipse. Furthermore, we extended the original ORCA algo- rithm (Van den Berg et al., 2011) to elliptical agents by formulating appropriate constraints (Section 2.4.2.1 and Section 2.4.2.2) that preserve the properties of the velocity obstacles. The set of permitted velocities

EORCAτ_A|B andEORCAτ_B|A for agentsAandB, respectively, are reciprocally maximal. The overall

approach is conservative, but it guarantees collision-free navigation. In densely packed conditions, the EORCA set of feasible velocities may be empty, in which case the 2D linear program will not find a solution. One possibility is to change the orientation of the ellipses (see EORCA-F in Section 2.6) to find a solution. If that does not find a feasible solution, we can select the velocity that minimally penetrates the constraints generated by neighboring robots, by using 3D linear programming (Van den Berg et al., 2011).