Определите новую позицию GPS, используя крен, шаг, рыскание и длину

Question

Я использую устройство Reach RS+ для захвата данных о местоположении GPS, а также данных IMU (крен, тангаж и рыскание); см. изображение "Коллекция точек" на сайте производителя.
Я пытаюсь определить координаты GPS нижней точки (пустой конец стержня, на котором закреплен приемник).
Чтобы иметь возможность делать вычисления в метрах, я конвертирую долготу (X) и широту (Y) в UTM, сохраняя при этом высоту (Z) без изменений.
Когда стержень в вертикальном положении, X и Y остаются одинаковыми, в то время как

Z1 = Z - ROD_LENGTH  

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

X1 = X + ROD_LENGTH * func_X(roll, pitch, yaw)
Y1 = Y + ROD_LENGTH * func_Y(roll, pitch, yaw)
Z1 = Z + ROD_LENGTH * func_Z(roll, pitch, yaw)

Крен, угол наклона и рыскание имеют значения от -180° до 180°.

Answer 1

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

Прежде чем посмотреть код, приведенный ниже, ОБРАТИТЕ ВНИМАНИЕ! Предположения важны, и вы должны убедиться, что они верны в вашем случае. Есть десятки (по крайней мере!) Способов определения ориентации и местоположения в пространстве. Основными предположениями, которые вы должны убедиться в том, что выровнены с вашим устройством, являются пространственные рамки, в которых мы работаем. Эта статья поможет вам понять, почему это так важно! Наиболее очевидным является то, как мы маркируем свои оси, какой путь вверх (положительный Z, как я выбрал ниже, но если бы мы говорили о подводных лодках, например, мы могли бы выбрать отрицательный Z).

1. Исходные предположения: Представьте себе самолет (я знаю, что это не самолет, но так проще объяснить) с длинным стержнем, свисающим прямо вниз. Мы определим ось Z как вверх (положительная) и вниз (отрицательная). Ось X указывает вперед (положительно) и назад (отрицательно). Ось Y - это ось вращения вокруг крыльев, положительная от левого крыла и отрицательная от правого крыла - это " правосторонняя система координат". Таким образом, оси пересекаются в середине самолета примерно там, где прикреплены крылья. Вращение определяется как против часовой стрелки вокруг оси для положительных углов и по часовой стрелке, будучи отрицательным. Так...

   • "рыскание" представляет изменение абсолютного курса (поэтому, даже если вы находитесь в наклонном положении и покатитесь, это направление, которое вы указываете относительно земли, фактической.
   • "тангаж" представляет угол вокруг крыльев - в основном, направлен ли нос вверх или вниз.
   • "крен" обозначает крен самолета - так, параллельна ли ось крыла поверхности земли или наклонена вокруг фюзеляжа.

   Важно сделать все правильно, особенно знак (+/-), связанный с вашими углами - попробуйте наклонить его и повернуть на 30 градусов и убедиться, что результаты совпадают с выходными данными - в противном случае измените знак угла. Для рыскания вам нужно будет изменить как курс, так и тангаж и крен, так как сам курс не повлияет на местоположение конца стержня, если он направлен вверх и вниз. Данные, которые вы описываете для "самолета", - это местоположение (три числа) в той же структуре XYZ, как описано выше, и три угла (в градусах, от -180 до 180), как описано выше.

2. Предположения об устройстве: это вещи, которые вам, возможно, придется проверить у своего поставщика. Если числа малы относительно ожидаемой (или допустимой) ошибки GPS, это может не иметь большого значения.
   • Кодекс предполагает, что все оси встречаются прямо в нижней части устройства, и что стержень свисает вертикально вниз от этой точки. Например, если длина стержня составляет 2 метра, а оси на самом деле встречаются на три сантиметра выше точки соединения, вы должны отрегулировать длину стержня до 2,03 метра. Если стержень фактически прикреплен к точке, не совсем под пересекающимися осями, необходимо немного изменить программное обеспечение, чтобы учесть, что конец не находится непосредственно под ним. Опять же, несколько миллиметров могут не иметь значения для вас в общей схеме вещей.
   • Код предполагает, что местоположение, в котором устройство утверждает, что находится на самом деле, пересекается с осями. Если нет, вам нужно будет настроить местоположение, чтобы быть в этой точке (или изменить программное обеспечение, чтобы учесть это).
   • Вы должны указать длину стержня в тех же единицах, что и местоположение устройства.
3. Другие предположения:
   • Это НЕ касается кривизны земли - если ваш стержень не слишком длинный, это не должно иметь значения и не будет иметь никакого значения, если вы держите его прямо (или почти так).

Код:

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

function presentresult(location, length, yaw, pitch, roll) {
    console.log("Starting point");
    console.log(location);
    console.log("Rod length = " + length);
    console.log("Yaw = " + yaw + ", Pitch = " + pitch + ", Roll = " + roll);
    console.log("Result:");
    console.log(rodloc(location, length, yaw, pitch, roll));
}

presentresult([100, 100, 100], 2, 0, 0, 0); // Result:  [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 30, 0, 0); // Result:  [100, 100, 98] (3)
presentresult([100, 100, 100], 2, -30, 0, 0); // Result:  [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 0, 30, 0); // Result:  [99, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, -30, 0); // Result:  [101, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, 30); // Result:  [100, 101, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, -30); // Result:  [100, 99, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 30, 30, 30); // Result:  [98.75, 100.43301270189222, 98.5] (3)
presentresult([100, 100, 100], 2, -30, -30, -30); // Result:  [100.25, 98.70096189432334, 98.5] (3)
presentresult([100, 100, 100], 2, -30, 30, -30); // Result:  [98.75, 99.56698729810778, 98.5] (3)

/** @typedef {Array<number,number,number>} */ var Vector3D;
/** @typedef {Array<Vector3D,vector3D,Vector3D>} */ var Matrix3D;

/**
 * @param {Vector3D} location - The location (3 coordinates) of the "plane"
 * @param {number} length - The length of the rod
 * @param {number} yaw - the yaw (heading) in degrees
 * @param {number} pitch - the pitch in degrees
 * @param {number} roll - the roll in degrees
 * @returns {Vector3D} - the location of the end of the rod
 */
function rodloc(location, length, yaw, pitch, roll) {
    let ryaw = yaw * Math.PI / 180.0;       // Convert to radians
    let rpitch = pitch * Math.PI / 180.0;
    let rroll = roll * Math.PI / 180.0;

    // This is where our axes start
    let x = [1, 0, 0];
    let y = [0, 1, 0];
    let z = [0, 0, 1];

    // NOTE:  ORDER MATTERS - your data may mean different things (see
    //        assumptions in answer!
    // Rotate axes around z by yaw
    let yprime = rotatearound([0, 1, 0], [0, 0, 1], ryaw);
    let xprime = rotatearound([1, 0, 0], [0, 0, 1], ryaw);
    let zprime = z;     // rotating around itself

    // Next we need to rotate for pitch (around the Y axis...)
    let x2prime = rotatearound(xprime, yprime, rpitch); 
    let y2prime = yprime; // dont need this
    let z2prime = rotatearound(zprime, yprime, rpitch);

    // Now we need to roll around the new x axis...
    let x3prime = x2prime   // dont need this
    let y3prime = rotatearound(y2prime, x2prime, rroll); // dont need this
    let z3prime = rotatearound(z2prime, x2prime, rroll);

    // now take what started out as [0, 0, 1] and place the end of the rod
    // (at what started out as [0, 0, -length])
    let rotend = [0,1,2].map(n=>-length*z3prime[n]);

    // now take that and add it to the original location of the plane 
    // and return it as the result
    return [0,1,2].map(n=>location[n]+rotend[n]);
}

/** Multiply a vector times a matrix
 * @param {Vector3D} offset - The vector of the offset
 * @param {Matrix3D} rotate - The rotation vector
 * @returns {Vector3D} - The new offset vector
 */
function vmmult(offset, rotate) {
    return [0,1,2].map(x=>xmult(offset,rotate[x]));
}

/** dot product of two vectors
 * @param {Vector3D} col
 * @param {Vector3D} row
 * @returns {number}
 */
function xmult(col, row) {
    return [0,1,2].reduce((a,c)=>a+col[c]*row[c],0);
}

/** Rotate a point around a vector projecting from the origin
 * @param {Vector3D} point - the we want to rotate
 * @param {Vector3D} vec - the vector (from origin to here) to rotate around
 * @param {number} angle - the angle (in radians) to rotate
 * @returns {Vector3D} - the new point location
 */
function rotatearound(point, vec, angle) {
    let rotmat = setuprotationmatrix(angle, vec);
    return vmmult(point, rotmat);
}

/**
 * Adapted from C courtesy of Bibek Subedi
 * https://www.programming-techniques.com/2012/03/3d-rotation-algorithm-about-arbitrary.html
 * @param {number} angle - the angle to rotate around the vector
 * @param {Vector3D} vec - the vector around which to rotate
 * @returns {Matrix3D} - the rotation matrix
 */
function setuprotationmatrix(angle, vec) {
    // Leaving L in for reusability, but it should always be 1 in our case
    let u = vec[0], v = vec[1], w = vec[2]; 
    let L = (u*u + v * v + w * w);
    let u2 = u * u;
    let v2 = v * v;
    let w2 = w * w; 

    let rotmat = [[],[],[]];
    rotmat[0][0] = (u2 + (v2 + w2) * Math.cos(angle)) / L;
    rotmat[0][1] = (u * v * (1 - Math.cos(angle)) - w * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[0][2] = (u * w * (1 - Math.cos(angle)) + v * Math.sqrt(L) * Math.sin(angle)) / L;

    rotmat[1][0] = (u * v * (1 - Math.cos(angle)) + w * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[1][1] = (v2 + (u2 + w2) * Math.cos(angle)) / L;
    rotmat[1][2] = (v * w * (1 - Math.cos(angle)) - u * Math.sqrt(L) * Math.sin(angle)) / L;

    rotmat[2][0] = (u * w * (1 - Math.cos(angle)) - v * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[2][1] = (v * w * (1 - Math.cos(angle)) + u * Math.sqrt(L) * Math.sin(angle)) / L;
    rotmat[2][2] = (w2 + (u2 + v2) * Math.cos(angle)) / L;
    return rotmat;
} 

Answer 2

В данный момент я тестирую решение, основанное на three.js, которое работает примерно так:

function getCorrectedPosition(x, y, z, dist, roll, pitch, yaw) {
    let matrix = new THREE.Matrix4().makeRotationFromEuler(new THREE.Euler(toRadians(pitch), toRadians(roll), toRadians(yaw)));
    let moveVector = new THREE.Vector3(0, 0, -dist);
    moveVector.applyMatrix4(matrix);
    let position = new THREE.Vector3(z, y, x).add(moveVector);
    return [position.x, position.y, position.z]
}

Я опубликую обновление с результатами, как только они у меня будут.

Answer 3

Я не думаю, что ваша проблема связана с блокировкой карданного подвеса, поскольку вы говорите, что X и Y одинаковы, когда удилище выпрямлено Вы уверены, что располагаете ИДУ параллельно земле? Я рекомендую сначала попытаться измерить результаты IMU, а когда вы уверены, что он дает точные результаты, то подключите его к RS.

Если вы хотите измерить координаты GPS и угол наклона рыскания, я рекомендую использовать модуль GPS и MPU-6050 с Arduino mini. Он достаточно мал, чтобы зацепиться за что-либо, а также намного дешевле по сравнению с очень дорогим RS+. Также с таким гаджетом вы найдете гораздо больше доступной поддержки, чем при использовании RS+.