⛄一、卡尔曼滤波路径追踪优化简介
割草机器人通过比对当前t时刻位置、导航方程之间偏移角度θ和偏移距离d,确定t+1时刻的运动方向属于递推型路径追踪。割草机器人工作过程中受到地面起伏等环境因素影响,在采用上述追踪方法时会和预测值产生偏差,造成机器人偏离导航方程,称之为系统预测误差。采用图像处理规划导航路径时,同样也会产生偏差,称为检测误差。由于地形环境因素和割草边界线图像检测误差均为偶然性误差,符合高斯白噪声,因此采用卡尔曼滤波的方法对割草机器人路径追踪进行优化。其核心思想为:比对预测值和检测值协方差,进而采用递推的方式计算出下一时刻系统路径最优解。
由于机器人在运动过程中受地面起伏影响,因此噪声矩阵Wt均值为0,协方差为Q。
机器人检测只关心当前位置,进而和导航方程进行比对,因此系统检测模型为
结合式(11)、式(12)构建卡尔曼滤波系统预测方程为
预测值与检测值协方差为