[发明专利]基于改进粒子滤波算法估算机器人方位的方法和装置

权利要求书

1.基于改进粒子滤波算法估算机器人方位的方法，其特征在于包括：

S1:根据系统动力学特性建立移动机器人的运动模型；

S2:通过传感器采集移动机器人的方位信息数据，采用改进粒子滤波算法处理系统中的显著误差，对其进行相应的补偿操作，得到精确估计移动机器人的方位状态参数；

采用改进粒子滤波算法处理系统中的显著误差方法为先对采集的方位信息数据进行检测，判断是否存在显著误差，如果不存在，就迭代进行下一次滤波估计；如果存在，就进行显著误差识别判别属于哪一类显著误差，判别出来之后，根据所属的显著误差类型，对其进行相应的补偿操作；

所述步骤S1具体为：

设定移动机器人方位估计参数，建立非线性方位估计系统的数学模型，选取移动机器人的方位变量时，选取线速度v和转向角速度w来作为测量数据，得到它的位置和姿态：x，y和θ，其非线性描述表述如下：

根据移动机器人的动力学模型，建立其在全局坐标系下的数学模型，根据数学模型，建立粒子滤波算法的状态空间模型，其表述如下所示：

其中状态空间模型包括6个状态变量，分别为：x，y，θ，v_x，v_y，v_θ，这里的v_x和v_y分别表示x轴和y轴的线速度，v_θ表示转向角速度，即v_θ＝w；

所述步骤S2具体为：

1)首先根据粒子滤波的原理方法，从先验概率p(x_k|x_k-1)中得到一组随机样本，称为粒子i表示第i个粒子，并设初始粒子的权值为

2)在预测阶段，利用k-1时刻的这些粒子根据状态转移方程计算k时刻的先验样本集：

3)在更新阶段，根据测量数据y_k和先验样本计算每个粒子的权重其中是似然概率，之后将权重进行归一化处理，以便于统一样本的分布特性，更新之后的后验分布：

4)通过定义一个重要性密度q(x_k|y_1：k)，则联合权重表示为：

5)利用状态转移概率函数为建议分布，归一化权重：

6)引入重采样，将权重大的亲代粒子按照权重大小进行复制作为子代粒子，将权重小的亲代粒子舍弃，设定有效粒子数Neff来衡量粒子权值的退化程度：通过重采样以后，子代粒子的后验估计表示为：

7)计算状态估计向量和校正的测量值

8)对校正的测量值进行显著误差检测的方法是计算其残差大小，残差的表示方法是：

9)在检测出显著误差并确定其大小之后应该消除显著误差，即实现对测量值的补偿，补偿后的测量值y′_k表示为：

y′_k＝y_k-C_m，k

更新相应的权重如下所示：

将更新后的权重值用于粒子滤波的重采样阶段，并推导得到状态变量估计值和校正后的测量值利用校正测量值通过更新得到测量残差信息，由于C_m，k是由测量残差时间序列信息估计得到，因此更新后的测量残差用于改善后续量的测量补偿。

2.根据权利要求1所述的基于改进粒子滤波算法估算机器人方位的方法，其特征在于：将补偿后的方位信息数据作为下一次滤波的初始数据，进行下一次状态估计。

3.根据权利要求1所述的基于改进粒子滤波算法估算机器人方位的方法，其特征在于，所述的显著误差设定为异常值、静差和漂移三种类型，其中异常值表现为在测量数据中出现若干突发峰值；静差是指完成过渡过程之后的残余偏差，也就是被控变量的稳定值与给定值之差，其值可正可负，静差要求限制在给定值附近的某一允许的小范围内，其表现为在测量器件上产生了持续且相对稳定的错误值；漂移反映了在规定的条件下，测量仪器量测特性在一段时间内的连续或增量变化。

4.基于改进粒子滤波算法估算机器人方位的装置，其特征在于该装置采用如权利要求1-3任一项所述的方法进行估算移动机器人精确方位，包括方位信号收集模块、信号处理模块以及上位机，其中通过方位信号收集模块对移动机器人的方位信息进行采集，并输入到信号处理模块进行改进粒子滤波算法处理，对其存在的显著误差进行识别类型，判别出来之后，该显著误差设定为异常值、静差和漂移三种类型，根据所属的显著误差类型对其进行相应的补偿操作，得到精确估计移动机器人的方位状态参数。