321型正交位移测量系统的正解（已知位姿求位移传感器位移）

function H=ODMS_zhengjie(Q,L,T_B_E0,T_P_E)
% Q为RPY角位姿表达式，6*1向量
% L为六根直线表达式，6*6矩阵
% T_B_E0及T_P_E为齐次坐标变换矩阵，4*4矩阵
M0=L_for_M(T_B_E0,L);%求零位触点
T_B_P=zhengjie_for_T(Q);%求齐次位姿
T_B_E=T_B_P*T_P_E;%求被测基准块在定系中的位姿
M1=L_for_M(T_B_E,L);%求非零位触点
H=zhengjie_for_H(M0,M1);%程序输出