一种基于多传感器融合的机器人自抗扰定位方法以及定位系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN108253963A(43)申请公布日2018.07.06(21)申请号201711385817.5(22)申请日2017.12.20(71)申请人广西师范大学地址541004广西壮族自治区桂林市七星区育才路15号(72)发明人胡维平龚佳鹏吴磊(74)专利代理机构北京轻创知识产权代理有限公司11212代理人杨立周玉婷(51)Int.Cl.G01C21/16(2006.01)权利要求书4页说明书10页附图2页(54)发明名称一种基于多传感器融合的机器人自抗扰定位方法以及定位系统(57)摘要本发明涉及一种基于多传感器融合的机器人自抗扰定位方法，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位方法包括以下步骤：步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；步骤S3、若α≥Threshold(α)且β≥Threshold(β)时，采用视觉惯导融合定位，否则采用轮式惯导融合定位。本发明提供的机器人自抗扰定位方法融合多种传感器的优势，优化机器人定位的计算方法,消除定位过程中出现的误差。CN108253963ACN108253963A权利要求书1/4页1.一种基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位方法包括以下步骤：步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；步骤S3、若α≥Threshold(α)且β≥Threshold(β)时，采用视觉惯导融合定位，否则采用轮式惯导融合定位。2.根据权利要求1所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述视觉惯导融合定位具体包括以下子步骤：步骤S31、通过所述双目视觉里程计获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵；步骤S32、通过所述惯导获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息；步骤S33、建立扩展卡尔曼滤波模型，并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。3.根据权利要求2所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述子步骤S31进一步包括以下子步骤：步骤S311、设定所述双目视觉里程计的双目相机平行安装，通过所述双目相机获取双目图像，对所述双目图像的特征点进行匹配，利用视差计算出所述特征点的三维坐标；步骤S312、以所述双目相机的左相机为基准，建立相机坐标系下前后时刻对应特征点的运动方程组:Pk＝RPk-1+T式中:Pk表示k时刻所述特征点在相机坐标系中的三维坐标值，Pk-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值，R表示旋转矩阵，T表示平移矩阵；去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。4.根据权利要求3所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述姿态角信息包括俯仰角θ、侧倾角φ以及航向角ψ，所述子步骤S32进一步包括以下子步骤：步骤S321、通过所述惯导获取三轴加速度ax、ay以及az，仅重力作用时所述三轴加速度ax、ay以及az与俯仰角θ以及侧倾角φ之间满足以下关系式：其中，g为重力加速度,求解上式得到俯仰角θ和侧倾角φ；步骤S322、通过所述惯导获取角速度ωg，在k时刻至k+1时刻之间的时间段内对角速度ωg积分，得到航向角变化量Δψ，再将所述航向角变化量Δψ累加得到航向角ψ。5.根据权利要求4所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述2CN108253963A权利要求书2/4页子步骤S33进一步包括以下子步骤：步骤S331、利用所述姿态角信息计算姿态四元素e0、e1、e2以及e3，定义所述扩展卡尔曼T滤波模型的系统状态变量D＝[e0e1e2e3]，并建立系统的状态方程：Dk+1＝q*Dk+w(k)其中，DK表示k时刻的状态变量，DK+1表示k+1时刻的状态变量，q表示k时刻到k+1时刻机器人的旋转向量，q由旋转矩阵R计算得到，*为四元素乘法，w(k)表示所述状态方程的噪声向量，求解所述状态方程得到k+1时刻的状态变量Dk+1；步骤S332、建立观测方程：其中，v(k)表示观测过程的高斯噪声矩阵，求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角，所述融合姿态角包括融合