本帖最后由 wy2589 于 2018-11-27 15:26 编辑
5 卡尔曼滤波应用 在机器人基于测距仪的跟踪中，激光测距仪的采样周期为 100ms，假设在 k 时刻获取了实测值并结合 k－1时刻的距离预测值（方程 1），则可以根据方程 4 对 k 时刻的测量值进行滤波估计修正。修正之后就可以对k+1 时刻的距离进行预测，相当于提前了一个采样周期。因而可以提前进行运动控制。
以机器人上的激光测距仪检测跟踪动态目标为例，说明卡尔曼滤波在滤波和预测上的应用。
考虑机器人的运动状态，可以分三种情况分别进行机器人坐标系下的障碍目标状态估计： 1 、机器人静止而目标运动； 2 、机器人运动而目标静止，相当于机器人对静止的环境或障碍、目标进行相对位置的估计，有对机器人定位修正的作用。（因为机器人运动时通过里程计等确定的相对运动位置会有误差，需要校订；另外相邻两个运动周期和激光采样周期内机器人与目标的相对位置只通过激光测距仪来确定存在误差，结合里程计对机器人位置的计算可以通过卡尔曼滤波更加准确估计二者的相对位置。） 3 、当机器人和目标都处于运动状态时，预测运动目标相对于机器人的相对运动，从而进行机器人控制。当然第三种情况可以包含前两种，如果能够通过在第三种情况的算法中改变不同的参数得到前两种情况的计算，则就可以用一套方法对动态环境中的动、静目标进行相对运动预测了。
目前认为机器人是静止的，机器人的传感器坐标采用极坐标系。 设系统的状态向量
观测系统的状态方程为（没有输入量，所以 B 为 0）：X (k) = AX (k −1) +W (k −1) 。 由于激光测距仪的采样周期 t 为 100ms，而机器人跟踪的目标的运动速度比较平缓，所以在局部时间段内设定目标为匀速运动，所以
t 为采样周期。 而沿 x 方向的运动速度是变化的，可以取前一个采样周期内速度的均值（或者前两个周期内的速度均值）。
式中：
W(k −1)为过程噪声，是均值为零的高斯白噪声，协方差为 Q(k-1)。 如果过程噪声W (k −1)用 T(k-1)v(k-1)来代替，T(k)为过程噪声的分布矩阵，v(k)为各状态变量对应的误差向量，由于各状态变量的误差符合 0 均值的高斯分布（0 均值的正态分布），即符合
则 Q(k)变为
测量方程为： Z(k) = CX (k) +V (k) ，式中：
ρ 和θ 分别为极坐标下激光雷达的目标径向距离和方位角的测量数据。
在上面的过程方程中，以前后两个采样周期的位置差来求速度，进行考虑目标加速运行的状态存在一定的不足。因为每次的估计的位置仍然存在一定的误差，那么两次的位置估计值相减，有可能减小总体的误差，也可能放大总体误差（即相减之后的误差），所以可能会造成速度的振荡变化，即使目标匀速运动也可能使滤波出现跟踪中的波动，降低跟踪及预测效果。
| 
投票
