[发明专利]一种基于EKF的多传感器融合温室巡检机器人跟踪方法有效
申请号: | 202010274834.7 | 申请日: | 2020-04-09 |
公开(公告)号: | CN111536967B | 公开(公告)日: | 2022-12-16 |
发明(设计)人: | 潘逸轩;姚瑶;王增辉 | 申请(专利权)人: | 江苏大学 |
主分类号: | G01C21/16 | 分类号: | G01C21/16;G01C21/20;G06K9/62 |
代理公司: | 暂无信息 | 代理人: | 暂无信息 |
地址: | 212013 江*** | 国省代码: | 江苏;32 |
权利要求书: | 查看更多 | 说明书: | 查看更多 |
摘要: | |||
搜索关键词: | 一种 基于 ekf 传感器 融合 温室 巡检 机器人 跟踪 方法 | ||
1.一种基于EKF的多传感器融合温室巡检机器人跟踪方法,其特征包括如下步骤:
步骤1:基于UWB测距模块的基站到目标距离信息获取:基于DS-TWR测距技术实时获取已知坐标的三个基站到机器人的距离信息;
步骤2:目标机器人加速度,航向角信息获取:利用MPU6050传感器,经计算获取机器人的加速度和航向角信息;
步骤3:初始观测坐标获取:使用三边定位模型,基于最小二乘法利用三边定位模型从三个距离值得到机器人坐标初始观测值;
步骤4:坐标观测值修正:利用泰勒展开在观测点进行迭代修正,当修正值小于设定阈值或达到迭代次数时结束迭代;
步骤5:融合定位:将修正后的坐标观测值,实时测量的机器人加速度和航向角同时传入CTRV运动模型,利用扩展的卡尔曼滤波对传感器数据进行融合处理,得到目标机器人的最优估计坐标;
步骤4具体过程为:
设(x,y)为修正后的观测坐标,则(x,y)到3个基站的距离分别记作:
且
f1(x0,y0)=d1 (10)
f2(x0,y0)=d2 (11)
f3(x0,y0)=d3 (12)
将fn(x,y)在(x0,y0)处进行泰勒展开,将第一次使用Taylor算法优化得到的观测值令P1=[x1 y1]T,将由步骤3得到的初始观测坐标表示为P0=[x0 y0]T,设优化观测值与初始观测值之差为ΔP1-0=P1-P0=[Δx1-0 Δy1-0]T,用矩阵形式表达为:
记式(13)为:
F0·ΔP1-0=η1-0 (14)
则由最小二乘法解得修正值ΔP1-0为:
则这一轮泰勒展开计算后得到的新的观测坐标值为P1=P0+ΔP1-0,重复上述泰勒展开计算,迭代步骤可以表示为:
将上一次经Taylor算法优化后得到的值设为Pk-1=[xk-1 yk-1]T,作为下一次迭代的初始观测值,将下一次迭代后得到的优化观测值设为Pk=[xk yk]T,设优化观测值与初始观测值之差为ΔPk-(k-1)=Pk-Pk-1=[Δxk-(k-1) Δyk-(k-1)]T,将fn(x,y)在Pk-1处进行泰勒展开,用矩阵形式表达为:
将式(16)记作:
Fk-1·ΔPk-(k-1)=ηk-(k-1) (17)
则由最小二乘法解得修正值ΔPk-(k-1)为:
重复上述迭代步骤,直到ΔPk-(k-1)≤ε,其中ε为提前设定的经验阈值,满足此条件时认为算法收敛,得到优化后的观测坐标值Pk:
Pk=Pk-1+ΔPk-(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次的最终结果Xk=[xk yk vk θk ωk]T与根据第k次预测得到的第k+1次的预测值将看作是自变量为Xk的函数f(Xk);
当ω≠0时可以写成:
当ω=0时可以写成:
利用一阶泰勒展开对模型在Xk进行线性化:
f(X)=f(Xk)+(X-Xk)JA(Xk) (24)
式(24)中的JA(Xk)代表由f(Xk)的一阶偏导数组成的雅可比矩阵;
当ω≠0时,计算得到雅可比矩阵JA(Xk)为:
当ω≠0时,计算得到雅可比矩阵JA(Xk)为:
预测误差协方差表示为:
其中是由上一个状态的误差协方差矩阵Pk得到的表示预测误差的协方差矩阵,Q代表噪声的协方差矩阵,而噪声主要来自于直线加速度a和角加速度假定a和均满足0均值的高斯分布,方差分别为和a和对状态影响表达为W:
将式(28)表示为:
W=G·λ (29)
则Q为处理噪声的协方差矩阵:
Q=E[W·WT]=G·E[λ·λT]·GT (30)
其中,E[W·WT]代表对W·WT求数学期望,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可以叙述为:
根据上一次的状态Xk-1计算这一次的状态预测值
根据上一次的误差协方差矩阵Pk-1计算这一次的预测值与真实值之间的误差协方差矩阵将式(25)中的雅可比矩阵简写为JA,其转置矩阵为JAT,Q为式(30)中的处理噪声的协方差矩阵,则可以表示为:
计算这一次的卡尔曼增益Kk:
其中H是式(34)中将预测状态映射到测量模型空间的测量矩阵,R是式(35)中的测量噪声的协方差矩阵;
根据这一次的测量模型Zk=[xk yk θk]T更新状态,其中xk,yk为步骤4得到的这一次的坐标观测值,θk为步骤2得到的这一次的航向角,得到当前状态Xk:
更新误差协方差矩阵:
其中Pk为这一次的更新后的误差协方差矩阵。
2.根据权利要求1所述的一种基于EKF的多传感器融合温室巡检机器人跟踪方法,其特征在于,步骤3具体过程为:
已知固定的三个基站坐标分别为(x1,y1),(x2,y2),(x3,y3),设初始观测目标位置为(x0,y0),由步骤1中UWB测距模块测得三个基站到目标(x0,y0)的距离分别为d1,d2,d3,则由三边定位模型可知:
上述三式两两相减得到线性方程,写成矩阵形式为:
记作:
X·θ=Y (5)
由最小二乘法解得初始观测坐标:
其中XT为X的转置矩阵,(XTX)-1意为对X′和X矩阵相乘的结果进行求逆矩阵运算。
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于江苏大学,未经江苏大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/pat/books/202010274834.7/1.html,转载请声明来源钻瓜专利网。