[发明专利]一种基于VCKF的多机器人协同导航定位方法

权利要求书

1.一种基于VCKF的多移动机器人协同导航定位方法，包括以下步骤：

步骤1：根据多移动机器人工作环境，确定协同导航系统的初始值，包括多移动机器人的初始位姿和路标点的位置；

步骤2：建立多移动机器人协同导航系统的非线性系统方程；

构建包括多机器人位置和姿态在内的状态向量：

X＝[X1 X2 … Xn]^T

其中Xi＝[xi,yi,θi]^T表示第i个机器人的位姿，根据机器人运动学方程可以表示为：

构建包括距离和方位角度的量测向量Z＝[ZRR ZRL]^T，包括机器人与机器人之间的相互观测ZRR和机器人与固定路标点之间的观测ZRL，则ZRR和ZRL可表示为：

其中[xi,yi,θi]^T和[xj,yj,θj]^T是第i个机器人和第j个机器人的位姿，(xl,yl)是路标点l的位置；

在此基础上，综合考虑多移动机器人协同导航的非线性和不确定性因素，根据移动机器人动力学原理，建立多移动机器人的非线性系统方程：

其中，f(·)是系统的非线性状态转移方程，w(k)是系统的噪声矩阵，w(k)～N(0,Q(k))；h(·)是系统的非线性观测方程，η(k)是系统的观测噪声矩阵，且有η(k)～N(0,R(k))；

步骤3：对多移动机器人协同导航系统按照CKF滤波框架进行时间更新；

首先根据多移动机器人协同导航系统的维数N来确定CKF滤波所需要的容积点集[ξi，ωi]，其中i＝1,2,…,2N：

按照CKF滤波框架对系统进行时间更新：

步骤4：多移动机器人对工作环境中的固定路标点以及其它机器人进行实时观测，获取相对距离和方位角作为观测信息；

步骤5：利用观测到的量测信息和系统方程，利用VCKF算法完成多移动机器人协同导航系统的量测更新；

步骤6：对多移动机器人的位姿信息进行更新；

步骤7：重复步骤3～步骤6，完成多移动机器人的高精度协同导航定位。

2.如权利要求1所述的一种基于VCKF的多移动机器人协同导航定位方法，其特征在于，步骤5中利用改进的VCKF算法完成协同导航系统的量测更新过程包括以下步骤：

首先利用CKF框架完成协同导航系统的量测更新，估计出多机器人的位姿信息；

然后利用VCE方法对系统的过程噪声和量测噪声进行估计：

1)在多移动机器人非线性系统时间更新的基础上建立包含系统过程噪声和观测噪声在内的三组伪观测向量；

2)根据残差理论计算出伪观测向量的残差方程以及残差向量的方差矩阵；

3)计算出对应伪观测向量的多余观测分量，以及过程噪声方差分量和量测噪声方差分量的多余观测分量；

4)根据Herlmet方差分量估计方法计算出系统的过程噪声方差阵和量测噪声方差阵，完成系统噪声的实时估计。