Python 矩阵运算笔记

目的

从Excel的单元格中读取数据，根据Yaw、Roll、Pitch、世界坐标、A点，通过计算出偏移后的点A'，最后将结果输出在Excel的单元格中。

参考文章

在实现过程中，4*4矩阵计算参考了部分公式但是还是计算错误。后来看了Java的Matrix库以及以下链接的文章，才算出正确的结果。

• 旋转矩阵（Rotation Matrix)的推导 - 无名氏的文章 - 知乎
  https://zhuanlan.zhihu.com/p/433389563

绕X轴

绕Y轴

绕Z轴

代码实现

import xlwings as xw
import math
import numpy as np
 
def process_data(sheet, row_number):

    # 读取单元格数据
    pitch_angle = sheet.range(f"A{row_number}").value
    roll_angle = sheet.range(f"B{row_number}").value
    avr_angle = sheet.range(f"C{row_number}").value

        # 定位天线坐标
    posX = sheet.range(f"E{row_number}").value # E
    posY = sheet.range(f"D{row_number}").value # N
    posZ = sheet.range(f"F{row_number}").value

    # 需要计算的左点坐标
    relativePtX_left = sheet.range(f"H{row_number}").value # E
    relativePtY_left = sheet.range(f"G{row_number}").value # N
    relativePtZ_left = sheet.range(f"I{row_number}").value

    relativePtX_right = sheet.range(f"K{row_number}").value # E
    relativePtY_right = sheet.range(f"J{row_number}").value # N
    relativePtZ_right = sheet.range(f"L{row_number}").value

    
    # 将角度转换为弧度
    avr_angle = math.radians(-(avr_angle - 90))
    roll_angle = math.radians(roll_angle)
    pitch_angle = math.radians(pitch_angle)
   
    pt = np.zeros(3, dtype=float)

    worldPos_left = np.array([relativePtX_left - posX, relativePtY_left - posY, relativePtZ_left - posZ, 1], dtype=float)
    localPos_left = np.zeros(4, dtype=float)

    worldPos_right = np.array([relativePtX_right - posX, relativePtY_right - posY, relativePtZ_right - posZ, 1], dtype=float)
    localPos_right = np.zeros(4, dtype=float)

    mat = np.identity(4, dtype=np.float32)
    # print(worldPos)

    # 创建旋转矩阵
    rotation_matrix_yaw = np.array([
        [np.cos(avr_angle), np.sin(avr_angle), 0, 0],
        [-np.sin(avr_angle), np.cos(avr_angle), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ], dtype=np.float32)

    rotation_matrix_roll = np.array([
        [np.cos(roll_angle), 0, -np.sin(roll_angle), 0],
        [0, 1, 0, 0],
        [np.sin(roll_angle), 0, np.cos(roll_angle), 0],
        [0, 0, 0, 1]
    ], dtype=np.float32)

    rotation_matrix_pitch = np.array([
        [1, 0, 0, 0],
        [0, np.cos(pitch_angle), np.sin(pitch_angle), 0],
        [0, -np.sin(pitch_angle), np.cos(pitch_angle), 0],
        [0, 0, 0, 1]
    ], dtype=np.float32)

    # 将旋转矩阵应用到原始矩阵
    mat = np.dot(rotation_matrix_yaw, mat)
    mat = np.dot(rotation_matrix_roll, mat)
    mat = np.dot(rotation_matrix_pitch, mat)

    # 计算逆矩阵
    invert = np.linalg.inv(mat)
    # 计算本地坐标
    localPos_left = np.dot(worldPos_left, invert)
    localPos_right = np.dot(worldPos_right, invert)
    # 提取结果
    pt_left = localPos_left[:3]
    pt_right = localPos_right[:3]
    # 打印结果
    # print("pt_left:", pt_left)
    # print("pt_right:", pt_right)
    
    # 将结果写回Excel
    sheet.range(f"M{row_number}").value = pt_left[0]
    sheet.range(f"N{row_number}").value = pt_left[1]
    sheet.range(f"O{row_number}").value = pt_left[2]

    sheet.range(f"P{row_number}").value = pt_right[0]
    sheet.range(f"Q{row_number}").value = pt_right[1]
    sheet.range(f"R{row_number}").value = pt_right[2]

    sheet.range(f"T{row_number}").value = pt_left[0] + posX # E
    sheet.range(f"S{row_number}").value = pt_left[1] + posY # N
    sheet.range(f"U{row_number}").value = pt_left[2] + posZ

    sheet.range(f"W{row_number}").value = pt_right[0] + posX # E
    sheet.range(f"V{row_number}").value = pt_right[1] + posY # N
    sheet.range(f"X{row_number}").value = pt_right[2] + posZ


def main():
    wb = xw.Book.caller()
    sheet = wb.sheets[0]

    # 遍历处理多行数据，从第2行到最后一行
    current_row = 2
    while sheet.range(f"A{current_row}").value:
        process_data(sheet, current_row)
        current_row += 1

    # 保存并关闭工作簿
    wb.save()

if __name__ == "__main__":
    xw.Book("table.xlsm").set_mock_caller()
    main()