[发明专利]非视距环境下基于时域测量的固态体定位方法

权利要求书

1.一种非视距环境下基于时域测量的固态体定位方法，其特征在于由下述步骤组成：

(1)提取时域测量信息

根据下式非视距传输环境下的测量模型，提取固态体节点向锚节点发射信号的时域测量信息r_mi：

r_mi＝||s_i-a_m||+v_mi+b_mi  (1)

其中s_i表示全局参考系中第i个固态体节点的位置坐标，为[x_i,y_i,z_i]^T,i取值为1,2,…,N；a_m表示第m个布设在周围环境中锚节点的位置坐标，为[x_m,y_m,z_m]^T，m取值为1,2,…,M；N为固态体节点的个数，M为锚节点的个数，M,N为有限的正整数，且M≥4,N≥3；||*||为欧几里得范数；v_mi为测量误差，其服从均值为0、方差为的高斯分布，b_mi为非视距偏差，非视距偏差存在确定的边界值，即0≤b_mi≤b_max，b_mi为0～b_max之间任意取值；

(2)确定修正后时域测量信息

按下式确定修正后的时域测量信息

其中取值为r_mi-b_max/2，是修正后的时域非视距偏差，取值为b_mi-b_max/2；

(3)确定全局参考系中固态体节点位置

按下式得到广义信赖域子问题：

其中，||W_i(Ay_i-h_i)||²为最小化的目标函数，y_i^TDy_i+2g^Ty_i＝0为约束条件，y_i为含有全局参考系中第i个固态体节点位置的辅助变量，加权矩阵W_i表示为：

广义信赖域子问题中的矩阵A、h_i分别为：

约束条件中的矩阵D、g分别为：

D＝diag([1_1×K,0])

其中1_1×K表示1×K的全1矩阵，0_1×K表示1×K的全零矩阵，K表示维度，K取值为3；采用二分法得y_i，全局参考系中固态体节点位置s_i由下式确定：

s_i＝y_i(1:3)  (4)

(4)确定固态体位姿

1)全局参考系中固态体节点位置s_i在局部参考系和全局参考系的位置关系由下式表示：

s_i＝Qc_i+t  (5)

其中c_i为局部参考系中固态体节点位置，Q为固态体旋转矩阵，t为固态体位移矢量；

2)按下式确定固态体方向参数α^o、β^o、γ^o

采用最小二乘法确定固态体旋转矩阵Q：

Q＝Vdiag([1^T,det(VU^T)]^T)U^T  (7)

其中，SVD表示特征值分解，为全局参考系中第i个固态体节点位置坐标与的差值，为局部参考系中第i个固态体节点位置坐标与的差值，为全局参考系中N个固态体节点位置坐标的加权平均，为局部参考系中N个固态体节点位置坐标的加权平均；R_i为s_i的协方差矩阵，I为单位矩阵，q_i为R_i的逆矩阵与I的逆矩阵的乘积，由公式(6)确定固态体的偏航角为α^o、滚转角为β^o、俯仰角为γ^o；

3)按下式确定固态体位移矢量t：

确定了在非视距环境下的固态体位姿。