Raspberry Pi & Arch Linux - Day 15 - Obstacle Avoidance Method (1)

Let us talk about the obstacle avoidance algorithm today.

We start from some simple ideas. Here is a paper illustrating the idea of vector field histogram:

http://www-personal.umich.edu/~johannb/Papers/paper16.pdf

The main idea is to use a two-dimentional Cartesian histogram grids to represent the environment. Each cell of grid has a certainty value which represents the confidence level in the existence of an obstacle. We can think of this grid as a probabilistic view of the obstacle location.

One of the steps in the Vector Field Histogram (VFH) approach is that we need to reduce the two dimentional Cartesian histogram grids to one-dimentional polar histogram. Honestly I do not quite understand why we need to convert from two-dimentional Cartesian system to one-dimentional polar system instead of having the polar system from the beginning. In our case, we have a rotational distance radar; hence it is very natural to work with polar system in the first place.

Here is a draft version of code that creates the polar histogram. There is nothing specially here. What we need to do is to align the data from radar motor and the data from distance sensor based on the timestamp. The data from the radar motor provides the angle of the distance sensor and the data from the sensor is just the distance. Once the timestamp is aligned, we will know the distance measured when the radar is positioned at a particular angle. And this is essentially the polar histogram.

def create_distance_map(df_radar_base, df_distance_sensor):
    """
    Create the distance map. The map represents the environment in front of the robot. It is the association between position of
    radar (in degree) and the distance detected in that position.

    Args:
        df_radar_base:      data sent from distance radar component.
        df_distance_sensor: data sent from the distance radar sensor component.

    Return:
        A pd.Series. The index is the position of the radar base and the value is the distance deteced. 

    Note:
        The map is discrete. The value of index in the retured Series is integer.
    """



    df_sensor = df_distance_sensor
    df_base = df_radar_base

    df_sensor['timestamp'] = format_timestamp(df_sensor['timestamp'])
    df_base['timestamp']   = format_timestamp(df_base['timestamp'])

    df = pd.merge(df_sensor, df_base, on='timestamp', how='outer', suffixes=('_sensor','_base')).sort_values('timestamp')
    
    # smooth the distance.
    # window = int(df.shape[0] / df['pos'].notnull().sum())
    # df['avg_distance'] = df['distance'].rolling(window=window, min_periods=1).mean()
    # df['distance'] = df['distance'].combine_first(df['avg_distance'])
    # df = df.drop('avg_distance', axis=1)

    # clean the data
    df['distance'] = df['distance'].fillna(method='ffill')
    df['status']   = df['distance'].fillna(method='ffill')

    df_work = df[(df['pos'].notnull()) & (df['distance'].notnull())]

    # select useful information
    df_work = df_work[['timestamp','status','distance','pos','degree']]


    # create the distance map
    df_work['pos_bin'] = df_work['pos'].astype(int)

    ts_distance_map = df_work.gropuby(by='pos_bin')['distance'].mean().sort_index()

    return ts_distance_map

To align the timestamp, we merge the two data frame on timestamp and then fill the NaN. In pandas-0.19, there is a new function called merge_asof and it can merge two data frames without an exact match on keys.

In the context of the VFH method, obstacles exert repulsive forces and the magnitude of the such forces depends on the distance between obstacles and the robot. All these repulsive forces form a kind of filed and this is reason why we called this method Vector Field Histogram.

VFH is a simple idea but it is that easy to implement. The algorithm actually has three steps:

1. collect and transform data
2. generate signal/command
3. physical steer controllers

We will even experience difficulties for the first item for our ultrasonic sensor is a cheap one and it is not very stable. Also the measurement itself may not be accurate enough. For the second and third item, we have more serious problems. We can control the speed and the direction of the robot but we do not have any idea of the absolute speed and we loosely control the angle of the turn. 

I think the next big question is how we can continuously update the polar histogram without fine control on the speed and angle of the robot.

--END--