Интересный метод построения карт на основе движения робота

В современном мире, где робот пылесос является обыденностью, а статьи про новые автопилотируемые автомобили публикуются регулярно, давно не секрет, что для своих задач эти чудо машины строят карту движения, чтобы знать куда ехать.

Проблема современных автопилотируемых систем

Современным машинам с полным (или почти полным) автопилотом приходится работать с огромным потоком данных, чтобы не "заблудиться". Посмотрите, сколько различных датчиков/сенсоров находятся в автомобилях компании Tesla и беспилотных автомобилях Яндекса.

Естественно, для таких дел нужны большие вычислительные мощности, что повышает цену продукта.

Автопилот в промышленности

Мы живём в эпоху четвёртой промышленной революции - когда производство становится полностью цифровым и самоуправляемым. Но громоздить небольшие погрузчики и другую технику десятками камер и лидарами слишком дорого в промышленном масштабе.

Построение карты движения робота по данным двух энкодеров, применяя одометрию

Одометрия — использование данных о движении приводов для оценки перемещения.

Энкодер — устройство для замеров тех или иных параметров цифровыми методами. К таковым могут относиться параметры передвижения деталей, углы их поворота, направление перемещения, скорость. Два энкодера стоят гораздо меньше датчиков и дают меньше данных, которые нужно обработать. Энкодеры можно комбинировать с камерой для более точного результата

Я работал с уткоботом (duckiebot) из интересного проекта MIT — Duckietown. Алгоритм построения карты движения робота будет основан на формуле, предложенной MIT. Данные с энкодеров были получены с помощью ROS.

Разберём код:

import numpy as np
import matplotlib.pyplot as plt


class Odometry: # создаём класс
    """
        Creates a map based on file input.
    """

функция getInfo возвращает сортированные данные из файла

 def __init__(self):
        self.file = "Test.txt"  # путь к файлу сюда
        self.robotLength = 0.1

    def getInfo(self):
        encoderL = []
        encoderR = []
        time = []
        with open(self.file) as f:
            content = f.readlines()
        for line in content:
            lineCut = line.split(" ")
            encoderL.append(float(lineCut[0]))
            encoderR.append(float(lineCut[1]))
            time.append(float(lineCut[2]) / 10 ** 9)
        time_diff = np.diff(time)
        return encoderL, encoderR, time_diff

Магическая формула расчёта движения относительно координат и времени прошлого "тика". Подробное объяснение на 8 слайде

def odometry(self, x, y, date, theta, left, right):
        d = (left + right) / 2
        fi = (right - left) / (2 * self.robotLength)
        x1 = x + (date * d * np.cos(theta))
        y1 = y + (date * d * np.sin(theta))

        theta1 = theta + fi * date

        return x1, y1, theta1

функция, создающая карту

def createMap(self):
        x = [0]
        y = [0]
        theta = [0]
        encoderL, encoderR, time_diff = self.getInfo()

        for i in range(len(encoderL) - 1):
            x1, y1, theta1 = self.odometry(x[i], y[i], time_diff[i], theta[i], encoderL[i], encoderR[i])
            x.append(x1)
            y.append(y1)
            theta.append(theta1)

        plt.plot(x, y)
        plt.show()

напрямую вывести карту нельзя, нужно передать данные в другую переменную

output = Odometry()
output.createMap()

Вывод программы, используя тестовые данные, такой: (это норма)

Вывод

Энкодеры могут заменить дорогие датчики/камеры в некоторых задачах, но могут давать серьёзную погрешность (скользкий пол, или плохо работающий в данных условиях алгоритм) как здесь. Для простого погрузчика, которому нужна карта склада, чтобы знать, куда надо погрузить какой либо предмет, хватит.