Определение направления по данным акселерометра IMU

Я учусь интегрировать датчики в ардуино и считывать их данные.

Моя конечная цель - установить ИДУ на робота и использовать его для определения местоположения роботов в окружающей среде.

Используя следующий код, я могу получить данные акселерометра и температуры:

// (c) Michael Schoeffler 2017, http://www.mschoeffler.de

#include "Wire.h" // Эта библиотека позволяет вам взаимодействовать с устройствами I2C.

const int MPU_ADDR = 0x68; // I2C-адрес MPU-6050. Если для вывода AD0 установлено значение HIGH, адрес I2C будет равен 0x69.

int16_t accelerometer_x, accelerometer_y, accelerometer_z; // переменные для исходных данных
int16_t gyro_x, gyro_y, gyro_z; // переменные для исходных данных гироскопа
int16_t temperature; // переменные для данных о температуре

char tmp_str[7]; // временная переменная, используемая в функции преобразования

char* convert_int16_to_str(int16_t i) { // преобразует int16 в строку. Более того, результирующие строки будут иметь одинаковую длину в мониторе отладки.
  sprintf(tmp_str, "%6d", i);
  return tmp_str;
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR); // Начинает передачу на ведомое устройство I2C (плата GY-521)
  Wire.write(0x6B); // Регистр PWR_MGMT_1
  Wire.write(0); // устанавливается на ноль (пробуждает MPU-6050)
  Wire.endTransmission(true);
}
void loop() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // начиная с регистра 0x3B (ACCEL_XOUT_H) [Карта и описания регистров MPU-6000 и MPU-6050 Редакция 4.2, стр.40]
  Wire.endTransmission(false); // параметр указывает, что Arduino отправит перезапуск. В результате соединение остается активным.
  Wire.requestFrom(MPU_ADDR, 7*2, true); // запросить в общей сложности 7*2=14 регистров

  // "Wire.read()<<8 | Wire.read();" означает, что два регистра считываются и сохраняются в одной переменной
  accelerometer_x = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x3B (ACCEL_XOUT_H) и 0x3C (ACCEL_XOUT_L)
  accelerometer_y = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x3D (ACCELE_YOUT_H) и 0x3E (ACCEL_YOUT_L)
  accelerometer_z = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x3F (ACCELER_ZOUT_H) и 0x40 (ACCELER_ZOUT_L)
  temperature = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x41 (TEMP_OUT_H) и 0x42 (TEMP_OUT_L)
// gyro_x = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x43 (GYRO_XOUT_H) и 0x44 (GYRO_XOUT_L)
// gyro_y = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x45 (GYRO_YOUT_H) и 0x46 (GYRO_YOUT_L)
// gyro_z = Wire.read()<<8 | Wire.read(); // регистры чтения: 0x47 (GYRO_ZOUT_H) и 0x48 (GYRO_ZOUT_L)

// // распечатать данные
  Serial.print("aX = "); Serial.print(convert_int16_to_str(accelerometer_x));
  Serial.print(" | aY = "); Serial.print(convert_int16_to_str(accelerometer_y));
  Serial.print(" | aZ = "); Serial.print(convert_int16_to_str(accelerometer_z));
// // следующее уравнение было взято из документации [Карта и описание регистра MPU-6000/ MPU-6050, стр.30]
  Serial.print(" | tmp = "); Serial.print(temperature/340.00+36.53);
// Serial.print(" | gX = "); Serial.print(convert_int16_to_str(gyro_x));
// Serial.print(" | gY = "); Serial.print(convert_int16_to_str(gyro_y));
// Serial.print(" | gZ = "); Serial.print(convert_int16_to_str(gyro_z));
  Serial.println();

  // задержка
  delay(1000);
}

Давая мне вывод, подобный следующему:

aX =   3526 | aY =   -968 | aZ = -11988 | tmp = 25.89
aX =   3478 | aY =   -936 | aZ = -12020 | tmp = 25.91
aX =   3530 | aY =   -922 | aZ = -12002 | tmp = 25.91
aX =   3490 | aY =   -896 | aZ = -12006 | tmp = 25.94
aX =   3498 | aY =   -898 | aZ = -12036 | tmp = 25.91
aX =   3520 | aY =   -916 | aZ = -12050 | tmp = 25.91
aX =   3466 | aY =   -924 | aZ = -11960 | tmp = 25.91
aX =   3492 | aY =   -944 | aZ = -12026 | tmp = 25.92
aX =   3510 | aY =   -924 | aZ = -12048 | tmp = 25.91
aX =   3440 | aY =   -942 | aZ = -11980 | tmp = 25.91
aX =   3514 | aY =   -924 | aZ = -12066 | tmp = 25.91
aX =   3484 | aY =   -944 | aZ = -11968 | tmp = 25.92
aX =   3490 | aY =   -944 | aZ = -12012 | tmp = 25.92
aX =   3458 | aY =   -952 | aZ = -11944 | tmp = 25.92

Что мне нужно знать, так это как мне определить, в каком направлении движется мой робот, основываясь на выходных данных акселерометра?

, 👍0


3 ответа


0

Сначала вы должны проверить, в какой ориентации установлен датчик на плате IMU. Для этого вы можете использовать свой тестовый скетч. Если вы не будете двигать датчик, он покажет вам гравитационную силу, которая всегда направлена вниз. Сориентируйте ИДУ таким образом, чтобы aX и aY были близки к нулю. Теперь вы знаете, где находятся верх и низ. Перемещайте IMU в стороны, пока не найдете направление, в котором значительно изменяется только aX, в то время как aY остается близким к нулю. Тогда вы нашли следующее направление.

Наконец, установите IMU в вашем роботе в найденной ориентации. Поскольку вы знаете, какие из aX, aY и aZ изменяются в каком направлении, вы знаете направление движения роботов.

,

1

Вы не можете сделать это с какой-либо большой точностью.

Вектор ускорения от акселерометра представляет собой комбинацию двух вещей:

  • Гравитационное ускорение
  • Ускорение из-за изменения скорости робота.

Если робот движется с постоянной скоростью, ускорение из-за изменения скорости отсутствует, поэтому акселерометр будет измерять только силу тяжести. Тогда я понятия не имею, как далеко зашел робот.

Если робот поворачивается на месте, опять же, изменения ускорения не происходит. Таким образом, вы не знаете, в какую сторону смотрит робот!

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

Чтобы получить скорость (скорость с направлением) или абсолютное положение, вам понадобится магнитометр или гироскоп для оценки вращения.

,

1

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

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

Если ваша цель - знать ориентацию, то есть направление движения, не окажет ли вам большую помощь электронный компас (магнитометр)?

,