Try again - using resampling and weighted observation model
This commit is contained in:
Danny Staple
@@ -0,0 +1,84 @@
|
||||
"""Represent the lines and target zone of the arena"""
|
||||
import math
|
||||
|
||||
boundary_lines = [
|
||||
[(0,0), (0, 1500)],
|
||||
[(0, 1500), (1500, 1500)],
|
||||
[(1500, 1500), (1500, 500)],
|
||||
[(1500, 500), (1000, 500)],
|
||||
[(1000, 500), (1000, 0)],
|
||||
[(1000, 0), (0, 0)],
|
||||
]
|
||||
|
||||
width = 1500
|
||||
height = 1500
|
||||
|
||||
|
||||
def point_is_inside_arena(x, y):
|
||||
"""Return True if the point is inside the arena.
|
||||
if the point is inside the rectangle, but not inside the cutout, it's inside the arena.
|
||||
"""
|
||||
# is it inside the rectangle?
|
||||
if x < 0 or x > width \
|
||||
or y < 0 or y > height:
|
||||
return False
|
||||
# is it inside the cutout?
|
||||
if x > 1000 and y < 500:
|
||||
return False
|
||||
return True
|
||||
|
||||
## intention - we can use a distance squared function to avoid the square root, and just square the distance sensor readings too.
|
||||
def get_ray_distance_to_segment_squared(ray, segment):
|
||||
"""Return the distance squared from the ray origin to the intersection point along the given ray heading.
|
||||
The segments are boundary lines, which will be horizontal or vertical, and have known lengths.
|
||||
The ray can have any heading, and will be infinite in length.
|
||||
Ray -> (x, y, heading)
|
||||
Segment -> ((x1, y1), (x2, y2))
|
||||
"""
|
||||
ray_x, ray_y, ray_heading = ray
|
||||
segment_x1, segment_y1 = segment[0]
|
||||
segment_x2, segment_y2 = segment[1]
|
||||
# if the segment is horizontal, the ray will intersect it at a known y value
|
||||
if segment_y1 == segment_y2:
|
||||
# if the ray is horizontal, it will never intersect the segment
|
||||
if ray_heading == 0:
|
||||
return None
|
||||
# calculate the x value of the intersection point
|
||||
intersection_x = ray_x + (segment_y1 - ray_y) / math.tan(ray_heading)
|
||||
# is the intersection point on the segment?
|
||||
if intersection_x < segment_x1 or intersection_x > segment_x2:
|
||||
return None
|
||||
# calculate the distance from the ray origin to the intersection point
|
||||
return (intersection_x - ray_x) ** 2 + (segment_y1 - ray_y) ** 2
|
||||
# if the segment is vertical, the ray will intersect it at a known x value
|
||||
if segment_x1 == segment_x2:
|
||||
# if the ray is vertical, it will never intersect the segment
|
||||
if ray_heading == math.pi / 2:
|
||||
return None
|
||||
# calculate the y value of the intersection point
|
||||
intersection_y = ray_y + (segment_x1 - ray_x) * math.tan(ray_heading)
|
||||
# is the intersection point on the segment?
|
||||
if intersection_y < segment_y1 or intersection_y > segment_y2:
|
||||
return None
|
||||
# calculate the distance from the ray origin to the intersection point
|
||||
return (segment_x1 - ray_x) ** 2 + (intersection_y - ray_y) ** 2
|
||||
else:
|
||||
raise Exception("Segment is not horizontal or vertical")
|
||||
|
||||
def get_ray_distance_squared_to_nearest_boundary_segment(ray):
|
||||
"""Return the distance from the ray origin to the intersection point along the given ray heading.
|
||||
The segments are boundary lines, which will be horizontal or vertical, and have known lengths.
|
||||
The ray can have any heading, and will be infinite in length.
|
||||
Ray -> (x, y, heading)
|
||||
"""
|
||||
# find the distance to each segment
|
||||
distances = []
|
||||
for segment in boundary_lines:
|
||||
distance_squared = get_ray_distance_to_segment_squared(ray, segment)
|
||||
if distance_squared is not None:
|
||||
distances.append(distance_squared)
|
||||
# return the minimum distance
|
||||
if distances:
|
||||
return min(distances)
|
||||
else:
|
||||
return None
|
||||
@@ -0,0 +1,230 @@
|
||||
import asyncio
|
||||
import json
|
||||
import random
|
||||
from ulab import numpy as np
|
||||
from guassian import get_gaussian_sample
|
||||
import arena
|
||||
import robot
|
||||
import pid_controller
|
||||
|
||||
# initial sample set - uniform
|
||||
# then apply sensor model
|
||||
# then resample
|
||||
# then apply motion model
|
||||
# and repeat
|
||||
|
||||
class Simulation:
|
||||
def __init__(self):
|
||||
self.population_size = 200
|
||||
self.left_distance = 100
|
||||
self.right_distance = 100
|
||||
self.occupancy_grid = arena.get_binary_occupancy_grid()
|
||||
self.time_step = 0.1
|
||||
# Poses - each an array of [x, y, heading]
|
||||
self.poses = np.array(
|
||||
[(random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360)) for _ in range(self.population_size)],
|
||||
dtype=np.float,
|
||||
)
|
||||
# use pids to avoid collisions
|
||||
# speed is proportional to distance from wall -> further we are from wall, faster we can go
|
||||
# turn is proportional to difference between left and right distance sensors.
|
||||
|
||||
self.forward_distance_pid = pid_controller.PIDController(0.1, 0.01, 0.01)
|
||||
self.turn_pid = pid_controller.PIDController(0.1, 0.01, 0.01)
|
||||
self.distance_aim = 100
|
||||
|
||||
def apply_sensor_model(self, distance_left, distance_right):
|
||||
# Based on vl53l1x sensor readings, create weight for each pose.
|
||||
# vl53l1x standard dev is +/- 5 mm. Each distance is a mean reading
|
||||
# we will first determine sensor positions based on poses
|
||||
# project forward based on distances sensed, introducing noise (based on standard dev)
|
||||
# then check this projected position against occupancy grid
|
||||
# and weight accordingly
|
||||
|
||||
# distance sensor positions projected forward. left_x, left_y, right_x, right_y
|
||||
distance_sensor_positions = np.array(
|
||||
(self.poses.shape[0], 6), dtype=np.float)
|
||||
# sensors - they are facing forward, either side of the robot. Project them out to the sides
|
||||
# based on each poses heading
|
||||
# left sensor
|
||||
poses_left_90 = np.radians(self.poses[:, 2] + 90)
|
||||
distance_sensor_positions[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle
|
||||
distance_sensor_positions[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle
|
||||
# right sensor
|
||||
poses_right_90 = np.radians(self.poses[:, 2] - 90)
|
||||
distance_sensor_positions[:, 2] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle
|
||||
distance_sensor_positions[:, 3] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle
|
||||
# for each sensor position, find the distance to the nearest obstacle
|
||||
distance_sensor_standard_dev = 5
|
||||
dl_squared = distance_left ** 2
|
||||
dr_squared = distance_right ** 2
|
||||
|
||||
# weighted poses a numpy array of weights for each pose
|
||||
weights = np.empty(self.poses.shape[0], dtype=np.float)
|
||||
|
||||
for index, sensor_position in enumerate(distance_sensor_positions):
|
||||
# difference between this distance and the distance sensed is the error
|
||||
# add noise to this error
|
||||
|
||||
# left sensor
|
||||
left_ray = sensor_position[0], sensor_position[1], self.poses[index, 2]
|
||||
noise = get_gaussian_sample(0, distance_sensor_standard_dev)
|
||||
left_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(left_ray) + noise
|
||||
left_error = abs(left_actual - dl_squared) # error
|
||||
# right sensor
|
||||
right_ray = sensor_position[2], sensor_position[3], self.poses[index, 2]
|
||||
noise = get_gaussian_sample(0, distance_sensor_standard_dev)
|
||||
right_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(right_ray) + noise
|
||||
right_error = abs(right_actual - dr_squared) #error
|
||||
# weight is the inverse of the error
|
||||
weights[index] = 1 / (left_error + right_error)
|
||||
#normalise the weights
|
||||
weights = weights / np.sum(weights)
|
||||
return weights
|
||||
|
||||
def resample(self, weights):
|
||||
# based on the weights, resample the poses
|
||||
# weights is a numpy array of weights
|
||||
# resample is a numpy array of indices into the poses array
|
||||
samples = []
|
||||
# use low variance resampling
|
||||
start = random.uniform(0, 1 / self.population_size)
|
||||
cumulative_weights = weights[0]
|
||||
source_index = 0
|
||||
for current_index in range(self.population_size):
|
||||
sample_index = start + current_index / self.population_size
|
||||
while sample_index > cumulative_weights:
|
||||
source_index += 1
|
||||
cumulative_weights += weights[source_index]
|
||||
samples.append(source_index)
|
||||
# set poses to the resampled poses
|
||||
self.poses = self.poses[samples]
|
||||
|
||||
async def move_robot(self):
|
||||
"""move forward, apply the motion model"""
|
||||
starting_heading = robot.imu.euler[0]
|
||||
encoder_left = robot.left_encoder.read()
|
||||
encoder_right = robot.right_encoder.read()
|
||||
|
||||
# move forward - use distance sensor to determine how far to go
|
||||
distance_error = self.distance_aim - min(self.left_distance, self.right_distance)
|
||||
forward_speed = self.forward_distance_pid.calculate(distance_error, self.time_step)
|
||||
turn_error = self.left_distance - self.right_distance
|
||||
turn_speed = self.turn_pid.calculate(turn_error, self.time_step)
|
||||
|
||||
robot.set_left(forward_speed + turn_speed)
|
||||
robot.set_right(forward_speed - turn_speed)
|
||||
|
||||
await asyncio.sleep(self.time_step)
|
||||
# record sensor changes
|
||||
left_movement = robot.left_encoder.read() - encoder_left
|
||||
right_movement = robot.right_encoder.read() - encoder_right
|
||||
speed_in_mm = robot.ticks_to_m * ((left_movement + right_movement) / 2) * 1000
|
||||
new_heading = robot.imu.euler[0]
|
||||
if new_heading:
|
||||
heading_change = starting_heading - new_heading
|
||||
else:
|
||||
print("Failed to get heading")
|
||||
heading_change = 0
|
||||
|
||||
# move poses (this is a bit cheeky, and should be using icc)
|
||||
heading_standard_dev = 2 # degrees
|
||||
speed_standard_dev = 5 # mm
|
||||
|
||||
radians = np.radians(self.poses[2])
|
||||
heading_model = [get_gaussian_sample(0, heading_standard_dev) for _ in range(self.poses.shape[1])]
|
||||
speed_model = [get_gaussian_sample(speed_in_mm, speed_standard_dev) for _ in range(self.poses.shape[1])]
|
||||
self.poses[:,0] += speed_model * np.cos(radians)
|
||||
self.poses[:,1] += speed_model * np.sin(radians)
|
||||
self.poses[:,2] += np.full(self.poses[2].shape, heading_change + heading_model)
|
||||
self.poses[:,2] = np.vectorize(lambda n: n % 360)(self.poses[2])
|
||||
|
||||
async def distance_sensor_updater(self):
|
||||
robot.left_distance.start_ranging()
|
||||
robot.right_distance.start_ranging()
|
||||
while True:
|
||||
if robot.left_distance.data_ready and robot.left_distance.distance:
|
||||
self.left_distance = robot.left_distance.distance * 10 # convert to mm
|
||||
robot.left_distance.clear_interrupt()
|
||||
if robot.right_distance.data_ready and robot.right_distance.distance:
|
||||
self.right_distance = robot.right_distance.distance * 10
|
||||
robot.right_distance.clear_interrupt()
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
async def run(self):
|
||||
asyncio.create_task(self.distance_sensor_updater())
|
||||
try:
|
||||
for _ in range(15):
|
||||
weights = self.apply_sensor_model()
|
||||
self.resample(weights)
|
||||
await self.move_robot()
|
||||
finally:
|
||||
robot.stop()
|
||||
|
||||
|
||||
def send_json(data):
|
||||
robot.uart.write((json.dumps(data) + "\n").encode())
|
||||
|
||||
|
||||
def read_command():
|
||||
data = robot.uart.readline()
|
||||
try:
|
||||
decoded = data.decode()
|
||||
except UnicodeError:
|
||||
print("UnicodeError decoding :")
|
||||
print(data)
|
||||
return None
|
||||
try:
|
||||
request = json.loads(decoded)
|
||||
except ValueError:
|
||||
print("ValueError reading json from:")
|
||||
print(decoded)
|
||||
return None
|
||||
return request
|
||||
|
||||
|
||||
async def updater(simulation):
|
||||
print("starting updater")
|
||||
while True:
|
||||
sys_status, gyro, accel, mag = robot.imu.calibration_status
|
||||
if sys_status < 3:
|
||||
send_json(
|
||||
{
|
||||
"imu_calibration": {
|
||||
"gyro": gyro,
|
||||
"accel": accel,
|
||||
"mag": mag,
|
||||
"sys": sys_status,
|
||||
}
|
||||
}
|
||||
)
|
||||
send_json(
|
||||
{
|
||||
"poses": simulation.poses.tolist(),
|
||||
}
|
||||
)
|
||||
await asyncio.sleep(0.5)
|
||||
|
||||
|
||||
async def command_handler(simulation):
|
||||
update_task = asyncio.create_task(updater(simulation))
|
||||
simulation_task = asyncio.create_task(simulation.run())
|
||||
print("Starting handler")
|
||||
while True:
|
||||
if robot.uart.in_waiting:
|
||||
print("Receiving data...")
|
||||
request = read_command()
|
||||
if not request:
|
||||
print("no request")
|
||||
continue
|
||||
if request["command"] == "arena":
|
||||
send_json(
|
||||
{
|
||||
"arena": arena.boundary_lines
|
||||
}
|
||||
)
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
|
||||
simulation = Simulation()
|
||||
asyncio.run(command_handler(simulation))
|
||||
@@ -0,0 +1,16 @@
|
||||
import random
|
||||
import math
|
||||
|
||||
def get_standard_normal_sample():
|
||||
"""Using the Marsaglia Polar method"""
|
||||
# timeit on mac says - 0.915
|
||||
while True:
|
||||
u = random.uniform(-1, 1)
|
||||
v = random.uniform(-1, 1)
|
||||
s = u * u + v * v
|
||||
if s >= 1:
|
||||
continue
|
||||
return u * math.sqrt(-2 * math.log(s) / s)
|
||||
|
||||
def get_gaussian_sample(mean, standard_deviation):
|
||||
return get_standard_normal_sample() * standard_deviation + mean
|
||||
@@ -0,0 +1,27 @@
|
||||
class PIDController:
|
||||
def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.d_filter_gain = d_filter_gain
|
||||
self.imax = imax
|
||||
self.imin = imin
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.integral = 0
|
||||
self.error_prev = 0
|
||||
self.derivative = 0
|
||||
|
||||
def calculate(self, error, dt):
|
||||
self.integral += error * dt
|
||||
if self.imax is not None and self.integral > self.imax:
|
||||
self.integral = self.imax
|
||||
if self.imin is not None and self.integral < self.imin:
|
||||
self.integral = self.imin
|
||||
# Add a low pass filter to the difference
|
||||
difference = (error - self.error_prev) * self.d_filter_gain
|
||||
self.error_prev += difference
|
||||
self.derivative = difference / dt
|
||||
|
||||
return self.kp * error + self.ki * self.integral + self.kd * self.derivative
|
||||
@@ -0,0 +1,84 @@
|
||||
import rp2pio
|
||||
import adafruit_pioasm
|
||||
import array
|
||||
import asyncio
|
||||
|
||||
|
||||
program = """
|
||||
; use the osr for count
|
||||
; input pins c1 c2
|
||||
|
||||
set y, 0 ; clear y
|
||||
mov osr, y ; and clear osr
|
||||
read:
|
||||
; x will be the old value
|
||||
; y the new values
|
||||
mov x, y ; store old Y in x
|
||||
in null, 32 ; Clear ISR - using y
|
||||
in pins, 2 ; read two pins into y
|
||||
mov y, isr
|
||||
jmp x!=y, different ; Jump if its different
|
||||
jmp read ; otherwise loop back to read
|
||||
|
||||
different:
|
||||
; x has old value, y has new.
|
||||
; extract the upper bit of X.
|
||||
in x, 31 ; get bit 31 - old p1 (remember which direction it came in)
|
||||
in null, 31 ; keep only 1 bit
|
||||
mov x, isr ; put this back in x
|
||||
jmp !x, c1_old_zero
|
||||
|
||||
c1_old_not_zero:
|
||||
jmp pin, count_up
|
||||
jmp count_down
|
||||
|
||||
c1_old_zero:
|
||||
jmp pin, count_down
|
||||
; fall through
|
||||
count_up:
|
||||
; for a clockwise move - we'll add 1 by inverting
|
||||
mov x, ~ osr ; store inverted OSR on x
|
||||
jmp x--, fake ; use jump to take off 1
|
||||
fake:
|
||||
mov x, ~ x ; invert back
|
||||
jmp send
|
||||
count_down:
|
||||
; for a clockwise move, just take one off
|
||||
mov x, osr ; store osr in x
|
||||
jmp x--, send ; dec and send
|
||||
send:
|
||||
; send x.
|
||||
mov isr, x ; send it
|
||||
push noblock ; put ISR into input FIFO
|
||||
mov osr, x ; put X back in OSR
|
||||
jmp read ; loop back
|
||||
"""
|
||||
|
||||
assembled = adafruit_pioasm.assemble(program)
|
||||
|
||||
|
||||
class QuadratureEncoder:
|
||||
def __init__(self, first_pin, second_pin, reversed=False):
|
||||
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
||||
self.sm = rp2pio.StateMachine(
|
||||
assembled,
|
||||
frequency=0,
|
||||
first_in_pin=first_pin,
|
||||
jmp_pin=second_pin,
|
||||
in_pin_count=2,
|
||||
)
|
||||
self.reversed = reversed
|
||||
self._buffer = array.array("i", [0])
|
||||
asyncio.create_task(self.poll_loop())
|
||||
|
||||
async def poll_loop(self):
|
||||
while True:
|
||||
await asyncio.sleep(0)
|
||||
while self.sm.in_waiting:
|
||||
self.sm.readinto(self._buffer)
|
||||
|
||||
def read(self):
|
||||
if self.reversed:
|
||||
return -self._buffer[0]
|
||||
else:
|
||||
return self._buffer[0]
|
||||
Executable
+78
@@ -0,0 +1,78 @@
|
||||
import board
|
||||
import pwmio
|
||||
import pio_encoder
|
||||
import busio
|
||||
import adafruit_vl53l1x
|
||||
import math
|
||||
import busio
|
||||
import adafruit_bno055
|
||||
|
||||
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
|
||||
|
||||
wheel_diameter_mm = 70
|
||||
wheel_circumference_mm = math.pi * wheel_diameter_mm
|
||||
gear_ratio = 298
|
||||
encoder_poles = 28
|
||||
ticks_per_revolution = encoder_poles * gear_ratio
|
||||
ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000
|
||||
m_to_ticks = 1 / ticks_to_m
|
||||
|
||||
|
||||
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
|
||||
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
|
||||
motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
|
||||
motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
|
||||
|
||||
right_motor = motor_A1, motor_A2
|
||||
left_motor = motor_B1, motor_B2
|
||||
|
||||
right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
|
||||
left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
|
||||
|
||||
i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
|
||||
i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
|
||||
|
||||
left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
|
||||
right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
|
||||
|
||||
distance_sensor_from_middle = 40 # approx mm
|
||||
|
||||
imu = adafruit_bno055.BNO055_I2C(i2c0)
|
||||
imu.mode = adafruit_bno055.NDOF_MODE # should be in chapter 12!
|
||||
|
||||
def stop():
|
||||
motor_A1.duty_cycle = 0
|
||||
motor_A2.duty_cycle = 0
|
||||
motor_B1.duty_cycle = 0
|
||||
motor_B2.duty_cycle = 0
|
||||
|
||||
|
||||
def set_speed(motor, speed):
|
||||
# Swap motor pins if we reverse the speed
|
||||
if abs(speed) < 0.1:
|
||||
motor[0].duty_cycle = 0
|
||||
motor[1].duty_cycle = 1
|
||||
return
|
||||
if speed < 0:
|
||||
direction = motor[1], motor[0]
|
||||
speed = -speed
|
||||
else:
|
||||
direction = motor
|
||||
speed = min(speed, 1) # limit to 1.0
|
||||
max_speed = 2 ** 16 - 1
|
||||
|
||||
direction[0].duty_cycle = int(max_speed * speed)
|
||||
direction[1].duty_cycle = 0
|
||||
|
||||
|
||||
def set_left(speed):
|
||||
set_speed(left_motor, speed)
|
||||
|
||||
|
||||
def set_right(speed):
|
||||
set_speed(right_motor, speed)
|
||||
|
||||
def check_imu_status():
|
||||
sys_status, gyro, accel, mag = imu.calibration_status
|
||||
uart.write(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}\n".encode())
|
||||
return sys_status == 3
|
||||
Reference in New Issue
Block a user