关于任取2个机器人位置坐标反推坐标系参数技术实现（2年前笔记，归档）

关于任取2个机器人位置坐标反推坐标系参数技术实现

 

需求：

机器人和无人机等设备有自己的坐标系，要在GIS地图上划一块空地展示它们的运动。由于某些未知的异常原因导致无法获取它们坐标系参数（原点，x轴方向，y轴方向），只能根据它们的位置坐标来推演。

 

技术方案简介：

坐标系高度问题好解决，这里只用考虑平面坐标(x, y, 0)，采用平面几何解三角形方案。

1）取2个位置点且按横坐标x从小到大排序得到序列A、B；

2）根据A、B两点易得向量AB、A和B的相对位置在x轴和y轴上的偏移差dx、dy，计算向量与x轴、y轴的偏转角α = arctan(dy/dx)，β = Math.PI/2 – α;

 

3）我们只要把向量AB绕以垂直地面A点的法线ANormal旋转-α就得到平行于实际的x轴的向量xVec，旋转β得到平行于实际Y轴的向量yVec，这里逆时针旋转角度是正数；

4）我们把前面旋转的得到的平行于坐标轴的2个向量归一化为单位向量IX, IY，即实际坐标系的X和Y的方向向量。我们只要把A点沿着-IX、-IY平移自身位置坐标对应的x和y距离就得到实际坐标系的原点坐标。

 

案例验证：

在GIS上定义一个平面坐标系如下：

 

A、 B是任取的2个点，位置坐标已知，假设我们不知道那个白圈是原点、那个红绿线是坐  标轴。 

 

1） 按A、B两点的x坐标增序排列，B在A的右侧，x轴向量正方向一定在在AB向量的右侧。

 

2）计算相对位置差值dx，dy、向量AB并归一化AB、旋转角度α和β，代码如下：

const [p1, p2] = positions;

const dx = p2.x - p1.x;

const dy = p2.y - p1.y;

const AB = Cesium.Cartesian3.subtract(B, A,new Cesium.Cartesian3());

const unitAB =Cesium.Cartesian3.normalize(AB, new Cesium.Cartesian3()); // 归一化

const alpha = Math.atan(dy/dx);

const beta = Math.PI/2 - alpha;

 

3）在A点建立本地坐标系，取法线向量ANormal

const enu =Cesium.Transforms.eastNorthUpToFixedFrame(A);

const ANormal = newCesium.Cartesian3(enu[8], enu[9], enu[10]);

 

4）利用四元数将向量AB绕法向量ANormal旋转得坐标轴向量

let q =Cesium.Quaternion.fromAxisAngle(ANormal, -alpha);

let m3 = Cesium.Matrix3.fromQuaternion(q);

let m4 =Cesium.Matrix4.fromRotationTranslation(m3);

const xVec =Cesium.Matrix4.multiplyByPoint(m4, unitAB, new Cesium.Cartesian3());

 

q =Cesium.Quaternion.fromAxisAngle(ANormal, beta);

m3 = Cesium.Matrix3.fromQuaternion(q);

m4 =Cesium.Matrix4.fromRotationTranslation(m3);

const yVec =Cesium.Matrix4.multiplyByPoint(m4, unitAB, new Cesium.Cartesian3());

 

5）沿着坐标轴单位向量反方向平移A点的坐标值距离得原点坐标：

Cesium.Cartesian3.subtract(A,Cesium.Cartesian3.multiplyByScalar(xVec,p1.x,newCesium.Cartesian3()), p0);

Cesium.Cartesian3.subtract(p0,Cesium.Cartesian3.multiplyByScalar(yVec,p1.y,newCesium.Cartesian3()), p0);

console.log(‘坐标原点’,p0);

 

最终原点绘制的位置青蓝色点和白圈重合，计算OK