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

权利要求书

1.一种用于双移动机器人刚体协作搬运的协同定位方法，其特征在于，包括以下步骤：

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

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

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

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

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

S6、将更新后的刚体状态估计值转换为双机器人系统中的两个移动机器人的位姿状态；

所述的步骤S1具体包括以下步骤：

S1.1、设定刚体状态向量并对刚体状态向量赋初值，刚体状态向量记为表示所搬运刚体的位置，其中(x_c,y_c)为两个移动机器人的几何中心连线中点在导航坐标系下的坐标；/为(x_c,y_c)到其中一个移动机器人R_i几何中心(x_i,y_i)的延长线与导航坐标系横轴X_Nav正半轴的夹角，表示刚体的姿态；θ为移动机器人的朝向与X_Nav正半轴的夹角，表示刚体的运动方向；

S1.2、基于刚体状态向量构建带有刚性约束的双机器人系统离散运动方程，并得到相应的运动学模型：

X_k+1＝f(X_k,u_k,w_k)＝X_k+T·Φ_k(u_k+w_k)；

其中f(·)为非线性状态转移方程；u_k＝[v_i,k,v_j,k,ω_k]^T为控制向量；v_i,k,v_j,k分别表示两个移动机器人R_i和R_j的线速度，ω_k表示两个移动机器人共同的角速度，w_k＝[δv_i,k,δv_j,k,δω_k]^T为过程噪声并且服从高斯分布N(0,Q)，为过程噪声方差阵，k表示时刻，X_k为k时刻的刚体状态，T为采样时间，L为刚体约束距离；

S1.3、构建绝对量测与相对量测相联合的量测模型：

其中Z_k为k时刻的量测向量，h(·)为非线性量测方程，为移动机器人R_i的绝对位置量测值，γ_i,k,γ_j,k分别是移动机器人R_i和R_j的旋转机构角度传感器的测量值，v_k～N(0,R)为量测噪声，/为量测噪声方差阵，/为两机器人所搬运刚体的姿态角；

所述的步骤S2具体包括以下步骤：

S2.1、初始化刚体状态向量及其协方差矩阵：

其中，X₀为刚体状态向量的初值，E(·)为求期望操作；

S2.2、将高斯-厄米特求积分卡尔曼滤波器的积分点个数记为m，计算厄米特多项式H_m(λ)的m个根

S2.3、计算对应的单变量配置高斯积分点ξ_i及其系数a_i：

S2.4、通过克罗内克张量积将单变量配置扩展得到n维状态，得到n维变量的高斯积分点配置向量及系数/其中l＝mⁿ，/

所述的步骤S3具体包括以下步骤：

/

其中，S_k-1|k-1为对k-1时刻的协方差矩阵P_k-1|k-1进行Cholesky分解得到的下三角矩阵，X_i表示刚体状态向量的第i个积分点。