[发明专利]一种室内三维定位方法及计算机可读存储介质

权利要求书

1.一种室内三维定位方法，其特征在于，包括如下步骤：

S1：获取基于超宽带的多个基站到被测物体的第一距离值，对所述第一距离的误差进行校正得到第二距离值；

S2：根据所述第二距离值采用Chan算法得到所述被测物体位置坐标的初始估计，将所述初始估计作为Taylor算法迭代的初始值并获取所述被测物体的位置坐标估计；

S3：根据所述位置坐标估计得到所述被测物体的定位轨迹曲线。

2.如权利要求1所述的室内三维定位方法，其特征在于，对所述第一距离值的误差进行校正得到所述第二距离值包括如下步骤：

S11：获取多个所述基站到所述被测物体的真实值d₁,d₂,…d_n和对应的所述基站到所述被测物体测量平均值得到测距误差其中，n是正整数；

S12：利用最小二乘法求得因变量为所述测距误差e_i与自变量为所述真实值d_i的拟合多项式，即误差拟合函数：

f(d_i)＝w₀+w₁d_i+w₂d_i²+…

其中，W＝(w₀,w₁,…)是多项式的系数；

S13：根据所述误差拟合函数对所述第一距离值d进行校正得到所述第二距离值

3.如权利要求2所述的室内三维定位方法，其特征在于，所述误差拟合函数采用经验误差拟合公式：

4.如权利要求2或3任一所述的室内三维定位方法，其特征在于，将所述第二距离值带入Chan算法得到所述被测物体位置坐标的所述初始估计，具体包括：

S21：将初始的基于飞行时间的非线性二次方程转化为线性方程组：ψ＝H-G_aZ_a，其中ψ表示误差，Z_a是未知量向量，G_a是系数，是第二距离值，k_i是基站坐标平方和；

S22：利用加权最小二乘法解出算法的初始解其中

S23：使用所得初始解附加距离和误差的约束条件

其中，R₀是附加距离参数，e₁,e₂,e₃,e₄分别为x,y,z,R₀与所述被测物体真实位置的误差；构造方程组ψ'＝H'-G_a'Z_p，其中，Z_p是所述被测物体真实位置坐标的平方，ψ'表示误差，G_a'和H'是系数；

S24：使用加权最小二乘法解出定位结果：

根据先验信息选择更优的解，所述更优的解为所述被测物体位置坐标的初始估计

5.如权利要求4所述的室内三维定位方法，其特征在于，将所述初始估计作为Taylor算法的初始值并使用残值加权函数构造误差的协方差矩阵迭代运算得到所述被测物体的所述位置坐标估计，具体包括：

S25：将所述第二距离值在所述初始估计处泰勒展开并忽略二次以上的项:

S26：利用偏导数的值得测量误差ψ＝h_t-G_tδ，由加权最小二乘法得到误差：

δ＝(G_t^TQ^-1G_t)^-1G_t^TQ^-1h_t；

其中，δ＝(δ_x,δ_y,δ_z)；

S27：判断残差与给定门限值得关系，若小于门限值，则此时所得就为所述被测物体的位置坐标估计(x,y,z)；否则令重新进行S25、S26。