CN111890373A - 车载机械臂的感知定位方法 - Google Patents

车载机械臂的感知定位方法 Download PDF

Info

Publication number
CN111890373A
CN111890373A CN202011047302.6A CN202011047302A CN111890373A CN 111890373 A CN111890373 A CN 111890373A CN 202011047302 A CN202011047302 A CN 202011047302A CN 111890373 A CN111890373 A CN 111890373A
Authority
CN
China
Prior art keywords
positioning
mechanical arm
vector
inertial
vehicle
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.)
Pending
Application number
CN202011047302.6A
Other languages
English (en)
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.)
Changzhou Weishi Intelligent Iot Innovation Center Co ltd
Original Assignee
Changzhou Weishi Intelligent Iot Innovation Center Co ltd
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 Changzhou Weishi Intelligent Iot Innovation Center Co ltd filed Critical Changzhou Weishi Intelligent Iot Innovation Center Co ltd
Priority to CN202011047302.6A priority Critical patent/CN111890373A/zh
Publication of CN111890373A publication Critical patent/CN111890373A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/04—Viewing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种车载机械臂的感知定位方法。其包括如下步骤：步骤1、得到机械臂的视觉定位姿态向量
Figure 530142DEST_PATH_IMAGE001
与视觉定位位置向量
Figure 292430DEST_PATH_IMAGE002
；步骤2、得到机械臂的惯性定位姿态向量
Figure 504100DEST_PATH_IMAGE003
以及惯性定位位置向量
Figure 508571DEST_PATH_IMAGE004
，利用AFEKF对视觉定位姿态向量
Figure 613931DEST_PATH_IMAGE001
、视觉定位位置向量
Figure 501115DEST_PATH_IMAGE002
、惯性定位姿态向量
Figure 246086DEST_PATH_IMAGE003
以及惯性定位位置向量
Figure 40867DEST_PATH_IMAGE005
进行融合，以得到机械臂的联合状态矩阵
Figure 733DEST_PATH_IMAGE006
；步骤3、利用UWB对车载机械臂进行室内定位，以得到UWB测量定位向量
Figure 606289DEST_PATH_IMAGE007
；步骤4、利用SHFAF方法对联合状态矩阵
Figure 58130DEST_PATH_IMAGE006
、UWB测量定位向量
Figure 640290DEST_PATH_IMAGE007
进行融合，并对UWB测量定位向量
Figure 454662DEST_PATH_IMAGE007
进行异常值检测与校正，以得到准确状态矩阵
Figure 745966DEST_PATH_IMAGE008
。本发明能有效实现对车载机械臂的定位，提高定位精度，安全可靠。

Description

车载机械臂的感知定位方法
技术领域
本发明涉及一种感知定位方法，尤其是一种车载机械臂的感知定位方法。
背景技术
随着机器人领域的发展，移动机器人近些年越来越普遍。对于这些机器人来说，能够正确的知道周围的环境状况和自身的位置是其正常工作的保障。然而传统的GPS定位技术，受环境的限制太多。
过去人们通常使用雷达来收集环境的信息，但是激光雷达的成本太高。最近几年，人们采用相机作为视觉定位的工具，而相机易受光照条件的影响，并且图像处理算法处理大量的图像数据速度很慢无法达到实时性的要求。相比之下，惯性导航***（INS）采用惯性测量装置（IMU）既可以满足实时性，又可以弥补相机的处理速度问题，不过它有累积误差的问题，因此并不适合作为远程室内导航定位来使用。
发明内容
本发明的目的是克服现有技术中存在的不足，提供一种车载机械臂的感知定位方法，其能有效实现对车载机械臂的定位，提高定位精度，安全可靠。
按照本发明提供的技术方案，车载机械臂的感知定位方法，所述感知定位方法包括如下步骤：
步骤1、利用视觉装置对车载机械臂进行室内定位，以能得到机械臂的视觉定位姿态向 量
Figure 824413DEST_PATH_IMAGE001
与视觉定位位置向量
Figure 210395DEST_PATH_IMAGE002
；
步骤2、利用惯性装置对车载机械臂进行室内定位，以能得到机械臂的惯性定位姿态向 量
Figure 98455DEST_PATH_IMAGE003
以及惯性定位位置向量
Figure 493664DEST_PATH_IMAGE004
，利用AFEKF对视觉定位姿态向量
Figure 776878DEST_PATH_IMAGE001
、视觉定位位置向量
Figure 396078DEST_PATH_IMAGE002
、惯性定位姿态向量
Figure 663111DEST_PATH_IMAGE005
以及惯性定位位置向量
Figure 206219DEST_PATH_IMAGE006
进行融合，以得到机械臂的联合状态矩 阵
Figure 609519DEST_PATH_IMAGE007
；
步骤3、利用UWB对车载机械臂进行室内定位，以得到UWB测量定位向量
Figure 196358DEST_PATH_IMAGE008
；
步骤4、利用SHFAF方法对联合状态矩阵
Figure 950687DEST_PATH_IMAGE009
、UWB测量定位向量
Figure 733704DEST_PATH_IMAGE010
进行融合，并对UWB测 量定位向量
Figure 991510DEST_PATH_IMAGE011
进行异常值检测与校正，以得到准确状态矩阵
Figure 421355DEST_PATH_IMAGE012
。
步骤1中，包括如下步骤：
步骤1.1、在进行室内定位时，利用Kinect相机实时获取多帧连续的彩色图像与深度图像；
步骤1.2、利用SURF方法对两帧连续的彩色图像提取公共特征点，并得到特征向量，且通过特征向量能得到机械臂姿态改变对应的方向向量；
步骤1.3、利用绝对定向方法计算连续两帧图像中机械臂方向的旋转矩阵，根据所述旋 转矩阵以及上述的方向向量，能得到机械臂的视觉定位姿态向量
Figure 459718DEST_PATH_IMAGE001
；利用连续两帧深度图 像中机械臂的偏移值，能得到视觉定位位置向量
Figure 344628DEST_PATH_IMAGE002
。
将惯性装置IMU安装于车载机械臂的底座上，并通过捷联惯导方法得到惯性定位 姿态向量
Figure 456941DEST_PATH_IMAGE013
以及惯性定位位置向量
Figure 385583DEST_PATH_IMAGE004
。
本发明的优点：视觉定位和IMU定位两者结合，实现优势的互补，从而提高惯性导航***的导航精度。同时，采用超宽带（UWB）来补偿IMU的累积误差。由于UWB在复杂的室内环境中易受多径效应和非视距因素影响的问题，采用模糊自适应滤波器（SHFAF）来解决复杂室内环境中时变噪声的问题，并通过检测和纠正异常值，提高车载机械臂的定位性能。
附图说明
图1为本发明的流程图。
图2本发明视觉装置对车载机械臂进行室内定位的流程图。
具体实施方式
下面结合具体附图和实施例对本发明作进一步说明。
如图1所示，为了能有效实现对车载机械臂的定位，提高定位精度，本发明的感知定位方法包括如下步骤：
步骤1、利用视觉装置对车载机械臂进行室内定位，以能得到机械臂的视觉定位姿态向 量
Figure 583346DEST_PATH_IMAGE014
与视觉定位位置向量
Figure 239324DEST_PATH_IMAGE002
；
具体地，机械臂的底座是不方便移动的，我们把机械臂固定到小车上，使机械臂跟小车 一起运动，因此叫做车载机械臂，需要对其进行定位。在利用视觉装置进行室内定位时，使 用Kincet相机实时获取多帧的连续彩色图像和深度图像，采用SURF方法（加速健壮尺度不 变特征变化）对两帧连续彩色图像进行特征匹配，以提取公共特征点和相应的特征向量。两 个相邻公共特征点的三维坐标相减，乘以利用绝对定向方法计算得到的旋转矩阵，即得到 当前视觉定位位置向量
Figure 206143DEST_PATH_IMAGE015
。通过特征向量的改变得到机械臂姿态改变在模型坐标系下的方 向向量，采用绝对定向方法计算两帧连续彩色图像中机械臂的旋转矩阵，将模型坐标系下 的方向向量乘以该旋转矩阵，从而得到目标坐标系下的方向向量，即机械臂的姿态改变信 息，结合上一姿态向量即得到新的视觉定位姿态向量
Figure 977790DEST_PATH_IMAGE016
。
本发明实例中，目标坐标系即绝对坐标系，是在当前区域内规定的一个原点坐标，水平向右方向为x轴，竖直向前方向为y轴，垂直地面向上方向为z轴，下面所有的坐标都是在这个坐标系下相对于原点的坐标。
模型坐标系即相对坐标系，为各个机械臂自身的坐标系。直接获取到的坐标都是模型坐标系下的坐标，需要统一转换成目标坐标系的向量坐标进行计算。其中，绝对定向方法就是把模型坐标系下的坐标转换到目标坐标系下的过程。
具体实施时，为了避免视野阻挡，将Kinect相机固定在机械臂的抓手上方且不会 被机械臂遮挡的任意位置。设通过Kincet相机获取到的特征点的像素坐标为
Figure 521904DEST_PATH_IMAGE017
，在采用常 规手段进行失真校正之后，通过针孔相机模型和相机的内部参数获得点在模型坐标系下的 三维坐标
Figure 138830DEST_PATH_IMAGE018
。
SURF算法是经典SIFT算法的加速版，是特征匹配的常用算法。采用SURF算法，从连续帧彩色图像的像素坐标数据中提取公共特征点和特征向量，在彩色图像中分别提取出特征点的像素坐标，并利用本技术领域常规的技术手段得到这些特征点的三维坐标。
利用SURF算法从彩色图像中提取出特征点后，由于受到噪声、光照等外部条件的影响，仍然存在许多不匹配点，需要进一步提高精度。采用RANSAC算法消除特征点中的不匹配点，得到受外部条件影响较小的匹配特征点。
由于照相机采用红外照相机，红外辐射的距离会受到天气影响，使红外辐射强度过高，传感器饱和，从而降低定位精度，为了提高数据的准确性，可采用冒泡排序法，选取四个较大间距的特征点，然后取所述四个特征点的三维坐标平均值作为正确的结果。
绝对定向方法分为三步：模型坐标系相对于目标坐标系旋转；模型坐标系相对于目标坐标系平移；确定模型标度因子。
在使用SURF方法和RANSAC方法之后，Kinect相机在第一个位置捕捉到的特征点的 三维坐标被标记为data1，在第二位置得到的特征点的三维坐标为data2；然后，采用冒泡排 序法对data1、data2进行处理和平均，两者相减，乘以利用绝对定向方法计算得到的旋转矩 阵，结合上一位置向量即得到当前视觉定位位置向量
Figure 491314DEST_PATH_IMAGE019
。data2和data1对应的 特征向量相减得到在模型坐标系下的方向向量，采用绝对定向方法计算两帧连续彩色图像 中机械臂的旋转矩阵，将模型坐标系下的方向向量乘以该旋转矩阵，从而得到目标坐标系 下的方向向量，即机械臂的姿态改变信息，结合上一姿态向量得到机械臂当前的视觉定位 姿态向量
Figure 981332DEST_PATH_IMAGE020
，其中
Figure 12742DEST_PATH_IMAGE021
，
Figure 433359DEST_PATH_IMAGE022
，
Figure 640349DEST_PATH_IMAGE023
分别代表机械臂姿态与绝对坐标系在X轴，Y轴， Z轴的夹角。
机械臂最初位于坐标原点。当经第一位置和第二位置移动到第三位置时，将获得 新的特征点作为新的data2，在第二位置获取的特征点作为新的data1。数据更新后，用新的 data1和data2计算出机械臂从第二位置到第三位置的相对运动参数，结合上一位置向量计 算得到机械臂的视觉定位姿态向量
Figure 614120DEST_PATH_IMAGE024
与视觉定位位置向量
Figure 539351DEST_PATH_IMAGE025
；视觉定位流程，如图2所示。
步骤2、利用惯性装置对车载机械臂进行室内定位，以能得到机械臂的惯性定位姿 态向量
Figure 560396DEST_PATH_IMAGE003
以及惯性定位位置向量
Figure 90735DEST_PATH_IMAGE026
，利用AFEKF对视觉定位姿态向量
Figure 375085DEST_PATH_IMAGE027
、视觉定位位置向量
Figure 397399DEST_PATH_IMAGE025
、惯性定位姿态向量
Figure 222136DEST_PATH_IMAGE028
以及惯性定位位置向量
Figure 403718DEST_PATH_IMAGE029
进行融合，以得到机械臂的联合状态矩 阵
Figure 62233DEST_PATH_IMAGE030
；
具体地，惯性装置IMU一般安装在被测物体的重心上，因此，将IMU安装到机械臂底座 上。IMU的核心部件是陀螺仪和加速度计，其中陀螺仪测量机械臂运动的角速度
Figure 70378DEST_PATH_IMAGE031
，加速 度计测量机械臂运动的加速度
Figure 105330DEST_PATH_IMAGE032
，根据加速度和基础物理运动学公式得到速度
Figure 406998DEST_PATH_IMAGE033
。通过 捷联惯导算法得到机械臂的惯性定位姿态向量
Figure 564310DEST_PATH_IMAGE028
以及惯性定位位置向量
Figure 951429DEST_PATH_IMAGE034
。
捷联惯导方法的基本思想是使用前一时刻的导航参数（姿态、速度和位置）作为初 始值，并使用IMU的输出（角速度
Figure 665438DEST_PATH_IMAGE035
和加速度
Figure 290455DEST_PATH_IMAGE036
），通过递归积分的方式计算得到机械臂 当前的导航参数，即机械臂的惯性定位姿态向量
Figure 87510DEST_PATH_IMAGE037
和惯性定位位置向量
Figure 289821DEST_PATH_IMAGE038
。当前的导航参数将被用作下一次解的已知值。
由于视觉传感器受光线条件的限制，不能在黑暗环境中工作，长期工作也产生累积误差。另外，IMU也存在***误差及随机误差。为了结果更准确，可利用自适应衰落扩展卡尔曼滤波器AFEKF对Kinect相机和IMU输出的位置和姿态向量进行融合，也就是从原本因为误差的存在而不确定性高的分布中，得到一个不确定性更小的分布。AFEKF是广泛应用在传感器数据融合等方面的算法，具体为本技术领域人员所熟知，此处不再赘述。
至此，使用AFEKF得到Kinect相机和IMU的融合结果，即得到联合状态矩阵
Figure 509318DEST_PATH_IMAGE039
，其中包含了由Kinect相机和IMU融合得到的位置向量
Figure 520000DEST_PATH_IMAGE040
和姿态向量
Figure 222376DEST_PATH_IMAGE041
， 联合状态矩阵
Figure 911984DEST_PATH_IMAGE030
是由位置向量
Figure 623588DEST_PATH_IMAGE040
和姿态向量
Figure 36246DEST_PATH_IMAGE041
组合而成的矩阵。在后续步骤中会对该结 果进行处理。
步骤3、利用UWB对车载机械臂进行室内定位，以得到UWB测量定位向量
Figure 643945DEST_PATH_IMAGE042
；
本发明实例中，将利用UWB测量方法获得新的位置和速度向量。在UWB测量***中，标签（Tag）固定在机械臂上，用来发出脉冲信号，锚点（Anchor）是用来接收标签发出的信号的基站，至少需要三个以上。采用最小二乘法估计UWB标签的位置，具体计算过程如下：
设UWB锚点的位置向量为
Figure 227373DEST_PATH_IMAGE043
，其中
Figure 336143DEST_PATH_IMAGE044
表示锚点的 序列号，N是锚点的总数，锚点是UWB的测量基站，因此，锚点的位置向量
Figure 790258DEST_PATH_IMAGE045
是固定的，需要 给定。
Figure 100017DEST_PATH_IMAGE046
表示标签的位置向量，通过UWB测量得到，就是公式（3-6）得到的结果。则在时间k 处UWB标签的位置向量表示为
Figure 216746DEST_PATH_IMAGE047
，则在时间k处从UWB标签到第n 个锚点的距离
Figure 863628DEST_PATH_IMAGE048
计算公式为
Figure 703408DEST_PATH_IMAGE049
（3-1）
根据等式（3-1）有
Figure 918489DEST_PATH_IMAGE050
（3-2）
设G和
Figure 758400DEST_PATH_IMAGE051
如下，
Figure 615498DEST_PATH_IMAGE052
（3-3）
Figure 903259DEST_PATH_IMAGE053
（3-4）
则，用公式（3-2）中的第二行，第三行，...，第
Figure 289241DEST_PATH_IMAGE054
行分别减去第一行，结合算式（3-3）， （3-4），得到线性方程
Figure 334558DEST_PATH_IMAGE055
（3-5）
解该线性方程得UWB标签位置向量为
Figure 578369DEST_PATH_IMAGE056
（3-6）
设
Figure 127162DEST_PATH_IMAGE057
为标签的速度向量，
Figure 11942DEST_PATH_IMAGE058
是第k个时刻的时间，则根据基础物理运动学原理，在 时刻k处标签的速度向量
Figure 747817DEST_PATH_IMAGE057
为
Figure 946717DEST_PATH_IMAGE059
（3-7）
设由IMU进行室内定位得到的机械臂在时间
Figure 959803DEST_PATH_IMAGE060
的位置向量和速度向量分别为
Figure 15484DEST_PATH_IMAGE061
和
Figure 769813DEST_PATH_IMAGE062
，则UWB测量结果
Figure 975667DEST_PATH_IMAGE063
为
Figure 607374DEST_PATH_IMAGE064
（3-8）
至此，得到UWB的测量结果
Figure 37218DEST_PATH_IMAGE042
，并将在后续对
Figure 482106DEST_PATH_IMAGE042
进行异常值校正。
步骤4、利用SHFAF方法对联合状态矩阵X、UWB测量定位向量
Figure 819547DEST_PATH_IMAGE065
进行融合，并对 UWB测量定位向量
Figure 463018DEST_PATH_IMAGE065
进行异常值检测与校正，以得到准确状态矩阵
Figure 142392DEST_PATH_IMAGE066
。
由于室内环境通常很复杂（动态，遮挡因子等），UWB测量结果通常受多径效应和 NLOS因子影响，可能带来异常值，导致性能下降，甚至导致滤波器发散。因此，使用SHFAF估 计精度和鲁棒性可在计算复杂度较低的情况下得以提高。通过使用SHFAF方法，对机械臂的 联合状态矩阵
Figure 605734DEST_PATH_IMAGE030
和UWB测量定位向量
Figure 278024DEST_PATH_IMAGE067
进行融合，既对机械臂的联合状态矩阵
Figure 776001DEST_PATH_IMAGE030
进行矫 正补偿，又对UWB测量定位向量
Figure 328074DEST_PATH_IMAGE068
的异常值进行校正，从而得到更精准的位置和姿态向量。
SHFAF（模糊自适应滤波器）用来解决在复杂室内环境中时变噪声导致UWB测量存 在异常值的问题。SHFAF包括异常值检测和校正、自适应估计器以及误差状态卡尔曼滤波 器。异常值检测和校正模块旨在通过对原始测量值进行预处理来识别异常值并减少其离差 程度。自适应估计器准确估计随时间变化的误差。此外，校正后的测量结果及其估计的噪声 协方差是误差状态卡尔曼滤波器的输入。异常值检测和校正利用联合状态矩阵
Figure 544292DEST_PATH_IMAGE030
对UWB测 量结果进行异常值判断对异常值进行校正。自适应估计器利用下述协方差矩阵和误差状态 矩阵对结果进行误差估计。SHFAF的具体情况与现有相一致，具体为本领域技术人员所熟 知，此处不再累述。
步骤2得到机械臂的联合状态矩阵为
Figure 161218DEST_PATH_IMAGE069
，测量出来的联合状态矩阵
Figure 107177DEST_PATH_IMAGE070
存在误差，因此，联合状态矩阵
Figure 49726DEST_PATH_IMAGE070
可以看作由准确状态矩阵
Figure 35130DEST_PATH_IMAGE071
和误差状态矩阵
Figure 924589DEST_PATH_IMAGE072
组成，由 过渡方程式
Figure 397159DEST_PATH_IMAGE073
对准确状态矩阵
Figure 104083DEST_PATH_IMAGE074
进行递归预测，得到最终结果。具体步骤说明 如下：
（1）、预测误差状态和观测协方差矩阵
定义
Figure 498156DEST_PATH_IMAGE075
是k-1时刻的误差状态转移矩阵，用于中预测k时刻的误差状态，得到
Figure 722464DEST_PATH_IMAGE076
（4-1）
其中，
Figure 95545DEST_PATH_IMAGE077
为三阶单位矩阵，
Figure 973371DEST_PATH_IMAGE078
为时间间隔，
Figure 385898DEST_PATH_IMAGE079
是k-1时刻测量的加速度，通过惯性 装置IMU测量得到，
Figure 882738DEST_PATH_IMAGE080
为加速度的测量误差；
Figure 64321DEST_PATH_IMAGE081
是
Figure 67043DEST_PATH_IMAGE082
时刻测量的角速度，通过惯 性装置IMU测量得到，
Figure 560341DEST_PATH_IMAGE083
为角速度的测量误差，
Figure 126452DEST_PATH_IMAGE084
为k-1时刻的旋转变换矩阵，所述 旋转变换矩阵由步骤1中绝对定向方法得到。
则k时刻的误差状态预测方程即为
Figure 896962DEST_PATH_IMAGE082
时刻的误差状态转移矩阵左乘
Figure 359999DEST_PATH_IMAGE082
时刻的误 差状态，计算公式为
Figure 747118DEST_PATH_IMAGE085
（4-2）
噪声驱动矩阵
Figure 648078DEST_PATH_IMAGE086
为自定义的矩阵，为了计算协方差矩阵，需要将两个不同维度的矩阵 相加，因此定义
Figure 273094DEST_PATH_IMAGE086
来完成式下述中的矩阵加法。
Figure 70149DEST_PATH_IMAGE087
（4-3）
其中，
Figure 288772DEST_PATH_IMAGE077
为三阶单位矩阵。
过程噪声协方差矩阵
Figure 259002DEST_PATH_IMAGE088
用于计算噪声带来的误差。由于测量结果受到噪声影响， 因此，需要在预测时去掉噪声的影响。
Figure 269683DEST_PATH_IMAGE089
（4-4）
其中，
Figure 706481DEST_PATH_IMAGE090
表示线性代换，
Figure 910935DEST_PATH_IMAGE091
，
Figure 356960DEST_PATH_IMAGE092
，为1时刻的加速度值，为k时刻的加速 度值，其他的类同；为1时刻的角速度值，为k时刻的角速度值，其他类同。
因此，k时刻观测协方差矩阵为
Figure 789209DEST_PATH_IMAGE082
时刻的观测协方差矩阵与误差状态矩阵相乘再 加上噪声影响，其方程为
Figure 98968DEST_PATH_IMAGE097
（4-5）
观测协方差矩阵在计算测量结果的期望值和计算卡尔曼增益时是非常重要的一个参数。
（2）、异常值检测及校正
超宽带可能会受到未知和不确定的干扰，如多径效应和NLOS因子。在这种情况下，很可能会出现异常值。这种现象对滤波器性能的不利影响难以消除。为了避免较大的估计误差甚至滤波发散，我们需要进行异常点的检测与校正。
Figure 763167DEST_PATH_IMAGE060
时刻UWB测量结果的估计误差为
Figure 862579DEST_PATH_IMAGE098
，其中，
Figure 436780DEST_PATH_IMAGE099
是为了矩阵运算所定义的矩阵，
Figure 917440DEST_PATH_IMAGE100
是步骤3中UWB的测量结果，
Figure 803356DEST_PATH_IMAGE101
是
Figure 207924DEST_PATH_IMAGE102
的转置。
Figure 902210DEST_PATH_IMAGE103
的估计期望值是误差期望和
Figure 22613DEST_PATH_IMAGE082
时刻期望的加和，计算公式是
Figure 926984DEST_PATH_IMAGE104
（4-6）
Figure 791035DEST_PATH_IMAGE103
的理论期望值则需要将误差状态和预测到的协方差和测量噪声考虑进去，计算 公式是
Figure 339828DEST_PATH_IMAGE105
（4-7）
测量噪声协方差
Figure 473875DEST_PATH_IMAGE106
是根据
Figure 334384DEST_PATH_IMAGE082
时刻的测量误差和协方差计算出来的
Figure 2125DEST_PATH_IMAGE060
时刻下的噪声协 方差，
Figure 671004DEST_PATH_IMAGE107
，其中
Figure 477417DEST_PATH_IMAGE108
，修正的创新贡献权（MICW） 为
Figure 966167DEST_PATH_IMAGE109
，其中
Figure 562234DEST_PATH_IMAGE110
。当
Figure 820040DEST_PATH_IMAGE111
时，
Figure 984305DEST_PATH_IMAGE112
减小到传统的ICW。
Figure 74533DEST_PATH_IMAGE113
是遗忘 因子（通常在0.95和0.99之间）。
通过公式（4-6）和（4-7）计算
Figure 411973DEST_PATH_IMAGE103
的估计期望值与理论期望值之差，判断在时间k 处的测量值是否为异常值。设
Figure 789865DEST_PATH_IMAGE114
（4-8）
那么定义异常系数
Figure 859452DEST_PATH_IMAGE115
如下：
Figure 932581DEST_PATH_IMAGE116
（4-9）
其中，
Figure 276975DEST_PATH_IMAGE117
是异常值识别的预定灵敏度阈值，需要通过具体实验获得。
若
Figure 571690DEST_PATH_IMAGE115
不等于1，则测量值
Figure 546599DEST_PATH_IMAGE118
为异常值，被校正后的新的测量值
Figure 762817DEST_PATH_IMAGE119
为
Figure 753644DEST_PATH_IMAGE120
（4-10）校正后的数据重新注入上述
Figure 574970DEST_PATH_IMAGE121
的计算。若测量值
Figure 251939DEST_PATH_IMAGE122
为非异常 值，进行下述的计算。
（3）、计算卡尔曼增益
经过以上步骤，计算出误差状态为
Figure 486611DEST_PATH_IMAGE123
的卡尔曼增益
Figure 517015DEST_PATH_IMAGE124
，
Figure 724006DEST_PATH_IMAGE125
是观测协方差矩阵和预 测协方差矩阵的比，用于更新误差状态，即
Figure 571876DEST_PATH_IMAGE126
（4-11）
其中，
Figure 90582DEST_PATH_IMAGE127
是公式（4-5）中的观测协方差矩阵，
Figure 314890DEST_PATH_IMAGE128
是去除了噪声协 方差之后的预测协方差矩阵。
（4）、更新误差状态
估计的误差状态矩阵
Figure 687971DEST_PATH_IMAGE123
是卡尔曼增益
Figure 706743DEST_PATH_IMAGE125
和当前UWB估计误差
Figure 853690DEST_PATH_IMAGE129
的乘积，
Figure 475165DEST_PATH_IMAGE130
（4-12）
（5）、更新准确状态，重置误差状态为0
Figure 204217DEST_PATH_IMAGE131
（4-13）
至此，得到k时刻车载机械臂最精准的机械臂的联合状态矩阵
Figure 659469DEST_PATH_IMAGE132
，实现k时刻的定 位。

Claims (3)

1.一种车载机械臂的感知定位方法，其特征是，所述感知定位方法包括如下步骤：
步骤1、利用视觉装置对车载机械臂进行室内定位，以能得到机械臂的视觉定位姿态向 量
Figure 722563DEST_PATH_IMAGE001
与视觉定位位置向量
Figure 187043DEST_PATH_IMAGE002
；
步骤2、利用惯性装置对车载机械臂进行室内定位，以能得到机械臂的惯性定位姿态向 量
Figure 541407DEST_PATH_IMAGE003
以及惯性定位位置向量
Figure 206875DEST_PATH_IMAGE004
，利用AFEKF对视觉定位姿态向量
Figure 226784DEST_PATH_IMAGE001
、视觉定位位置向量
Figure 416325DEST_PATH_IMAGE002
、 惯性定位姿态向量
Figure 80656DEST_PATH_IMAGE003
以及惯性定位位置向量
Figure 464495DEST_PATH_IMAGE005
进行融合，以得到机械臂的联合状态矩阵
Figure 971700DEST_PATH_IMAGE006
；
步骤3、利用UWB对车载机械臂进行室内定位，以得到UWB测量定位向量
Figure 653348DEST_PATH_IMAGE007
；
步骤4、利用SHFAF方法对联合状态矩阵
Figure 827977DEST_PATH_IMAGE006
、UWB测量定位向量
Figure 615674DEST_PATH_IMAGE008
进行融合，并对UWB测 量定位向量
Figure 751120DEST_PATH_IMAGE009
进行异常值检测与校正，以得到准确状态矩阵
Figure 95514DEST_PATH_IMAGE010
。
2.根据权利要求1所述的车载机械臂的感知定位方法，其特征是，步骤1中，包括如下步骤：
步骤1.1、在进行室内定位时，利用Kinect相机实时获取多帧连续的彩色图像与深度图像；
步骤1.2、利用SURF方法对两帧连续的彩色图像提取公共特征点，并得到特征向量，且通过特征向量得到机械臂姿态改变对应的方向向量；
步骤1.3、利用绝对定向方法计算连续两帧图像中机械臂方向的旋转矩阵，根据所述旋 转矩阵以及上述的方向向量，能得到机械臂的视觉定位姿态向量
Figure 562134DEST_PATH_IMAGE001
；利用连续两帧深度图 像中机械臂的偏移值，能得到视觉定位位置向量
Figure 474727DEST_PATH_IMAGE002
。
3.根据权利要求1所述的车载机械臂的感知定位方法，其特征是，将惯性装置IMU安装 于车载机械臂的底座上，并通过捷联惯导方法得到惯性定位姿态向量
Figure 956523DEST_PATH_IMAGE011
以及惯性定位位 置向量
Figure DEST_PATH_IMAGE012
。
CN202011047302.6A 2020-09-29 2020-09-29 车载机械臂的感知定位方法 Pending CN111890373A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011047302.6A CN111890373A (zh) 2020-09-29 2020-09-29 车载机械臂的感知定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011047302.6A CN111890373A (zh) 2020-09-29 2020-09-29 车载机械臂的感知定位方法

Publications (1)

Publication Number Publication Date
CN111890373A true CN111890373A (zh) 2020-11-06

Family

ID=73224015

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011047302.6A Pending CN111890373A (zh) 2020-09-29 2020-09-29 车载机械臂的感知定位方法

Country Status (1)

Country Link
CN (1) CN111890373A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124856A (zh) * 2021-05-21 2021-07-16 天津大学 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN113729655A (zh) * 2021-09-26 2021-12-03 重庆邮电大学 一种分离uwb雷达传感器接收信号的方法
WO2023168849A1 (zh) * 2022-03-08 2023-09-14 江南大学 机械臂动作捕捉方法、介质、电子设备及***
US11766784B1 (en) 2022-03-08 2023-09-26 Jiangnan University Motion capture method and system of robotic arm, medium, and electronic device
WO2024002276A1 (zh) * 2021-11-01 2024-01-04 华人运通(江苏)技术有限公司 脚本序列的确定方法、装置、电子设备及车辆

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106584466A (zh) * 2017-02-15 2017-04-26 湖南天特智能科技有限公司 自动运载移动机器人***
CN107179080A (zh) * 2017-06-07 2017-09-19 纳恩博（北京）科技有限公司 电子设备的定位方法和装置、电子设备、电子定位***
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN110609311A (zh) * 2019-10-10 2019-12-24 武汉理工大学 基于车载环视图像与毫米波雷达融合的智能车定位方法
CN110926460A (zh) * 2019-10-29 2020-03-27 广东工业大学 一种基于IMU的uwb定位异常值处理方法
US20200192362A1 (en) * 2018-12-12 2020-06-18 GM Global Technology Operations LLC System and method for assisting a vehicle to park in alignment with a wireless battery charging pad

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106584466A (zh) * 2017-02-15 2017-04-26 湖南天特智能科技有限公司 自动运载移动机器人***
CN107179080A (zh) * 2017-06-07 2017-09-19 纳恩博（北京）科技有限公司 电子设备的定位方法和装置、电子设备、电子定位***
US20200192362A1 (en) * 2018-12-12 2020-06-18 GM Global Technology Operations LLC System and method for assisting a vehicle to park in alignment with a wireless battery charging pad
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN110609311A (zh) * 2019-10-10 2019-12-24 武汉理工大学 基于车载环视图像与毫米波雷达融合的智能车定位方法
CN110926460A (zh) * 2019-10-29 2020-03-27 广东工业大学 一种基于IMU的uwb定位异常值处理方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
JIANFENG LIU等: "An Approach to Robust INS/UWB Integrated Positioning for Autonomous Indoor Mobile Robots", 《SENSORS》 *
温熙: "Kinect和惯性导航***组合的室内定位技术研究", 《中国优秀硕士学位论文全文数据库》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124856A (zh) * 2021-05-21 2021-07-16 天津大学 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN113124856B (zh) * 2021-05-21 2023-03-14 天津大学 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN113729655A (zh) * 2021-09-26 2021-12-03 重庆邮电大学 一种分离uwb雷达传感器接收信号的方法
CN113729655B (zh) * 2021-09-26 2024-03-08 重庆邮电大学 一种分离uwb雷达传感器接收信号的方法
WO2024002276A1 (zh) * 2021-11-01 2024-01-04 华人运通(江苏)技术有限公司 脚本序列的确定方法、装置、电子设备及车辆
WO2023168849A1 (zh) * 2022-03-08 2023-09-14 江南大学 机械臂动作捕捉方法、介质、电子设备及***
US11766784B1 (en) 2022-03-08 2023-09-26 Jiangnan University Motion capture method and system of robotic arm, medium, and electronic device

Similar Documents

Publication Publication Date Title
CN111595333B (zh) 视觉惯性激光数据融合的模块化无人车定位方法及***
CN111890373A (zh) 车载机械臂的感知定位方法
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
CN111210477B (zh) 一种运动目标的定位方法及***
CN112233177B (zh) 一种无人机位姿估计方法及***
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN103020952A (zh) 信息处理设备和信息处理方法
CN114755662B (zh) 一种路车融合感知的激光雷达和gps的标定方法和装置
CN112254729A (zh) 一种基于多传感器融合的移动机器人定位方法
Zhang et al. Vision-aided localization for ground robots
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN112388635B (zh) 机器人多传感器融合感知与空间定位的方法、***及装置
CN113436261B (zh) 一种面向封闭园区自动驾驶的单目视觉惯性定位方法
CN114234967A (zh) 一种基于多传感器融合的六足机器人定位方法
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN113899364A (zh) 定位方法及装置、设备、存储介质
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN110515088B (zh) 一种智能机器人的里程计估计方法及***
CN113362377B (zh) 一种基于单目相机的vo加权优化方法
CN114397642A (zh) 一种基于图优化的三维激光雷达与imu外参标定方法
CN112762929A (zh) 一种智能导航方法、装置和设备
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
CN116310991A (zh) 一种基于强化学习的篮板落点预测方法及***
CN111862146A (zh) 一种目标对象的定位方法及装置

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20201106

RJ01 Rejection of invention patent application after publication