[发明专利]一种基于EKF的多传感器融合温室巡检机器人跟踪方法

权利要求书

1.一种基于EKF的多传感器融合温室巡检机器人跟踪方法，其特征包括如下步骤：

步骤1：基于UWB测距模块的基站到目标距离信息获取：基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息；

步骤2：目标机器人加速度，航向角信息获取：利用MPU6050传感器，经计算获取机器人的加速度和航向角信息；

步骤3：初始观测坐标获取：使用三边定位模型，基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值；

步骤4：坐标观测值修正：利用泰勒展开在观测点进行迭代修正，当修正值小于设定阈值或达到迭代次数时结束迭代；

步骤5：融合定位：将修正后的坐标观测值，实时测量的机器人加速度和航向角同时传入CTRV运动模型，利用扩展的卡尔曼滤波对传感器数据进行融合处理，得到目标机器人的最优估计坐标；

步骤4具体过程为：

设(x,y)为修正后的观测坐标，则(x,y)到3个基站的距离分别记作：

且

f₁(x₀,y₀)＝d₁ (10)

f₂(x₀,y₀)＝d₂ (11)

f₃(x₀,y₀)＝d₃ (12)

将f_n(x,y)在(x₀,y₀)处进行泰勒展开，将第一次使用Taylor算法优化得到的观测值令P₁＝[x¹ y¹]^T,将由步骤3得到的初始观测坐标表示为P₀＝[x₀ y₀]^T，设优化观测值与初始观测值之差为ΔP_1-0＝P₁-P₀＝[Δx_1-0 Δy_1-0]^T,用矩阵形式表达为：

记式(13)为：

F₀·ΔP_1-0＝η_1-0 (14)

则由最小二乘法解得修正值ΔP_1-0为：

则这一轮泰勒展开计算后得到的新的观测坐标值为P₁＝P₀+ΔP_1-0，重复上述泰勒展开计算，迭代步骤可以表示为：

将上一次经Taylor算法优化后得到的值设为P_k-1＝[x^k-1 y^k-1]^T，作为下一次迭代的初始观测值，将下一次迭代后得到的优化观测值设为P_k＝[x^k y^k]^T，设优化观测值与初始观测值之差为ΔP_k-(k-1)＝P_k-P_k-1＝[Δx_k-(k-1) Δy_k-(k-1)]^T，将f_n(x,y)在P_k-1处进行泰勒展开，用矩阵形式表达为：

将式(16)记作：

F_k-1·ΔP_k-(k-1)＝η_k-(k-1) (17)

则由最小二乘法解得修正值ΔP_k-(k-1)为：

重复上述迭代步骤，直到ΔP_k-(k-1)≤ε，其中ε为提前设定的经验阈值，满足此条件时认为算法收敛，得到优化后的观测坐标值P_k：

P_k＝P_k-1+ΔP_k-(k-1) (19)；

步骤5具体过程为：

将步骤4得到的修正后的坐标观测值(x,y)和MPU6050传感器测得的航向角θ传入恒定转率和速度模型(CTRV运动模型)，可以将目标机器人的状态表示为：

X＝[x y v θ ω]^T (20)

其中，x代表目标机器人横坐标，y代表纵坐标，v代表运动速度，θ代表航向角，ω代表角速度，在CTRV运动模型中，目标的运动速度v和角速度ω被假设为恒定值，而模型的噪声主要来自于直线加速度a和角加速度由于扩展的卡尔曼滤波的工作是由第k次的结果预测第k+1次的结果，结合第k+1次的测量数据对预测结果进行修正，得到第k+1次的最终结果，CTRV运动模型的状态转移函数为：

上式给出了第k次的最终结果X_k＝[x_k y_k v_k θ_k ω_k]^T与根据第k次预测得到的第k+1次的预测值将看作是自变量为X_k的函数f(X_k)；

当ω≠0时可以写成：

当ω＝0时可以写成：

利用一阶泰勒展开对模型在X_k进行线性化：

f(X)＝f(X_k)+(X-X_k)J_A(X_k) (24)

式(24)中的J_A(X_k)代表由f(X_k)的一阶偏导数组成的雅可比矩阵；

当ω≠0时，计算得到雅可比矩阵J_A(X_k)为：

当ω≠0时，计算得到雅可比矩阵J_A(X_k)为：

预测误差协方差表示为：

其中是由上一个状态的误差协方差矩阵P_k得到的表示预测误差的协方差矩阵，Q代表噪声的协方差矩阵，而噪声主要来自于直线加速度a和角加速度假定a和均满足0均值的高斯分布，方差分别为和a和对状态影响表达为W：

将式(28)表示为：

W＝G·λ (29)

则Q为处理噪声的协方差矩阵：

Q＝E[W·W^T]＝G·E[λ·λ^T]·G^T (30)

其中，E[W·W^T]代表对W·W^T求数学期望，E[λ·λ^T]则是对λ·λ^T求数学期望；

由于a和均满足0均值的高斯分布，所以E[λ·λ^T]可以表示为：

此处假设为定值0.3而通过MPU6050传回的加速度值得到，处理噪声的协方差矩阵Q的计算公式为：

由步骤2和步骤4获取的线性测量模型为：

Z＝[x y θ]^T (33)

将预测状态映射到测量模型空间的测量矩阵为：

测量噪声的协方差矩阵R为:

式(35)中的分别表示测量模型Z＝[x y θ]^T中x,y,θ测量值的方差，表示测量噪声，此值根据经验设置为定值；

整个步骤5可以叙述为：

根据上一次的状态X_k-1计算这一次的状态预测值

根据上一次的误差协方差矩阵P_k-1计算这一次的预测值与真实值之间的误差协方差矩阵将式(25)中的雅可比矩阵简写为J_A，其转置矩阵为J_A^T，Q为式(30)中的处理噪声的协方差矩阵，则可以表示为：

计算这一次的卡尔曼增益K_k：

其中H是式(34)中将预测状态映射到测量模型空间的测量矩阵，R是式(35)中的测量噪声的协方差矩阵；

根据这一次的测量模型Z_k＝[x_k y_k θ_k]^T更新状态，其中x_k，y_k为步骤4得到的这一次的坐标观测值，θ_k为步骤2得到的这一次的航向角，得到当前状态X_k：

更新误差协方差矩阵：

其中P_k为这一次的更新后的误差协方差矩阵。

2.根据权利要求1所述的一种基于EKF的多传感器融合温室巡检机器人跟踪方法，其特征在于，步骤3具体过程为：

已知固定的三个基站坐标分别为(x₁,y₁),(x₂,y₂),(x₃,y₃),设初始观测目标位置为(x₀,y₀),由步骤1中UWB测距模块测得三个基站到目标(x₀,y₀)的距离分别为d₁,d₂,d₃,则由三边定位模型可知：

上述三式两两相减得到线性方程，写成矩阵形式为：

记作：

X·θ＝Y (5)

由最小二乘法解得初始观测坐标：

其中X^T为X的转置矩阵，(X^TX)^-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。