[发明专利]基于校准源的运动固态体定位方法

权利要求书

1.一种基于校准源的运动固态体定位方法，其特征在于，包括如下步骤：

(1)构建运动固态体的定位场景并初始化参数：

构建包括安装有N个传感器的运动固态体、分布在运动固态体周围处于静止状态的M个锚节点和一个校准源的运动固态体的三维定位场景；初始化所述N个传感器在以运动固态体几何中心为原点的局部参考系Oq₁q₂q₃下的位置坐标为c＝[c₁,c₂,…,c_n,…,c_N]^T；所述M个锚节点、校准源在全球参考系Oxyz下的位置坐标分别为a＝[a₁,a₂,…,a_m,…,a_M]^T、p＝[p_x,p_y,p_z]^T，a、p对应的位置误差分别为Δa＝[Δa₁,Δa₂,…,Δa_m,…,Δa_M]^T、Δp＝[Δp_x,Δp_y,Δp_z]^T，Δa、Δp的标准差分别为σ_a、σ_p；所述运动固态体在全球参考系下含有误差的方向信息、位置信息、角速度信息、速度信息分别为q＝[α,β,γ]^T、t＝[t_x,t_y,t_z]^T、w＝[w_x,w_y,w_z]^T、其中，N≥5，M≥6，c_n表示第n个传感器在局部参考系下的位置坐标，a_m表示第m个锚节点在全球参考系下位置坐标，a_m＝[a_mx,a_my,a_mz],Δa_m表示a_m对应的位置误差，Δa_m＝[Δa_mx,Δa_my,Δa_mz]，α、β、γ表示所述固态体在全球参考系中的偏航角、翻滚角、俯仰角，·^T表示转置操作；

(2)构建测量向量：

通过每个传感器在局部参考系下的位置坐标c_n、全球参考系下每个锚节点的位置坐标a_m和运动固态体含有误差的方向信息q、位置信息t、角速度信息w和速度信息计算每个传感器和每个锚节点之间的距离测量值r_mn及多普勒测量值得到距离测量向量r＝[r₁₁,r₂₁,…,r_mn,…,r_MN]^T和多普勒测量向量同时通过在全球坐标系下每个锚节点的位置信息a_m和校准源的位置信息p，计算每个锚节点与校准源之间的距离测量值d_m，得到距离测量向量d＝[d₁,d₂,...,d_m,...,d_M]^T，并通过r、d以及步骤(1)中校准源的位置信息p和M个锚节点的位置坐标a，构建测量向量

(3)构造估计向量并获取估计向量的最大似然解：

(3a)通过全球参考系下固态体的方向估计量、位置估计量、角速度估计量、速度估计量q^o、t^o、w^o、以及M个锚节点位置坐标a的估计量a^o和校准源位置坐标p的估计量p^o，构造估计向量同时通过M个锚节点与N个传感器之间距离测量向量r、多普勒测量向量的误差协方差矩阵R_v、M个锚节点位置误差Δa的协方差矩阵R_a、校准源位置误差Δp的协方差矩阵R_p、M个锚节点与校准源之间的距离测量误差协方差矩阵R_d，构建测量误差矩阵其中，Bdiag[]表示分块对角矩阵；

(3b)通过测量向量ζ、估计向量ψ^o及测量误差矩阵R，构建最小二乘模型ψ：

ψ＝arg_ψmin(ζ-ζ(ψ^o))^TR^-1(ζ-ζ(ψ^o))

其中，ζ(ψ^o)表示包含估计向量ψ^o的测量向量，R^-1表示测量误差矩阵R的逆矩阵，arg_ψmin(*)表示(*)取得最小值时ψ的值；

(3c)基于高斯-牛顿迭代法则，对最小二乘模型ψ中的测量向量ζ(ψ^o)进行k_max次更新，实现对ψ的迭代优化，得到估计向量ψ^o的最大似然解

其中，k_max≥100；

(4)获取运动固态体的定位结果：

通过最大似然解计算全球参考系下运动固态体无误差的方向信息位置信息角速度信息速度信息