使用滚动、俯仰、偏航和长度确定新的 GPS 位置

Question

我正在使用 Reach RS+ 设备捕获 GPS 位置数据以及 IMU 数据（滚动、俯仰和偏航）；查看制造商 website 上的 "Point Collection" 图片。
我正在尝试确定底部点（接收器固定在杆的空端）的 GPS 坐标。
为了能够以米为单位进行计算，我将经度 (X) 和纬度 (Y) 转换为 UTM，同时保持高度(Z) 不变。
当杆直立时，X 和 Y 保持不变，而

Z1 = Z - ROD_LENGTH

然而，当杆倾斜时，所有坐标都会受到影响，我需要一种方法来计算杆的末端位置。
我看过旋转矩阵、三角方程、我自己的基于实验观察的 sin 和 cos 公式，但我没有 3D 背景几何形状，我不确定该走哪条路（例如，我不知道如何将杆长与旋转矩阵一起使用）。
基本上我需要以下公式：

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

我不认为你的问题恰好是万向节锁，因为你说当杆直立时X和Y是相同的。您确定将 IMU 与地面平行放置吗？我建议首先尝试只测量 IMU 的结果，当你确定它给出准确的结果时，然后将它与 RS 连接起来。

如果你想测量 GPS 坐标和偏航俯仰角，那么我建议使用 GPS 模块和带 Arduino mini 的 MPU-6050。它足够小，可以挂在任何东西上，而且与非常昂贵的 RS+ 相比也便宜很多。此外，使用这样的小工具，您会发现比使用 RS+ 提供更多支持。

Answer 2

我必须说，这比我预期的要复杂得多。我想我已经把这一切都弄清楚了，但请在评论中告诉我任何更正，我会尽力修复。

在您查看下面的代码之前，请注意！这些假设很重要，您需要验证它们是否适用于您的情况。 space 中有数十种（至少！）定义方向和位置的方法。您需要确保与您的设备保持一致的主要假设是我们正在运行的空间框架。 This article 会让您明白为什么这如此重要！最明显的是我们如何标记我们的轴，哪条路是向上的（正 Z，正如我在下面选择的，但如果我们谈论的是潜艇，例如，我们可能会选择负 Z）。

框架假设：想象一架飞机（我知道它不是飞机，但这样解释更容易），一根长杆笔直向下。我们将 Z 轴定义为向上（正）和向下（负）。 X 轴指向前（正）和后（负）。 Y 轴是围绕机翼的旋转轴，左翼为正，右翼为负 - 这是“right handed coordinate system”。所以轴线相交在飞机的中间，大致就是机翼所在的位置。旋转被定义为绕轴逆时针旋转为正角，顺时针旋转为负角。所以...
- "yaw" 代表绝对航向的变化（所以即使你倾斜和滚动，这也是你相对于地球实际指向的方向。
- "pitch" 表示机翼周围的角度 - 基本上是鼻子朝上还是朝下。
- "roll"表示飞机的倾斜度——所以机翼轴线是平行于地球表面还是围绕机身倾斜。
确保所有这些都正确很重要，尤其是与角度相关的符号 (+/-) - 尝试倾斜它并滚动它大约 30 度并确保结果与输出一致 - 否则改变符号的角度。对于偏航，您需要同时更改航向以及俯仰和横滚，因为航向本身不会影响杆末端的位置，如果它是直线上下的话。您描述 "airplane" 的数据是位置（三个数字），在与上述相同的 XYZ 框架中，以及三个角度（以度为单位，-180 到 180），如上所述。
设备假设：这些是您可能需要与供应商核实的事项。如果数字相对于预期（或允许的）GPS 误差而言较小，则可能无关紧要。
- 代码假定所有轴都在设备底部恰好相交，并且杆从该点垂直垂下。例如，如果杆长 2 米，轴实际上在连接点上方 3 厘米处相遇，您可以将杆长调整为 2.03 米。如果杆实际上连接到一个不完全在轴相交下方的点，则需要稍微更改软件以说明末端不在其正下方。同样，在宏伟的计划中，几毫米对您来说可能无关紧要。
- 代码假定设备声称所在的位置实际上是轴相交的位置。如果不是，您将需要将位置调整到该点（或更改软件以允许这样做）。
- 您需要使用与设备位置相同的单位来指定杆长。
其他假设：
- 这不涉及地球曲率 - 除非你的杆非常长，否则这无关紧要，如果你将它竖直（或接近竖直）则根本无关紧要。

代码：

我留下了一些不必要的东西（如果你需要重组它，你可能需要这些东西）并且也没有试图让它更有效率（例如不断重新计算相同的正弦和余弦）以让它更清楚一点。我在闭包编译器中留下了输入，既是为了一些文档，也是为了以后你想缩小它。 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 3

目前我正在测试一个基于 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]
}

我会 post 获得结果后进行更新。

使用滚动、俯仰、偏航和长度确定新的 GPS 位置

Determine new GPS position using roll, pitch, yaw and length

javascript

math

gps

angle

imu