基于平方根无迹卡尔曼滤波的多机器人协同定位算法.pdf

摘要
申请专利号：	CN201410581627.0	申请日：	2014.10.27
公开号：	CN104330083A	公开日：	2015.02.04
当前法律状态：	撤回	有效性：	无权
法律详情：	发明专利申请公布后的视为撤回IPC(主分类):G01C 21/00申请公布日:20150204\|\|\|实质审查的生效IPC(主分类):G01C 21/00申请日:20141027\|\|\|公开
IPC分类号：	G01C21/00	主分类号：	G01C21/00
申请人：	南京理工大学
发明人：	王碧霞; 李银伢
地址：	210094江苏省南京市玄武区孝陵卫200号
优先权：
专利代理机构：	南京理工大学专利中心32203	代理人：	朱显国
PDF下载：	PDF下载

内容摘要

本发明提供一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤：步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。本发明提供的算法，能同时兼顾系统的定位精度与实时性要求，计算复杂度下降，大大缩短算法耗时的同时保证了协方差矩阵的半正定性和数值稳定性，可为后续多移动机器人的协同定位提供技术支撑。

权利要求书

权利要求书
1.  一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，包括以下步骤：
步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；
步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。

2.  根据权利要求1所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1的具体过程为：
步骤1.1，收集机器人群中每个机器人的信息；
步骤1.2，建立机器人Ri的状态方程；
步骤1.3，建立量测方程。

3.  根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1.1收集的信息包括：
t时刻机器人Ri，i∈[1,N]在t时刻的位置状态其中分别为机器人Ri在t时刻的横轴坐标，纵轴坐标和运动方向；
t时刻整个机器人群的位置状态
t时刻机器人Ri，i∈[1,N]观测到的机器人Rj,j≠i,j＞i,j∈[1,N]的相对方位角

4.  根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1.2中机器人Ri的状态方程为

其中，f(·)为状态方程，为机器人Ri的控制输入，为t到t+1时刻内机器人Ri的位移，为t到t+1时刻内机器人Ri的旋转角度，为非加性的高斯过程噪声，其均值为σD为单位时间内位移D的误差，σα为单位时间内旋转角的误差。

5.  根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1.3中量测方程为

h(·)为量测方程，为量测噪声，服从且与状态方程中的过程噪声不相关，为t时刻机器人Ri与机器人Rj的相对方位，σφ相对方位φ的误差。

6.  根据权利要求1所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤2的具体过程为：
步骤2.1，给定机器人群初始状态向量初始协方差阵以及初始协方差矩阵的cholesky因子

状态维数为nx；
步骤2.2，对位置状态以及进行扩维，即

其中的实际维数为na＝nx+nw，nw为过程噪声的维数；
步骤2.3，采用UT变换中的对称性采样，生成表示机器人群的Sigma点，具体过程为：
由位置状态变量的实际维数na计算权值

其中，和分别为第一个采样点的位置状态权值和误差协方差阵权值，和表示第ui个采样点对应的位置状态权值和误差协方差阵权值，ui＝1,2,...,2na；
β为描述先验分布信息，表示最小化高阶项影响的参数；
其中，κ为尺度参数，α2为正值比例缩放因子；
经UT变换后t时刻机器人Ri的Sigma点其中
机器人群所有机器人经过UT变换后得到的Sigma点组合成机器人群的Sigma点；
步骤2.4，对生成的Sigma点进行位置状态变量的一步预测，具体过程为：
通过非线性状态函数f(·)传播为加权求和得机器人Ri位置状态的一步预测值

误差协方差阵的平方根一步预测：

其中qr{S}返回矩阵P的cholesky因子，P＝SST，因子更新 cholupdate{S,B,±v}为返回矩阵的上三角cholesky因子；
步骤2.5，重新计算表示机器人Ri的新Sigma点，具体过程为：

其中，和为第一个新采样点的位置状态权值和误差协方差阵权值，和表示第ui个新采样点对应的的位置状态权值和误差协方差阵权值，ui＝1,2,...,2nb，λb＝α22(nb+κ)-nb，nb＝nx；
新Sigma点其中
步骤2.6，机器人Ri新的采样点以及机器人Rj新的采样点通过非线性量测方程传播得到量测变量预测值加权求和得到输出预测

步骤2.7，计算自协方差矩阵的平方根因子预测值

其中为t时刻机器人Ri观测其他机器人的量测噪声的协方差矩阵

计算互协方差阵
计算滤波增益阵
步骤2.8，更新机器人Ri的位置状态向量和协方差矩阵的平方根因子即

其中是机器人Ri在t+1时刻观测其它N-1个机器人的相对方位角的实际量测值。

说明书

说明书基于平方根无迹卡尔曼滤波的多机器人协同定位算法
技术领域
本发明涉及协同定位算法领域，特别为一种基于相对方位的多移动机器人协同定位算法。
背景技术
多机器人协同系统是近年来机器人研究的热点，在工业、军事、航空航天等领域具有非常广阔的应用前景。多机器人之间相互观测，通过信息交换，实现信息共享，减少对外部环境的依赖性，增强机器人群中单个机器人的感知能力，从而可获得比单个机器人更精确的定位信息,称为多机器人的协同定位。大多数机器人系统在环境探索时要求机器人有自定位功能，而其中的自主移动机器人的自身定位问题尤为重要，是其最重要的能力之一。
D.Fox等人把单机器人马尔科夫定位方法扩展到多机器人，每个机器人使用概率分布描述自身位姿信息，再用其它机器人的观测值重新定义概率分布。A.Howard等应用最大似然估计和数值优化方法，提出了基于相对观测的、以自我为中心的机器人队列自定位算法。文献1：王玲，刘云辉，万建伟.基于相对方位的多机器人协同合作定位算法.传感器技术学报.2007,20(4):794-799。在此基础上研究了并行式结构中，利用扩展卡尔曼滤波融合内部和外部相对观测信息，有效地提高了系统定位精度。
现有EKF的定位算法及其改进算法实时性好，但易产生误差积累，初始值不当和系统的强非线性都易导致滤波发散。有学者提出了UKF及CDKF的机器人定位算法来解决此问题，改善了算法的鲁棒性、可实现性，提高了定位精度，但在多机器人的高维系统中，协方差传播会出现非正定性，进而导致数值不稳定。粒子滤波算法也被广泛应用于多机器人系统定位中，PF定位精度较好但迭代算法的复杂性导致其耗时为EKF的2～3倍，而很多实时应用中计算资源有限。在真实的多机器人系统中，由于现有技术手段、平台资源等实际因素的制约，除定位精度外，系统对定位算法的实时性仍有较高要求。综上所述，现有算法存在难以兼顾定位精度和实时性的问题。
发明内容
为了克服现有技术的不足，本发明提供一种基于平方根无迹卡尔曼滤波 (square root unscented Kalman filter,SR-UKF)的多移动机器人协同定位算法，能同时兼顾系统的定位精度与实时性要求。
一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤：
步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；
步骤1.1，收集机器人群中每个机器人的信息；
步骤1.2，建立机器人Ri的状态方程；
步骤1.3，建立量测方程。
步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位；
步骤2.1，给定机器人群初始状态向量、初始协方差阵，以及初始协方差矩阵的cholesky因子；
步骤2.2，对位置状态以及初始协方差矩阵的cholesky因子进行扩维；
步骤2.3，采用UT变换中的对称性采样，生成表示机器人群的Sigma点；
步骤2.4，对生成的Sigma点进行位置状态变量的一步预测；
步骤2.5，重新计算表示机器人Ri的新Sigma点；
步骤2.6，机器人Ri新的采样点以及机器人新的采样点，通过非线性量测方程传播得到量测变量预测值，加权求和得到输出预测；
步骤2.7，计算自协方差矩阵的平方根因子预测值；
步骤2.8，更新机器人Ri的位置状态向量和协方差矩阵的平方根因子。
本发明与现有技术相比，其优点在于：(1)用无迹变换(unscented transform，UT)生成的Sigma点表示概率分布，避免了线性化采样点带来的近似误差，从而提高了定位精度；(2)采用相对方位信息作为量测输入量，进一步提高了定位精度；(3)为满足实时性要求，SR-UKF算法避免了计算雅克比矩阵，降低了计算复杂度，滤波增益的计算中采用自协方差阵平方根因子的嵌套逆，该因子是方阵且为三角阵，计算复杂度下降，大大缩短算法耗时；(4)更新中直接传递协方差矩阵的平方根因子，保证了协方差矩阵的半正定性和数值稳定性。
下面结合附图进一步说明本发明。
附图说明
图1是本发明的仿真系统中所用相对方位观测量的含义图；
图2是本发明中SR-UKF滤波算法实现的流程图。
具体实施方式
结合图2，一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，首先考虑N个机器人组成的队列在二维环境中沿不同方向运动的情况，且N个机器人在环境探索时均处于同等地位。为了满足协同定位以及系统可观测要求，每个机器人需具备下列条件：(1)在二维环境中无障碍运动，每个机器人配有测量自身位置变化的内置传感器(采用旋转编码器测量旋转角，直尺编码器测量单位时间内位移)；(2)每个机器人都配有外部传感器，能探测并识别周围的机器人并测量相对方位信息(采用激光测距仪探测，用全景摄像机配合机器人上不同的可视标记来确定测得的信息来源于哪个机器人)；(3)每个机器人配有通信设备，能进行必要的信息交互，掌握自身在机器人群中的相对位置。
一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤：
步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；
步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。
步骤1的具体过程为：
步骤1.1，收集机器人群中每个机器人的信息，包括：
t时刻机器人Ri，i∈[1,N]在t时刻的位置状态其中分别为机器人Ri在t时刻的横轴坐标，纵轴坐标和运动方向；
t时刻整个机器人群的位置状态
t时刻机器人Ri，i∈[1,N]观测到的机器人Rj,j≠i,j＞i,j∈[1,N]的相对方位角
步骤1.2，建立机器人Ri的状态方程
Xt+1i=f(Xti,uti,wti)=xti+(Dti+wDi)cos(θti+αti+wαi)yti+(Dti+wDi)sin(θti+αti+wαi)θti+αti+wαi,]]>
其中，f(·)为状态方程，为机器人Ri的控制输入，为t到t+1时刻内机器人Ri的位移，为t到t+1时刻内机器人Ri的旋转角度，为非加性的高斯过程噪声，其均值为Qwti=diag{σD2,σα2},]]>δtl=1,t=l0,t&NotEqual;l,]]>σD为单位时间内位移D的误差，σα为单位时间内旋转角的误差；
步骤1.3，建立量测方程，结合图1，在机器人群移动过程中，t时刻机器人Ri通过外部传感器探测到机器人Rj，并测量与机器人Rj的相对方位如图1所示，其相对方位角为
γtij=h(Xti,Xtj)+vφi=φtij=tan-1(ytj-ytixtj-xti)-θti+vφi]]>
其中，为t时刻Ri观测到的机器人Rj的相对方位角，h(·)为量测方程，为量测噪声，服从且与状态方程中的过程噪声不相关，为t时刻机器人Ri与机器人Rj的相对方位，σφ相对方位φ的误差。
步骤2的具体过程为：
步骤2.1，给定机器人群初始状态向量初始协方差阵以及初始协方差矩阵的cholesky因子
X^0i=E[X0i]]]>
P0i=E[(X0i-X^0i)(X0i-X^0i)T]]]>
S0X,i=chol{P0i}]]>
状态维数为nx。
步骤2.2，对位置状态以及进行扩维，即
X^0a,i=E[X0i,w0i]=[X^0i,0]]]>
S0a,i=S0X,i00S0w0i,i]]>
其中S0w0i,i=chol{Qw0i},]]>Qw0i=diag{σD2,σα2},]]>的实际维数为na＝nx+nw，nw为过程噪声的维数。
步骤2.3，生成表示机器人Ri的Sigma点：采用UT变换中的对称性采样，选取一系列确定的采样点对状态向量的概率密度函数进行近似，计算相应权值，具体过程为：
由位置状态变量的实际维数na计算权值
ωa0m=λa/(na+λa)]]>
ωa0c=λa/(na+λa)+(1-α22+β)]]>
ωauim=ωauic=λa/(2na+2λa)]]>
其中，和分别为第一个采样点的位置状态权值和误差协方差阵权值，和表示第ui个采样点对应的位置状态权值和误差协方差阵权值，ui＝1,2,...,2na；β为描述先验分布信息，表示最小化高阶项影响的参数；其中，λa＝α22(na+κ)-na，κ为尺度参数，α2为正值比例缩放因子；
经UT变换后t时刻机器人Ri的Sigma点为
χti=X^ta,iX^ta,i+ηaSta,iX^ta,i-ηaSta,i,]]>
其中ηa=(na+λa);]]>
机器人群所有机器人经过UT变换后得到的Sigma点组合成机器人群的Sigma点。
步骤2.4，对生成的Sigma点进行位置状态变量的一步预测，具体过程为：
通过非线性状态函数f(·)传播为加权求和得机器人Ri位置状态的一步预测值
χt+1/t*i=f(χti,uti)]]>
X^t+1/ti=Σui=02naωauimχui,t+1/t*i;]]>
误差协方差阵的平方根一步预测：
St+1|ti=qr{[ωa1:2nac(χ1:2na,t+1|t*i-X^t+1|ti)]}]]>
St+1|ti=cholupdate{St+1|ti,χ0,t+1|t*i-X^t+1|ti,ωa0c}]]>
其中qr{S}返回矩阵P的cholesky因子，P＝SST，因子更新cholupdate{S,B,±v}为返回矩阵的上三角cholesky因子。
步骤2.5，利用和重新计算表示机器人Ri的新Sigma点，其维数为nb＝nx，个数为Lb＝2nb+1，相应权值计算，具体过程为：
ωb0m=λb/(nb+λb)]]>
ωb0c=λb/(nb+λb)+(1-α22+β)]]>
ωbuim=ωbuic=λb/(2nb+2λb)]]>
其中，和为第一个新采样点的位置状态权值和误差协方差阵权值，和表示第ui个新采样点对应的的位置状态权值和误差协方差阵权值，ui＝1,2,...,2nb，λb＝α22(nb+κ)-nb，nb＝nx；
新Sigma点χt+1|ti=X^t+1|tiX^t+1|ti+ηbSt+1|tiX^t+1|ti-ηbSt+1|ti,]]>其中ηb=(nb+λb).]]>
步骤2.6，结合机器人Ri新的采样点以及机器人Rj新的采样点通过非线性量测方程传播得到量测变量预测值加权求和得到输出预测
γui,t+1|ti=h(χt+1|ti,χt+1|tj)+vφi]]>
Y^t+1|ti=Σui=02nbωbuimγui,t+1|ti.]]>
步骤2.7，计算自协方差矩阵的平方根因子预测值
St+1Y,i=qr{[ωb1:2nbc(γ1:2nb,t+1|ti-Y^t+1|ti)Rφi]}]]>
St+1Y,i=cholupdate{St+1Y,i,γ0,t+1|ti-Y^t+1|ti,ωb0c}]]>
其中为t时刻机器人Ri观测其他机器人的量测噪声的协方差矩阵
Rφi=diag{[σφi12...σφij2...σφiN2]},i&NotEqual;j,j=1,2,...,N,]]>
其中表示第i个机器人与第j个机器人之间相对方位φij的误差；
计算互协方差阵PXt+1Yt+1i=Σui=02nbωbuic,i(χui,t+1|t*i-X^t+1|ti)(γui,t+1|ti-Y^t+1|ti)T]]>
计算滤波增益阵Kt+1=(PXt+1Yt+1i/(St+1Y,i)T)/St+1Y,i.]]>
步骤2.8，更新机器人Ri的位置状态向量和协方差矩阵的平方根因子即
X^t+1i=X^t+1|ti+Kt+1(Yt+1measure,i-Y^t+1|ti)]]>
U=Kt+1St+1Y,i]]>
St+1X,i=cholupdate{St+1|ti,U,-1}]]>
其中是机器人Ri在t+1时刻观测其它N-1个机器人的相对方位角的实际量测值。
每当机器人Ri获得与其它机器人的相对量测信息，其状态信息就会得到更新，机器人间的相对关系就被体现在了机器人的位置估计中；反之当量测信息缺失时，机器人Ri的状信息在该时刻就不更新，但其它机器人的状态会更新。而在下一时刻，通过新的量测更新，其它机器人更新的结果能影响到前一时刻量测信息缺失的那个机器人。由于i＝1,2,...N,t∈{0,1,2,...,∞}，在每个时刻t其它机器人的状态信息也是按上述流程更新的。

资源描述

《基于平方根无迹卡尔曼滤波的多机器人协同定位算法.pdf》由会员分享，可在线阅读，更多相关《基于平方根无迹卡尔曼滤波的多机器人协同定位算法.pdf（12页珍藏版）》请在专利查询网上搜索。

1、(10)申请公布号 CN 104330083 A (43)申请公布日 2015.02.04 CN 104330083 A (21)申请号 201410581627.0 (22)申请日 2014.10.27 G01C 21/00(2006.01) (71)申请人南京理工大学地址 210094 江苏省南京市玄武区孝陵卫 200 号 (72)发明人王碧霞李银伢 (74)专利代理机构南京理工大学专利中心 32203 代理人朱显国 (54) 发明名称基于平方根无迹卡尔曼滤波的多机器人协同定位算法 (57) 摘要本发明提供一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步。

2、骤：步骤 1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。本发明提供的算法，能同时兼顾系统的定位精度与实时性要求，计算复杂度下降，大大缩短算法耗时的同时保证了协方差矩阵的半正定性和数值稳定性，可为后续多移动机器人的协同定位提供技术支撑。 (51)Int.Cl. 权利要求书 4 页说明书 6 页附图 1 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请权利要求书4页说明书6页附图1页 (10。

3、)申请公布号 CN 104330083 A CN 104330083 A 1/4 页 2 1. 一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，包括以下步骤：步骤 1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；步骤 2，采用 SR-UKF 滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。 2. 根据权利要求 1 所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤 1 的具体过程为：步骤 1.1，收集机器人群中每个机器人的信息；步骤 1.2，建。

4、立机器人 Ri的状态方程；步骤 1.3，建立量测方程。 3. 根据权利要求 2 所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤 1.1 收集的信息包括： t时刻机器人Ri， i1,N在t时刻的位置状态其中分别为机器人 Ri在 t 时刻的横轴坐标，纵轴坐标和运动方向； t 时刻整个机器人群的位置状态 t 时刻机器人 Ri， i 1,N 观测到的机器人 Rj,j i,j i,j 1,N 的相对方位角 4. 根据权利要求 2 所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤 1.2 中机器人 Ri的状态方程为其中， f() 。

5、为状态方程，为机器人 Ri的控制输入，为 t 到 t+1 时刻内机器人 Ri的位移，为 t 到 t+1 时刻内机器人 Ri的旋转角度，为非加性的高斯过程噪声，其均值为D为单位时间内位移 D 的误差，为单位时间内旋转角的误差。 5. 根据权利要求 2 所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤 1.3 中量测方程为 h() 为量测方程，为量测噪声，服从且与状态方程中的过程噪声不相关，为 t 时刻机器人 Ri与机器人 Rj的相对方位，相对方位的误差。权利要求书 CN 104330083 A 2 2/4 页 3 6. 根据权利要。

6、求 1 所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤 2 的具体过程为：步骤 2.1，给定机器人群初始状态向量初始协方差阵以及初始协方差矩阵的 cholesky 因子状态维数为 nx；步骤 2.2，对位置状态以及进行扩维，即其中的实际维数为nanx+nw， nw为过程噪声的维数；步骤 2.3，采用 UT 变换中的对称性采样，生成表示机器人群的 Sigma 点，具体过程为：由位置状态变量的实际维数 na计算权值其中，和分别为第一个采样点的位置状态权值和误差协方差阵权值，和表示第 ui 个采样点对应的位置状态权值和误差协方差阵权。

7、值， ui 1,2,.,2na；为描述先验分布信息，表示最小化高阶项影响的参数；其中，为尺度参数， 2为正值比例缩放因子；经 UT 变换后 t 时刻机器人 Ri的 Sigma 点其中机器人群所有机器人经过 UT 变换后得到的 Sigma 点组合成机器人群的 Sigma 点；步骤 2.4，对生成的 Sigma 点进行位置状态变量的一步预测，具体过程为：通过非线性状态函数 f() 传播为加权求和得机器人 Ri位置状态的一步预权利要求书 CN 104330083 A 3 3/4 页 4 测值误差协方差阵的平方根一步预测：其中 qrS 返回矩阵 P 的 c。

8、holesky 因子， P SST，因子更新 cholupdateS,B,v 为返回矩阵的上三角 cholesky 因子；步骤 2.5，重新计算表示机器人 Ri的新 Sigma 点，具体过程为：其中，和为第一个新采样点的位置状态权值和误差协方差阵权值，和表示第ui个新采样点对应的的位置状态权值和误差协方差阵权值， ui1,2,.,2nb， b 22(nb+)-nb， nb nx；新 Sigma 点其中步骤 2.6，机器人 Ri新的采样点以及机器人 Rj新的采样点通过非线性量测方程传播得到量测变量预测值加权求和得到输出预测步骤 2.7，计算自协方差矩阵的平方根因子。

9、预测值其中为 t 时刻机器人 Ri观测其他机器人的量测噪声的协方差矩阵权利要求书 CN 104330083 A 4 4/4 页 5 计算互协方差阵计算滤波增益阵步骤 2.8，更新机器人 Ri的位置状态向量和协方差矩阵的平方根因子即其中是机器人 Ri在 t+1 时刻观测其它 N-1 个机器人的相对方位角的实际量测值。权利要求书 CN 104330083 A 5 1/6 页 6 基于平方根无迹卡尔曼滤波的多机器人协同定位算法技术领域 0001 本发明涉及协同定位算法领域，特别为一种基于相对方位的多移动机器人协同定位算法。背景技术 0002 多机器人协同系统。

10、是近年来机器人研究的热点，在工业、军事、航空航天等领域具有非常广阔的应用前景。多机器人之间相互观测，通过信息交换，实现信息共享，减少对外部环境的依赖性，增强机器人群中单个机器人的感知能力，从而可获得比单个机器人更精确的定位信息 , 称为多机器人的协同定位。大多数机器人系统在环境探索时要求机器人有自定位功能，而其中的自主移动机器人的自身定位问题尤为重要，是其最重要的能力之一。 0003 D.Fox 等人把单机器人马尔科夫定位方法扩展到多机器人，每个机器人使用概率分布描述自身位姿信息，再用其它机器人的观测值重新定义概率分布。A.Howard 等应用最大似然估计和。

11、数值优化方法，提出了基于相对观测的、以自我为中心的机器人队列自定位算法。文献 1 ：王玲，刘云辉，万建伟 . 基于相对方位的多机器人协同合作定位算法 . 传感器技术学报.2007,20(4):794-799。在此基础上研究了并行式结构中，利用扩展卡尔曼滤波融合内部和外部相对观测信息，有效地提高了系统定位精度。 0004 现有 EKF 的定位算法及其改进算法实时性好，但易产生误差积累，初始值不当和系统的强非线性都易导致滤波发散。有学者提出了 UKF 及 CDKF 的机器人定位算法来解决此问题，改善了算法的鲁棒性、可实现性，提高了定位精度，但在多机器人的高维系。

12、统中，协方差传播会出现非正定性，进而导致数值不稳定。粒子滤波算法也被广泛应用于多机器人系统定位中， PF 定位精度较好但迭代算法的复杂性导致其耗时为 EKF 的 2 3 倍，而很多实时应用中计算资源有限。在真实的多机器人系统中，由于现有技术手段、平台资源等实际因素的制约，除定位精度外，系统对定位算法的实时性仍有较高要求。综上所述，现有算法存在难以兼顾定位精度和实时性的问题。发明内容 0005 为了克服现有技术的不足，本发明提供一种基于平方根无迹卡尔曼滤波 (square root unscented Kalman fi lter,SR-UKF) 的多移动机器人协。

13、同定位算法，能同时兼顾系统的定位精度与实时性要求。 0006 一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤： 0007 步骤 1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型； 0008 步骤 1.1，收集机器人群中每个机器人的信息； 0009 步骤 1.2，建立机器人 Ri的状态方程； 0010 步骤 1.3，建立量测方程。 0011 步骤 2，采用 SR-UKF 滤波算法，以相对方位为输入对系统状态进行整体更新，实现说明书 CN 104330083 A 6 2/6 页 7 多移动机器人的协同自定位。

14、； 0012 步骤 2.1，给定机器人群初始状态向量、初始协方差阵，以及初始协方差矩阵的 cholesky 因子； 0013 步骤 2.2，对位置状态以及初始协方差矩阵的 cholesky 因子进行扩维； 0014 步骤 2.3，采用 UT 变换中的对称性采样，生成表示机器人群的 Sigma 点； 0015 步骤 2.4，对生成的 Sigma 点进行位置状态变量的一步预测； 0016 步骤 2.5，重新计算表示机器人 Ri的新 Sigma 点； 0017 步骤2.6，机器人Ri新的采样点以及机器人新的采样点，通过非线性量测方程传播得到量测变量预测值，加权求和。

15、得到输出预测； 0018 步骤 2.7，计算自协方差矩阵的平方根因子预测值； 0019 步骤 2.8，更新机器人 Ri的位置状态向量和协方差矩阵的平方根因子。 0020 本发明与现有技术相比，其优点在于： (1) 用无迹变换 (unscented transform， UT) 生成的 Sigma 点表示概率分布，避免了线性化采样点带来的近似误差，从而提高了定位精度； (2) 采用相对方位信息作为量测输入量，进一步提高了定位精度； (3) 为满足实时性要求， SR-UKF 算法避免了计算雅克比矩阵，降低了计算复杂度，滤波增益的计算中采用自协方差阵平方根因子的嵌套逆。

16、，该因子是方阵且为三角阵，计算复杂度下降，大大缩短算法耗时； (4) 更新中直接传递协方差矩阵的平方根因子，保证了协方差矩阵的半正定性和数值稳定性。 0021 下面结合附图进一步说明本发明。附图说明 0022 图 1 是本发明的仿真系统中所用相对方位观测量的含义图； 0023 图 2 是本发明中 SR-UKF 滤波算法实现的流程图。具体实施方式 0024 结合图 2，一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，首先考虑 N 个机器人组成的队列在二维环境中沿不同方向运动的情况，且 N 个机器人在环境探索时均处于同等地位。为了满足协同定位以及系统可观测要求，每。

17、个机器人需具备下列条件： (1) 在二维环境中无障碍运动，每个机器人配有测量自身位置变化的内置传感器 ( 采用旋转编码器测量旋转角，直尺编码器测量单位时间内位移 ) ； (2) 每个机器人都配有外部传感器，能探测并识别周围的机器人并测量相对方位信息 ( 采用激光测距仪探测，用全景摄像机配合机器人上不同的可视标记来确定测得的信息来源于哪个机器人 ) ； (3) 每个机器人配有通信设备，能进行必要的信息交互，掌握自身在机器人群中的相对位置。 0025 一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤： 0026 步骤 1，根据机器人的运动学方程和基于相对方。

18、位的量测方程，给出多移动机器人自定位的动态模型； 0027 步骤 2，采用 SR-UKF 滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。 0028 步骤 1 的具体过程为：说明书 CN 104330083 A 7 3/6 页 8 0029 步骤 1.1，收集机器人群中每个机器人的信息，包括： 0030 t 时刻机器人 Ri， i 1,N 在 t 时刻的位置状态其中分别为机器人 Ri在 t 时刻的横轴坐标，纵轴坐标和运动方向； 0031 t 时刻整个机器人群的位置状态 0032 t 时刻机器人 Ri， i 1,N 观测到的机器人。

19、 Rj,j i,j i,j 1,N 的相对方位角 0033 步骤 1.2，建立机器人 Ri的状态方程 0034 0035 其中， f() 为状态方程，为机器人 Ri的控制输入，为 t 到 t+1 时刻内机器人 Ri的位移，为 t 到 t+1 时刻内机器人 Ri的旋转角度，为非加性的高斯过程噪声，其均值为D 为单位时间内位移 D 的误差，为单位时间内旋转角的误差； 0036 步骤 1.3，建立量测方程，结合图 1，在机器人群移动过程中， t 时刻机器人 Ri通过外部传感器探测到机器人Rj，并测量与机器人Rj的相对方位如图1所示，其相对方位角为 0037 0038 其。

20、中，为 t 时刻 Ri观测到的机器人 Rj的相对方位角， h() 为量测方程，为量测噪声，服从且与状态方程中的过程噪声不相关，为t时刻机器人Ri与机器人 Rj的相对方位，相对方位的误差。 0039 步骤 2 的具体过程为： 0040 步骤2.1，给定机器人群初始状态向量初始协方差阵以及初始协方差矩阵的 cholesky 因子 0041 0042 0043 0044 状态维数为 nx。 0045 步骤 2.2，对位置状态以及进行扩维，即说明书 CN 104330083 A 8 4/6 页 9 0046 0047 0048 其中的实际维数为nanx+nw， nw为过。

21、程噪声的维数。 0049 步骤2.3，生成表示机器人Ri的Sigma点：采用UT变换中的对称性采样，选取一系列确定的采样点对状态向量的概率密度函数进行近似，计算相应权值，具体过程为： 0050 由位置状态变量的实际维数 na计算权值 0051 0052 0053 0054 其中，和分别为第一个采样点的位置状态权值和误差协方差阵权值，和表示第 ui 个采样点对应的位置状态权值和误差协方差阵权值， ui 1,2,.,2na；为描述先验分布信息，表示最小化高阶项影响的参数；其中， a 22(na+)-na，为尺度参数， 2为正值比例缩放因子； 0055 经 UT 变。

22、换后 t 时刻机器人 Ri的 Sigma 点为 0056 0057 其中 0058 机器人群所有机器人经过 UT 变换后得到的 Sigma 点组合成机器人群的 Sigma 点。 0059 步骤 2.4，对生成的 Sigma 点进行位置状态变量的一步预测，具体过程为： 0060 通过非线性状态函数 f() 传播为加权求和得机器人 Ri位置状态的一步预测值 0061 0062 0063 误差协方差阵的平方根一步预测： 0064 0065 0066 其中qrS返回矩阵P的cholesky因子， PSST，因子更新cholupdateS,B,v 说明书 CN 104330083 A 9。

23、 5/6 页 10 为返回矩阵的上三角 cholesky 因子。 0067 步骤 2.5，利用和重新计算表示机器人 Ri的新 Sigma 点，其维数为 nb nx，个数为 Lb 2nb+1，相应权值计算，具体过程为： 0068 0069 0070 0071 其中，和为第一个新采样点的位置状态权值和误差协方差阵权值，和表示第ui个新采样点对应的的位置状态权值和误差协方差阵权值， ui1,2,.,2nb， b 22(nb+)-nb， nb nx； 0072 新 Sigma 点其中 0073 步骤 2.6，结合机器人 Ri新的采样点以及机器人 Rj新的采样点通过非线性量测方程传播得。

24、到量测变量预测值加权求和得到输出预测 0074 0075 0076 步骤 2.7，计算自协方差矩阵的平方根因子预测值 0077 0078 0079 其中为 t 时刻机器人 Ri观测其他机器人的量测噪声的协方差矩阵 0080 0081 其中表示第 i 个机器人与第 j 个机器人之间相对方位 ij的误差； 0082 计算互协方差阵 0083 计算滤波增益阵 0084 步骤 2.8，更新机器人 Ri的位置状态向量和协方差矩阵的平方根因子即 0085 0086 说明书 CN 104330083 A 10 6/6 页 11 0087 0088 其中是机器人 Ri在 t+1 时刻观测其它 N-1。

25、个机器人的相对方位角的实际量测值。 0089 每当机器人 Ri获得与其它机器人的相对量测信息，其状态信息就会得到更新，机器人间的相对关系就被体现在了机器人的位置估计中；反之当量测信息缺失时，机器人 Ri 的状信息在该时刻就不更新，但其它机器人的状态会更新。而在下一时刻，通过新的量测更新，其它机器人更新的结果能影响到前一时刻量测信息缺失的那个机器人。由于 i 1,2,.N,t 0,1,2,., ，在每个时刻 t 其它机器人的状态信息也是按上述流程更新的。说明书 CN 104330083 A 11 1/1 页 12 图 1 图 2 说明书附图 CN 104330083 A 12 。

展开阅读全文