Как рассчитать компенсацию отклонения наклона? - PullRequest
Новая
       101

Как рассчитать компенсацию отклонения наклона?

0 голосов
/ 22 февраля 2020

В течение некоторого времени я боролся за то, как правильно рассчитать угол рыскания от IMU, но не могу заставить его работать. Я использую LSM9DS1, если это имеет значение.

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

Я рассчитываю углы Эйлера следующим образом: 

    double weight = 0.05f;

    private void calculateEulerAngles() {
        // Measured angle by the accelerometer
        double rollXLMeasured = Math.atan2(this.accelerometer.getX(), this.accelerometer.getZ()) / 2 / Math.PI * 360;
        double pitchXLMeasured = Math.atan2(this.accelerometer.getY() / 9.81f, this.accelerometer.getZ() / 9.81f) / 2 / Math.PI * 360;

        // Adding a low pass filter
        double rollXLFiltered = (1 - this.weight) * rollXLFilteredOld + this.weight * rollXLMeasured;
        double pitchXLFiltered = (1 - this.weight) * pitchXLFilteredOld + this.weight * pitchXLMeasured;
        this.rollXLFilteredOld = rollXLFiltered;
        this.pitchXLFilteredOld = pitchXLFiltered;

        // Calculating deltaTime
        long time = System.nanoTime();
        int difference = (int) ((time - this.deltaTime) / 1000000000);
        this.deltaTime = time;

        // Adding a complementary filter
        this.roll = ((1 - this.weight) * (this.roll + this.gyroscope.getY() * difference)) + (this.weight * rollXLMeasured);
        this.pitch = ((1 - this.weight) * (this.pitch - this.gyroscope.getX() * difference)) + (this.weight * pitchXLMeasured);

        // Calculating yaw using the magnetometer and applying a low pass filter
        double rollRadians = this.roll / 360 * (2 * Math.PI);
        double pitchRadians = this.pitch / 360 * (2 * Math.PI);
        double magnetometerXCompensated = (-this.magnetometer.getX() * Math.cos(rollRadians)) - (this.magnetometer.getY() * Math.sin(pitchRadians) *
                Math.sin(rollRadians)) + (this.magnetometer.getZ() * Math.cos(pitchRadians) * Math.sin(rollRadians));
        double magnetometerYCompensated = (this.magnetometer.getY() * Math.cos(pitchRadians)) + (this.magnetometer.getZ() * Math.sin(pitchRadians));
        double yawMeasured = Math.atan2(magnetometerYCompensated, magnetometerXCompensated) / (2 * Math.PI) * 360;
        double yawFiltered = (1 - this.weight) * yawFilteredOld + this.weight * yawMeasured;
        this.yawFilteredOld = yawFiltered;
        this.yaw = yawFiltered;

        // Print roll, pitch and yaw for debug purposes
        System.out.println(this.roll + ", " + this.pitch + ", " + this.yaw);
    }

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

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

Должен ли я выполнять свои вычисления с необработанными значениями, такими как данные внутри IMU, или использовать уже обработанные данные? Например, this.accelerometer.getX() на самом деле возвращает x_raw * 0.061f / 1000 * 9.81f, где x_raw - это значение, хранящееся внутри IMU, а 0.061f - это некоторый коэффициент. Я в основном скопировал расчет из библиотеки Adafruit, но я не уверен на 100%, почему вы должны умножать / делить эти значения.

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

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

...