[发明专利]车载机械臂的感知定位方法

说明书

本发明涉及一种车载机械臂的感知定位方法。其包括如下步骤：步骤1、得到机械臂的视觉定位姿态向量与视觉定位位置向量；步骤2、得到机械臂的惯性定位姿态向量以及惯性定位位置向量，利用AFEKF对视觉定位姿态向量、视觉定位位置向量、惯性定位姿态向量以及惯性定位位置向量进行融合，以得到机械臂的联合状态矩阵；步骤3、利用UWB对车载机械臂进行室内定位，以得到UWB测量定位向量；步骤4、利用SHFAF方法对联合状态矩阵、UWB测量定位向量进行融合，并对UWB测量定位向量进行异常值检测与校正，以得到准确状态矩阵。本发明能有效实现对车载机械臂的定位，提高定位精度，安全可靠。

技术领域

本发明涉及一种感知定位方法，尤其是一种车载机械臂的感知定位方法。

背景技术

随着机器人领域的发展，移动机器人近些年越来越普遍。对于这些机器人来说，能够正确的知道周围的环境状况和自身的位置是其正常工作的保障。然而传统的GPS定位技术，受环境的限制太多。

过去人们通常使用雷达来收集环境的信息，但是激光雷达的成本太高。最近几年，人们采用相机作为视觉定位的工具，而相机易受光照条件的影响，并且图像处理算法处理大量的图像数据速度很慢无法达到实时性的要求。相比之下，惯性导航系统（INS）采用惯性测量装置（IMU）既可以满足实时性，又可以弥补相机的处理速度问题，不过它有累积误差的问题，因此并不适合作为远程室内导航定位来使用。

发明内容

本发明的目的是克服现有技术中存在的不足，提供一种车载机械臂的感知定位方法，其能有效实现对车载机械臂的定位，提高定位精度，安全可靠。

按照本发明提供的技术方案，车载机械臂的感知定位方法，所述感知定位方法包括如下步骤：

步骤1、利用视觉装置对车载机械臂进行室内定位，以能得到机械臂的视觉定位姿态向量与视觉定位位置向量；

步骤2、利用惯性装置对车载机械臂进行室内定位，以能得到机械臂的惯性定位姿态向量以及惯性定位位置向量，利用AFEKF对视觉定位姿态向量、视觉定位位置向量、惯性定位姿态向量以及惯性定位位置向量进行融合，以得到机械臂的联合状态矩阵；

步骤3、利用UWB对车载机械臂进行室内定位，以得到UWB测量定位向量；

步骤4、利用SHFAF方法对联合状态矩阵、UWB测量定位向量进行融合，并对UWB测量定位向量进行异常值检测与校正，以得到准确状态矩阵。

步骤1中，包括如下步骤：

步骤1.1、在进行室内定位时，利用Kinect相机实时获取多帧连续的彩色图像与深度图像；

步骤1.2、利用SURF方法对两帧连续的彩色图像提取公共特征点，并得到特征向量，且通过特征向量能得到机械臂姿态改变对应的方向向量；

步骤1.3、利用绝对定向方法计算连续两帧图像中机械臂方向的旋转矩阵，根据所述旋转矩阵以及上述的方向向量，能得到机械臂的视觉定位姿态向量；利用连续两帧深度图像中机械臂的偏移值，能得到视觉定位位置向量。

将惯性装置IMU安装于车载机械臂的底座上，并通过捷联惯导方法得到惯性定位姿态向量以及惯性定位位置向量。

本发明的优点：视觉定位和IMU定位两者结合，实现优势的互补，从而提高惯性导航系统的导航精度。同时，采用超宽带（UWB）来补偿IMU的累积误差。由于UWB在复杂的室内环境中易受多径效应和非视距因素影响的问题，采用模糊自适应滤波器（SHFAF）来解决复杂室内环境中时变噪声的问题，并通过检测和纠正异常值，提高车载机械臂的定位性能。

附图说明

图1为本发明的流程图。

图2本发明视觉装置对车载机械臂进行室内定位的流程图。