使用横滚，俯仰，偏航和长度确定新的GPS位置 [英] Determine new GPS position using roll, pitch, yaw and length
问题描述
我正在使用Reach RS +设备来捕获GPS位置数据以及IMU数据(侧倾，俯仰和偏航)；请参阅制造商的网站上的积分收集"图像.
我正在尝试确定底部点(接收器固定在其上的杆的空端)的GPS坐标.
为了能够以米为单位进行计算，我将经度( X )和纬度( Y )转换为
I'm using a Reach RS+ device to capture GPS position data as well as IMU data (roll, pitch and yaw); see the "Point Collection" image on the manufacturer's website.
I'm trying to determine the GPS coordinates of the bottom point (the empty end of the rod that the receiver is fixed on).
To be able to make calculations in meters I'm converting longitude (X) and latitude (Y) to UTM while keeping altitude (Z) unaltered.
When the rod is upright, X and Y stay the same while
Z1 = Z - ROD_LENGTH
但是，当杆倾斜时，所有坐标都会受到影响，我需要一种方法来计算杆的最终位置.
我已经根据实验观察了旋转矩阵，三角方程，我自己的 sin 和 cos 公式，但是我没有3D几何背景，并且我没有确定走哪条路线(例如，我不知道如何将杆长与旋转矩阵一起使用).
基本上，我需要以下公式:
However when the rod is tilted all coordinates are affected and I need a way to calculate the rod's end position.
I have looked at rotation matrices, triangle equations, my own sin and cos formulas based on experimental observations but I don't have a background in 3D geometry and I'm not sure which route to take (for example I don't know how to use the rod length with a rotation matrix).
Basically I need the following formulas:
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°之间.
Roll, pitch and yaw have values between -180° and 180°.
推荐答案
我必须说，事实证明这比我预期的要复杂得多.我想我已经很直截了当了，但是在任何更正的评论中让我知道，我将尝试修复.
I must say, this turned out to be a lot more complex than I expected. I think I have this all straight, but let me know in comments of any corrections and I will try to fix.
在看下面的代码之前，请注意！这些假设很重要，您需要验证它们在您的情况下是正确的.有数十种(至少！)定义空间方向和位置的方法.您需要确保与设备对齐的主要假设是我们在其中进行操作的空间框架. 这篇文章将为您提供一定的赞赏重要的！最明显的是如何标记轴，即向上标记(正Z号，如我在下面选择的，但是例如，在谈论潜艇时，我们可能会选择负Z号).
Before you look at the code, below, PLEASE NOTE! The assumptions are important and you need to verify they are true in your case. There are dozens (at least!) of ways to define orientation and locations in space. The main assumpions you need to make sure are aligned with your device are the spatial frame in which we are operating. This article will give you some appreciation for why this is so important! The most obvious being how are we labelling our axes, which way is up (positive Z, as I have chosen below, but if we were talking about submarines, for instance, we might choose negative Z).
-
框架假设:想象一下一架飞机(我知道它不是飞机，但用这种方式更容易解释)，上面长着一根直立的杆子.我们将Z轴定义为向上(正)和向下(负). X轴指向向前(正)和向后(负). Y轴是围绕机翼的旋转轴，左翼为正，右翼为负-这是一个"右手坐标系".因此，轴相交在飞机的中部，机翼大约在此位置.旋转定义为绕轴逆时针旋转，角度为正，顺时针旋转为负.所以...
Framework assumptions: Picture an airplane (I know its not an airplane, but its easier to explain this way) with a long rod hanging straight down. We will define the Z axis as up (positive) and down (negative). The X axis points forwards (positive) and backwards (negative). The Y axis is the axis of rotation about the wings, with positive off the left wing, and negative off the right wing - this is a "right handed coordinate system". So the axes intersect is in the middle of the airplane roughly where the wings are attached. Rotation is defined as counter-clockwise around the axis for positive angles and clockwise being negative. So...
- 偏航"表示绝对航向的变化(因此，即使您俯仰和滚动，这也是您相对于实际地球指向的方向.
- 俯仰"表示机翼周围的角度-基本上是鼻子是向上还是向下.
- 侧倾"代表飞机的倾斜方向-机翼轴线平行于地面还是围绕机身倾斜.
所有这些都非常重要，尤其是与您的角度相关的符号(+/-)-尝试将其倾斜并滚动约30度，并确保结果与输出一致-否则请更改符号角度.进行偏航时，您需要同时更改航向以及俯仰和横滚，因为航向本身不会影响杆端的位置(如果杆是向上或向下的话). 如上所述，您描述飞机"的数据是位置(三个数字)，位于相同的XYZ框架中，三个角度(以度为-180到180度).
It's important to get all this right, particularly the sign (+/-) associated with your angles - try to pitch it and roll it about 30 degrees and make sure the results agree with the output - otherwise change the sign of the angle. For yaw, you will need to change both the heading and either the pitch and roll, as the heading itself will not affect the location of the end of the rod, if its straight up and down. The data you have describing the "airplane" is location (three numbers), in the same XYZ framework as described above, and the three angles (in degrees, -180 to 180), as described above.
- 该代码假定所有轴都在设备底部恰好相交，并且杆从该点垂直垂下.例如，如果杆长度为2米，并且轴实际上在连接点上方相交三厘米，则可以将杆长度调整为2.03米.如果杆实际上连接到的轴线不完全相交的点，则需要对软件进行一些更改以说明末端不在其正下方.同样，在宏伟的设计方案中，几毫米对您可能并不重要.
- 代码假定设备声称的位置实际上是轴相交.如果不是，则需要将位置调整为该位置(或更改软件以允许该位置).
- 您需要以与设备位置相同的单位指定焊条长度.
- 这不会处理地球曲率-除非您的杆异常长，这无关紧要，并且如果您将其笔直(或几乎垂直)将其完全无关紧要.
代码:
我遗留了一些不必要的东西(如果您需要对其进行完全重组，可能会需要这些东西)，并且也没有尝试使其变得更有效率(例如，不断地重新计算相同的正弦和余弦)来使其变得更有效.清楚一点.我留下了闭包编译器的类型，既有一些文档说明，也有待以后缩小的情况. rodloc是您想要的功能...
I left in some unnecessary things (which you might need in case you need to restructure it at all) and also didn't try to make it more efficient (for example constant recalculation of the same sins and cosines) to make it a little clearer. I left in the closure compiler typing, both for a little documentation and in case you want to minify it later. rodloc is the function you want...
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;
}
这篇关于使用横滚，俯仰，偏航和长度确定新的GPS位置的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！
