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

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

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

Современным машинам с полным (или почти полным) автопилотом приходится работать с огромным потоком данных, чтобы не «заблудиться». Посмотрите, сколько различных датчиков/сенсоров находятся в автомобилях компании 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()

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

Вывод

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