北云组合导航设备X1使用排坑

北云组合导航设备X1使用排坑

带着问题，去寻找答案，以及在过程中解决意想不到的问题。

---

一. 问题

1. 使用北云组合导航设备，如何进行各类参数设置？以及需要设置哪些参数？
2. 拿到X1设备提供的InsPVA原始数据，如何对三个欧拉角进行解析，旋转顺序是怎样的？

二. 北云组合导航设备bynav X1使用问题

参考：
1. 官方视频教程
2. 产品用户手册下载

这里新手主要结合这两个资料进行相关设置，关于串口的相关指令，在产品手册中写的比较详细。主要注意关于天线杆臂值、旋转参数、RBV设置、byoffset软件使用等。

三. 北云X1 原始欧拉角信息，如何正确转换为相对于ENU（东北天）坐标系的旋转数据

参考：
北云官方提供的三个角度说明

原本只是一个普普通通欧拉角旋转问题，只需要提供对应顺序和角度值即可，但是事情的排查过程经历了一波三折，排查问题的过程分成了以下三个阶段：

1. 供应商提供信息，欧拉角正确旋转顺序

按照ZXY旋转顺序，对应Yaw、Pitch、Roll； Z是左手法则正方向，其他是右手法则正方向
但是我们测试过程中，发现按照此方式进行位姿+点云数据无法拼接成一张点云图（除去位姿误差影响）。

## python
from scipy.spatial.transform import Rotation as R
angle = [-yaw, pitch, roll]   # 角度
q = R.from_euler('ZXY', angle, degrees=True)  # 注意：大写ZXY代表内旋，xyz代表外旋

供应商并未提供详细代码，根据供应商提供顺序进行解析；请假其他认识的人，说yaw可能存在加减90度的问题，因为apollo工程gnss驱动 novatel数据解析有提到关于角度需要-90，再解析的情况，尝试无果。

constexpr double DEG_TO_RAD = M_PI / 180.0;
constexpr double azimuth_deg_to_yaw_rad(double azimuth) {
  return (90.0 - azimuth) * DEG_TO_RAD;
}

2. 其他同事根据各种图像、点云、场地等信息推理导航参考系并非ENU,而是NED

思路：
1）原始位姿数据解析，按照ZYX顺序，Yaw、Pitch、Roll，认为此得到的位姿是相对于NED参考系；
2）将位姿数据转换到ENU坐标系下。

NED2ENU = np.array([[0,1,0],[1,0,0],[0,0,-1]])

3）激光外参采用的是sensor相对于INS坐标系（NED坐标系）。
至此，得到的激光拼接的点云地图结果是完整的，但细想有许多地方与供应商提供的信息和现场实际信息都有矛盾，比如现场INS载体坐标系是严格右前上的，又或者供应商所说INS的导航参考系就是ENU，不支持NED的设置。

3. 根据第二步推断的正确结果，做反向推导，求出正确的欧拉角解析方法

根据原理，R_NED2ENU * R_ZYX * R_lidar2NED = R_ZXY * R_lidar2ENU, 反向求出R_ZXY。
eg.
例如R_ZYX取值[200,10,20]（yaw,pitch,roll）, 则 R_ZXY [-20,-10,-20], 刚好满足[180°-yaw， -pitch，-roll]的角度计算，点云地图验证后也是拼接正常, 并非完全是供应商提供的 RZ(-yaw)*RX(pitch)*RY(roll)。（不排除设备设置的某些不注意的参数导致，供应商远程设置的，自己并未参与，只是告诉我们都是默认参数）

def test():
    angle = [200, 10, 20]
    true_q  = R.from_euler('ZYX', angle, degrees=True) # shaowei
    q = true_q.as_quat()
    print("true qx qy qz qw: ", q[0],q[1],q[2],q[3])
    
    NED2ENU = np.array([[0,1,0],[1,0,0],[0,0,-1]])
    frontlidar2NEDins = np.array([[1,0,0],[0,-1,0],[0,0,-1]])
    frontlidar2ENUins = np.array([[0,1,0],[-1,0,0],[0,0,1]])
    part0 = np.linalg.inv(frontlidar2ENUins)
    part1 = np.dot(NED2ENU, true_q.as_matrix())
    part2 = np.dot(part1, frontlidar2NEDins)
    part3 = np.dot(part2, part0)
    print(part3)
    m = np.matrix(part3)
    ret = R.from_matrix(m)
    euler = ret.as_euler('ZXY', degrees=True)
    print(euler) # -20 -10 -20

总结

在使用北云组合导航设备bynav X1时，首先务必遵照手册进行相关参数的正确设置，其次注意角度解析时，按照ZXY旋转顺序，对应Yaw、Pitch、Roll；Z是左手法则正方向，XY是右手法则正方向，即解析方式如下：

angle = [180 - yaw, -pitch, -roll]
q = R.from_euler('ZXY', angle, degrees=True)