[发明专利]一种基于旋转矩阵的因子图协同定位方法

权利要求书

1.一种基于旋转矩阵的因子图协同定位方法，其特征在于：所述方法包括以下步骤：

步骤1：构建基于旋转矩阵的协同定位方法因子图模型，在因子图模型中加入旋转矩阵因子节点；

在步骤1中，主从艇的距离信息通过节点F_i进入因子图模型中，从艇位置的先验估计通过节点A和B进入因子图模型中，然后通过节点T_i对从艇的位置进行旋转变换，通过C_i和D_i节点将主从艇的绝对位置信息转换为相对位置信息，最后通过节点E_i将主从艇的位置信息进行融合；

在协同定位方法因子图模型中，因子节点E_n的作用是将x组坐标系与y组坐标系结合起来，变量节点Δx_n与Δy_n之间的约束满足：

从因子节点E_n传递到变量节点Δy_n的概率密度函数表示为：

式中，表示上一次迭代过程中的主艇与从艇的x坐标差；

表示对应的方差；

从因子节点E_n传递到变量节点Δx_n的概率密度函数表示为：

式中，表示上一次迭代过程中的主艇与从艇的y坐标差；

表示对应的方差；

根据从因子节点传递到变量节点Δy_n的概率密度函数，可以进一步表示出Δy_n的期望E(Δy_n)为：

设函数将其进行一阶泰勒展开得到：

则有：

其中，x₀为初始时刻的x坐标，表示从艇在原坐标系x轴的误差；

由于当主艇与从艇的y轴坐标趋于相同时，那么有根据上式，此时Δx_n的微小误差将会引起E(Δy_n)的巨大误差，这样的结果进一步使得以状态变量的期望、方差作为信度信息的因子图协同定位方法在实际运行过程中当出现主从艇y坐标相近或相同时协同定位误差突然增大的现象；

当主艇与从艇的x轴坐标趋于相同或相近时，与上述情况相同，将会引起E(Δx_n)的巨大误差，导致定位误差变大；

步骤2：建立基于旋转矩阵的协同定位方法，构造旋转矩阵以及设计旋转角度，并对协同定位系统的位置状态信息进行滤波更新；

引入的旋转矩阵的形式如下：

θ为所构造的旋转矩阵的旋转角，值大小的确定方法如下：

假设某主艇的坐标为(x_m,y_m)，从艇的坐标为(x_s,y_s)，经过转换矩阵C将坐标转换为(x′_m,y′_m)和(x′_s,y′_s)，转换过程如下：

当估计的从艇的坐标中带有误差时，会有下式：

式中，表示从艇在原坐标系x轴的误差；

表示从艇在原坐标系y轴的误差；

表示从艇经过转换矩阵之后在x轴的误差；

表示从艇经过转换矩阵之后在y轴的误差；

进一步求得误差项为：

结合f(x)一阶泰勒展开式可得：

为主、从艇坐标经过转换矩阵之后在x轴方向相对距离的误差；为主、从艇坐标经过转换矩阵之后在y轴方向相对距离的误差；为了不出现误差增大的情况，需要满足下式：

由公式的约束关系将上式进一步简化为：

综合上式可得到θ应满足的条件式如下：

考虑到实际情况，显然有θ₁≠θ₂，那么选择θ为：θ＝(θ₁+θ₂)/2，此时，θ恒满足以上条件；

步骤3：通过传递的概率密度函数对协同定位系统的位置状态进行滤波融合估计；

在常规的因子图协同定位方法的基础上添加旋转矩阵因子节点T_i与变换后的坐标变量节点x_i，从因子节点T_i传递到变量节点x_i的概率密度函数表示为：

从因子节点T_i传递到变量节点y_i的概率密度函数表示为：

式中，表示变量节点x传递给因子节点T_i的方差；

表示变量节点y传递给因子节点T_i的方差。