1.一种室内机器人定位与建图方法,其特征在于,包括以下步骤:步骤1、基于环境信息进行粒子采样,获取k时刻每个粒子的位姿,并且根据粒子个数平均分配各粒子的权值;步骤2、将获取的k时刻下机器人与目标或障碍物之间距离和偏离角度作为k时刻的实际观测值,根据k时刻的实际观测值和每个粒子的位姿,对每个粒子的权值进行更新并归一化处理;步骤3、根据步骤2更新并归一化处理后每个粒子的权值,计算得到有效粒子数;判断有效粒子数小于设定的阈值时,对粒子进行重采样,并结合k时刻的实际观测值和重采样粒子的位姿,计算并更新重采样粒子的权值;及判断有效粒子数大于设定的阈值时,保持原采样粒子及每个粒子的权值;步骤4、根据步骤3所得重采样粒子权值或原采样粒子权值,利用加权公式计算出当前状态的期望值对k时刻的机器人的实际位姿进行更新;步骤5、根据步骤1所获取k时刻每个粒子的位姿和k时刻的实际观测值,利用卡尔曼滤波算法更新每个粒子地图的均值和方差,以得全部粒子的地图信息,具体为:根据更新所得重采样粒子权值或原采样粒子权值的大小对粒子进行排序,判断更新后各粒子组成的粒子集是否为空,如果判断为空则结束更新,反之依次取出粒子集中的粒子;判断所取出的粒子集中的每个粒子与上一个粒子是否相似,即判断两个粒子的位姿和权值是否在设定范围内保持一致,是则对上一个粒子更新后的权值进行复制操作,反之则重新利用卡尔曼滤波进行粒子地图的均值和方差更新,以得到全部粒子的地图信息;并由步骤4所更新k时刻机器人的实际位姿结合k时刻的实际观测值、机器人所在实际位姿对应粒子的地图信息,建立机器人实际地图。
2.根据权利要求1所述室内机器人定位与建图方法,其特征在于,所述步骤1还包括判断k时刻是否为初始时刻,是则生成初始时刻的粒子位姿,否则根据上一代粒子集中粒子的位姿结合运动模型得到k时刻每个粒子的预估位姿。
3.根据权利要求1所述室内机器人定位与建图方法,其特征在于,所述步骤2中对每个粒子的权值进行更新,采用公式:其中, 表示更新后的k时刻第i个粒子的权值; 表示在k-1时刻第i个粒子的权值;p表示k时刻的概率密度函数; 表示k时刻第i个粒子的实际观测值; 表示k时刻第i个粒子的位姿。
4.根据权利要求1所述室内机器人定位与建图方法,其特征在于,所述步骤2中对更新的每个粒子权值进行归一化处理,采用公式:其中, 表示归一化后的k时刻第i个粒子的权值; 表示更新后的k时刻第i个粒子的权值;N表示粒子个数。
5.根据权利要求1所述室内机器人定位与建图方法,其特征在于,所述步骤3计算有效粒子数N eff ,采用公式:其中, 表示归一化后的k时刻第i个粒子的权值;N表示粒子个数。
6.根据权利要求1所述室内机器人定位与建图方法,其特征在于,所述步骤5中利用卡尔曼滤波算法更新每个粒子地图的均值和方差,采用公式:其中, 表示第i个粒子在t时刻更新后的位姿;m为卡尔曼增益, 表示第i个粒子在t时刻的预估位姿,z t 代表t时刻的实际观测值; 表示第i个粒子在t时刻的预估观测值; 表示第i个粒子在t时刻预估位姿与实际观测值之间的协方差;I表示单位矩阵;H表示转换矩阵; 表示第i个粒子在t时刻预估位姿与实际观测值之间的预估协方差。
7.一种室内机器人定位与建图装置,其特征在于,包括:运动控制模块,用于获取机器人的速度信息,并结合机器人的速度信息获取k时刻每个粒子的位姿;激光雷达测距模块,用于获取k时刻下机器人与目标或障碍物之间距离和偏离角度并作为k时刻的实际观测值;定位与建图模块,用于根据粒子个数平均分配各粒子的权值;并根据k时刻的实际观测值和每个粒子的位姿,对分配的每个粒子权值进行更新并归一化处理后,计算得到有效粒子数;判断有效粒子数小于设定的阈值时,控制运动控制模块对粒子进行重采样,并计算和更新重采样粒子的权值;及判断有效粒子数大于设定的阈值时,保持原采样粒子及每个粒子权值;根据所得重采样粒子权值或原采样粒子权值,利用加权公式计算出当前状态的期望值对k时刻的机器人的实际位姿进行更新;根据所获取k时刻每个粒子的位姿和k时刻的实际观测值,利用卡尔曼滤波算法更新每个粒子地图的均值和方差,以得到 全部粒子的地图信息;并由所更新k时刻机器人的实际位姿结合k时刻的实际观测值、机器人所在实际位姿对应粒子的地图信息,建立机器人实际地图;其中,所述得到全部粒子的地图信息,具体为:根据更新所得重采样粒子权值或原采样粒子权值的大小对粒子进行排序,判断更新后各粒子组成的粒子集是否为空,如果判断为空则结束更新,反之依次取出粒子集中的粒子;判断所取出的粒子集中的每个粒子与上一个粒子是否相似,即判断两个粒子的位姿和权值是否在设定范围内保持一致,是则对上一个粒子更新后的权值进行复制操作,反之则重新利用卡尔曼滤波进行粒子地图的均值和方差更新,以得到全部粒子的地图信息。