[发明专利]一种用于双移动机器人刚体协作搬运的协同定位方法

说明书

本发明提供一种用于双移动机器人刚体协作搬运的协同定位方法，包括以下步骤：S1、构建基于刚性约束的双机器人系统的运动学模型和量测模型；S2、初始化高斯‑厄米特求积分卡尔曼滤波器；S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态，其能够融合由不变刚性约束产生的相对距离与方位约束条件限制误差上界，使得估计结果更加接近真实运动轨迹。

技术领域

本发明涉及多机器人协同定位领域，具体涉及一种用于双移动机器人刚体协作搬运的协同定位方法。

背景技术

近年来，多机器人系统受到了广泛关注，其不仅能够克服单机器人系统在信息获取、处理和控制等方面的局限，而且运行效率高、灵活性好，可用于执行各种复杂任务。多机器人协作搬运是其主要应用之一，并在生产制造、物流仓储等工业自动化应用中扮演着重要角色，除此之外，多机器人系统还能够在一些高危场景中代替人执行搬运任务，如太空与深海探索、核泄漏废弃物清理、战场排雷等。因此，多机器人协作搬运具有重要的研究价值。

在多机器人刚体协作搬运中，如何对移动机器人团队进行精确定位是其顺利完成搬运任务的基础和关键。本领域有的技术人员利用放置于环境上方的相机采集机器人图像，并结合计算机视觉以实现搬运机器人定位。然而，该方法中有限的相机视野极大地限制了目标定位范围，且使用较为繁琐，需要对其进行标定。与借助外部设备不同，本领域有的技术人员利用机器人自身配备的传感器进行多传感器数据融合，以实现精确状态估计。然而，这些方法并没有考虑协作机器人之间的相关性，有可能导致根据定位结果计算出的相对信息具有较大方差，不利于协同控制，从而致使搬运任务失败。

为了充分利用机器人之间的相对信息来改善定位性能，多机器人协同定位技术应运而生，其能够通过机器人之间相互通信实现导航资源共享，进而减少对外部环境的依赖性，增强单个机器人的感知能力。此外，协同定位将个体定位误差分散至协同网络，提高整体定位精度，同时利用分布式架构提高计算效率和鲁棒性。本领域有的技术人员利用扩展卡尔曼滤波器(EKF)对主从自主水下航行器(AUV)之间的相对距离信息进行融合，有效提高了仅配备低精度导航设备的从AUV定位精度，但由于EKF使用一阶泰勒展开对非线性系统进行近似表示，会产生较大的截断误差，滤波精度仍有待于提高。本领域有的技术人员将能够对任意非线性系统进行逼近的高斯-厄密特粒子滤波器(GHPF)应用于协同定位中，可获得比EKF更高的滤波精度，但由于滤波器在每次递归中需要对多个粒子求高斯-厄米特积分，显著增加了计算成本，难以实现对机器人团队位置的实时估计。综上所述，以上协同定位方法没有对定位精度和效率进行较好地权衡，且没有考虑刚体协作搬运任务的特殊性，不适合直接应用于协作搬运任务中。

发明内容

本发明的目的是提供一种用于双移动机器人刚体协作搬运的协同定位方法，其能够融合由不变刚性约束产生的相对距离与方位约束条件限制误差上界，使得估计结果更加接近真实运动轨迹；同时，其能够利用约束建立的耦合关系实现状态降维，可显著提高计算效率；而且，机器人之间的相对信息无需测量，有助于降低应用成本，便于实际应用。

为了达到上述目的，本发明所采用的技术方案是：一种用于双移动机器人刚体协作搬运的协同定位方法，包括以下步骤：

S1、构建基于刚性约束的双机器人系统的运动学模型和量测模型；

S2、初始化高斯-厄米特求积分卡尔曼滤波器；

S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；

S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；

S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；