专利名称：一种基于卡尔曼滤波器预测的机器人避障方法
技术领域：
本发明涉及一种基于卡尔曼滤波器预测的机器人避障方法。
背景技术：
随着人工智能、电子通讯技术的不断发展，机器人除了已进入无人驾驶飞行器和 无人驾驶水下机器人等军事领域外，也逐渐进入天气预报、救生任务侦察，搜索及森林野火 等环境监测诸多民用生活领域。在复杂的应用环境中，机器人行进是动态变化和不确定的， 因此，当它执行一个计划任务时，一旦确定预定任务的环境发生重大变化，它应该修改任务 目标，并重新实时规划任务，以适应环境的变化。避障是机器人路径规划中的难点之一，其 任务是在具有障碍物的环境中，按照一定的评价标准，寻找一条从起始状态到达目标状态 的无碰撞路径。能够在具有静态和动态障碍物的复杂环境中完成局部在线避碰是机器人自 主性的重要体现。常用的机器人路径规划方法可以分为环境信息完全知道的全局路径规划法和环 境信息完全未知或部分未知的局部路径规划法。Voronoi图法对环境约束条件进行表达， 是一种常用、有效的全局路径规划方法。然而机器人对于环境信息，特别是动态障碍物的信 息往往很难具有先验知识，同时移动机器人只能执行静态环境中的地图创建，不能在复杂 环境中完成在线避碰，使Voronoi图法这种全局路径规划法的使用受到限制。
发明内容
本发明的目的是提供一种基于卡尔曼滤波器预测的机器人避障方法，保障机器人 在复杂、不确定环境中实时实施避障，直至机器人完成既定任务。本发明所采用的技术方案是
一种基于卡尔曼滤波器预测的机器人避障方法，其特征在于，包含以下步骤 步骤一设定初始的机器人任务环境的数字地图，并传送到机器人路径规划器； 步骤二 传感器系统对机器人所处状态的测量信号作为机器人路径规划器的输入信
号；
步骤三机器人的路径规划器根据目前掌握的数字地图、机器人所处状态、任务集及约 束条件，对任务集进行排序，规划出行进无碰撞路径，并产生一系列机器人伺服系统的行进 点序列信号；
步骤四基于伺服系统的输出信号，调整传感器系统对环境和机器人自身运动状态进 行监测并采集相应实时数据；
步骤五路径规划器根据传感器系统的反馈，进行状态判断；若自身运动状态与上次 路径规划器目标点位置坐标相同，规划结束；否则，进行步骤六的内容；
步骤六判定有无新障碍物出现，若无新障碍物出现，继续执行步骤四中伺服系统的行 进动作；否则，进行步骤七的内容；
步骤七建立新障碍物卡尔曼滤波器模型，根据传感器系统的观测数据，对模型参数进行辨识和修正；
步骤八根据辨识的模型、机器人位置和周围环境之间关系的预测，检查机器人行进过 程中是否会发生碰撞，如预测到不会发生碰撞，继续执行步骤四中伺服系统的行进动作；否 则，进行步骤一的内容，更新数字地图，供路径规划器进行新一轮的路径局部重规划。步骤三中，机器人对任务集进行排序时，根据已知给定机器人出发点、目标点及部 分已知障碍物的初始位置，建立Voronoi图状态空间，根据Di jkstra算法或DoubleSwe印 算法，可沿Voronoi图顶点搜到一条从机器人出发点至目标点的一条无碰撞路径，并产生 一系列机器人伺服系统行进点序列信号。步骤七中，根据传感器系统2的观测数据，对新障碍物的卡尔曼滤波器模型参数 进行辨识和修正时，采用线性动态系统期望最大化(EM)模型辨识算法，对目标系统用动态 系统方程和测量方程进行表征如下
r x r = A x xf + v,
u }}t 二 C x x, -f- w,
其中乂时刻状态变量巧ef，观测变量，系统噪声V广所'0，6>,测量噪声
补,t N(0，R') ；^是动态模型传递函数义是测量模型传递函数…和分别是系统噪声方差
和测量噪声方差；根据观测数据可用已有的线性动态系统期望最大化(EM)模型辨识算法 迭代算出A、C、Q和R等模型及参数，完成模型辨识。所述的卡尔曼滤波模型，是由测量方程所得到的观测信息求解系统状态的最优估 计，整个滤波过程包括预测过程和更新过程；所述的预测过程负责推算当前状态变量和误 差协方差估计值，以便为下一个时间状态构造先验估计；所述的更新过程负责反馈，它将先 验估计和新的测量变量结合以构造改进的后验估计，其预测和更新方程表征如下
单步预测彳\ ,,本发明具有以下优点
本发明所涉及的机器人避障方法在Voronoi图法基础上加进了对机器人位置和周围 环境之间的关系的预测，产生对周围环境中的机动目标预测沿某方向前进信息的先验知 识，因而使机器人可以在不确定环境中动态生成局部避障路径，实现简单，实时性较好。
图1为基于卡尔曼滤波器预测的机器人避障方法原理图。图2为机器人初始路径与机动目标卡尔曼滤波器模型状态预测轨迹图。图3为根据机动目标卡尔曼滤波器模型状态预测进行的机器人避障局部重规划 路径图。
具体实施例方式为了在复杂环境中实现有效地避障，必须通过传感器等来收集周边环境的信息数 据，根据机动障碍物的运动规律有效地进行其位置状态预知势在必行，而动态环境中路径 规划法的核心在于确定目标点和执行任务区域中的障碍物状态。本发明的避障方法对传统 Voronoi图法进行了完善，利用对机动障碍物的运动观测，根据辨识的卡尔曼滤波器模型 对机动障碍物的运动规律进行有效的预测，一旦环境发生重大变化，机器人可以修改任务 目标，并重新实时规划任务，以适应环境的变化，经过局部多次重规划来得到机器人可行的 安全路径并实施避障策略。本发明采用“感知_规划_执行”的闭环框架，提出一种基于卡尔曼滤波器预测的 机器人避障方法。机器人的路径规划器3根据目前掌握的数字地图1、任务集、传感器系统 2所得机器人自身所处状态及约束条件，对任务集进行排序，并产生一系列机器人行进点序 列，由控制器控制伺服系统4执行。当机器人传感器系统2探测到有新障碍物出现，根据观 测数据建立卡尔曼滤波器模型，并利用观测数据和经典的线性动态系统期望最大化(EM)模 型辨识算法对模型参数进行辨识和修正。根据卡尔曼滤波模型辨识5，对机器人位置和周围 环境之间的关系进行状态预测，检查机器人行进过程中是否会发生碰撞。如预测到会发生 碰撞，机器人便及时用预测到的对周围环境中障碍物的位置信息更新数字地图1，供路径规 划器3进行新一轮的局部重规划。“感知-规划-执行”的优化过程周而复始，直至机器人 完成既定任务。本发明所述的避障方法所涉及的避障系统，包含有传感器系统、路径规划器、伺服 系统。其中，传感器系统2收集周边环境的信息数据，根据机动障碍物的运动规律有效地进行其位置状态的预知；路径规划器3根据数字地图、任务集、传感器系统得到的机器人自身 所处状态及约束条件，对任务集进行排序，并产生一系列机器人行进的点序列，规划出机器 人从出发点至目标点的最优行进轨迹；伺服系统4执行由路径规划器产生的机器人行进的 点序列。本发明的具体实施步骤如下
步骤一设定初始的机器人任务环境的数字地图1，并传送到机器人路径规划器3。步骤二 传感器系统2对机器人所处状态的测量信号作为机器人路径规划器3的 输入信号，包括机器人的相对位置及速度等。步骤三机器人的路径规划器3根据目前掌握的数字地图1、机器人所处状态、任 务集及约束条件，对任务集进行排序，即根据已知给定机器人出发点、目标点及部分已知障 碍物的初始位置，建立Voronoi图状态空间。要回避障碍、安全地完成预定任务，需要综合 考虑机器人导航精度和机动能力的限制，从机器人出发点至目标点的最优或次优的行进轨 迹。根据Di jkstra算法或DoubleSwe印算法，可沿Voronoi图顶点搜到一条从机器人出发 点至目标点的一条无碰撞路径，并产生一系列机器人伺服系统4行进点序列信号。步骤四伺服系统4按更新的行进点序列执行行进动作，基于伺服系统4的输出 信号调整传感器系统2对机动目标或障碍物、自身运动状态进行监测并采集相应的实时数 据。步骤五路径规划器3根据内环反馈回路，进行状态判断。若自身运动状态的位置 坐标与上次路径规划器目标位置坐标相同，表明到达目标，规划结束；否则，进行步骤六的 内容。步骤六判定有无新障碍物出现若无新障碍物出现，继续执行步骤四伺服系统4 行进动作；否则，进行步骤七的内容。步骤七建立新障碍物卡尔曼滤波器模型，根据传感器系统2的观测数据，并利用 观测数据和现有的线性动态系统期望最大化(EM)模型辨识算法对模型参数进行辨识和修 正。对目标系统用动态系统方程和测量方程进行表征如下
其中乂时刻状态变量巧ef，观测变量乃eiT，系统噪声、~#(0,辺，测量噪声
wt~N(0,R) ；^是动态模型传递函数义是测量模型传递函数…和分别是系统噪声方差 和测量噪声方差；
根据观测数据可用已有的线性动态系统期望最大化(EM)模型辨识算法迭代算出A、C、 Q和R等模型及参数，即模型的辨识。步骤八根据辨识的模型，对机器人位置和周围环境之间关系的预测，检查机器人 行进过程中是否会发生碰撞。如预测到不会发生碰撞，继续执行步骤四中伺服系统4的行 进动作；否则，进行步骤一内容，更新数字地图1，供路径规划器3进行新一轮的路径局部重 规划。卡尔曼滤波问题往往是由测量方程所得到的观测信息求解系统状态的最优估计，整个滤波过程包括预测过程和更新过程。预测过程负责推算当前状态变量和误差协方差估 计值，以便为下一个时间状态构造先验估计；更新过程负责反馈，它将先验估计和新的测量 变量值结合以构造改进的后验估计。单步预测
权利要求
一种基于卡尔曼滤波器预测的机器人避障方法，其特征在于，包含以下步骤步骤一设定初始的机器人任务环境的数字地图(1)，并传送到机器人路径规划器(3)；步骤二传感器系统(2)对机器人所处状态的测量信号作为机器人路径规划器(3)的输入信号；步骤三机器人的路径规划器(3)根据目前掌握的数字地图(1)、机器人所处状态、任务集及约束条件，对任务集进行排序，规划出行进无碰撞路径，并产生一系列机器人伺服系统(4)的行进点序列信号；步骤四基于伺服系统(4)的输出信号，调整传感器系统(2)对环境和自身运动状态进行监测并采集相应的实时数据；步骤五路径规划器(3)根据传感器系统(2)的反馈，进行状态判断；若自身运动状态与上次路径规划器目标位置坐标相同，规划结束；否则，进行步骤六的内容；步骤六判定有无新障碍物出现若无新障碍物出现，继续执行步骤四中伺服系统(4)的行进动作；否则，进行步骤七的内容；步骤七建立新障碍物卡尔曼滤波器模型，根据传感器系统(2)的观测数据，对模型参数进行辨识和修正；步骤八根据辨识的模型、机器人位置和周围环境之间关系的预测，检查机器人行进过程中是否会发生碰撞，如预测到不会发生碰撞，继续执行步骤四中伺服系统(4)的行进动作；否则，进行步骤一的内容，更新数字地图(1)，供路径规划器(3)进行新一轮的路径局部重规划。
2.根据权利要求1所述的一种基于卡尔曼滤波器预测的机器人避障方法，其特征在 于步骤三中，机器人对任务集进行排序时，根据已知给定机器人出发点、目标点及部分已 知障碍物的初始位置，建立Voronoi图状态空间，根据Dijkstra算法或DoubleSwe印算法， 沿Voronoi图顶点搜到一条从机器人出发点至目标点的一条无碰撞路径，并产生一系列机 器人伺服系统(4)行进点序列信号。
3.根据权利要求1所述的一种基于卡尔曼滤波器预测的机器人避障方法，其特征在 于步骤七中，根据传感器系统(2)的观测数据，对新障碍物的卡尔曼滤波器模型参数进行 辨识和修正时，采用线性动态系统期望最大化模型辨识算法，对目标系统用动态系统方程 和测量方程进行表征如下r x ++ f = A X xf + vf ,yt 二 C X + ^r其中时刻状态变量，观测变量乃ef，系统噪声iv 测量噪声wt-N(0,R) ；^是动态模型传递函数义是测量模型传递函数…和分别是系统噪声方差和测量噪声方差；根据观测数据用已有的线性动态系统期望最大化模型辨识算法迭代算出 A、C、Q和R等模型及参数，完成模型辨识。
4.根据权利要求1所述的一种基于卡尔曼滤波器预测的机器人避障方法，其特征在 于所述的卡尔曼滤波模型，是由测量方程所得到的观测信息求解系统状态的最优估计，整个滤波过程包括预测过程和更新过程；所述的预测过程负责推算当前状态变量和误差协方 差估计值，以便为下一个时间状态构造先验估计；所述的更新过程负责反馈，它将先验估计 和新的测量变量值结合以构造改进的后验估计，其预测和更新方程表征如下 单步预测
全文摘要
本发明涉及一种基于卡尔曼滤波器预测的机器人避障方法。复杂环境中，机器人行进的环境是动态变化的，当确定预定任务的环境发生重大变化，应修改任务目标，实时规划任务、修改路径。本发明所述避障方法中的路径规划器根据数字地图、任务集、传感器系统所得机器人所处状态，对任务集进行排序，产生机器人行进点序列，由伺服系统执行，当传感器系统探测到有新障碍物出现，根据观测数据建立卡尔曼滤波器模型，利用观测数据和经典的线性动态系统期望最大化模型辨识算法对参数进行辨识和修正，更新数字地图，供路径规划器进行新一轮的局部重规划。本发明可实现机器人在不确定环境下局部动态生成的避障路径规划，实现简单，实时性较好。
文档编号G05D3/12GK101943916SQ20101027346
公开日2011年1月12日 申请日期2010年9月7日 优先权日2010年9月7日
发明者侯勇严, 郭文强 申请人:陕西科技大学