Cesium 根据飞机航线计算飞机的Heading(偏航角)、Pitch(俯仰角)、Roll(翻滚

欧飞

需求

设置飞机的一些坐标位置(经纬度高度)，插值得到更多的坐标位置，然后飞机按照这些坐标集合形成的航线飞行，飞机的朝向、俯仰角以及飞机转弯时的翻转角根据坐标集合计算得出，而不需要手动设置heading、pitch、roll。
坐标插值

不知道为什么，可能是飞行速度变化太大，我用Cesium自带的插值，计算出的航线很奇怪

1. // 如下代码插值计算出的航线有问题
2. property.setInterpolationOptions({
3.     interpolationDegree : 5,
4.     interpolationAlgorithm : Cesium.LagrangePolynomialApproximation
5. });

复制代码

自己写的插值计算，效果等同于Cesium自带的线性插值。
思路很简单，每次插值，就是取时间的中点，两个坐标的中点。
代码如下：

1. /**
2. * 重新采样this.DronePositions
3. */
4. DetectsDrones.prototype.sameple = function () {
5.     for (let i = 0; i < 3; i++) {
6.         this.samepleOnce();
7.     }
8. }
10. /**
11. * 重新采样this.DronePositions
12. */
13. DetectsDrones.prototype.samepleOnce = function () {
14.     for (let i = 0; i < this.DronePositions.length - 1; i += 2) {
15.         let pos1 = this.DronePositions[i];
16.         let pos2 = this.DronePositions[i + 1];
17.         let time1 = dayjs(pos1.time, 'YYYY-MM-DD HH:mm:ss');
18.         let time2 = dayjs(pos2.time, 'YYYY-MM-DD HH:mm:ss');
19.         let time = time1.add(time2.diff(time1) / 2.0, 'millisecond');
20.         let lng = (pos1.targetPosition.lng + pos2.targetPosition.lng) / 2.0;
21.         let lat = (pos1.targetPosition.lat + pos2.targetPosition.lat) / 2.0;
22.         let height = (pos1.targetPosition.height + pos2.targetPosition.height) / 2.0;
23.         let heading = (pos1.targetPosition.heading + pos2.targetPosition.heading) / 2.0;
24.         let pitch = (pos1.targetPosition.pitch + pos2.targetPosition.pitch) / 2.0;
25.         let roll = (pos1.targetPosition.roll + pos2.targetPosition.roll) / 2.0;
26.         let pos = {
27.             time: time.format('YYYY-MM-DD HH:mm:ss.SSS'),
28.             targetPosition: {
29.                 lng: lng,
30.                 lat: lat,
31.                 height: height,
32.                 heading: heading,
33.                 pitch: pitch,
34.                 roll: roll,
35.             }
36.         }
37.         this.DronePositions.splice(i + 1, 0, pos);
38.     }
39. }

复制代码

根据航线坐标集合计算heading、pitch、roll

从网上抄的计算heading和pitch的方法(参考博客：https://blog.csdn.net/u010447508/article/details/105562542?_refluxos=a10)：

1. /**
2. * 根据两个坐标点,获取Heading(朝向)
3. * @param { Cesium.Cartesian3 } pointA
4. * @param { Cesium.Cartesian3 } pointB
5. * @returns
6. */
7. function getHeading(pointA, pointB) {
8.     //建立以点A为原点，X轴为east,Y轴为north,Z轴朝上的坐标系
9.     const transform = Cesium.Transforms.eastNorthUpToFixedFrame(pointA);
10.     //向量AB
11.     const positionvector = Cesium.Cartesian3.subtract(pointB, pointA, new Cesium.Cartesian3());
12.     //因transform是将A为原点的eastNorthUp坐标系中的点转换到世界坐标系的矩阵
13.     //AB为世界坐标中的向量
14.     //因此将AB向量转换为A原点坐标系中的向量，需乘以transform的逆矩阵。
15.     const vector = Cesium.Matrix4.multiplyByPointAsVector(
16.         Cesium.Matrix4.inverse(transform, new Cesium.Matrix4()),
17.         positionvector,
18.         new Cesium.Cartesian3()
19.     );
20.     //归一化
21.     const direction = Cesium.Cartesian3.normalize(vector, new Cesium.Cartesian3());
22.     //heading
23.     let heading = Math.atan2(direction.y, direction.x) - Cesium.Math.PI_OVER_TWO;
24.     heading = Cesium.Math.TWO_PI - Cesium.Math.zeroToTwoPi(heading);
25.     return Cesium.Math.toDegrees(heading);
26. }
28. /**
29. * 根据两个坐标点,获取Pitch(仰角)
30. * @param { Cesium.Cartesian3 } pointA
31. * @param { Cesium.Cartesian3 } pointB
32. * @returns
33. */
34. function getPitch(pointA, pointB) {
35.     let transfrom = Cesium.Transforms.eastNorthUpToFixedFrame(pointA);
36.     const vector = Cesium.Cartesian3.subtract(pointB, pointA, new Cesium.Cartesian3());
37.     let direction = Cesium.Matrix4.multiplyByPointAsVector(Cesium.Matrix4.inverse(transfrom, transfrom), vector, vector);
38.     Cesium.Cartesian3.normalize(direction, direction);
39.     //因为direction已归一化，斜边长度等于1，所以余弦函数等于direction.z
40.     let pitch = Cesium.Math.PI_OVER_TWO - Cesium.Math.acosClamped(direction.z);
41.     return Cesium.Math.toDegrees(pitch);
42. }

复制代码

根据航线坐标集合计算heading、pitch、roll：
代码中this.DronePositions是无人机群的坐标集合，坐标放在targetPosition属性中

1. /**
2. * 计算无人机群的heading
3. */
4. DetectsDrones.prototype.calcHeading = function () {
5.     // 清空原有heading
6.     this.DronePositions.map(pos => {
7.         pos.targetPosition.heading = undefined;
8.     });
10.     for (let i = 0; i < this.DronePositions.length - 1; i++) {
11.         let pos1 = this.DronePositions[i];
12.         let pos2 = this.DronePositions[i + 1];
13.         let heading = -90 + getHeading(Cesium.Cartesian3.fromDegrees(pos1.targetPosition.lng, pos1.targetPosition.lat), Cesium.Cartesian3.fromDegrees(pos2.targetPosition.lng, pos2.targetPosition.lat));
14.         if (!pos1.targetPosition.heading) {
15.             pos1.targetPosition.heading = heading;
16.         }
17.         pos2.targetPosition.heading = heading;
18.     }
19. }
21. /**
22. * 计算无人机群的pitch
23. */
24. DetectsDrones.prototype.calcPitch = function () {
25.     // 清空原有pitch
26.     this.DronePositions.map(pos => {
27.         pos.targetPosition.pitch = undefined;
28.     });
30.     for (let i = 0; i < this.DronePositions.length - 1; i++) {
31.         let pos1 = this.DronePositions[i];
32.         let pos2 = this.DronePositions[i + 1];
33.         let pitch = getPitch(Cesium.Cartesian3.fromDegrees(pos1.targetPosition.lng, pos1.targetPosition.lat, pos1.targetPosition.height), Cesium.Cartesian3.fromDegrees(pos2.targetPosition.lng, pos2.targetPosition.lat, pos2.targetPosition.height));
34.         if (!pos1.targetPosition.pitch) {
35.             pos1.targetPosition.pitch = pitch;
36.         }
37.         pos2.targetPosition.pitch = pitch;
38.     }
39. }
41. /**
42. * 计算无人机群的roll(不支持转弯大于90度)
43. */
44. DetectsDrones.prototype.calcRoll = function () {
45.     // 清空原有roll
46.     this.DronePositions.map(pos => {
47.         pos.targetPosition.roll = undefined;
48.     });
50.     for (let i = 1; i < this.DronePositions.length - 1; i++) {
51.         let pos1 = this.DronePositions[i];
52.         let pos2 = this.DronePositions[i + 1];
53.         let deltaHeading = pos2.targetPosition.heading - pos1.targetPosition.heading;
54.         pos2.targetPosition.roll = deltaHeading / 1.5;
55.     }
56. }

复制代码

效果

主要是飞机的朝向和转弯时的翻滚，俯仰角这里没体现。

遇到的问题

• 插值计算的问题，就是设置的坐标集合，是拆线，最好把它插值成平滑曲线，但是Cesium自带的插值，有时间参数，而我想仅仅通过经纬度集合来插值。
  
• 我写的计算roll的方法有问题，不支持转弯大于90度的情况，花了一些时间，没搞定。转弯小于90度，凑合用，测试了几组数据没问题，但仍不确定有没有BUG。严格来讲，根据这些参数，这个roll是算不出来的，但是，该算法要求根据飞机的转弯半径及方向，给出一个相对合理的roll值。
  抛砖引玉，有没有高手给个提示，插值问题怎么解决？roll的正确的通用的计算方法？
  

来源:https://www.cnblogs.com/s0611163/p/18107778
免责声明：由于采集信息均来自互联网，如果侵犯了您的权益，请联系我们【E-Mail:cb@itdo.tech】 我们会及时删除侵权内容，谢谢合作！