问题
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
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)
Roll, pitch and yaw have values between -180° and 180°.
回答1:
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.
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).
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...
- "yaw" represents a change in absolute heading (so even if you are pitched and rolled, this is the direction you are pointing with respect to earth actual.
- "pitch" represents the angle around the wings - basically whether the nose is pointing up or down.
- "roll" represents the banking of the airplane - so whether the wing axis is parallel to the earth surface or tilted around the fuselage.
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.
- Device assumptions: These are things you may need to check with your vendor. If the numbers are small relative to the expected (or permissable) GPS error, it may not matter very much.
- The code assumes that the axes all meet right at the bottom of the device, and that the rod hangs vertically down from that point. If, for example, the rod is 2 meters long and the axes actually meet three centimeters above the connection point, you would adjust the rod length to 2.03 meters. If the rod is actually attached to a point not quite under the axes intersect, the software needs to be changed a bit to account for the end not being directly under it. Again, a few millimeters may not matter to you in the grand scheme of things.
- The code assumes that the location the device claims to be at is actually the axes intersect. If not, you will need to adjust the location to be at that point (or change the software to allow for that).
- You need to specify the rod length in the same units as the device location.
- Other assumptions:
- This does NOT deal with earth curvature - unless your rod is unusually long, this shouldn't matter, and won't matter at all if you are holding it straight up (or nearly so).
The code:
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;
}
回答2:
At the moment I'm testing a solution based on three.js which works something along these lines:
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]
}
I'll post an update with results once I have them.
回答3:
I don't think your problem happens to be gimbal lock as you say X and Y are the same when rod is upright. Are you sure you have places the IMU parallel to the ground. I recommend first trying to just measure the results of the IMU and when you are certain it is giving accurate results then hook it up with the RS.
If you want to measure GPS coordinates and yaw pitch roll then I recommend using a GPS module and MPU-6050 with an Arduino mini. It is small enough to be hooked with anything and also a lot lot cheaper compared to a very expensive RS+. Also with such a gadget you will find a lot more support available rather than using RS+.
来源：https://stackoverflow.com/questions/52370496/determine-new-gps-position-using-roll-pitch-yaw-and-length