Определение положения предмета с помощью датчиков расстояния

(C)

	Понятие триангуляция знакомо многим, кто хоть раз разбирался как работают GPS приемникиили тем, кто изучал что такое радиопеленгация. На самом деле это понятие пришло из геометрии и геодезии. Но сейчас пойдет речь именно о триангуляции, как способе определения координат или местоположении предмета.

Aswin - блогер из Дании, регулярно публикующий свои заметки на блоге NXT Time. Недавно опубликовал очень интересный материал - он показал, как с помощью двух датчиков расстояния (он использовал стандартные NXT Ultrasonic датчики) можно с допустимой точностью определить местоположение предмета. Для определения местоположения как раз и использовался метод триангуляции. 

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

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

Автор также делиться видео, демонстрирующим, что его подход работает:


Есть мнение, что подобный подход используется в роботе "Глазастик", подготовленный для Международной Робототехнической Олимпиады командой ФМЛ №239 г. Санкт-Петербург.


Но вернемся, к первоначальному устройству... Хотя автор и использует язык программирования Java для программирования своей лабораторной установки, код достаточно прост для понимания и может быть легко интерпретирован в другие языки программирования. Единственное, нужно отметить, что в виртуальная машина leJOS, используемая автором на NXT блоке, предоставляет продвинутые средства для использования сенсора расстояния - в ней есть автоматическая фильтрация показаний, так что в программу уже поступают более "надежные" показания. 

package lejos.nxt.sensor.example;
 
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.sensor.filter.AdditionFilter;
import lejos.nxt.sensor.sensor.LcUltrasonic;
import lejos.util.Delay;
 
public class TwoUSSensors {
 
     // Коэффициент для П-регулятора, управляющего моторами
     static final int P=5;
     // Погрешность измерения
     static final double MARGIN=0.8;
     // Максимальное расстояние определения
     static final float MAXRANGE=150;
     // Минимальный угол, на который могут повернуться сенсоры, чтобы не попасть
     // в зону видимости друг друга
     static final int MINANGLE=25;
     // Дистанция между двумя сенсорами - измеряется от осей мотора 
     static final double b=20.5;
     // Смещение датчика от оси мотора
     static final float SENSOROFFSET=2.5f;
 
     // Инициализация датчиков и фильтров
     static final LcUltrasonic sensorA=new LcUltrasonic(SensorPort.S1);
     static final LcUltrasonic sensorC=new LcUltrasonic(SensorPort.S4);
     static final AdditionFilter corrrectedA=new AdditionFilter(sensorA,SENSOROFFSET);
     static final AdditionFilter corrrectedC=new AdditionFilter(sensorC,SENSOROFFSET);
 
     static final NXTRegulatedMotor C=Motor.C;
     static final NXTRegulatedMotor A=Motor.A;
 
     // Место запуска основной программы
     public static void main(String[] args) {
 
          Object run=new TwoUSSensors();
 
     }
 
     public TwoUSSensors() {
          double targetC=0, targetA=0;
          double  a,c;
          // Перевести датчики в режим PING (single shot)
          // подробнее на http://nxttime.wordpress.com/2012/09/12/the-ultrasonic-sensor/
          sensorA.setMode(LcUltrasonic.MODE_PING);
          sensorC.setMode(LcUltrasonic.MODE_PING);
 
          // Перевести датчики в начальное положение
          C.rotateTo(90, true);
          A.rotateTo(90, false);
          Delay.msDelay(500);
 
          // Выполнять программу до нажатия кнопки
          while(!Button.ESCAPE.isDown()) {
 
               // Получить данные от датчиков
               c=corrrectedA.fetchSample();
               a=corrrectedС.fetchSample();
 
               LCD.clear();
               LCD.drawString("       A     C", 0, 0);
               LCD.drawString("dist", 0, 1);
               LCD.drawString("dist'", 0, 2);
               LCD.drawString("target", 0, 3);
               LCD.drawString("error", 0, 4);
               LCD.drawString("state", 0, 5);
 
               LCD.drawInt((int) c, 7, 1);
               LCD.drawInt((int) a, 12, 1);
 
               if (a>MAXRANGE && c> MAXRANGE) {
                    // Датчики ничего не видят перед собой
                    targetA = Math.PI/2;
                    targetC = Math.PI/2;
                         LCD.drawString("-", 7, 5);
                         LCD.drawString("-", 12, 5);
               }
               else {
                    // Есть предмет в области видимости датчиков.
                    // Исходя из правила что a+c>b можно определить видят датчики
                    // два разных предмета или один
                    if (Math.abs(a-c)>b*MARGIN) {
                         // Датчики видят два разных предмета - выбираем ближайший
                         if (a<c) {
                              // Ближайший перед датчиком С
                              LCD.drawString("-", 7, 5);
                              LCD.drawString("+", 12, 5);
                              targetC =Math.toRadians(C.getPosition());
                              // расстояние от датчика А до предмета
                              c = Math.sqrt(a*a+b*b-2*a*b*Math.cos(targetC));
                              targetA = Math.acos((a*a-b*b-c*c)/(-2*c*b));
                         }
                         else {
                              // Ближайший перед датчиком A
                              LCD.drawString("+", 7, 5);
                              LCD.drawString("-", 12, 5);
                              targetA =Math.toRadians(A.getPosition());
                              // расстояние от датчика А до предмета
                              a=Math.sqrt(b*b+c*c-2*b*c*Math.cos(targetA));
                              targetC =Math.acos((c*c-a*a-b*b)/(-2*a*b));
                         }
                         LCD.drawInt((int) c, 7, 2);
                         LCD.drawInt((int) a, 12, 2);
                    }
                    else {
                         // Датчики видят один предмет
                         LCD.drawString("+", 7, 5);
                         LCD.drawString("+", 12, 5);
                         targetC =Math.acos((c*c-a*a-b*b)/(-2*a*b));
                         targetA =Math.acos((a*a-b*b-c*c)/(-2*c*b));
                    }
               }
 
               LCD.drawInt((int) Math.toDegrees(targetA),7,3);
               LCD.drawInt((int) Math.toDegrees(targetC),12,3);
 
               // К какой позиции нужно повернуть моторы
               LCD.drawInt(rotateTo(A, targetA),7,4);
               LCD.drawInt(rotateTo(C, targetC),12,4);
               Delay.msDelay(20);
          }
 
          // Перед окончанием программы - вернуться в начальное положение
          A.setSpeed(200);
          C.setSpeed(200);
          C.rotateTo(0, true);
          A.rotateTo(0, false);
          Delay.msDelay(500);
     }
 
// Функция поворота двигателя на заданную позицию. Поворот выполняется через
// регулирование мощности на моторе. Чем ближе требуемая позиция, тем меньше мощность.
int rotateTo(NXTRegulatedMotor motor,double target){
     int targetDeg=(int) Math.toDegrees(target);
 
     if (targetDeg<MINANGLE) target=MINANGLE;
     int error=targetDeg-motor.getPosition();
     if (Math.abs(error)<=1) return error;
     motor.setSpeed(error*P);
     motor.rotateTo(targetDeg, true);
     return error;
}