无人机避障——4DMmvRadar三维点云坐标转换到无人机坐标系（三）

在4D毫米波雷达数据读取完：

数据读取可看上篇：

4D 毫米波雷达读取数据篇（一）-CSDN博客

进行雷达三维点云通过旋翼无人机的偏航、滚转、俯仰进行点云坐标转换：

参考博客：

无人机导航中的各类坐标系_无人机 坐标系-CSDN博客

PX4无人机控制坐标系基本概念_无人机坐标系统-CSDN博客

 [重点]：

固联在无人机上的雷达传感器的数据类似于东北天坐标系，而无人机的机体坐标系类似于北东地坐标系。

参考博客：

东北天（ENU）和北东地（NED）_东北天坐标系-CSDN博客

1、先将雷达传感器的数据转换成无人机的机体坐标系（ENU->NED）

从坐标系定义中，可以看出，ENU和NED的坐标变换关系是：
X(ENU) = Y(NED)
Y(ENU) = X(NED)
Z(ENU) = -Z(NED)
Yaw(ENU) = -Yaw（NED）+ 90

注意：虽然ENU和NED旋转顺序不同，但是都是根据yaw-pitch-roll顺序进行旋转的。

2、从无人机的机体坐标系转换到世界坐标系（NED->NED）经过了欧拉角的转换。

# # 测试三维旋转矩阵
                rotate_matrix_z = np.asarray([[math.cos(yaw),-math.sin(yaw),0],
                                              [math.sin(yaw),math.cos(yaw),0],
                                              [0,0,1]])
                rotate_matrix_y = np.asarray([[math.cos(pitch),0,math.sin(pitch)],
                                              [0,1,0],
                                              [-math.sin(pitch),0,math.cos(pitch)]])
                rotate_matrix_x = np.asarray([[1,0,0],
                                              [0,math.cos(roll),math.sin(roll)],
                                              [0,math.sin(roll),math.cos(roll)]])

内旋还是外旋：

 每次旋转是绕固定轴（一个固定参考系，比如世界坐标系）旋转，称为外旋。每次旋转是绕自身旋转之后的轴旋转，称为内旋。下图说明了内旋和外旋的区别。

[注意]：无人机从机体坐标系转世界坐标系是外旋，因为世界坐标系是固定坐标系

外旋：（问题还差这里！！！）

无人机常用坐标系、旋转矩阵、欧拉角、四元数以及相互转换关系_姿态角希腊字母-CSDN博客

                        # 旋转
                    nedlidarPos = np.dot(points_ned,rotate_matrix_z)
                    nedlidarPos = np.dot(nedlidarPos,rotate_matrix_y)
                    nedlidarPos = np.dot(nedlidarPos,rotate_matrix_x)

 因为外旋要左乘，但是涉及到维度的问题，还需要解决！！！！

参考三角函数转换公式：

三角函数之间的转换公式_三角函数之间的转换关系-CSDN博客

[注意] ：https://zhuanlan.zhihu.com/p/598881190

Airsim中的激光雷达传感器用的坐标系是北东地坐标系NED坐标系，所以在仿真中其不需要先转为机体系那一步骤，直接就可以旋转矩阵了。 

nedlidarPos = np.dot(rotate_matrix_x,points_ned.T)
                    nedlidarPos = np.dot(rotate_matrix_y,nedlidarPos)
                    nedlidarPos = np.dot(rotate_matrix_z,nedlidarPos)

 机体坐标系：

机体坐标系固连飞机，其原点 取在多旋翼的重心位置上。 x轴在多旋翼对称平面内指向机头（机头方向与多旋+字形或X字形相关）。 z轴在飞机对称平面内，垂直轴向下。然后，按右手定则确定y轴 。

地球固联坐标系：

通常以多旋翼起飞位置作为坐标原点 。先让x轴在水平面内指向某一方向，z轴垂直于地面向下。然后，按右手定则确定y轴，坐标原点还有用地心的？比如NED坐标系为x轴为正北方向，y轴为正东方向，z轴指向下。
飞机的欧拉角就是基于上面两个坐标系的转换。pitch+为抬头，roll+为右旋转，yaw+为右偏航。

GPS坐标系 (WGS-84)：

定义：全球定位系统的坐标系，以地球中心为原点。
应用：用于全球定位和长距离导航。
特点：提供经纬度和高度坐标。

北东地坐标系 (NED, North-East-Down)：

定义：局部笛卡尔坐标系，原点通常设置在无人机的起始位置。
应用：导航和控制算法中最常用的坐标系之一。
特点：x轴指向正北方向，y轴指向正东方向，z轴向下指向地面。

为什么使用NED坐标系：

高度控制的重要性：

NED坐标系的Z轴指向地面（向下），这使得高度控制变得更为直观和直接。在定点控制模式下，高度控制是非常关键的功能之一，确保无人机能够稳定地保持在设定的高度上。
ENU坐标系的Z轴指向天空（向上），虽然也可以用于高度控制，但是在无人机上下移动时，使用NED坐标系更加直观，更容易理解和实现。

导航和控制：在无人机和飞机的导航系统中，NED 坐标系被用来表示相对于起点的位置变化，方便进行路径规划和控制。
传感器融合：无人机上的传感器（如加速度计、陀螺仪）通常以机体坐标系输出数据，需要转换到 NED 坐标系才能进行有效的融合和导航解算。
地图匹配：在地图匹配和地理信息系统（GIS）应用中，NED 坐标系可以用来将传感器数据和地图数据进行对齐。

从机体坐标系到 NED：这通常涉及到无人机姿态的变换，即根据无人机的姿态角（俯仰角、偏航角、滚转角）来旋转机体坐标系中的向量，使其对准 NED 坐标系。

代码：

在 NumPy 中使用 np.dot 函数或 @ 运算符进行矩阵乘法时，操作是按照左乘进行的。这意味着，如果您有两个矩阵 A 和 B，表达式 np.dot(A, B) 或 A @ B 会将 A 乘以 B。

nedlidarPos = np.dot(pointsxyz, rotate_matrix_z)

nedlidarPos = pointsxyz @ rotate_matrix_z

pointsxyz 是左乘以 rotate_matrix_z 的。这意味着 rotate_matrix_z 被应用于 pointsxyz 的列向量上。这通常是用来将一个变换（例如旋转、缩放、剪切等）应用到一组点上的常见方式。

在这种情况下，假设 pointsxyz 是一个 M x 3 的数组，其中包含 M 个点的 X、Y、Z 坐标，而 rotate_matrix_z 是一个 3x3 的旋转矩阵（针对三维空间中的 X、Y、Z 坐标），那么 np.dot(pointsxyz, rotate_matrix_z) 会执行以下操作：

1. 取出 pointsxyz 的每一列（一个 3x1 的向量）。
2. 将这个向量与 rotate_matrix_z 进行矩阵乘法。
3. 计算结果是一个 M x 3 的矩阵，其中每一列都是原始 pointsxyz 中对应列向量与旋转矩阵相乘的结果。 

举例 ：

import numpy as np

# 创建一个旋转矩阵，旋转角度为 theta（以弧度为单位）
theta = np.radians(angle)  # 将角度转换为弧度
rotate_matrix_z = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
])

# 假设 points 是一个 N x 3 的数组
# points = np.array([...])
# pointsxyz = points[:, 0:3]  # 提取 X、Y、Z 坐标

# 应用旋转矩阵
nedlidarPos = np.dot(pointsxyz, rotate_matrix_z)

# nedlidarPos 现在是旋转后的点坐标