[发明专利]一种融合里程计与UWB的多机器人协作定位方法

权利要求书

1.一种融合里程计与UWB的多机器人协作定位方法，其特征在于，包括以下步骤：

S1：通过里程计和UWB数据采集模块分别采集机器人的位姿信息和多组UWB节点之间的距离信息；

S2：基于采集的多组UWB节点之间的距离信息，通过非线性优化算法实现机器人协作定位；

所述步骤S2包括以下子步骤：

S21：根据多组UWB节点之间的距离信息，计算机器人i和机器人j之间距离测量值和计算值的残差函数；

S22：根据机器人i和机器人j之间距离测量值和计算值的残差函数，利用列文伯格-马夸特法进行迭代，得到机器人的最佳位姿，实现多机器人协作定位；

所述步骤S21中，机器人i和机器人j之间距离测量值和计算值的残差函数的表达式为：

其中，表示机器人i和机器人j之间残差函数值最小的机器人位姿，argmin(·)表示能够使误差值最小的x的值，f(x)表示残差函数，x＝(x,y,θ)表示机器人的位姿，x表示机器人i相对于机器人j的横坐标，y表示机器人i相对于机器人j的纵坐标，θ表示机器人i相对于机器人j的方向角度，K表示机器人i上的UWB节点的数量，L表示机器人j上的UWB节点的数量，表示在t时刻机器人i上UWB节点k和机器人j上UWB节点l之间的实际测量值，d(·)表示给定机器人i与机器人j相对位姿的情况下计算机器人i上UWB节点k和机器人j上UWB节点l之间的距离运算，表示机器人i上的UWB节点k的相对位置，表示机器人j上的UWB节点l的相对位置，k∈[1,…,K]，l∈[1，…,L]，i∈[1,…,N],j∈[1，…,N]，N代表机器人的数量；

所述步骤S22包括以下子步骤：

S221：根据残差函数，建立近似指标评价模型，动态调整信赖区域μ的大小；

S222：将步长Δx限制在信赖半径为μ的区域内，计算Δx；

S223：在信赖半径为μ的区域内进行k次迭代，得到机器人的最佳位姿，实现多机器人协作定位；

所述步骤S221中，在信赖半径为μ的区域内进行k次迭代的方法为：通过建立近似指标评价模型动态调整信赖半径为μ的区域，近似指标评价模型的表达式为：

其中，Δx表示步长，J(x)表示f(x)关于x的一阶导数，f(x+Δx)表示f(x)的增量，ρ表示近似指标；

根据近似指标ρ动态调整信赖半径为μ的区域的方法为：若0ρ≤0.25，则将信赖半径μ缩小一半，若ρ0.75，则将信赖半径μ扩大一半，若0.25ρ≤0.75，则不调整信赖半径μ；

在信赖半径为μ的区域内进行k次迭代，若ρ0，则令x_k+1＝x_k，并将信赖半径μ缩小一半继续迭代，若ρ＞0，则令x_k+1＝x_k+Δx_k，当Δx_k小于设定阈值时，得到机器人的初步最佳位姿x_k+1，其中，Δx_k表示迭代k次后步长，x_k表示上一次迭代的位姿；

所述步骤S222中，步长Δx的计算公式为：

(H(x)+λI)Δx＝-J(x)^Tf(x)

其中，H(x)表示f(x)海塞矩阵的近似，H(x)＝J(x)^TJ(x)，I表示单位矩阵，λ表示系数因子，J(x)表示f(x)关于x的一阶导数；

S3：基于非线性优化后的多机器人定位信息，融合机器人里程计提供的位姿信息，构建位姿图，并进行优化，实现多机器人协作的精确定位；

所述步骤S3中，构建位姿图的方法为：将机器人的位姿作为顶点，不同时刻位姿之间的约束关系作为边；

基于位姿图，通过最小二乘法计算约束最小情况下的位姿，其计算公式为：

其中，T表示时长，表示机器人i在t时刻的里程计数据，表示机器人i在t+1时刻的里程计数据，表示机器人i在t至t+1时刻的里程计数据，表示机器人i在t至t+1时刻观察值的信息矩阵，表示机器人i在t时刻的里程计约束，表示机器人j在t时刻的里程计数据，表示机器人i和机器人j之间残差函数值最小的机器人位姿，表示机器人i和机器人j之间的位态约束，是机器人i和机器人j之间在t时刻观察值的信息矩阵，表示机器人j在t+1时刻的里程计数据，代表机器人j在t至t+1时刻的里程计数据，是机器人j在t时刻的里程计约束，表示机器人j之间在t至t+1时刻观察值的信息矩阵，(i,j)∈[1,…,N]，N代表机器人的数量。

2.根据权利要求1所述的融合里程计与UWB的多机器人协作定位方法，其特征在于，所述步骤S1中，通过里程计采集机器人的位姿信息的方法为：将编码器搭建在机器人上进行采集，得到里程计数据；

通过UWB数据采集模块采集多组UWB节点之间的距离信息的方法为：在每个机器人不同的位置上搭载UWB标签，采集多组UWB节点之间的距离信息。