Chapter 2 16
2.5 Multi-Policy Decision Making
2.5.2 The Cost Function
Progress: The term encourages the robot for going toward the goal. With the unit vector 𝑛𝑟→𝑔𝑟 toward its goal and a future position applying a policy 𝑥𝑟(𝑡𝐻, 𝜉), the cost of progress can be expressed as:
PG (𝑋̂(ξ)) = (𝑥𝑟(𝑡𝐻, ξ) − 𝑥𝑟) ⋅ 𝑛𝑟→𝑔𝑟 (2.)
Chapter 3
Socially-Aware Navigation
3.1 System Architecture
The overview of our system is shown in Figure 3.1. From the multi-human indoor environment, the robot will operate in a proper action and movement based on the current situation which can be described by the state of human and obstacles. Each control loop starts from sensing module, which combines a laser-based submodule with a vision-based one, and collects data including human information and obstacles position. About the second stage, we are going to construct models of human and static obstacles from the collected data. After modeling the environment, the main procedure of decision making is applied. This multi-policy decision making procedure will choose an optimal policy that matches the social situation that the robot is currently facing. Recall that the velocities controlled by the system behind are translation along x-axis, y-axis and rotation about z-axis, namely [𝑣𝑇 𝑣𝐿 𝜔]𝑇 , the control module perform to send proper velocity command to actuators. Following the optimal policy, the robot navigates in the environment by controlling the robot’s behaved velocities to achieve socially-aware navigation. The description of each function modules is shown below.
In the sensing module block, it is composed of laser-based and vision-based models. For laser-based model, we collect the laser data as geometry feature such as distance and angle. For vision-based model, we use the multi-person
2D keypoints detection method proposed by [42] to calculate human heading direction. The detail is described in Chapter 2.
Figure 3.1. Overall system architecture.
In the human detection module, we use the sensing data to construct individual human model and go one step further to construct group model from it. This individual and group models together will be considered as the social situation faced by the robot. After the construction, this information will form a joint state of robot and pedestrian which will be a part of decision input.
For other static obstacles in the environment, such as wall, sofa, chair, we can also collect the geometry data from laser. Here, we use the idea of gap analysis [6]. This method clusters the laser data into several groups based on the Euclidean distance between two adjacent laser points. From the analysis, a sequence of gaps is confirmed and describes the free space of the robot configuration. After clustering, we also do the line detection to figure out where a wall is. The detail is described in Chapter 2.
The policy set consists of several policies, also known as robot behaviors, including “Go-Forward”, “Follow Agent”, “Wall Following”, “Stop”. Actually, the size of executable policy list is dynamic because the number of agents who can be followed is yet to be determined. Therefore, this module also takes human states as input to construct the current policy set.
In Multi-Policy Decision Making module, we take the human model, gaps, and a list of executable policies into consideration. First, this module does sampling from the current observation of joint state. Second, it simulates all the agents including human and robot forward time for some duration with the executable policies and the calculate the total cost of each individual policy. Based on the cost, an optimal one is chosen by this decision making procedure for the robot to execute. The submodule of this procedure is shown in Figure 3.2.
As has been described in Chapter 2, the control module is composed of robot
control and actuator model. Robot control model follow the optimal policy ξ𝑜𝑝𝑡𝑖𝑚𝑎𝑙 to generate the velocity [𝑣𝑇 𝑣𝐿 𝜔] . The actuator model then generates the output velocity [𝑣𝑙 𝑣𝑟 𝑣𝑏] to each wheel. The hardware design and system detail can be seen in Chapter 2 as well.
3.2 Human Model Construction
To achieve a socially-aware navigation, we take human feeling and nature into consideration. We first define an individual model which represents a human with several assisting information including position, orientation, and velocity. Constructed from sensing data, this individual model can express the human trajectory and behavior that become the social cues. Different from previous works, we go one step further to construct the group model that describes the dynamics and behavior of a crowd. The behavior between pedestrians in a group can give us more information about human crowds. In the real environment, pedestrians walk either in individual way or with others to form a group.
Following this concept, we propose a human model including individual and dynamic group model that show the social situation surrounding the robot.
3.2.1 Individual Model Construction
To construct an individual human model, we first define the component of this model.
The information of an individual model includes position, orientation, and velocity. The model can be written as:
ℎ = [𝑝𝑥 𝑝𝑦 𝜃 𝑣𝑥 𝑣𝑦] (3.)
As mentioned in Chapter 2, we can get the information of position and velocity from laser data. Extracting the geometry features from laser points, we first detect human legs and calculate the position. Afterward, a SIR particle filter is used to predict human state and calculate human velocity. In this subsection, we focus on how to figure out human orientation.
To obtain human heading or body orientation, we utilize the multi-person pose estimation proposed by [42] to collect the keypoints of human body. A list of keypoints can be obtained from the networks and represented in image coordinate. Recalled from Chapter 2, the keypoints representing the shoulders, arms are used to calculate the human orientation. We first utilize the property that the length of arm and width of shoulder would be proportional for all human. This property can also be applicable to the situation where the proportion of human body in image is changed by the distance between RGB sensor and human. A predefined parameter 𝑅𝑎𝑡𝑖𝑜𝑤/𝑙 is the ratio of width of shoulder 𝑤𝑟𝑒𝑓 and length of arm 𝑙𝑟𝑒𝑓 in a normal situation. With this ratio, for each sampling data {𝑤′, 𝑙′} we can calculate the angle of orientation in [0, 𝜋] . The formula and illustration are shown below.
𝑅𝑎𝑡𝑖𝑜𝑤/𝑙 =𝑤𝑟𝑒𝑓
𝑙𝑟𝑒𝑓 (3.)
𝜃𝐻 = 𝑐𝑜𝑠−1( 𝑤′
𝑙′× 𝑅𝑎𝑡𝑖𝑜𝑤/𝑙) (3.)
Figure 3.3. Illustration of The Width 𝑤𝑟𝑒𝑓 And Length 𝑙𝑟𝑒𝑓.
Then, we translate the angle of orientation into [0, 2𝜋] in order to project the orientation into 2D plane. By doing this, we need to determine whether the rotation is made clockwise or counterclockwise. By the fact that the nearer arm should be longer than the farer one in image, we compare the length of both arms to figure out the rotation direction. Therefore, we get the orientation of individual projecting in 2D plane.
In general case, the calculation above is precise. However, the detection may have some noise. In order to be more robust, we use the image pixels on the head which are more stable during detection. By comparing with the distance 𝑙𝑙𝑒𝑓𝑡_𝑒𝑦𝑒 and 𝑙𝑟𝑖𝑔ℎ𝑡_𝑒𝑦𝑒 , we can distinguish the orientation into left-oriented or right-oriented. Using this information, we can avoid the situation when the key points of the arms cannot be detected.
Figure 3.4. Illustration of Facial Distances.
After the detection procedures of these two module, we need to match the results of them. Because the sensing range of the webcam is much smaller than the Laser Rangefinder, we just match pedestrians who are detected by the vision-based module.
First, we align the central line of these two sensors. Second, we calculate the direction of human in the image regards of the central line. An example is shown in Figure. Also, the result of laser-based module can show the direction of human regards of the heading direction of robot, which is the same as the central line. Third, with the calculation result of second part, we can match the detection result to form the human state [𝑝𝑥 𝑝𝑦 𝜃 𝑣𝑥 𝑣𝑦].
3.2.2 Dynamic Group Model Construction
Pedestrians belong to a group behave in a different way compared to individuals and can give more extra information for robot to interpret social situation. In the sociology research of pedestrian behavior [45 - 48], a group of pedestrian is composed of many types with different walking characteristics and objectives.
Before construction, we need to discuss some observation of pedestrian near robot.
Figure 3.5 is the illustration of a robot move in multi-human situation. The diagram shows that the pedestrians near robot is distinguished into two classifications. The yellow area is the sensing area of robot and the white area is out of sensing range. The green cycle will perform social forces to robot and the blue rectangle would not. The dotted line shows the sensing range of robot. In that case, there are three types of pedestrian who would not generate social force to robot to be described below:
the pedestrian who is out of the sensing range of robot.
the pedestrian who is blocked by others.
the pedestrian who is farther than a threshold of distance.
Even these humans have been detected by our sensing module, we do not consider them as the input of our group construction. Thus, we focus on constructing the detected pedestrians who are sufficiently close to the robot into groups.
Figure 3.5. Illustration of Robot Navigating in Multi-Human Environment
First, we divide the detected humans into groups by considering three conditions:
the relative distance between individuals.
the difference of moving direction between individuals and the group.
the magnitude of velocity.
By the conditions, we define three thresholds, 𝐷𝑡ℎ𝑟𝑒, 𝜃𝑡ℎ𝑟𝑒 and 𝑅𝑎𝑡𝑖𝑜𝑣. The type of pedestrian can be decided by the moving direction and velocity. We assume that the humans belonging to a group have the same type of pedestrian who maintains a similar moving direction and velocity. Therefore, a group formed by one object is characterized by two informative elements, {|𝑣𝐺|, 𝑛𝐺}, group velocity and its moving direction. Given a list of detected humans, we check if a person ℎ́ belongs to a group 𝐺́ in the group list 𝐺̂ by calculating the value of conditions. If the conditions are matched, we append the human to the group and update the properties{|𝑣𝐺|, 𝑛𝐺}. On the contrary, we build up a new object of group with this person and calculate the properties. After that, a list of groups, 𝐺̂, is built. The procedure is shown in the following Algorithm.
Algorithm Group_Construction(𝑯̂ ) 1. 𝑯̂ is the list of detected human.
2. 𝑮̂ is a two-dimension vector stores each groups.
3. 𝑮́ is a one-dimension vector stores each individual belong the group 4. 𝑮̂ = {}
21. 𝑮́. 𝒖𝒑𝒅𝒂𝒕𝒆() 22. 𝑮̂. 𝒂𝒑𝒑𝒆𝒏𝒅(𝑮́) 23. end if
24. end for 25. return 𝑮̂
After categorizing the individuals, we define a parameter that describes the interacting state between people in a group. By the sociology research [49], pedestrians can interact with others by rotating their heads. Therefore, to decide the interacting state defined as a continues value, the eyes-contact becomes an important cue. From the individual model, the orientation 𝜃𝐻 of each pedestrian in a group is used to confirm the eyes-contact. The Figure 3.6 illustrates of this situation.
Figure 3.6. (a) Two People Interacting. (b) The Situation in Three People.
The interacting factor used in later section can be expressed as:
η𝑖𝑛𝑡 = { 1 𝑖𝑓 𝐶ℎ𝑒𝑐𝑘_𝐹𝑎𝑐𝑒_𝐸𝑛𝑔𝑎𝑔𝑒 = 𝑡𝑢𝑟𝑒
0 𝑖𝑓 𝐶ℎ𝑒𝑐𝑘_𝐹𝑎𝑐𝑒_𝐸𝑛𝑔𝑎𝑔𝑒 = 𝑓𝑎𝑙𝑠𝑒 (3.)
and orientation data. Here, we use the concept of vector geometry and vector algebra.
Given two human states including their position and orientation information, the example is shown in Figure 3.7 below.
Figure 3.7. Example of The Face-Engagement Checking.
Figure 3.8. The Vectors of Calculation.
As shown in Figure 3.8 above, 𝑎⃗ and 𝑏⃗⃗ are the vectors along each orientation, 𝑃⃗⃗⃗⃗⃗⃗⃗⃗⃗ 1𝑃2 and 𝑃⃗⃗⃗⃗⃗⃗⃗⃗⃗ are the vectors from one person toward another. By the calculation of vector 2𝑃1 geometry, we can figure out whether the extensions of the vectors 𝑎⃗ and 𝑏⃗⃗ will cross each other or not. The detail of calculation is shown in Algorithm below.
After that, we borrow the concept of the “motion of bands” from Hughes [47, 48]
which states that the crowds would form several virtual lanes in which human try to maintain the same type of pedestrian, as known as the property of a group. Therefore, we can calculate the width of a group that can be seen as the width of virtual lane. In the previous paragraph, a group of people would form a rectangle rather than an egg-shaped or ellipse-shaped. An example is shown in Figure 3.9. Therefore, we set the width of dynamic grouping model as the distance of the people on the sides.
Figure 3.9. Example of A Rectangle-Shaped Grouping Model.
The virtual lane would provide an information that a group of pedestrians would maintain walking inside the lane. This information would be used in the Group Following behavior described in Section 3.4.
Finally, we can construct a list of groups and their components including a list of human, velocity, moving direction, width, and the interacting factor. For the group with
Algorithm 𝑪𝒉𝒆𝒄𝒌_𝑺𝒊𝒈𝒉_𝑪𝒓𝒐𝒔𝒔(𝑮́)
1. 𝑮́ is a group including several human states 2. Recalled that 𝒉𝒊 = [𝒑𝒙𝒊, 𝒑𝒚𝒊, 𝜽𝒊, 𝒗𝒙𝒊, 𝒗𝒚𝒊] ∀ 𝒉𝒊 ∈ 𝑮́
3.3 Extended Social Force Model
In tradition, Social Force Model (SFM) is the description of interaction [1], such like agent-to-target, agent-to-agent and agent-to-object. For an agent in the environment, he/she can be attracted toward a target or repulsed by other agents and static obstacles.
Most of the previous works using a non-holonomic system with SFM do not take the orientation issue into consideration since the robot will always face to the moving direction. However, we use a holonomic mobile robot instead that the facing orientation does not always align with the moving direction. This characteristic is actually one of the important feature of human motion. Therefore, to achieve a natural movement and
model describing the orientation of individual agent applied by a holonomic system.
3.3.1 Observation and Discussion of Social Force Model
Several methods have been developed based on social force model to achieve socially-awareness during navigation. Applying a non-holonomic mobile robot, these methods not only drive the robot but also model the human by social force. By the control system, a non-holonomic robot, such as Pioneer 3-DX Mobile Robot, can imitate parts of human motions and predict future trajectory of neighboring people. In our approach, we use a holonomic mobile robot instead to achieve more natural way in avoidance behavior and walking characteristic. In that case, the difference between these two system using a traditional social force model becomes the question.
Figure 3.10. Illustration of Non-Holonomic and Holonomic Applying SFM.
A non-holonomic robot and holonomic one applied with social force model are
shown in Figure 3.10 (a) and (b), respectively. Driven by the overall force 𝐹𝑖, these two system perform different moving behavior. Solid lines represent the social force and the dotted lines show the future trajectory. Due to the mechanism, a non-holonomic robot driven by the overall social force 𝐹𝑖 does not move along the direction of force immediately. Instead of that, it takes a curve movement while considering the radius of gyration and align the orientation with direction of tangential velocity 𝑣𝑇. The forward kinematic of nonholonomic is described in equation:
( characteristic makes robot move along the direction of resultant force without rotating its orientation. Though the robot can still behavior in a social way, it is weird to face a fixed direction during moving. Actually, facing or body orientation is an important social signal in human motion. This phenomenon shows the problem of holonomic system with traditional social force that is caused by an unexpected and unnatural behavior.
Therefore, we summarize several conclusions based on the observation:
Traditional Social Force Model maps the interaction between two objects into force filed. An object is affected by the sum of these forces following the Newton’s law.
This property shows that the SFM drives an object toward a direction without describing the orientation.
Previous approaches use a non-holonomic mobile robot and predict future trajectory
the non-holonomic system.
From the observation, a holonomic mobile system applying the SFM would cause a problem that robot will move along the direction of resultant force without rotating its orientation. Thus, the traditional SFM cannot be applied directly to a holonomic mobile robot.
3.3.2 Rotation Social Force
According to the observations and conclusions above, we propose a rotation social force to form an extended Social Force Model. To define an additional social force which describes the rotation behavior, we follow the concept of traditional social force functions.
A social force would have several parameters, such as strength, range and direction which correspond to {𝛼𝑖, 𝛽𝑖, 𝑛𝑖 | 𝑖 ∈ 𝑝, 𝑜} described in Section 2.3. The Figure 3.11 shows that an agent driven by SFM toward a direction, where 𝜃𝐹 and 𝜃𝐻 are the angle of resultant social force and heading. Based on the concept of parameters, a force 𝑓𝑟𝑜𝑡 performing the rotation can be defined as:
𝑓𝑟𝑜𝑡 = 𝛼𝑟𝑜𝑡|𝐹⃗⃗⃗| exp (𝑖 𝛽𝑟𝑜𝑡
|𝜃𝐹− 𝜃𝐻| + 𝜀) ⋅ 𝐷𝑜𝑅(𝜃𝐹, 𝜃𝐻), 𝜀 → 0 (3.)
where |𝐹⃗⃗⃗| is the magnitude of the resultant force, {𝛼𝑖 𝑟𝑜𝑡, 𝛽𝑟𝑜𝑡} are the rotational parameters of strength and range, ε is a positive number closed to zero and the output of 𝐷𝑜𝑅 (Direction of Rotation) function shows if it is clockwise or counterclockwise .
Figure 3.11. Example Shows The Angle of Social Force And Heading.
We first define the sense of rotation. A positive and negative rotation social force would cause a counterclockwise and clockwise rotation, respectively. Afterwards we define the 𝐷𝑜𝑅 function which takes 𝜃𝐹 and 𝜃𝐻 as input. From sensing module, the human orientation 𝜃𝐻 can be calculated in global coordinate. In 𝐷𝑜𝑅 function, we first transform 𝜃𝐹 and 𝜃𝐻 into local coordinate 𝜃 ∈ {0,2𝜋}. We set 𝜃𝐻 as zero reference degree and θF1 as the angle transformed from 𝜃𝐹. Second, we transform the coordinate system into 𝜃 ∈ {−𝜋, 𝜋} . Then, 𝜃𝐹2 ∈ {−𝜋, 𝜋} is transformed from 𝜃𝐹1 . Finally, the output of 𝐷𝑜𝑅 function is the sign of 𝜃𝐹2 and the 𝑠𝑔𝑛() is the sign function. The equations can be written as:
𝜃𝐹1 = {𝜃𝐹− 𝜃𝐻 𝑖𝑓 𝜃𝐹− 𝜃𝐻 > 0∘
𝜃𝐹 − 𝜃𝐻+ 360 𝑖𝑓 𝜃𝐹− 𝜃𝐻 < 0∘ (3.)
𝜃𝐹2 = {𝜃𝐹1 𝑖𝑓 0∘≤ 𝜃𝐹1 ≤ 180∘
𝜃𝐹1− 360 𝑖𝑓 180∘< 𝜃𝐹1 < 360∘ (3.)
𝐷𝑜𝑅(𝜃𝐹, 𝜃𝐻) = 𝑠𝑔𝑛(𝜃𝐹2) (3.)
From the sociology and the research on human motion, we human being, a hybrid system of holonomic and non-holonomic, would walk with/without rotation. As mentioned in Chapter 2, the lateral and diagonal movement would not change the orientation but the “rotation without changing translation direction” movement does.
Therefore, when robot has to rotate and apply the rotation social force become the next thing to consider about. Here, we follow the idea proposed by Hughes et al. [20] that a sidestepping, the lateral movement, and the diagonal movement is mainly used as an emergency collision avoidance strategy. In laboratory experiments, they concluded that the use of holonomic movement, the lateral and diagonal one, is constrained by the time to avoid collision and the moving direction. In Figure 3.12 (a), the circle of time to collision window proposed by [20] is performed. The white and yellow areas represent non-holonomic and holonomic solution spaces respectively. 𝑣𝑠𝑜𝑙1 and 𝑣𝑠𝑜𝑙2 are the examples of solution trajectories. The green kite shape is a representation of a person.
Figure 3.12. (a) Time to Collision Window. (b) One-Step Window
Based on the description of last paragraph, we complete the definition of rotational social force by modifying the concept of circle shown in Figure 3.12 (a). we define the solution space in white is the area that we apply the rotational social force directly. On the other hand, the solution space in yellow would execute a lateral or diagonal movement.
3.4 Robot Behavior Design
In this section we design several robot behaviors based on the extended Social Force Model and environment model including individual, group and obstacles. A behavior, also known as a policy, is designed to react to social situation and drive robot toward its goal.
In this section we design several robot behaviors based on the extended Social Force Model and environment model including individual, group and obstacles. A behavior, also known as a policy, is designed to react to social situation and drive robot toward its goal.