CN110967017A - 一种用于双移动机器人刚体协作搬运的协同定位方法 - Google Patents

一种用于双移动机器人刚体协作搬运的协同定位方法 Download PDF

Info

Publication number
CN110967017A
CN110967017A CN201911154494.8A CN201911154494A CN110967017A CN 110967017 A CN110967017 A CN 110967017A CN 201911154494 A CN201911154494 A CN 201911154494A CN 110967017 A CN110967017 A CN 110967017A
Authority
CN
China
Prior art keywords
rigid body
state
rigid
robot
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911154494.8A
Other languages
English (en)
Other versions
CN110967017B (zh
Inventor
刘剑锋
普杰信
孙力帆
张松灿
司彦娜
王燕玲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Henan University of Science and Technology
Original Assignee
Henan University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Henan University of Science and Technology filed Critical Henan University of Science and Technology
Priority to CN201911154494.8A priority Critical patent/CN110967017B/zh
Publication of CN110967017A publication Critical patent/CN110967017A/zh
Application granted granted Critical
Publication of CN110967017B publication Critical patent/CN110967017B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching

Abstract

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

Description

一种用于双移动机器人刚体协作搬运的协同定位方法
技术领域
本发明涉及多机器人协同定位领域，具体涉及一种用于双移动机器人刚体协作搬运的协同定位方法。
背景技术
近年来，多机器人系统受到了广泛关注，其不仅能够克服单机器人系统在信息获取、处理和控制等方面的局限，而且运行效率高、灵活性好，可用于执行各种复杂任务。多机器人协作搬运是其主要应用之一，并在生产制造、物流仓储等工业自动化应用中扮演着重要角色，除此之外，多机器人系统还能够在一些高危场景中代替人执行搬运任务，如太空与深海探索、核泄漏废弃物清理、战场排雷等。因此，多机器人协作搬运具有重要的研究价值。
在多机器人刚体协作搬运中，如何对移动机器人团队进行精确定位是其顺利完成搬运任务的基础和关键。本领域有的技术人员利用放置于环境上方的相机采集机器人图像，并结合计算机视觉以实现搬运机器人定位。然而，该方法中有限的相机视野极大地限制了目标定位范围，且使用较为繁琐，需要对其进行标定。与借助外部设备不同，本领域有的技术人员利用机器人自身配备的传感器进行多传感器数据融合，以实现精确状态估计。然而，这些方法并没有考虑协作机器人之间的相关性，有可能导致根据定位结果计算出的相对信息具有较大方差，不利于协同控制，从而致使搬运任务失败。
为了充分利用机器人之间的相对信息来改善定位性能，多机器人协同定位技术应运而生，其能够通过机器人之间相互通信实现导航资源共享，进而减少对外部环境的依赖性，增强单个机器人的感知能力。此外，协同定位将个体定位误差分散至协同网络，提高整体定位精度，同时利用分布式架构提高计算效率和鲁棒性。本领域有的技术人员利用扩展卡尔曼滤波器(EKF)对主从自主水下航行器(AUV)之间的相对距离信息进行融合，有效提高了仅配备低精度导航设备的从AUV定位精度，但由于EKF使用一阶泰勒展开对非线性系统进行近似表示，会产生较大的截断误差，滤波精度仍有待于提高。本领域有的技术人员将能够对任意非线性系统进行逼近的高斯-厄密特粒子滤波器(GHPF)应用于协同定位中，可获得比EKF更高的滤波精度，但由于滤波器在每次递归中需要对多个粒子求高斯-厄米特积分，显著增加了计算成本，难以实现对机器人团队位置的实时估计。综上所述，以上协同定位方法没有对定位精度和效率进行较好地权衡，且没有考虑刚体协作搬运任务的特殊性，不适合直接应用于协作搬运任务中。
发明内容
本发明的目的是提供一种用于双移动机器人刚体协作搬运的协同定位方法，其能够融合由不变刚性约束产生的相对距离与方位约束条件限制误差上界，使得估计结果更加接近真实运动轨迹；同时，其能够利用约束建立的耦合关系实现状态降维，可显著提高计算效率；而且，机器人之间的相对信息无需测量，有助于降低应用成本，便于实际应用。
为了达到上述目的，本发明所采用的技术方案是：一种用于双移动机器人刚体协作搬运的协同定位方法，包括以下步骤：
S1、构建基于刚性约束的双机器人系统的运动学模型和量测模型；
S2、初始化高斯-厄米特求积分卡尔曼滤波器；
S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；
S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；
S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；
S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态。
进一步的，所述的步骤S1具体包括以下步骤：
S1.1、设定刚体状态向量并对刚体状态向量赋初值，刚体状态向量记为
Figure BDA0002284431920000031
表示所搬运刚体的位置，其中(xc,yc)为两个移动机器人的几何中心连线中点在导航坐标系下的坐标；
Figure BDA0002284431920000032
为(xc,yc)到其中一个移动机器人Ri几何中心(xi,yi)的延长线与导航坐标系横轴XNav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与XNav正半轴的夹角，表示刚体的运动方向；
S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：
Figure BDA0002284431920000041
Xk+1＝f(Xk,uk,wk)＝Xk+T·Φk(uk+wk)；
Figure BDA0002284431920000042
其中f(·)为非线性状态转移方程；uk＝[vi,k,vj,k,ωk]T为控制向量；vi,k,vj,k分别表示两个移动机器人Ri和Rj的线速度，ωk表示两个移动机器人共同的角速度，wk＝[δvi,k,δvj,k,δωk]T为过程噪声并且服从高斯分布N(0,Q)，
Figure BDA0002284431920000043
为过程噪声方差阵，k表示时刻，Xk为k时刻的刚体状态，T为采样时间，L为刚体约束距离；
S1.3、构建绝对量测与相对量测相联合的量测模型：
Figure BDA0002284431920000044
其中Zk为k时刻的量测向量，h(·)为非线性量测方程，
Figure BDA0002284431920000045
为移动机器人Ri的绝对位置量测值，γi,k,γj,k分别是移动机器人Ri和Rj的旋转机构角度传感器的测量值，vk～N(0,R)为量测噪声，
Figure BDA0002284431920000051
为量测噪声方差阵，
Figure BDA0002284431920000052
为两机器人所搬运刚体的姿态角。
进一步的，所述的步骤S2具体包括以下步骤：
S2.1、初始化刚体状态向量及其协方差矩阵：
Figure BDA0002284431920000053
Figure BDA0002284431920000054
其中，X0为刚体状态向量的初值，E(·)为求期望操作；
S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式Hm(λ)的m个根
Figure BDA0002284431920000055
S2.3、计算
Figure BDA0002284431920000056
对应的单变量配置高斯积分点ξi及其系数ai：
Figure BDA0002284431920000057
Figure BDA0002284431920000058
S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量
Figure BDA0002284431920000059
及系数
Figure BDA00022844319200000510
其中l＝mn，
Figure BDA00022844319200000511
进一步的，所述的步骤S3具体包括以下步骤：
Figure BDA00022844319200000512
Figure BDA00022844319200000513
Figure BDA00022844319200000514
Figure BDA0002284431920000061
Figure BDA0002284431920000062
其中，Sk-1|k-1为对k-1时刻的协方差矩阵Pk-1|k-1进行Cholesky分解得到的下三角矩阵，Xi表示刚体状态向量
Figure BDA0002284431920000063
的第i个积分点。
进一步的，所述的步骤S4中计算卡尔曼增益的具体方法为：
Figure BDA0002284431920000064
Figure BDA0002284431920000065
Zi,k|k-1＝h(Xi,k|k-1)；
Figure BDA0002284431920000066
Figure BDA0002284431920000067
Figure BDA0002284431920000068
Figure BDA0002284431920000069
其中Zi表示量测向量的第i个积分点，PZZ,k|k-1,PXZ,k|k-1分别为新息协方差阵和互协方差阵，Kk为卡尔曼增益。
进一步的，S5的具体方法为：
Figure BDA00022844319200000610
Figure BDA00022844319200000611
其中
Figure BDA0002284431920000071
Pk|k分别为k时刻更新后的刚体状态估计值及协方差矩阵。
进一步的，所述的步骤S6的具体方法为：
Figure BDA0002284431920000072
其中
Figure BDA0002284431920000073
为k时刻两个移动机器人Ri和Rj的联合估计状态，Tran(·)表示将刚体状态X转换为机器人状态XR的非线性转换函数，x,y,θ分别表示移动机器人机体在导航坐标系下的横轴坐标、纵轴坐标以及方位角。
与现有技术相比，本发明的有益效果是：本发明通过刚性约束对漂移误差进行限制，同时利用耦合关系对待估计状态进行降维，有效提高了定位精度和计算效率。
附图说明
图1是本发明一种用于双移动机器人刚体协作搬运的协同定位方法的流程图；
图2是基于刚性约束的双机器人协作搬运模型示意图；
图3是仿真实验中本发明与几种现有技术的定位轨迹示意图；
图4是仿真实验中本发明与几种现有技术的定位误差图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，一种用于双移动机器人刚体协作搬运的协同定位方法，通过结合由刚性约束产生的相对距离及方位约束条件与高斯-厄米特求积分卡尔曼滤波器实现。协作搬运示意图如图2所示，两移动机器人对同一刚体进行协作搬运，其中刚体上的两点分别固定于机器人机体上的水平旋转机构。搬运过程中，由于刚体对机器人运动产生约束，两机器人之间相对距离为定值，且它们的运动方向需始终保持一致。机器人内部配备轮式编码器，旋转机构配有测量水平方向的角度传感器。其中轮式编码器用于测量机器人的运动线速度和角速度，水平位置的角度传感器用于量测所搬运刚体与机器人机体的相对方位。此外，其中一个机器人能够获取绝对位置量测值，可通过GPS、无线传感器网络等定位系统得到。基于刚性约束的双机器人协同定位方法利用刚性约束模型和量测信息实现对每个机器人的位置和姿态估计，该方法包括S1至S5。
一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，包括以下步骤：
S1、构建基于刚性约束的双机器人系统的运动学模型和量测模型；双机器人系统包括两个移动机器人，S1的具体方法包括S1.1至S1.3。
S1.1、设定刚体状态向量并对刚体状态向量赋初值(此处赋初值清楚，无需进一步说明)，刚体状态向量记为
Figure BDA0002284431920000091
表示所搬运刚体的位置，其中(xc,yc)为两个移动机器人的几何中心连线中点在导航坐标系下的坐标；
Figure BDA0002284431920000092
为(xc,yc)到其中一个移动机器人Ri几何中心(xi,yi)的延长线与导航坐标系横轴XNav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与XNav正半轴的夹角，表示刚体的运动方向；此处需要说明的是，赋初值是指对刚体状态量初始化，由于滤波开始时对双机器人系统的真实状态是未知的，需要对初始状态量进行设置以使之后用于定位的滤波器可正常工作，这个值可根据先验知识设置在真实初始状态附近，也可以设置为任意值，不需要特别精确(但是不能与真实初始值相差太大，否则可能导致滤波器发散)，因为初值会随着滤波器的运行逐渐收敛至真实值。设定了状态初值后，其根据运动模型进行演化(即预测)，然后利用量测信息对其进行修正，使得估计出的值与真实值之间的误差越来越小，从而实现对真实状态的精确估计，即实现精确定位。在本实施例中，刚体状态为刚体的位置、姿态及运动方向，设定初值时，机器人处于静止状态还未开始运动，滤波器也未开始运行。可将人对这些状态的主观估计量作为初值赋给它们，也可以任意指定，如全设置为0。当机器人开始运动时，滤波器同时开始运行，每隔固定采样时间T输出一次状态估计值，从而实现对双机器人位置与姿态的实时估计；
S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：
Figure BDA0002284431920000101
Xk+1＝f(Xk,uk,wk)＝Xk+T·Φk(uk+wk)；
Figure BDA0002284431920000102
其中f(·)为非线性状态转移方程；uk＝[vi,k,vj,k,ωk]T为控制向量；vi,k,vj,k分别表示两个移动机器人Ri和Rj的线速度，ωk表示两个移动机器人共同的角速度，wk＝[δvi,k,δvj,k,δωk]T为过程噪声并且服从高斯分布N(0,Q)，
Figure BDA0002284431920000103
为过程噪声方差阵，k表示时刻，Xk为k时刻的刚体状态，T为采样时间，L为刚体约束距离；
S1.3、构建绝对量测与相对量测相联合的量测模型：
Figure BDA0002284431920000104
其中Zk为k时刻的量测向量，h(·)为非线性量测方程，
Figure BDA0002284431920000105
为移动机器人Ri的绝对位置量测值，γi,k,γj,k分别是移动机器人Ri和Rj的旋转机构角度传感器的测量值，vk～N(0,R)为量测噪声，
Figure BDA0002284431920000111
为量测噪声方差阵，
Figure BDA0002284431920000112
为两机器人所搬运刚体的姿态角。
S2、初始化高斯-厄米特求积分卡尔曼滤波器，S2的具体方法包括S2.1至S2.4。
S2.1、初始化刚体状态向量及其协方差矩阵：
Figure BDA0002284431920000113
Figure BDA0002284431920000114
其中，X0为刚体状态向量的初值，E(·)为求期望操作；
S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式Hm(λ)的m个根
Figure BDA0002284431920000115
S2.3、计算
Figure BDA0002284431920000116
对应的单变量配置高斯积分点ξi及其系数ai：
Figure BDA0002284431920000117
Figure BDA0002284431920000118
S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量
Figure BDA0002284431920000119
及系数
Figure BDA00022844319200001110
其中l＝mn，
Figure BDA00022844319200001111
此处需要说明的是，使用克罗内克(Kronecker)张量积对单变量配置进行扩展的方法为现有技术，故不对其进行详细说明。
S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；步骤S3具体包括以下步骤：
Figure BDA0002284431920000121
Figure BDA0002284431920000122
Figure BDA0002284431920000123
Figure BDA0002284431920000124
Figure BDA0002284431920000125
其中，Sk-1|k-1为对k-1时刻的协方差矩阵Pk-1|k-1进行Cholesky分解得到的下三角矩阵，Xi表示刚体状态向量
Figure BDA0002284431920000126
的第i个积分点，此处需要说明的是，Cholesky分解是矩阵的一种分解方法，可以将对称正定的矩阵表示成一个下三角矩阵和其转置的乘积的分解，公式
Figure BDA0002284431920000127
即表示Cholesky分解，分解对象为k-1时刻的协方差矩阵Pk-1|k-1。
S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；
计算卡尔曼增益的具体方法为：
Figure BDA0002284431920000128
Figure BDA0002284431920000129
Zi,k|k-1＝h(Xi,k|k-1)；
Figure BDA00022844319200001210
Figure BDA0002284431920000131
Figure BDA0002284431920000132
Figure BDA0002284431920000133
其中Zi表示量测向量的第i个积分点，PZZ,k|k-1,PXZ,k|k-1分别为新息协方差阵和互协方差阵，Kk为卡尔曼增益。
S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值，具体方法如下：
Figure BDA0002284431920000134
Figure BDA0002284431920000135
其中
Figure BDA0002284431920000136
Pk|k分别为k时刻更新后的刚体状态估计值及协方差矩阵。
S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态，具体方法如下：
Figure BDA0002284431920000137
其中
Figure BDA0002284431920000138
为k时刻两个移动机器人Ri和Rj的联合估计状态，Tran(·)表示将刚体状态X转换为机器人状态XR的非线性转换函数，x,y,θ分别表示移动机器人机体在导航坐标系下的横轴坐标、纵轴坐标以及方位角。
本发明(在表1中简称为RCM-QKF)通过刚性约束对漂移误差进行限制，同时利用耦合关系对待估计状态进行降维，有效提高了定位精度和计算效率。为了验证本发明的效果，通过仿真实验将本发明与几种现有技术进行对比，具体仿真结果如图3和4所示，如图3所示，UM-EKF、UM-QKF以及本发明(RCM-QKF)的运动轨迹均与参考轨迹基本重合，如图4所示，UM-EKF的曲线位于最上方，本发明(RCM-QKF)的曲线位于最下方，UM-QKF的曲线基本位于两者之间，由此可以看出，本发明(RCM-QKF)具有最小的绝对定位误差，收敛后基本稳定在0.02m左右，而基于无约束模型的EKF协同定位方法(UM-EKF)由于对系统模型进行了一阶近似，定位误差较大。基于无约束模型的的QKF协同定位方法(UM-QKF)借助高斯-厄米特积分的高精度逼近能力，得到了较UM-EKF更小的定位误差，但其定位精度不及本发明，说明基于刚性约束的模型可以对误差上界进行有效限制。三种协同定位方法的计算时间如表1所示，可以看到，本发明的计算时间介于UM-EKF和UM-QKF之间。综上，本发明在保证较小定位误差的前提下仍具有较低的计算复杂度，实现对定位精度和效率的较好权衡。
Figure BDA0002284431920000141
表1不同协同定位方法计算时间对比
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，包括以下步骤：
S1、构建基于刚性约束的双机器人系统的运动学模型和量测模型；
S2、初始化高斯-厄米特求积分卡尔曼滤波器；
S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；
S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；
S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；
S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态。
2.如权利要求1所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S1具体包括以下步骤：
S1.1、设定刚体状态向量并对刚体状态向量赋初值，刚体状态向量记为
Figure FDA0002284431910000011
表示所搬运刚体的位置，其中(xc,yc)为两个移动机器人的几何中心连线中点在导航坐标系下的坐标；
Figure FDA0002284431910000012
为(xc,yc)到其中一个移动机器人Ri几何中心(xi,yi)的延长线与导航坐标系横轴XNav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与XNav正半轴的夹角，表示刚体的运动方向；
S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：
Figure FDA0002284431910000021
Xk+1＝f(Xk,uk,wk)＝Xk+T·Φk(uk+wk)；
Figure FDA0002284431910000022
其中f(·)为非线性状态转移方程；uk＝[vi,k,vj,k,ωk]T为控制向量；vi,k,vj,k分别表示两个移动机器人Ri和Rj的线速度，ωk表示两个移动机器人共同的角速度，wk＝[δvi,k,δvj,k,δωk]T为过程噪声并且服从高斯分布N(0,Q)，
Figure FDA0002284431910000023
为过程噪声方差阵，k表示时刻，Xk为k时刻的刚体状态，T为采样时间，L为刚体约束距离；
S1.3、构建绝对量测与相对量测相联合的量测模型：
Figure FDA0002284431910000024
其中Zk为k时刻的量测向量，h(·)为非线性量测方程，
Figure FDA0002284431910000025
为移动机器人Ri的绝对位置量测值，γi,k,γj,k分别是移动机器人Ri和Rj的旋转机构角度传感器的测量值，vk～N(0,R)为量测噪声，
Figure FDA0002284431910000026
为量测噪声方差阵，
Figure FDA0002284431910000027
为两机器人所搬运刚体的姿态角。
3.如权利要求2所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S2具体包括以下步骤：
S2.1、初始化刚体状态向量及其协方差矩阵：
Figure FDA0002284431910000031
Figure FDA0002284431910000032
其中，X0为刚体状态向量的初值，E(·)为求期望操作；
S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式Hm(λ)的m个根
Figure FDA0002284431910000033
S2.3、计算
Figure FDA0002284431910000034
对应的单变量配置高斯积分点ξi及其系数ai：
Figure FDA0002284431910000035
Figure FDA0002284431910000036
S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量
Figure FDA0002284431910000037
及系数
Figure FDA0002284431910000038
其中l＝mn，
Figure FDA0002284431910000039
4.如权利要求3所述的的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S3具体包括以下步骤：
Figure FDA00022844319100000310
Figure FDA00022844319100000311
Figure FDA0002284431910000041
Figure FDA0002284431910000042
Figure FDA0002284431910000043
其中，Sk-1|k-1为对k-1时刻的协方差矩阵Pk-1|k-1进行Cholesky分解得到的下三角矩阵，Xi表示刚体状态向量
Figure FDA0002284431910000044
的第i个积分点。
5.如权利要求4所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S4中计算卡尔曼增益的具体方法为：
Figure FDA0002284431910000045
Figure FDA0002284431910000046
Zi,k|k-1＝h(Xi,k|k-1)；
Figure FDA0002284431910000047
Figure FDA0002284431910000048
Figure FDA0002284431910000049
Figure FDA00022844319100000410
其中Zi表示量测向量的第i个积分点，PZZ,k|k-1,PXZ,k|k-1分别为新息协方差阵和互协方差阵，Kk为卡尔曼增益。
6.如权利要求5所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S5的具体方法为：
Figure FDA0002284431910000051
Figure FDA0002284431910000052
其中
Figure FDA0002284431910000053
Pk|k分别为k时刻更新后的刚体状态估计值及协方差矩阵。
7.如权利要求6所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S6的具体方法为：
Figure FDA0002284431910000054
其中
Figure FDA0002284431910000055
为k时刻两个移动机器人Ri和Rj的联合估计状态，Tran(·)表示将刚体状态X转换为机器人状态XR的非线性转换函数，x,y,θ分别表示移动机器人机体在导航坐标系下的横轴坐标、纵轴坐标以及方位角。
CN201911154494.8A 2019-11-22 2019-11-22 一种用于双移动机器人刚体协作搬运的协同定位方法 Active CN110967017B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911154494.8A CN110967017B (zh) 2019-11-22 2019-11-22 一种用于双移动机器人刚体协作搬运的协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911154494.8A CN110967017B (zh) 2019-11-22 2019-11-22 一种用于双移动机器人刚体协作搬运的协同定位方法

Publications (2)

Publication Number Publication Date
CN110967017A true CN110967017A (zh) 2020-04-07
CN110967017B CN110967017B (zh) 2023-03-31

Family

ID=70031221

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911154494.8A Active CN110967017B (zh) 2019-11-22 2019-11-22 一种用于双移动机器人刚体协作搬运的协同定位方法

Country Status (1)

Country Link
CN (1) CN110967017B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112394732A (zh) * 2020-11-20 2021-02-23 西北工业大学 一种适用于机器人集群协同搬运的接触力动态分配方法
CN113984073A (zh) * 2021-09-29 2022-01-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法
CN116608851A (zh) * 2023-07-19 2023-08-18 中山大学 地下管道双机器人步进式协同导航方法、系统及服务器

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160334793A1 (en) * 2015-04-09 2016-11-17 University Of New Hampshire POSE DETECTION AND CONTROL OF UNMANNED UNDERWATER VEHICLES (UUVs) UTILIZING AN OPTICAL DETECTOR ARRAY
CN106482736A (zh) * 2016-07-11 2017-03-08 安徽工程大学 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法
CN108759838A (zh) * 2018-05-23 2018-11-06 安徽科技学院 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法
US20190012801A1 (en) * 2017-07-07 2019-01-10 GameFace Labs Inc. Systems and methods for position and pose determination and tracking
CN109282817A (zh) * 2018-10-16 2019-01-29 中山大学 一种多机器人协同定位与控制方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160334793A1 (en) * 2015-04-09 2016-11-17 University Of New Hampshire POSE DETECTION AND CONTROL OF UNMANNED UNDERWATER VEHICLES (UUVs) UTILIZING AN OPTICAL DETECTOR ARRAY
CN106482736A (zh) * 2016-07-11 2017-03-08 安徽工程大学 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法
US20190012801A1 (en) * 2017-07-07 2019-01-10 GameFace Labs Inc. Systems and methods for position and pose determination and tracking
CN108759838A (zh) * 2018-05-23 2018-11-06 安徽科技学院 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法
CN109282817A (zh) * 2018-10-16 2019-01-29 中山大学 一种多机器人协同定位与控制方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
严浙平等: "模型辅助和洋流估计下的UUV导航定位方法", 《水下无人系统学报》 *
王碧霞等: "基于SR-UKF的纯方位多移动机器人协同定位算法", 《南京理工大学学报》 *
邵金鑫等: "基于高斯―施密特粒子滤波器的多机器人协同定位", 《计算机工程与科学》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112394732A (zh) * 2020-11-20 2021-02-23 西北工业大学 一种适用于机器人集群协同搬运的接触力动态分配方法
CN112394732B (zh) * 2020-11-20 2022-08-02 西北工业大学 一种适用于机器人集群协同搬运的接触力动态分配方法
CN113984073A (zh) * 2021-09-29 2022-01-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法
CN116608851A (zh) * 2023-07-19 2023-08-18 中山大学 地下管道双机器人步进式协同导航方法、系统及服务器
CN116608851B (zh) * 2023-07-19 2023-09-26 中山大学 地下管道双机器人步进式协同导航方法、系统及服务器

Also Published As

Publication number Publication date
CN110967017B (zh) 2023-03-31

Similar Documents

Publication Publication Date Title
Wu et al. Vins on wheels
Li et al. Adaptive trajectory tracking of nonholonomic mobile robots using vision-based position and velocity estimation
CN109885080B (zh) 自主控制系统及自主控制方法
CN110967017B (zh) 一种用于双移动机器人刚体协作搬运的协同定位方法
Liu et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble
CN107516326B (zh) 融合单目视觉和编码器信息的机器人定位方法和系统
Bloesch et al. The two-state implicit filter recursive estimation for mobile robots
Meng et al. Iterative-learning error compensation for autonomous parking of mobile manipulator in harsh industrial environment
Omari et al. Dense visual-inertial navigation system for mobile robots
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112113582A (zh) 时间同步处理方法、电子设备及存储介质
CN109724597B (zh) 一种基于函数迭代积分的惯性导航解算方法及系统
Yousuf et al. Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications
Zhou et al. Determining 3-D relative transformations for any combination of range and bearing measurements
Dang et al. Tightly-coupled data fusion of VINS and odometer based on wheel slip estimation
CN114111818A (zh) 一种通用视觉slam方法
CN111025229B (zh) 一种水下机器人纯方位目标估计方法
CN109443355B (zh) 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN114046800B (zh) 一种基于双层滤波框架的高精度里程估计方法
Zhang et al. A Robust Lidar SLAM System Based on Multi-Sensor Fusion
Fuchs et al. Advanced 3-D trailer pose estimation for articulated vehicles
Fu et al. A robust pose estimation method for multicopters using off-board multiple cameras
Dai et al. Unsupervised feature fusion combined with neural network applied to UAV attitude estimation
Huntsberger et al. Sensory fusion for planetary surface robotic navigation, rendezvous, and manipulation operations

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant