Реализация алгоритма определения ширины объекта с помощью ультразвукового дальномера и энкодеров на языке RobotC

Доброго дня!

Сегодня я бы хотел поделиться реализацией алгоритма определения ширины объекта с помощью ультразвукового дальномера и энкодеров на языке программирования RobotC (с использованием платформы VEX EDR).

Предистория

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

Элементная база

В качестве экспериментального полигона мной была использована среда Robot Virtual Worlds для платформы VEX, виртуальная модель робота VEX Launcher, виртуальные модели элементов соревнований VRC Turning Point (рисунок 1) и простейшие математические формулы (для определения длины хорды в окружности с известным центральным углом).


Рисунок 1. Виртуальный полигон с роботом

Задача

— Определение диаметра шара с использованием только ультразвукового дальномера и энкодеров, установленных на роботе

Теоретические основы

Для определения ширины объекта мной было использовано понятие диаграммы направленности (рисунок 2).


Рисунок 2. Чувствительность и диаграмма направленности ультразвукового дальномера

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


Рисунок 3. Геометрия алгоритма

Центральный угол дуги будем искать, исходя из соотношения между радиусом угла и дугой поворота робота. Итоговое значение ширины будет соответствовать m — хорде дуги.

Входные параметры

В качестве входных параметров мной были выбраны:

• dist = 100 мм (расстояние, на котором робот останавливается для измерения, параметр выбирается, исходя из наличия вокруг объекта других объектов, которые могут попасть в поле зрения датчика и создать помехи для измерения)
• d_wheel = 102 мм (диаметр колеса робота)
• d_ball= 102 мм (фактический диаметр шара)

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

• error = abs(d_ball_exper — d_ball)

Алгоритм работы программы

Были выделены следующие этапы работы алгоритма.

1. Движение до объекта.
2. Поворот в крайнюю правую точку объекта.
3. Поворот в крайнюю левую точку объекта.
4. Расчёт угла с использованием снятых с энкодеров показаний.
5. Расчёт ошибки.

Реализация алгоритма на языке RobotC

Первым делом, нам необходимо инициализировать стандартную конфигурацию виртуального робота:

#pragma config(StandardModel, "VEX Launchbot")

Реализуем функцию, которая будет отвечать за поворот робота в крайнее правое и/или крайнее левое положение. Функция возвращает показания энкодера после поворота, которое будет использовано в дальнейших расчётах:

int turn_to_over_object(int leftSpeed, int rightSpeed, int distance, bool need_object, bool left_side)
{
if(need_object == true)
{
if(left_side == true)
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
с}
}
else
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
else
{
if(left_side == true)
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}

return getMotorEncoder(leftMotor);
}

Познакомимся поближе с аргументами функции:

• leftSpeed — скорость левого привода
• rightSpeed — скорость правого привода
• rightSpeed — скорость правого привода
• distance — расстояние до объекта, на котором остановился робот
• need_object — режим движения: True, если необходимо наличие объекта в поле зрения датчика, False, если необходимо отсутствие объекта
• left_side — направление поворота робота. True — поворот налево, False — поворот направо

При значении need_object = True робот будет поворачивать до тех пор, пока объект не окажется на расстоянии distance от робота (в нашем случае 100 мм).

При значение need_object = False робот будет поворачивать до тех пор, пока объект не окажется на расстоянии (distance + 1) от робота. Параметр единица в сумме влияет на конечную точность определения отсутствия объекта в поле зрения датчика (возможно улучшение программы с помощью вынесения этого параметра в аргументы функции).

Напишем основное тело программы:

task main()
{
int d_ball = 10.2; //sm
int d_wheel = 10.2;

int dist = 10; //sm
//Подъем руки вверх
setMotor(armMotor, -50);
wait(1);

while(SensorValue[sonarSensor] > 10)
{
setMultipleMotors(50, leftMotor, rightMotor, frontLeftMotor, frontRightMotor);
}
stopAllMotors();
int enc = 0;
enc = turn_to_over_object(50, 50, dist, false, false);
enc = turn_to_over_object(50, 50, dist, true, true);
resetMotorEncoder(leftMotor);
enc = turn_to_over_object(50, 50, dist, false, true);
stopAllMotors();

float teta = abs(enc) * PI * d_wheel * d_wheel / (dist * 627);
float d_ball_exper = 2 * dist * sin(teta / 2);
	
float error = abs(d_ball_exper - d_ball);
while(1)
{
}
}

Первый цикл while работает до тех пор, пока робот не достигнет указанного расстояния до объекта. Далее нам необходимо вызвать поочередно написанную выше функцию для того, чтобы измерить показания энкодера между крайним правым и крайним левым положениями (обратите внимание, что перед измерением необходимо сбросить показания энкодера в ноль).

Для получения формулы угла я руководствовался следующими формулами:
— enc = N * 627, где
    N — число оборотов колеса,
    enc — показание энкодера,
    627 — разрешения энкодера => выразим N;
— S = N * l, где
    l — длина колеса, l = PI * d_wheel * d_wheel => подставим N и l => S = enc * PI * d_wheel * d_wheel / 627;
— с другой стороны, S = dist * teta, где
    S — величина дуги, на которую повернулся робот,
    teta — искомый центральный угол;
— исходя из приведенных выше данных, выразим teta => teta = enc * PI * d_wheel * d_wheel / (627 * dist)

Для нахождения хорды, равной искомому диаметру шара, воспользуемся формулой:
    d_ball_exper = 2 * dist * sin(teta / 2)

Бесконечный цикл while в конце программы необходим для того, чтобы увидеть полученные значения в отладчике.

Итоговые данные

На виртуальном поле получились следующие значения диаметра шара и ошибки:

    d_ball_exper = 10.6 см
    error = 0.4 см

Как видите, полученный метод измерения ширины даёт небольшое значение ошибки, которое является приемлемым для класса точности используемых датчиков. Значение ошибки можно уменьшить с помощью подбора значения параметра dist (например, запуска цикла с отъездом робота назад с определенным шагом, изменяющим на dist) или же использованием ПИД регулятора, который позволит уменьшить влияние инерции робота на конечное значение энкодера при торможении.

Перспективы

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