一种用于双移动机器人刚体协作搬运的协同定位方法
专利汇可以提供一种用于双移动机器人刚体协作搬运的协同定位方法专利检索，专利查询，专利分析的服务。并且本 发明 提供一种用于双移动 机器人 刚体协作搬运的协同 定位 方法，包括以下步骤：S1、构建基于刚性约束的双机器人系统的 运动学模型 和量测模型；S2、初始化高斯-厄米特求积分卡尔曼 滤波器 ；S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；S6、将更新后的刚体状态估计值转换为双机器人系统中的两个 移动机器人 的 位姿 状态，其能够融合由不变刚性约束产生的相对距离与方位约束条件限制误差上界，使得估计结果更加接近真实运动轨迹。，下面是一种用于双移动机器人刚体协作搬运的协同定位方法专利的具体信息内容。
1.一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，包括以下步骤：
S1、构建基于刚性约束的双机器人系统的运动学模型和量测模型；
S2、初始化高斯-厄米特求积分卡尔曼滤波器；
S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；
S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；
S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；
S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态。
2.如权利要求1所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S1具体包括以下步骤：
S1.1、设定刚体状态向量并对刚体状态向量赋初值，刚体状态向量记为
表示所搬运刚体的位置，其中(xc,yc)为两个移动机器人的几何中心连线中点在导航坐标系下的坐标； 为(xc,yc)到其中一个移动机器人Ri几何中心(xi,yi)的延长线与导航坐标系横轴XNav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与XNav正半轴的夹角，表示刚体的运动方向；
S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：
Xk+1＝f(Xk,uk,wk)＝Xk+T·Φk(uk+wk)；
其中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)， 为过程噪声方差阵，k
表示时刻，Xk为k时刻的刚体状态，T为采样时间，L为刚体约束距离；
S1.3、构建绝对量测与相对量测相联合的量测模型：
其中Zk为k时刻的量测向量，h(·)为非线性量测方程， 为移动机器人Ri的绝对位置量测值，γi,k,γj,k分别是移动机器人Ri和Rj的旋转机构角度传感器的测量值，vk～N(0,R)为量测噪声， 为量测噪声方差阵， 为两机器人所搬
运刚体的姿态角。
3.如权利要求2所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S2具体包括以下步骤：
S2.1、初始化刚体状态向量及其协方差矩阵：
其中，X0为刚体状态向量的初值，E(·)为求期望操作；
S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式Hm(λ)的m个根
S2.3、计算 对应的单变量配置高斯积分点ξi及其系数ai：
S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量 及系数 其中l＝mn，
4.如权利要求3所述的的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S3具体包括以下步骤：
其中，Sk-1|k-1为对k-1时刻的协方差矩阵Pk-1|k-1进行Cholesky分解得到的下三角矩阵，Xi表示刚体状态向量 的第i个积分点。
5.如权利要求4所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S4中计算卡尔曼增益的具体方法为：
Zi,k|k-1＝h(Xi,k|k-1)；
其中Zi表示量测向量的第i个积分点，PZZ,k|k-1,PXZ,k|k-1分别为新息协方差阵和互协方差阵，Kk为卡尔曼增益。
6.如权利要求5所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S5的具体方法为：
其中 Pk|k分别为k时刻更新后的刚体状态估计值及协方差矩阵。
7.如权利要求6所述的一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，所述的步骤S6的具体方法为：
其中 为k时刻两个移动机器人Ri和Rj的联合估计状态，Tran(·)表示将刚体状态X转换为机器人状态XR的非线性转换函数，x,y,θ分别表示移动机器人机体在导航坐标系下的横轴坐标、纵轴坐标以及方位角。
一种用于双移动机器人刚体协作搬运的协同定位方法
技术领域
背景技术
发明内容
S2、初始化高斯-厄米特求积分卡尔曼滤波器；
S3、基于运动学模型对双机器人系统进行状态推演，得到刚体状态预测值；
S4、获取双机器人系统的状态量测值，并且将状态量测值与量测模型相结合计算卡尔曼增益；
S5、利用卡尔曼增益对刚体状态预测值进行更新后得到刚体状态估计值；
S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态。
表示所搬运刚体的位置，其中(xc,yc)为两个移动机器人的几何中心连线中点在导航坐标系下的坐标； 为(xc,yc)到其中一个移动机器人Ri几何中心(xi,yi)的延长线与导航坐标系横轴XNav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与XNav正半轴的夹角，表示刚体的运动方向；
S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：
Xk+1＝f(Xk,uk,wk)＝Xk+T·Φk(uk+wk)；
其中f(·)为非线性状态转移方程；uk＝[vi,k,vj,k,ωk]T为控制向量；vi,k,vj,k分别表示两个移动机器人Ri和Rj的线速度，ωk表示两个移动机器人共同的角速度，wk＝[δvi,k,δvj,k,T
δωk] 为过程噪声并且服从高斯分布N(0,Q)， 为过程噪声方差阵，k
表示时刻，Xk为k时刻的刚体状态，T为采样时间，L为刚体约束距离；
S1.3、构建绝对量测与相对量测相联合的量测模型：
其中Zk为k时刻的量测向量，h(·)为非线性量测方程， 为移动机器人Ri的绝对位置量测值，γi,k,γj,k分别是移动机器人Ri和Rj的旋转机构角度传感器的测量值，vk～N(0,R)为量测噪声， 为量测噪声方差阵， 为两机器人所搬
运刚体的姿态角。
其中，X0为刚体状态向量的初值，E(·)为求期望操作；
S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式Hm(λ)的m个根
S2.3、计算 对应的单变量配置高斯积分点ξi及其系数ai：
S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量 及系数 其中l＝mn，
其中Zi表示量测向量的第i个积分点，PZZ,k|k-1,PXZ,k|k-1分别为新息协方差阵和互协方差阵，Kk为卡尔曼增益。
图3是仿真实验中本发明与几种现有技术的定位轨迹示意图；
图4是仿真实验中本发明与几种现有技术的定位误差图。
具体实施方式
S1.1、设定刚体状态向量并对刚体状态向量赋初值(此处赋初值清楚，无需进一步说明)，刚体状态向量记为 表示所搬运刚体的位置，其中(xc,yc)为两个移
动机器人的几何中心连线中点在导航坐标系下的坐标；为(xc,yc)到其中一个移动机器人Ri几何中心(xi,yi)的延长线与导航坐标系横轴XNav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与XNav正半轴的夹角，表示刚体的运动方向；此处需要说明的是，赋初值是指对刚体状态量初始化，由于滤波开始时对双机器人系统的真实状态是未知的，需要对初始状态量进行设置以使之后用于定位的滤波器可正常工作，这个值可根据先验知识设置在真实初始状态附近，也可以设置为任意值，不需要特别精确(但是不能与真实初始值相差太大，否则可能导致滤波器发散)，因为初值会随着滤波器的运行逐渐收敛至真实值。设定了状态初值后，其根据运动模型进行演化(即预测)，然后利用量测信息对其进行修正，使得估计出的值与真实值之间的误差越来越小，从而实现对真实状态的精确估计，即实现精确定位。在本实施例中，刚体状态为刚体的位置、姿态及运动方向，设定初值时，机器人处于静止状态还未开始运动，滤波器也未开始运行。可将人对这些状态的主观估计量作为初值赋给它们，也可以任意指定，如全设置为0。当机器人开始运动时，滤波器同时开始运行，每隔固定采样时间T输出一次状态估计值，从而实现对双机器人位置与姿态的实时估计；
S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：
Xk+1＝f(Xk,uk,wk)＝Xk+T·Φk(uk+wk)；
其中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)， 为过程噪声方差阵，k
表示时刻，Xk为k时刻的刚体状态，T为采样时间，L为刚体约束距离；
S1.3、构建绝对量测与相对量测相联合的量测模型：
其中Zk为k时刻的量测向量，h(·)为非线性量测方程， 为移动机器人Ri的绝对位置量测值，γi,k,γj,k分别是移动机器人Ri和Rj的旋转机构角度传感器的测量值，vk～N(0,R)为量测噪声， 为量测噪声方差阵， 为两机器人所
搬运刚体的姿态角。
其中，X0为刚体状态向量的初值，E(·)为求期望操作；
S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式Hm(λ)的m个根
S2.3、计算 对应的单变量配置高斯积分点ξi及其系数ai：
S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量 及系数 其中l＝mn， 此处需
要说明的是，使用克罗内克(Kronecker)张量积对单变量配置进行扩展的方法为现有技术，故不对其进行详细说明。
解是矩阵的一种分解方法，可以将对称正定的矩阵表示成一个下三角矩阵和其转置的乘积的分解，公式 即表示Cholesky分解，分解对象为k-1时刻的协方差
矩阵Pk-1|k-1。
Zi,k|k-1＝h(Xi,k|k-1)；
其中Zi表示量测向量的第i个积分点，PZZ,k|k-1,PXZ,k|k-1分别为新息协方差阵和互协方差阵，Kk为卡尔曼增益。
表1不同协同定位方法计算时间对比
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种地下电缆隧道巡检机器人 | 2020-05-13 | 870 |
| 用于将轴固定到安装部件的固定装置和机器人的驱动器 | 2020-05-14 | 367 |
| 控制机器人的圆弧动作的机器人控制装置 | 2020-05-14 | 970 |
| 一种3+4新构型双臂协作机器人加工系统及方法 | 2020-05-16 | 615 |
| 一种用于协作机器人的控制柜 | 2020-05-11 | 812 |
| 一种基于六自由度机器人的智能装配系统及方法 | 2020-05-13 | 932 |
| 用于训练相互依赖的自主机器的方法和设备 | 2020-05-14 | 756 |
| 一种基于情感识别的老人陪护机器人子系统 | 2020-05-08 | 321 |
| 一种变电运行协作机器人防撞装置 | 2020-05-13 | 878 |
| 一种智能化自动装卸型工业机器人 | 2020-05-15 | 399 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
