On-line Implementation for Dynamic Environments

The following describes an integrated on-line capable system that approximates the devel-oped optimal control problem in Subsec. 2.1.2 by applying a rule based brute force search.

This system was published in [7] and found application in the IURO project [4]. It allows to conduct user studies in order to find and confirm effects of parameters or to develop further aspects that affect readability.

For motion planning, B´ezier curves are used, since they feature properties which are beneficial for human-likeness and thus readability as well [60]. Firstly, the starting point b₀ and the endpoint b₃ of a B´ezier curve are freely controllable. For the robot-to-human approach, these points are fixed to the robot and the goal position and change since both move. Secondly, tangents at b₀ and b₃, that connect b₀, b₁ and b₂, b₃, allow for the definition of the final position and orientation. Thirdly, as the k-th derivative of a B´ezier curve is still continuous [41], the curve has continuous curvature. Accordingly, trajectories are consistent continuations of each other if their respective starting and ending point are the same. This is the case when continuous on-line re-planning is applied. Lastly, due to the k-fold differentiability [41] B´ezier curves supply smoothness and continuous jerk. The maximum time-frame for the trajectory construction is given by the fact that a person is only approachable until he/she reaches a distance which the robot is unable to catch up with. While the trajectory origin is always set at the robot position p_R(t), the final pose depends on the predicted movement (position, speed) for the person. Without loss of generality the person’s orientation is thereby assumed to be known.

Given these attributes, a cubic B´ezier curve is used for static scenarios, as shown in Fig. 2.2. The parameters of the final approach pose and the initial robot pose define the shape of the trajectory. The robot position controller then follows the curve with a constant velocity.

In a static collision case the degree of the curve is increased to four. Hence, one control point, hereb₂, pulls the curve away from the obstacle as shown in Fig. 2.3. For example, by checking for discontinuities in a forward-facing laser scan, object dimensions are assessed and extremal points found. Searching for a collision free curve, b₂ is shifted iteratively.

b₀ =p_R(t)

b₁

b₂

b₃

human robot

v_H

Fig. 2.2: Trajectory formed with B´ezier curve of degree three in free space

robot b₀ =p_R(t)

b₁

e_right

obstacle

b₄ b₃

b₂

e_left

human v_H

Fig. 2.3: Trajectory formed with B´ezier curve of degree four in the static collision case

The control points b₀ and b₁ remain on the line defined by the initial orientation. The pointsb₃,b₄ are still defined by the position of the humanp_H(t) and the goal posep_G(T).

Therefore, a simple turning on spot never results from this planning process. The positions of the B´ezier points may be defined as optimization constraints. For a curve of degree three it follows that:

b₁ =p_R(t) +η₁

(cos(φ_H(0)) sin(φ_H(0))

) ,

b₂ =b₃+η₂(b₃−p_H(T)),

where η₁ and η₂ are arbitrary parameters and b₃ = p_G(T) depicts the goal pose that employs social aspects like human-robot distance, positioning in the field of view and preference for sidewise approach. For degree four curves, the constraint on b2 and b3 is dependant on the obstacle dimension and its extremal points e_left and e_right. Pulling the curve out on the side ofe_left leads to the following:

b₂ =e_right+η₃(e_right−e_left), b₃ =b₄+η₄(b₄−p_H(T)),

where η3 and η4 are arbitrary parameters. Given the planned curve, it is discretized in time and space to form a discrete planar trajectory ξ_R^r(k) = [p^x_R(k), p^y_R(k), φ_R(k)]_k=1,...,K. The primary velocity profile assumes that maximum speed is possible due to absence of collisions. Deceleration is then necessary nearby the goal pose, as proposed in [9].

The constant velocity profile is adapted to provide a slow down or speed up for avoidance of moving obstacles. At first a safety region with radius r_safe is assumed around every discrete position on the trajectory. For a moving obstacle crossing the trajectory, the entrance time t_enter and the emission time t_leave are calculated by assuming a constant velocity. Based on this concept, the robot executes the trajectory either up to the colliding positionp_R(col−1) or the positionp_R(end+1) after the critical point, in order to avoid the obstacle as it leaves the zone or before it crosses. This requires accelerating or decelerating depending on the time the robot may arrive at either position. Generally, holding on to a constant velocity or slight deceleration is preferred over high accelerations, which may lead to a negative sensation for the approached person. Obstacle avoidance therefore follows a human-inspired approach [100]. Figure 2.4 illustrates the concept. Finally the arrival

pR(m)

pR(m+ 1) pR(col−1) pR(col)

tenter pR(end)

t_leave

pR(end + 1)

rsafe

Fig. 2.4: Collision zones around each discrete trajectory position are indicated by circles, while a continuous line around a way-point represents high collision potential

time at each trajectory point is estimated by the distance of two discrete points and the according robot velocity. After that, the velocity profile of the whole trajectory has to be considered in order to assess if the final goal pose is reached in time and the robot arrives simultaneously with the target person. In case the time constraint can not be met, the discrete trajectory needs to be re-planned. This leads to an iterative algorithm capable of adapting the trajectory, velocities and goal positions on-line. The separation of spacial and temporal planning entails that the trajectory shape does not change together with the velocity profiles. This renders the movement even more predictable. The real-time capability further enables the system to adapt in case of tracking errors or dynamic changes in the environment. This is necessary since the algorithm is developed with respect to the IURO project [4], where an autonomous mobile robot is required to approach pedestrians in an urban environment and initiate interactions. The capability of on-line re-planning and adaptation to dynamically changing environments is constrained to a limited number of dynamic obstacles and a certain spatial horizon. Due to this simplification the number of avoidable objects is constrained and the algorithm is thus not complete.