Лидар Delta2A от 3iRobotics. Пример на python

Delta2A — доступный сканирующий дальномер от Китайской компании 3iRobotics. В русском языке такие устройства часто называют лидарами, хотя дословный перевод английского слова — LiDAR, означает лишь непосредственно сам измеритель расстояния. Но мы будем всё же называть его лидаром, по то же причине, по которой сканирующий радиодальномер называем радаром.

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

Существуют дальномеры сканирующие пространство в 3-х измерениях. Цены на подобные устройства измеряются тысячами единиц иностранной валюты. Хотя, в последние годы, классические 2D и 3D лидары стали постепенно вытесняться так называемыми SolidState лидарами (они же — камеры глубины), которые являются более долговечными за счёт отсутствия движущихся частей, и по этой же причине менее дорогими.

Delta 2A может подключаться к компьютеру через USB (виртуальный COM-порт) или к микроконтроллеру через UART. Данные передаются со скоростью 230400 бод. С учетом того, что для хранения массива точек потребуется ещё и много оперативной памяти, к Arduino-подобным системам его подключать не имеет смысла. Потребуется ESP или STM32 с соответствующими параметрами. А лучше всего, иметь на борту робота полноценный, или хотя бы одноплатный компьютер.

Аппарат Delta2A обладает стандартными для устройств подобного класса характеристиками:

напряжение питания: 5 В;
рабочий ток: 500 мА;
частота вращения: 6,2 Гц;
измерения каждые: от 0,3 до 0,8 °;
измеряемые расстояния: от 0,13 до 8 м;
погрешность измерений: менее 1% (на расстоянии 5м);
DCV: менее 0,2%;
интерфейс: UART (скорость 230400 бод);
длина волны лазера: 780 нм;
диапазон рабочих температур: от 0 до 45 °C;
вес: 175 г;
размеры: 107 x 76 x 53 г.

В этой статье мы рассмотрим протокол общения датчика с вычислителем и посмотрим его в работе.

Совет

Лидар Delta2A и аналогичные сканирующие дальномеры, а также камеры глубины, можно приобрести в магазине Alash Electronics.

Протокол лидара Delta2A

За каждый оборот дальномер выдаёт серию из 16 пакетов данных. Каждый пакет представляет собой набор из некоторых служебных данных, параметров для оценки целостности пакета и массива значений измеренных расстояний (будем называть их точками). Длина пакета может варьироваться, в зависимости от скорости вращения мотора. В среднем, при питании о компьютера, каждый пакет содержит 51-52 точки.

С точки зрения программирования, пакет — это набор байт, из которого необходимо выделить нужные значения.

Индекс байта	Пример	Описание
0	0xAA	Индикатор начала пакета, всегда равен 0xAA
1-2	0x00, 0x91	Длина пакета без учёта двух байт CRC, но включая индикатор начала
3	0x01	Версия протокола: 0x01 — рабочий, 0x00 — ошибка
4	0x61	Типа пакета
5	0xAD	Индикатор начала полезных данных
6-7	0x00, 0x47	Длина полезных данных
8	0x82	Скорость вращения мотора (множитель 0,05): 0x82 = 130 * 0,05об/с = 6,5об/с
9-10	0x00, 0x87	Смещение угла для первого измерения в пакете (множитель 0,01)
11-12	0x69, 0x78	Начальный угол в пакете (множитель 0,01): 27000 * 0,01 = 270
13	0x46	Уровень сигнала №1
14-15	0x21, 0x3A	Дистанция №1 (множитель 0,25): 8506 * 0,25 = 2126мм
…
3N+2	0x5E	Уровень сигнала №47
3N+3, 3N+4	0x5E, 0x32	Дистанция №47: 2414 * 0,25 = 6028мм
3N+5, 3N+6	0x35, 0xBC	Сумма всех байтов пакета (CRC), за исключением текущих двух

Программа

Специально для этого дальномера я написал небольшую библиотеку для Python, которая позволит получить данные в удобоваримом для дальнейших манипуляций виде. А именно, в виде numpy массива пар данных [угол, дистанция]. Для работы библиотеки потребуется дополнительно установить пакеты: numpy и matplotlib

В первом примере будем сохранять данные с лидара в файл формата CVS

import LidarDelta2A
import numpy as np

port = 'COM10'
lidar = LidarDelta2A.LidarDelta2A(port, baudrate = 230400)

while not ready:
    data = lidar.handleData()

np.savetxt('output.csv', data, delimiter=',', fmt='%f')

COM10 — это порт, который определился в Windows при подключении лидара. В результате запуска скрипта, получим файл output.csv с результатами измерений за один оборот.

Второй пример интереснее — будем выводить точки на экран. Для этого потребуется с помощью matplotlib построить график типа scatter (набор несвязных точек). Если это делать в лоб, то мы получим весьма лагующую программу, даже на мощных ПК. Для ускорения используем функцию blit.

import LidarDelta2A
import signal
import time
import matplotlib.pyplot as plt
import math
import numpy as np

port = 'COM10'
lidar = LidarDelta2A.LidarDelta2A(port, baudrate = 230400)

points = np.zeros((16*52,2))

def on_close(event):
    print('Closed window!')
    ready = 0

def signal_handler(sig, frame):
    print('Pressed Ctrl+C!')
    global ready
    ready = 0

signal.signal(signal.SIGINT, signal_handler)

fig, ax = plt.subplots()

fig.canvas.mpl_connect('close_event', on_close)

ln = ax.scatter(points[:,0], points[:,1], animated=True)
ax.set_xlim([-5000,5000])
ax.set_ylim([-5000,5000])

plt.show(block=False)
plt.pause(0.1)

bg = fig.canvas.copy_from_bbox(fig.bbox)
ax.draw_artist(ln)
fig.canvas.blit(fig.bbox)

to = 0.01
t = time.time()
nxt = time.time() + to

ready = 1
while ready:
    data = lidar.handleData()
    for i,v in enumerate(data):
        points[i] = [v[1] * math.cos(math.radians(v[0])), v[1] * math.sin(math.radians(v[0]))]

    if time.time() > nxt:
        nxt = time.time() + to

        fig.canvas.restore_region(bg)
        ln.set_offsets(points)
        ax.draw_artist(ln)
        fig.canvas.blit(fig.bbox)
        fig.canvas.flush_events()

plt.close("all")
lidar.stop()

Запускаем скрипт и получаем примерно такую картину. Результат достаточно устойчив.

Дальнейшие размышления

Сам по себе массив точек с лидара не имеет особой ценности. Разве что можно использовать его как детектор препятствий. Основное применение сканирующих дальномеров — это SLAM, метод одновременной локализации и построения карты. То есть благодаря точкам с лидара на движущемся роботе можно построить карту помещения и ещё определить координаты робота на этой карте.

Примечание

Производитель Delta2A придаёт к своему изделию SDK, в котором есть и пакет для ROS, как раз для целей увязывания его со SLAM. К сожалению, всё SDK на китайском языке.

Полезные ссылки

Библиотека LidarDelta2A:

https://github.com/robotclass/LidarDelta2A

SDK от 3iRobotics:

https://download.robotclass.ru/SDK/Delta2A/Delta-2A.rar