CN113119112B - 适用于六自由度机器人视觉测量的运动规划方法及系统 - Google Patents

适用于六自由度机器人视觉测量的运动规划方法及系统 Download PDF

Info

Publication number
CN113119112B
CN113119112B CN202110291297.1A CN202110291297A CN113119112B CN 113119112 B CN113119112 B CN 113119112B CN 202110291297 A CN202110291297 A CN 202110291297A CN 113119112 B CN113119112 B CN 113119112B
Authority
CN
China
Prior art keywords
model
robot
planning
coordinate system
vision
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.)
Active
Application number
CN202110291297.1A
Other languages
English (en)
Other versions
CN113119112A (zh
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.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
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 Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202110291297.1A priority Critical patent/CN113119112B/zh
Publication of CN113119112A publication Critical patent/CN113119112A/zh
Application granted granted Critical
Publication of CN113119112B publication Critical patent/CN113119112B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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
    • 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
    • B25J9/1666—Avoiding collision or forbidden zones
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)

Abstract

本发明提供了一种适用于六自由度机器人视觉测量的运动规划方法及系统，包括建立环境模型、测量工件模型、机器人模型、视觉传感器模型，进而确定各个模型相互之间的位姿关系，定义视觉传感器模型的可测视锥，以及测量工件模型在视觉传感器模型视野中的视锥模型；所述机器人模型有关节空间，在所述关节空间内进行点到点的、满足约束的随机路径规划；所述随机路径规划中生成有路径点，对所述路径点构造B样条曲线，根据偏离误差以及关节角速度、加速度、跃度约束进行时间最优的轨迹规划。本方法可以解决标准商用六自由度机器人在环境约束下进行视觉测量的整套运动规划问题，并在生成路径下实现时间最优且满足到机器人跃度约束的速度规划。

Description

适用于六自由度机器人视觉测量的运动规划方法及系统
技术领域
本发明涉及主动视觉测量任务下的机器人运动规划技术领域，具体地，涉及一种适用于六自由度机器人视觉测量的运动规划方法及系统。
背景技术
主动视觉传感器测量规划问题在21世纪提出并引起各个专业领域的重视，串联机器人由于具有柔性好、易准确定位和工作空间大等优势，搭载视觉传感器进行视觉伺服已经是一个发展比较完整的解决方案。对于复杂零件，单一视角的被动测量已无法满足测量需求，主动视觉伺服的重要性逐渐被体现。因此，机器人搭载视觉传感器进行主动视觉伺服的应用，也变得逐渐广泛。但目前基于机器人的主动视觉伺服存在以下问题：1)没有考虑到机器人关节角加速度、跃度约束，导致机器人易振动、运动性能差2)大多视觉伺服问题没有考虑环境的约束，无法进行广泛运用3)解决路径规划问题的方法很多，但缺少完备的轨迹规划流程，很难用于实际工业需要。
中国专利CN108628310B(申请号：CN201810392600.5)“基于射影单应性矩阵的机器人无标定视觉伺服轨迹规划方法”，使用的是基于图像的视觉伺服方法，首先获取理想图像与参考图像，借助初始图像与理想图像、参考图像之间的特征匹配关系，计算初始与理想之间的射影单应性矩阵的真实值以及无穷远射影单应性矩阵的真实值，根据射影单应性矩阵的真实值在射影单应性矩阵空间内进行轨迹规划。此方法通过图像构造的规划空间是无法考虑视野约束的，所以在产生遮挡时视觉伺服是会发生错误甚至失败的，而且此方法无法考虑机器人自身的物理性约束，大多数应用场合无法适用。
中国专利CN107263484A(申请号：CN201710678376.1)“机器人关节空间点到点运动的轨迹规划方法”公开了一种根据从起始点到目标点的位移以及速度、加速度、跃度约束，在关节空间内进行的轨迹规划方法。它以机器人各轴为向量构造n维空间，根据n轴之间的矢量关系，通过投影对机器人各轴的运动参数物理量进行校验，使得轨迹满足限制要求。但是此方法只适用于点到点的路径规划，无法考虑环境约束以及对给定路径的轨迹跟踪。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种适用于六自由度机器人视觉测量的运动规划方法及系统。
根据本发明提供的一种适用于六自由度机器人视觉测量的运动规划方法，包括如下步骤：
步骤1：建立环境模型、测量工件模型、机器人模型、视觉传感器模型，进而确定各个模型相互之间的位姿关系，定义视觉传感器模型的可测视锥，以及测量工件模型在视觉传感器模型视野中的视锥模型；
步骤2：所述机器人模型有关节空间，在所述关节空间内进行点到点的、满足约束的随机路径规划；
步骤3：所述随机路径规划中生成有路径点，对所述路径点构造B样条曲线，根据偏离误差以及关节角速度、加速度、跃度约束进行时间最优的轨迹规划。
优选的，所述步骤1包括：
步骤1.1：所述环境模型上放置标定板进行标定，确定桌面坐标系、传感器坐标系和世界坐标系，利用相机标定得到视觉传感器模型的参数以及桌面坐标系相对于传感器坐标系的位姿，再根据手眼标定得到传感器坐标系到机器人模型末端的末端坐标系，再根据环境标定得到环境模型即既定放置的障碍物模型与测量工件模型相对于世界坐标系的位姿；
步骤1.2：用凸面体概括所述环境模型中的离散障碍物模型、视觉传感器模型以及六自由度机器人模型各个连杆模型，用离散顶点集表示测量工件模型；
步骤1.3：根据标定得到的所述视觉传感器模型参数建立相机模型，利用所述步骤1.1的标定结果，从而定义测量工件模型在视觉传感器模型视野中的视锥模型，再结合视觉传感器模型的可测范围定义可测视锥模型。
优选的，所述随机路径规划以所述凸面体、测量工件模型以及各个模型之间的关系为基础，在机器人模型的关节空间任选一个满足约束条件的起始点坐标，根据规定的视觉传感器模型相对于测量工件模型的终止点位姿，反解出满足约束条件的终止点坐标，通过快速随机搜索树方法，求出从起始点到终止点的满足约束的六维折线路径。
优选的，所述轨迹规划以所述六维折线路径得到的路径点为控制点，构造多维非均匀多次B样条参数曲线，根据所述多维非均匀多次B样条参数曲线参数构建变量，考虑偏离误差约束、曲线速度约束以及关节角的加速度约束，构造以时间最短为目标函数的优化模型，利用双向扫描获得初步的速度分布，再考虑关节角的跃度约束重构以时间最短为目标的优化模型，利用线性规划求得最终速度分布，再根据特定频率插补得到最终的轨迹。
优选的，所述多维非均匀多次B样条参数曲线包括六维非均匀五次B样条参数曲线。
根据本发明提供的一种适用于六自由度机器人视觉测量的运动规划系统，包括如下模块：
模块M1：建立环境模型、测量工件模型、机器人模型、视觉传感器模型，进而确定各个模型相互之间的位姿关系，定义视觉传感器模型的可测视锥，以及测量工件模型在视觉传感器模型视野中的视锥模型；
模块M2：所述机器人模型有关节空间，在所述关节空间内进行点到点的、满足约束的随机路径规划；
模块M3：所述随机路径规划中生成有路径点，对所述路径点构造B样条曲线，根据偏离误差以及关节角速度、加速度、跃度约束进行时间最优的轨迹规划。
优选的，所述模块1包括：
模块M1.1：所述环境模型上放置标定板进行标定，确定桌面坐标系、传感器坐标系和世界坐标系，利用相机标定得到视觉传感器模型的参数以及桌面坐标系相对于传感器坐标系的位姿，再根据手眼标定得到传感器坐标系到机器人模型末端的末端坐标系，再根据环境标定得到环境模型即既定放置的障碍物与测量工件模型相对于世界坐标系的位姿；
模块M1.2：用凸面体概括所述环境模型中的离散障碍物模型、视觉传感器模型以及六自由度机器人模型各个连杆模型，用离散顶点集表示测量工件模型；
模块M1.3：根据标定得到的所述视觉传感器模型参数建立相机模型，利用所述模块M1.1的标定结果，从而定义测量工件模型在视觉传感器模型视野中的视锥模型，再结合视觉传感器模型的可测范围定义可测视锥模型。
优选的，所述随机路径规划以所述凸面体、测量工件模型以及各个模型之间的关系为基础，在机器人模型的关节空间任选一个满足约束条件的起始点坐标，根据规定的视觉传感器模型相对于测量工件模型的终止点位姿，反解出满足约束条件的终止点坐标，通过快速随机搜索树方法，求出从起始点到终止点的满足约束的六维折线路径。
优选的，所述轨迹规划以所述六维折线路径得到的路径点为控制点，构造多维非均匀多次B样条参数曲线，根据所述多维非均匀多次B样条参数曲线参数构建变量，考虑偏离误差约束、曲线速度约束以及关节角的加速度约束，构造以时间最短为目标函数的优化模型，利用双向扫描获得初步的速度分布，再考虑关节角的跃度约束重构以时间最短为目标的优化模型，利用线性规划求得最终速度分布，再根据特定频率插补得到最终的轨迹。
优选的，所述多维非均匀多次B样条参数曲线包括六维非均匀五次B样条参数曲线。
与现有技术相比，本发明具有如下的有益效果：
1、此六自由度机器人视觉测量的运动规划方法及系统可以解决标准商用六自由度机器人在环境约束下进行视觉测量的整套运动规划问题，并在生成路径下实现时间最优且满足到机器人跃度约束的速度规划；
2、机器人关节加速度、跃度约束使机器人不易振动，有利于提高机器人的运动性能；
3、此六自由度机器人视觉测量的运动规划方法及系统中的视觉伺服问题根据环境的约束，有利于提高六自由度机器人的应用范围；
4、此六自由度机器人视觉测量的运动规划方法及系统具有完备的轨迹规划流程，容易适应实际工业需要。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明流程的流程图；
图2为模型及相互间位姿关系；
图3为RRT算法流程图；
图4为关节空间内路径以及优化后的轨迹在工作空间的3D路径对比；
图5为优化后机器人各关节角度变化曲线；
图6为优化后参数曲线速度变化曲线；
图7为优化后机器人各关节加速度变化曲线；
图8为优化后机器人各关节跃度变化曲线；
图9为优化后机器人关节轨迹每个离散点的约束检测可视化图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
以下结合附图对本发明的实施详细说明，但本发明的保护范围不限于下述的实施例。
一种适用于六自由度机器人视觉测量的运动规划方法及系统，如图1和图2所示，包括如下步骤：步骤1：建立环境模型、测量工件模型、机器人模型、视觉传感器模型，进而确定各个模型相互之间的位姿关系，定义视觉传感器模型的可测视锥，进而测量工件模型在视觉传感器模型视野中的视锥模型。步骤2：所述机器人模型有关节空间，在所述关节空间内进行点到点的、满足约束的随机路径规划。步骤3：所述随机路径规划中生成有路径点，对所述路径点构造B样条曲线，根据偏离误差以及关节角速度、加速度、跃度约束进行时间最优的轨迹规划。
首先，基于对各坐标系的标定，得到模型及其位姿；然后，输入关节空间内的起始点以及终止点，其中终止点一般是依据相机相对于测量工件的目标位姿，通过机器人运动学反解求出的且满足约束的关节空间坐标，依据图3所示的RRT算法求解满足约束的路径；其次，以路径点为控制点构造5次非均匀B样条曲线，通过对曲线参数构造出的变量进行优化，得到满足偏离误差、速度、加速度、跃度约束的时间最优速度分布；最后，根据泰勒公式以特定的频率进行插补，得到最终轨迹，即带有时序信息的位置、速度离散量。
所述步骤1，包括：
步骤1.1：如图2所示，建立世界坐标系为{O}，与机座坐标系重合，机器人末端坐标系为{E}，相机坐标系为{C}，标定板坐标系为{R}，与桌面坐标系重合，第i个桌面放置的物体(包括障碍物以及测量工件)坐标系为{Gi},i＝1,2,…,N.其中N为物体总数，且工件对应的序号i＝1。其中物体相对于桌面坐标系的位姿已知,即
Figure BDA0002982793630000051
已知。
利用标定板进行相机标定(比如张正友标定)，再结合机器人位姿进行手眼标定，求解AX＝XB问题得出传感器相对于末端的位姿
Figure BDA0002982793630000052
将机器人末端位姿
Figure BDA0002982793630000053
传感器相对于末端的位姿
Figure BDA0002982793630000054
标定板坐标系相对于传感器坐标系
Figure BDA0002982793630000055
的位姿联立，从而得出桌面坐标系(与标定板坐标系重合)相对于世界坐标系(与机座坐标系重合)的位姿
Figure BDA0002982793630000056
Figure BDA0002982793630000061
步骤1.2：用凸面体概括环境中的离散障碍物模型、视觉传感器模型以及机器人各个连杆模型，用离散顶点集表示测量工件模型；
步骤1.3:用相机标定得出的相机参数用于建立视觉传感器模型，由于
Figure BDA0002982793630000062
已知，进而通过
Figure BDA0002982793630000063
推算出各个障碍物与测量工件在世界坐标系下的位姿
Figure BDA0002982793630000064
有：
Figure BDA0002982793630000065
因此，根据
Figure BDA0002982793630000066
得出所有实物模型的位姿。相机坐标系相对于世界坐标系的位姿
Figure BDA0002982793630000067
有：
Figure BDA0002982793630000068
根据
Figure BDA0002982793630000069
结合传感器的可测范围可建立视觉传感器的可测视锥模型；根据工件坐标系
Figure BDA00029827936300000610
测量工件模型以及相机坐标系原点位置，可得到测量工件模型在相机视野中的视锥模型。
所述步骤2，具体地，根据关节的极限位置在六维空间内构造一个有界的关节空间Q，以L2范数定义关节空间中两个向量之间的距离。设定RRT算法中起始点、目标点分别为q0,q1，最大生长次数为MAX，判断是否到达目标点的阈值为δ(δ＞0)，tree代表算法中生成的随机树，由节点以及边信息构成，另外设置一个贪婪概率P(0<P<1)。
在最大生长次数之内，依据贪婪概率P进行随机生长或向目标生长。在小于概率P的情况下，即rand(0,1)<P(rand(0,1)表示随机生成一个0到1之间的数),选择随机生长：在Q中随机生成一个点qrand，找到tree中最接近qrand的qnear点，计算从qnear朝着方向qrand以一定步长λ经过的路径qpass，若qpass满足约束，则沿着路径qpass从qnear生长到qnew，即tree中新增节点qnew，并建立起qnear与qnew的边联系；当不小于概率P的情况下，选择直接向目标点生长，即找到tree中最接近q1的qnear2点，若qnear2到q1的路径qpass2满足约束，则直接生长到q1。以上两种情况下，若不满足约束，均不生长。其中，检测是否满足三个约束的数学方法是：利用凸多面体是否有交集判断是否碰撞；依据测量目标是否包含在视觉传感器可测视锥中判断目标是否在视野中；依据障碍物与测量目标在相机视野中的视锥是否有交集判断视野是否遮挡。
每次生长后即判断是否到达q1，其中随机生长情况下判断qnew与q1的距离是否小于δ，向目标生长则说明肯定到达目标点。若到达q1，则在tree中提取从q0到q1的所有经过节点，从而实现满足约束下点到点的路径规划。
步骤3，包括：
步骤3.1：首先，设u表示B样条曲线γ的参数，v(u)表示参数u对时间t的导数，v′(u)表示v(u)对参数u的导数。构建新的变量a(u),b(u)，有：
a(u)＝v′(u)v(u)，b(u)＝v(u)2 (4)
设轨迹完成时间为tf，则过程中每个时刻t满足0＜t＜tf。时长表达式为：
Figure BDA0002982793630000071
对u进行离散后，设n表示曲线参数离散后参数点数目，h表示步长，有：
Figure BDA0002982793630000072
b(ui)简写为bi，则目标函数重构为：
Figure BDA0002982793630000073
为建立优化模型，首先考虑曲线速度与加速度约束。设γ′i(6×1)表示路径曲线γ在参数为ui处对参数u的导数，γ′i,μ表示路径曲线γ在第μ(μ＝1,2,3,4,5,6)维度上，在参数为ui处对参数u的导数，γ″i,μ表示路径曲线γ在第μ(μ＝1,2,3,4,5,6)维度上，在参数为ui处对参数u的二阶导数。设速度矢量为
Figure BDA00029827936300000713
第μ(μ＝1,2,3,4,5,6)维度的加速度在参数为ui处为
Figure BDA0002982793630000074
构造变量ai,
Figure BDA0002982793630000075
ηi,μ，则有：
Figure BDA0002982793630000076
其中：ai＝(bi+1-bi)/2h,
Figure BDA0002982793630000077
设曲线速度最大值为Vmax，各维度上的加速度最大值为Amax，构造变量
Figure BDA0002982793630000078
Figure BDA0002982793630000079
所以速度约束
Figure BDA00029827936300000710
以及加速度约束
Figure BDA00029827936300000711
可以总结为：
Figure BDA00029827936300000712
Figure BDA0002982793630000081
由此，第一次重构时间最优模型，得到以下优化模型：
Figure BDA0002982793630000082
步骤3.2：加入跃度约束，设各维度上的跃度最大值为Jmax。由于曲线速度有：
Figure BDA0002982793630000083
令
Figure BDA0002982793630000084
其中
Figure BDA0002982793630000085
为不考虑跃度约束时即模型(11){bi}的最优解，得到第二次重构的优化模型：
Figure BDA0002982793630000086
步骤3.3：使用以下基于泰勒公式的近似方法，进行给定时间间隔TS的插补，将速度与位置信息转化为随时间的离散信息：
Figure BDA0002982793630000087
其中，
Figure BDA0002982793630000088
ti＝i·Ts(i＝1,2,…,n-1)，
Figure BDA0002982793630000089
表示
Figure BDA00029827936300000810
对时间的导数，u(3)(t)表示
Figure BDA00029827936300000811
对时间的导数。
在以下实施例中，如图2所示，以立体块为障碍物，圆盘为测量工件，棋盘格为标定板，以关节空间的坐标q0＝[2 -1 0.70 -0.52 -2.30 0.80]T为起始点，根据相机相对于测量工件的目标位姿
Figure BDA0002982793630000091
反解出关节空间的坐标：
q1＝[0.5353 -1.7950 1.5950 -1.3883 -1.5750 -1.0376]T
以此作为终止点，使用本发明提出的方法，得到一条优化的轨迹。注意：所有数值省略单位，其中角度单位为rad，长度单位为m，时间单位为s。具体步骤如下：
1、通过在桌面上放置棋盘格，进行标定，得到标定结果：
传感器坐标系{C}相对于机器人末端坐标系{E}的位姿
Figure BDA0002982793630000092
Figure BDA0002982793630000093
桌面坐标系{R}相对于世界坐标系{O}的位姿
Figure BDA0002982793630000094
Figure BDA0002982793630000095
在图2所示的模型中，测量工件以及障碍物1、2、3都以模型质心为原点，x轴与桌面坐标系x轴同向，z轴竖直向上建立自身坐标系{Gi},i＝1,2,3,4.由此推算出，它们相对于世界坐标系的位姿，在方向上，在一定误差范围内都是
Figure BDA0002982793630000096
在位置上，分别是：
Figure BDA0002982793630000097
2、根据图3所示的RRT算法，以q0＝[2 -1 0.70 -0.52 -2.30 0.80]T为起始点，q1＝[0.5353 -1.7950 1.5950 -1.3883 -1.5750 -1.0376]T为终止点，得到经过25个路径点的最终路径，如表1所示，将关节空间内得到的路径映射到机器人工作空间中，绘出末端扫略出的3D路径，如图4中的虚线所示。
表1路径点六维坐标表
路径点序号 关节1 关节2 关节3 关节4 关节5 关节6
1 2 -1 0.7 -0.52 -2.3 0.8
2 1.903021 -1.05264 0.759259 -0.57749 -2.252 0.678331
3 1.856287 -1.0413 0.813121 -0.70895 -2.16383 0.601977
4 1.884408 -1.09864 0.862425 -0.76914 -2.03134 0.510245
5 1.867146 -1.11305 1.016985 -0.73883 -2.08494 0.421289
6 1.775301 -1.16008 1.056845 -0.78362 -2.04978 0.320684
7 1.674049 -1.13427 1.09292 -0.82348 -2.11338 0.185606
8 1.751378 -1.09656 1.153101 -0.91526 -2.17649 0.073044
9 1.605101 -1.18057 1.206255 -0.97216 -2.10414 -0.06055
10 1.648573 -1.25099 1.264317 -0.93282 -2.13041 -0.21431
11 1.735203 -1.2888 1.313233 -0.87812 -2.17097 -0.35621
12 1.809678 -1.34514 1.387485 -0.89453 -2.22176 -0.49412
13 1.80801 -1.43827 1.350619 -0.97536 -2.20191 -0.63245
14 1.902887 -1.41846 1.270368 -1.07053 -2.1545 -0.72711
15 1.939816 -1.37719 1.391443 -1.11488 -2.18102 -0.85242
16 1.937266 -1.40774 1.472256 -1.27182 -2.22102 -0.80343
17 1.986775 -1.54815 1.534618 -1.26371 -2.25998 -0.71145
18 1.881932 -1.57212 1.636063 -1.34402 -2.33513 -0.66515
19 1.764507 -1.63704 1.683888 -1.45219 -2.3018 -0.61039
20 1.754739 -1.73186 1.814152 -1.45774 -2.23154 -0.53912
21 1.652206 -1.83072 1.863757 -1.45889 -2.18144 -0.43501
22 1.531062 -1.93225 1.872138 -1.46368 -2.10178 -0.50341
23 1.221518 -1.88958 1.785987 -1.44024 -1.93803 -0.66947
24 1.140301 -1.94014 1.759237 -1.46388 -1.81458 -0.77162
25 0.5353 -1.795 1.595 -1.3883 -1.575 -1.0376
3、将25个路径点作为控制点，构造6维的非均匀5次b样条参数曲线，根据曲线参数构建变量，构造以时间最短为目标函数的优化模型，考虑偏离误差以及曲线的速度(Vmax＝0.5rad/s)、加速度约束(amax＝0.5rad/s2)，重构优化模型，利用双向扫描方法获得初步速度分布，再考虑跃度约束(Jmax＝1rad/s3)对优化模型第二次重构，利用线性规划方法求得最终速度分布。再根据特定时间间隔Ts＝0.02s插补得到最终的轨迹,轨迹时长共14.56s。将关节空间内优化后的平滑路径映射到机器人工作空间中，绘出末端扫略出的3D路径，如图4中的实曲线所示，曲线速度如图6所示，机器人各关节的位置、加速度、跃度曲线如图5、7、8所示，对最终轨迹进行约束的二次验证，结果如图9所示，可以看出轨迹上的离散点均满足约束。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种适用于六自由度机器人视觉测量的运动规划方法，其特征在于，包括如下步骤：
步骤1：建立环境模型、测量工件模型、机器人模型、视觉传感器模型，进而确定各个模型相互之间的位姿关系，定义视觉传感器模型的可测视锥，以及测量工件模型在视觉传感器模型视野中的视锥模型；
步骤2：所述机器人模型有关节空间，在所述关节空间内进行点到点的、满足约束的随机路径规划；
步骤3：所述随机路径规划中生成有路径点，对所述路径点构造B样条曲线，根据偏离误差以及关节角速度、加速度、跃度约束进行时间最优的轨迹规划。
2.根据权利要求1所述的适用于六自由度机器人视觉测量的运动规划方法，其特征在于，所述步骤1包括：
步骤1.1：所述环境模型上放置标定板进行标定，确定桌面坐标系、传感器坐标系和世界坐标系，利用相机标定得到视觉传感器模型的参数以及桌面坐标系相对于传感器坐标系的位姿，再根据手眼标定得到传感器坐标系到机器人模型末端的末端坐标系，再根据环境标定得到环境模型即既定放置的离散障碍物模型与测量工件模型相对于世界坐标系的位姿；
步骤1.2：用凸面体概括所述环境模型中的离散障碍物模型、视觉传感器模型以及六自由度机器人模型各个连杆模型，用离散顶点集表示测量工件模型；
步骤1.3：根据标定得到的所述视觉传感器模型参数建立相机模型，利用所述步骤1.1的所有标定结果，从而定义测量工件模型在视觉传感器模型视野中的视锥模型，再结合视觉传感器模型的可测范围定义可测视锥模型。
3.根据权利要求2所述的适用于六自由度机器人视觉测量的运动规划方法，其特征在于，所述随机路径规划以所述凸面体、测量工件模型以及各个模型之间的关系为基础，在机器人模型的关节空间任选一个满足约束条件的起始点坐标，根据规定的视觉传感器模型相对于测量工件模型的终止点位姿，反解出满足约束条件的终止点坐标，通过快速随机搜索树方法，求出从起始点到终止点的满足约束的六维折线路径。
4.根据权利要求3所述的适用于六自由度机器人视觉测量的运动规划方法，其特征在于，所述轨迹规划以所述六维折线路径得到的路径点为控制点，构造多维非均匀多次B样条参数曲线，根据所述多维非均匀多次B样条参数曲线参数构建变量，考虑偏离误差约束、曲线速度约束以及关节角的加速度约束，构造以时间最短为目标函数的优化模型，利用双向扫描获得初步的速度分布，再考虑关节角的跃度约束重构以时间最短为目标的优化模型，利用线性规划求得最终速度分布，再根据特定频率插补得到最终的轨迹。
5.根据权利要求4所述的适用于六自由度机器人视觉测量的运动规划方法，其特征在于，所述多维非均匀多次B样条参数曲线包括六维非均匀五次B样条参数曲线。
6.一种适用于六自由度机器人视觉测量的运动规划系统，其特征在于，包括如下模块：
模块M1：建立环境模型、测量工件模型、机器人模型、视觉传感器模型，进而确定各个模型相互之间的位姿关系，定义视觉传感器模型的可测视锥，以及测量工件模型在视觉传感器模型视野中的视锥模型；
模块M2：所述机器人模型有关节空间，在所述关节空间内进行点到点的、满足约束的随机路径规划；
模块M3：所述随机路径规划中生成有路径点，对所述路径点构造B样条曲线，根据偏离误差以及关节角速度、加速度、跃度约束进行时间最优的轨迹规划。
7.根据权利要求6所述的适用于六自由度机器人视觉测量的运动规划系统，其特征在于，所述模块M1包括：
模块M1.1：所述环境模型上放置标定板进行标定，确定桌面坐标系、传感器坐标系和世界坐标系，利用相机标定得到视觉传感器模型的参数以及桌面坐标系相对于传感器坐标系的位姿，再根据手眼标定得到传感器坐标系到机器人模型末端的末端坐标系，再根据环境标定得到环境模型即既定放置的离散障碍物模型与测量工件模型相对于世界坐标系的位姿；
模块M1.2：用凸面体概括所述环境模型中的离散障碍物模型、视觉传感器模型以及六自由度机器人模型各个连杆模型，用离散顶点集表示测量工件模型；
模块M1.3：根据标定得到的所述视觉传感器模型参数建立相机模型，利用所述模块M1.1的标定结果，从而定义测量工件模型在视觉传感器模型视野中的视锥模型，再结合视觉传感器模型的可测范围定义可测视锥模型。
8.根据权利要求7所述的适用于六自由度机器人视觉测量的运动规划系统，其特征在于，所述随机路径规划以所述凸面体、测量工件模型以及各个模型之间的关系为基础，在机器人模型的关节空间任选一个满足约束条件的起始点坐标，根据规定的视觉传感器模型相对于测量工件模型的终止点位姿，反解出满足约束条件的终止点坐标，通过快速随机搜索树方法，求出从起始点到终止点的满足约束的六维折线路径。
9.根据权利要求8所述的适用于六自由度机器人视觉测量的运动规划系统，其特征在于，所述轨迹规划以所述六维折线路径得到的路径点为控制点，构造多维非均匀多次B样条参数曲线，根据所述多维非均匀多次B样条参数曲线参数构建变量，考虑偏离误差约束、曲线速度约束以及关节角的加速度约束，构造以时间最短为目标函数的优化模型，利用双向扫描获得初步的速度分布，再考虑关节角的跃度约束重构以时间最短为目标的优化模型，利用线性规划求得最终速度分布，再根据特定频率插补得到最终的轨迹。
10.根据权利要求9所述的适用于六自由度机器人视觉测量的运动规划系统，其特征在于，所述多维非均匀多次B样条参数曲线包括六维非均匀五次B样条参数曲线。
CN202110291297.1A 2021-03-18 2021-03-18 适用于六自由度机器人视觉测量的运动规划方法及系统 Active CN113119112B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110291297.1A CN113119112B (zh) 2021-03-18 2021-03-18 适用于六自由度机器人视觉测量的运动规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110291297.1A CN113119112B (zh) 2021-03-18 2021-03-18 适用于六自由度机器人视觉测量的运动规划方法及系统

Publications (2)

Publication Number Publication Date
CN113119112A CN113119112A (zh) 2021-07-16
CN113119112B true CN113119112B (zh) 2022-08-09

Family

ID=76773551

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110291297.1A Active CN113119112B (zh) 2021-03-18 2021-03-18 适用于六自由度机器人视觉测量的运动规划方法及系统

Country Status (1)

Country Link
CN (1) CN113119112B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114265418A (zh) * 2021-09-03 2022-04-01 国家电投集团江苏新能源有限公司 一种用于光伏电站的无人机巡检与缺陷定位系统及方法
CN114193449B (zh) * 2021-12-09 2023-08-11 中国煤炭科工集团太原研究院有限公司 一种锚杆支护机器人工作臂轨迹规划方法
CN114851209B (zh) * 2022-06-21 2024-04-19 上海大学 一种基于视觉的工业机器人工作路径规划优化方法及系统
CN117124335B (zh) * 2023-10-25 2024-01-05 山东工商学院 一种基于路径标记回溯策略的改进式rrt路径规划方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2301926A1 (en) * 2000-03-10 2000-07-05 Vladimir Besedic Method and apparatus for 3d visual cone examination
CN104020665A (zh) * 2014-06-25 2014-09-03 北京邮电大学 基于多目标粒子群算法的机械臂最小跃度轨迹优化方法
CN107918776A (zh) * 2017-11-01 2018-04-17 中国科学院深圳先进技术研究院 一种基于机器视觉的用地规划方法、系统及电子设备
CN109945885A (zh) * 2019-04-16 2019-06-28 清华大学 无人摩托的动态障碍物避障路径规划计算方法
CN110919626A (zh) * 2019-05-16 2020-03-27 广西大学 一种基于立体视觉的机器人手持示教装置及方法
CN111251297A (zh) * 2020-02-20 2020-06-09 西北工业大学 一种基于随机采样的双臂空间机器人协调路径规划方法
WO2020237890A1 (zh) * 2019-05-28 2020-12-03 上海钛米机器人科技有限公司 一种速度规划方法、装置、电子设备及存储介质

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111014594B (zh) * 2019-11-19 2021-11-26 中南大学 一种铸锭过程动态除渣的机器人轨迹规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2301926A1 (en) * 2000-03-10 2000-07-05 Vladimir Besedic Method and apparatus for 3d visual cone examination
CN104020665A (zh) * 2014-06-25 2014-09-03 北京邮电大学 基于多目标粒子群算法的机械臂最小跃度轨迹优化方法
CN107918776A (zh) * 2017-11-01 2018-04-17 中国科学院深圳先进技术研究院 一种基于机器视觉的用地规划方法、系统及电子设备
CN109945885A (zh) * 2019-04-16 2019-06-28 清华大学 无人摩托的动态障碍物避障路径规划计算方法
CN110919626A (zh) * 2019-05-16 2020-03-27 广西大学 一种基于立体视觉的机器人手持示教装置及方法
WO2020237890A1 (zh) * 2019-05-28 2020-12-03 上海钛米机器人科技有限公司 一种速度规划方法、装置、电子设备及存储介质
CN111251297A (zh) * 2020-02-20 2020-06-09 西北工业大学 一种基于随机采样的双臂空间机器人协调路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Thoma, J ; Paudel, DP ; Chhatkuli, A ; Probst, T ; Van Gool, L.Mapping, Localization and Path Planning for Image-based Navigation using Visual Features and Map.《IEEE Conference on Computer Vision and Pattern Recognition》.2020, *
钟泽杉 ; 杨敏 ; 赵现朝 ; 岳义 ; 时云 ; 杨天豪.轴空间多约束下的五轴B样条路径速度规划.《计算机集成制造系统》.2019, *

Also Published As

Publication number Publication date
CN113119112A (zh) 2021-07-16

Similar Documents

Publication Publication Date Title
CN113119112B (zh) 适用于六自由度机器人视觉测量的运动规划方法及系统
EP3410246B1 (en) Robot obstacle avoidance control system and method, robot, and storage medium
CN109521774B (zh) 一种基于强化学习的喷涂机器人轨迹优化方法
CN108356819B (zh) 基于改进a*算法的工业机械臂无碰撞路径规划方法
JP6879464B2 (ja) 干渉判定方法、干渉判定システム及びコンピュータプログラム
US9827675B2 (en) Collision avoidance method, control device, and program
US11724387B2 (en) Fast robot motion optimization with distance field
Yuan et al. A heuristic rapidly-exploring random trees method for manipulator motion planning
CN109434836B (zh) 一种结合球树模型的机械手人工势场空间路径规划方法
Ye et al. Collision-free motion planning for the litchi-picking robot
US20220379473A1 (en) Trajectory plan generation device, trajectory plan generation method, and trajectory plan generation program
Fang et al. A sampling-based motion planning method for active visual measurement with an industrial robot
JP5806105B2 (ja) パラメータ補正方法および制御システム
Birr et al. Oriented surface reachability maps for robot placement
CN113442144B (zh) 欠约束下的最优位姿确定方法、装置、存储介质及机械臂
JP2023084115A (ja) 点集合の干渉チェック
CN114734435A (zh) 一种基于超球面的编码器校准方法、装置及系统
US11865724B2 (en) Movement control method, mobile machine and non-transitory computer readable storage medium
CN110640723B (zh) 基于机器人末端指点示教的障碍物标定方法
CN113146637A (zh) 一种机器人笛卡尔空间的运动规划方法
Cheng et al. Analysis on the trajectory planning and simulation of six degrees of freedom manipulator
JP2000112510A (ja) ロボットの教示方法及びその装置
JP2021010994A (ja) センサ位置姿勢キャリブレーション装置及びセンサ位置姿勢キャリブレーション方法
CN117075617B (zh) 机器人轨迹规划方法、装置、存储介质及电子设备
Regal et al. Using Single Demonstrations to Define Autonomous Manipulation Contact Tasks in Unstructured Environments via Object Affordances

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
GR01 Patent grant
GR01 Patent grant