CN115781683B - 机械臂的在线轨迹规划方法、装置及计算机可读介质 - Google Patents
机械臂的在线轨迹规划方法、装置及计算机可读介质 Download PDFInfo
- Publication number
- CN115781683B CN115781683B CN202211640360.9A CN202211640360A CN115781683B CN 115781683 B CN115781683 B CN 115781683B CN 202211640360 A CN202211640360 A CN 202211640360A CN 115781683 B CN115781683 B CN 115781683B
- Authority
- CN
- China
- Prior art keywords
- point sequence
- mechanical arm
- spline
- target
- pose
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 239000011159 matrix material Substances 0.000 claims abstract description 58
- 230000001133 acceleration Effects 0.000 claims abstract description 34
- 238000005070 sampling Methods 0.000 claims abstract description 18
- 238000012545 processing Methods 0.000 claims abstract description 12
- 238000012952 Resampling Methods 0.000 claims abstract description 8
- 230000006870 function Effects 0.000 claims description 45
- 230000037361 pathway Effects 0.000 claims description 26
- 230000008569 process Effects 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 10
- 230000004044 response Effects 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 238000012986 modification Methods 0.000 description 8
- 230000004048 modification Effects 0.000 description 8
- 230000006872 improvement Effects 0.000 description 6
- 230000006978 adaptation Effects 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 238000009499 grossing Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000000644 propagated effect Effects 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 239000000725 suspension Substances 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001149 cognitive effect Effects 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000523 sample Substances 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本发明提供了一种机械臂的在线轨迹规划方法、装置及计算机可读介质。方法包括：接收每个采样周期内的主手末端位姿，将主手末端位姿转换为机械臂目标位姿矩阵；根据机械臂目标位姿矩阵得到原始途径点序列；对原始途径点序列进行增量式处理，得到目标途径点序列；根据所述目标途径点序列计算出光滑B样条的控制点序列；对所述控制点序列进行重采样得到第一控制点序列；将所述第一控制点序列输入B样条近似滤波器得到规划轨迹，将所述规划轨迹发送给机械臂。本发明采用光滑B样条曲线进行轨迹规划，可以保证规划轨迹的速度和加速度的平滑连续且轨迹平滑度可调。
Description
技术领域
本发明主要涉及机器人领域，尤其涉及一种机械臂的在线轨迹规划方法、装置及计算机可读介质。
背景技术
遥操作(Teleoperation)是指在相关机器人控制中把人类操作包含在控制回路中，任何的上层规划和认知决定都是由人类用户下达的，而机器人本体只是负责相应的实体应用。机器人遥操作广泛应用于远程医疗、远程服务、远程抓取、空间探测等领域中，而在线轨迹规划是遥操作过程中非常重要的一环。
CN112792808B提出了一种工业机器人在线轨迹规划的方法，该方法在每个控制周期内计算可以达到的最大加速度和最小加速度，并选择其中一种进行积分得到规划速度和规划位置。该方法的加速度并不连续，对于实际控制效果会有负面的影响；此外该方法未对曲线进行合理的平滑，在临近密集的途径点时会产生不可预料的大幅震荡。
发明内容
本发明要解决的技术问题是提供一种机械臂的在线轨迹规划方法、装置及计算机可读介质，解决规划轨迹的实时计算量大，加速度不连续且轨迹不平滑的问题。
为解决上述技术问题，本发明提供了一种机械臂的在线轨迹规划方法，方法包括：接收每个采样周期内的主手末端位姿，将所述主手末端位姿转换为机械臂目标位姿矩阵；根据所述机械臂目标位姿矩阵得到原始途径点序列；对所述原始途径点序列进行增量式处理，得到目标途径点序列；根据所述目标途径点序列计算出光滑B样条的控制点序列；对所述控制点序列进行重采样得到第一控制点序列；将所述第一控制点序列输入B样条近似滤波器得到规划轨迹，将所述规划轨迹发送给机械臂。
可选地，根据所述目标途径点序列计算出光滑B样条的控制点序列包括：根据所述目标途径点序列构造离散B样条曲线；对所述离散B样条曲线进行Z变换得到目标途径点的近似函数，根据所述近似函数计算出光滑B样条的控制点序列。
可选地，所述近似函数为如下形式：
其中，pk-M是第k-M个控制点，qk-n是第k-n个目标途径点，k是当前离散时间点，M是B样条离散近似过程的近似阶数，是脉冲响应系数。
可选地，所述B样条近似滤波器的Z域传递函数为P个均值滤波器M(z)的串联，P为B样条阶次，其中均值滤波器M(z)为如下形式：
其中，N为两个目标途径点之间所需插值点的数量。
可选地，对所述目标途径点序列进行增量式处理包括：将所述机械臂目标位姿矩阵离散化为第一增量矩阵，根据所述第一增量矩阵计算试探步位姿矩阵；对所述试探步位姿矩阵进行逆运动学计算，获取机械臂角关节值；判断所述机械臂角关节值是否超过角关节的工作范围，如果是，则让机械臂暂停并保持当前位置。
可选地，方法还包括：根据机械臂当前位姿计算笛卡尔空间下速度和加速度的上限；根据所述目标途径点序列计算规划速度和规划加速度；判断所述规划速度和规划加速度是否超过所述上限，如果是，则让机械臂暂停并保持当前位置。
为解决上述技术问题，本发明提供了一种机械臂的在线轨迹规划装置，包括：输入单元，配置为接收每个采样周期内的主手末端位姿，将所述主手末端位姿转换为机械臂目标位姿矩阵，根据所述机械臂目标位姿矩阵得到原始途径点序列；计算单元，配置为对所述原始途径点序列进行增量式处理，得到目标途径点序列，根据所述目标途径点序列计算出光滑B样条的控制点序列；重采样单元，配置为对所述控制点序列进行重采样得到第一控制点序列；B样条近似滤波器，配置为接收所述第一控制点序列，根据所述第一控制点序列得到规划轨迹，输出单元，配置为将所述规划轨迹发送给机械臂。
可选地，所述计算单元还配置为：根据所述目标途径点序列构造离散B样条曲线，对所述离散B样条曲线进行Z变换得到目标途径点的近似函数，根据所述近似函数计算出光滑B样条的控制点序列。
可选地，所述近似函数为如下形式：
其中，pk-M是第k-M个控制点，qk-n是第k-n个目标途径点，k是当前离散时间点，M是B样条离散近似过程的近似阶数，是脉冲响应系数。
可选地，所述B样条近似滤波器的Z域传递函数为P个均值滤波器M(z)的串联，P为B样条阶次，其中均值滤波器M(z)为如下形式：
其中，N为两个目标途径点之间所需插值点的数量。
可选地，所述计算单元还配置为：将所述机械臂目标位姿矩阵离散化为第一增量矩阵，根据所述第一增量矩阵计算试探步位姿矩阵；对所述试探步位姿矩阵进行逆运动学计算，获取机械臂角关节值；判断所述机械臂角关节值是否超过角关节的工作范围，如果是，则让机械臂暂停并保持当前位置。
可选地，所述计算单元还配置为：根据机械臂当前位姿计算笛卡尔空间下速度和加速度的上限；根据所述目标途径点序列计算规划速度和规划加速度；判断所述规划速度和规划加速度是否超过所述上限，如果是，则让机械臂暂停并保持当前位置。
为解决上述技术问题，本发明提供了一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如上所述的轨迹规划方法。
与现有技术相比，本发明具有以下优点：
本发明采用光滑B样条曲线进行轨迹规划，可以保证规划轨迹的速度和加速度的平滑连续且轨迹平滑度可调，根据不同的工况可以设定不同的平滑度；本发明采用离散B样条曲线的传递函数的近似函数快速计算控制点，避免了Deboor递归所带来的时间消耗，计算量小，保证了轨迹规划的实时性；本发明通过增量式的控制方式实现了在超过机械臂工作空间范围时进行动态暂停和恢复的功能，即当主手输入设备下发的指令超出机械臂工作范围时能够让机械臂暂停并保持当前位置，直到主手输入设备下发的指令回到机械臂空间范围内，机械臂又可自动立即恢复工作且无需下达额外指令。
附图说明
包括附图是为提供对本申请进一步的理解，它们被收录并构成本申请的一部分，附图示出了本申请的实施例，并与本说明书一起起到解释本发明原理的作用。附图中：
图1是根据本发明一实施例的机械臂的在线轨迹规划装置的***框图；
图2是根据本发明一实施例的机械臂的在线轨迹规划方法的流程图；
图3是根据本发明一实施例的增量式处理的流程图；
图4是图2优化实施例的机械臂的在线轨迹规划方法的流程图。
具体实施方式
为了更清楚地说明本申请的实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构或操作。
如本申请和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本申请保护范围的限制。此外，尽管本申请中所使用的术语是从公知公用的术语中选择的，但是本申请说明书中所提及的一些术语可能是申请人按他或她的判断来选择的，其详细含义在本文的描述的相关部分中说明。此外，要求不仅仅通过所使用的实际术语，而是还要通过每个术语所蕴含的意义来理解本申请。
本申请中使用了流程图用来说明根据本申请的实施例的***所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，可以按照倒序或同时处理各种步骤。同时，或将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。
图1是根据本发明一实施例的机械臂的在线轨迹规划***的***框图。如图1所示，机械臂的在线轨迹规划***100包括主手输入设备11、机械臂的在线轨迹规划装置12和机械臂13。轨迹规划装置12接收主手输入设备11发送的主手末端位姿，将主手末端位姿转换为机械臂目标位姿矩阵，根据机械臂目标位姿矩阵得到机械臂的目标途径点序列，根据目标途径点序列计算出规划轨迹，将规划轨迹发送给机械臂13。其中，主手输入设备11可以为力反馈设备，例如Geomagic Touch力反馈设备。在线轨迹规划装置12包括输入单元121、计算单元122、重采样单元123、B样条近似滤波器124和输出单元125。输入单元121配置为接收每个采样周期内的主手末端位姿，将主手末端位姿转换为机械臂目标位姿矩阵，根据所述机械臂目标位姿矩阵得到原始途径点序列；计算单元122配置为对原始途径点序列进行增量式处理，得到目标途径点序列，根据目标途径点序列计算出光滑B样条的控制点序列；重采样单元123配置为对控制点序列进行重采样得到第一控制点序列；B样条近似滤波器124配置为接收第一控制点序列，根据第一控制点序列得到规划轨迹；输出单元125配置为将规划轨迹发送给机械臂。
以下结合图2详细说明轨迹规划装置12进行在线轨迹规划的流程。图2是根据本发明一实施例的机械臂的在线轨迹规划方法的流程图。如图2所示，机械臂的在线轨迹规划方法200包括如下步骤：
步骤S21：接收每个采样周期内的主手末端位姿，将主手末端位姿转换为机械臂目标位姿矩阵，根据机械臂目标位姿矩阵得到原始途径点序列。
接收每个主手采样周期内的主手末端位姿，主手末端位姿处于主手基坐标系下。例如，在线规划开始的瞬间记录机械臂当前末端在机械臂基坐标系下位姿以及主手末端在主手基坐标系下位姿/>随后持续记录每个主手采样周期内获得的主手末端位姿主手采样周期可以为10ms，本申请对主手采样周期的值不作限制。每个采样点作为轨迹规划中的一个途径点。
通过工作空间坐标映射矩阵将每个主手末端位姿转换为机械臂目标位姿。工作空间坐标映射矩阵可以为如下形式：
其中，是工作空间坐标映射矩阵，/>是旋转矩阵，/>是位移矩阵。则求得的机械臂目标位姿为如下形式：
其中，是机械臂目标位姿，/>是机械臂基坐标系下初始时刻的械臂末端位姿，是工作空间坐标映射矩阵。将机械臂目标位姿/>转换为xyzabc形式，得到原始途径点序列。其中，xyzabc的含义分别为[X轴距离，Y轴距离，Z轴距离，偏转角，俯仰角，回转角]，即每个原始途径点具有6个自由度。
步骤S22：对所述原始途径点序列进行增量式处理，得到目标途径点序列。
原始途径点序列包括多个原始途径点，受到机械臂运动学的限制，一些原始途径点可能不存在于机械臂的可达空间内。为了确保下发的目标途径点存在于机械臂的可达空间内，方法还包括对原始途径点序列进行增量式处理。图3是根据本发明一实施例的增量式处理的流程图。如图3所示，增量式处理300包括如下步骤：
步骤301：将机械臂目标位姿矩阵离散化为第一增量矩阵，假设k时刻的机械臂目标位姿为则当前时刻的第一增量矩阵为：
其中是k-1时刻的机械臂目标位姿，dT是第一增量矩阵，dT描述了从k-1时刻到k时刻的机械臂末端目标位姿的变化。
步骤302：根据第一增量矩阵计算试探步位姿矩阵，可以通过如下公式计算试探步位姿矩阵：
Ttemp＝Tc(k-1)*dT
其中，Ttemp为试探步位姿矩阵，Tc(k-1)是k-1采样周期内机械臂末端在机械臂基坐标系下的位姿矩阵，dT是第一增量矩阵。
步骤303：对试探步位姿矩阵进行逆运动学计算，获取机械臂角关节值。
步骤304：判断机械臂角关节值是否超过角关节的工作范围，如果是，则舍弃当前的增量dT，使得Tc(k)＝Tc(k-1)，让机械臂暂停并保持当前位置；否则Tc(k)＝Ttemp。该过程确保下发的目标途径点始终保持在合理的机械臂的工作空间范围内。
步骤S23：根据目标途径点序列计算出光滑B样条的控制点序列。
B样条被广泛的用于描述各种不规则的任意曲线，通过设计不同的控制点位置来修改曲线的形状，在保证曲线能经过预设的途经点的同时能够保证速度和加速度的平滑连续。而光滑B样条则是在此基础上加上了一个平滑的作用，即通过修改平滑系数λ，调整曲线的平滑程度，这样可以有效的避免路径点发生跳变或者剧烈抖动的时候出现过大的加速度以及轨迹的大幅震荡。目标途径点序列中每个目标途径点同样可表示为xyzabc形式，其中，xyzabc的含义分别为[X轴距离，Y轴距离，Z轴距离，偏转角，俯仰角，回转角]，即每个目标途径点具有6个自由度。对于每个自由度分开规划，下述过程以单自由度为例说明根据目标途径点序列计算出光滑B样条的控制点序列的详细过程。
首先，根据目标途径点序列构造B样条曲线，B样条曲线可由n+1段基样条构成，每段基样条都由一个控制点来决定它的权重，将这一系列基样条加权后叠加，就构成了所需的B样条曲线。B样条曲线的基本构成函数为：
可由Deboor递归的形式构成，表示为如下形式：
其中，s(t是B样条曲线，pj是控制点，为基样条函数，p是B样条阶次，j是控制点序号，t是时间节点。
然后，将B样条曲线进行离散化处理得到离散B样条曲线。离散化处理是指将连续时间t用离散时间k代替，离散B样条曲线可表示为如下形式：
其中，pj是控制点，为基样条函数，N为节点之间的采样点数。其中N＝T/Ts，T是节点间的时间间隔，Ts是采样频率。
最后，对离散B样条曲线进行Z变换，可以得到控制点pj是关于目标途径点q的近似函数，根据近似函数计算出光滑B样条的控制点序列。近似函数可表示为如下形式：
其中，pk-M是第k-M个控制点，qk-n是第k-n个目标途径点，k是当前离散时间点，M是B样条离散近似过程的近似阶数，假设B样条曲线的样条次数为3，则近似阶数M的值为5；是脉冲响应系数，/> 的值取决于平滑系数λ的大小，λ越大生成的曲线越光滑。为了减少计算量，表1给出了针对不同λ值情况下的脉冲响应系数值，即 的值。
采用离散B样条曲线的传递函数的近似函数快速计算控制点，避免了Deboor递归所带来的时间消耗，计算量小，利于短间隔的实时轨迹规划；本发明可以通过修改平滑系数λ改变脉冲响应系数，进而调整曲线的平滑程度，这样可以有效的避免路径点发生跳变或者剧烈抖动的时候出现过大的加速度以及轨迹的大幅震荡。
表1脉冲响应系数与平滑系数对应表
步骤S24：对控制点序列进行重采样得到第一控制点序列。
以主手采样周期为10ms，机械臂的控制周期为1ms为例，每两个目标途径点之间需要插10个点。因此需要对得到的控制点进行重采样。具体地，将每个控制点重复10次得到第一控制点序列，第一控制点序列为多段常值离散点。
步骤S25：将第一控制点序列输入B样条近似滤波器得到规划轨迹，将规划轨迹发送给机械臂。
由前文可知，B样条曲线由控制点和基样条函数乘积求和得到，步骤S23已经求得了控制点，因此还需要计算基样条函数。而基样条函数离散化之后可以等效为p次0阶基样条函数连乘，离散基样条函数可表示为如下形式：
其中是离散基样条函数，/>为p次0阶基样条函数，p为B样条阶次，*是卷积符号。
在z域离散基样条函数的传递函数可以表示为p个均值滤波器M(z)的串联，P为B样条阶次，其中均值滤波器M(z)为如下形式：
其中，N为两个目标途径点之间所需插值点的数量。以主手采样周期为10ms，机械臂的控制周期为1ms为例，此时N＝10。
将串联的p个均值滤波器M(z)作为一个整体，命名为B样条近似滤波器。将第一控制点序列经过B样条近似滤波器之后即可得到规划好的光滑B样条轨迹。
图4是图2优化实施例的机械臂的在线轨迹规划方法的流程图。如图4所示，机械臂的在线轨迹规划方法400还包括：
步骤S211：对原始途径点序列进行平滑滤波。具体地，将xyzabc形式的原始途径点序列中的abc进行拼接，避免在达到±pi时发生反转跳变。例如在abc角度从0.9pi开始递增达到pi的时候会跳变为-pi，此时采用拼接方法将后续的点都+2pi，保证角度的连续性。随后采用低通滤波滤除采样过程中产生的高频部分以及人手抖动带来的低频分量。
方法还包括步骤S221：判断规划速度和规划加速度是否超过上限，如果是，则让机械臂暂停并保持当前位置。
具体地，根据机械臂当前位姿计算笛卡尔空间下速度和加速度的上限。可以通过如下公式计算速度和加速度的上限：
其中，其中x是操作空间位移，是笛卡尔空间下的速度，/>是笛卡尔空间下的加速度，J是雅克比矩阵，q是关节角。根据关节角最大转速和最大加速度，可以计算得到相应的操作空间下的速度和加速度的上限。
根据目标途径点序列计算规划速度和规划加速度。具体地，对目标途径点序列进行离散差分，计算得到出其在笛卡尔空间下的速度和加速度。例如从目标途径点序列P(t)获取其速度的离散差分可以通过如下公式：
其中dt是目标途径点序列时间间隔。
最后，判断规划速度和规划加速度是否超过上限，如果是，则让机械臂暂停并保持当前位置。
本发明采用光滑B样条曲线进行轨迹规划，可以保证规划轨迹的速度和加速度的平滑连续且轨迹平滑度可调，根据不同的工况可以设定不同的平滑度；本发明采用离散B样条曲线的传递函数的近似函数快速计算控制点，避免了Deboor递归所带来的时间消耗，计算量小，保证了轨迹规划的实时性；本发明通过增量式的控制方式实现了在超过机械臂工作空间范围时进行动态暂停和恢复的功能，即当主手输入设备下发的指令超出机械臂工作范围时能够让机械臂暂停并保持当前位置，直到主手输入设备下发的指令回到机械臂空间范围内，机械臂又可自动立即恢复工作且无需下达额外指令。
继续参考图1，在一些实施例中，计算单元122还配置为根据目标途径点序列构造离散B样条曲线，对离散B样条曲线进行Z变换得到目标途径点的近似函数，根据近似函数计算出光滑B样条的控制点序列。
可选地，近似函数为如下形式：
其中，p(k-M)是第k-M个控制点，q(k-n)是第k-n个目标途径点，k是当前离散时间点，M是B样条离散近似过程的近似阶数，是脉冲响应系数。
在一些实施例中，B样条近似滤波器的Z域传递函数为P个均值滤波器M(z)的串联，P为B样条阶次，其中均值滤波器M(z)为如下形式：
其中，N为两个目标途径点之间所需插值点的数量。
在一些实施例中，计算单元122还配置为将机械臂目标位姿矩阵离散化为第一增量矩阵，根据第一增量矩阵计算试探步位姿矩阵；对试探步位姿矩阵进行逆运动学计算，获取机械臂角关节值；判断机械臂角关节值是否超过角关节的工作范围，如果是，则让机械臂暂停并保持当前位置。
在一些实施例中，计算单元122还配置为根据机械臂当前位姿计算笛卡尔空间下速度和加速度的上限；根据目标途径点序列计算规划速度和规划加速度；判断规划速度和规划加速度是否超过上限，如果是，则让机械臂暂停并保持当前位置。
本申请还包括一种存储有计算机程序代码的计算机可读介质，该计算机程序代码在由处理器执行时实现前文所述的机械臂的在线轨迹规划方法。
机械臂的在线轨迹规划方法实施为计算机程序时，也可以存储在计算机可读存储介质中作为制品。例如，计算机可读存储介质可以包括但不限于磁存储设备(例如，硬盘、软盘、磁条)、光盘(例如，压缩盘(CD)、数字多功能盘(DVD))、智能卡和闪存设备(例如，电可擦除可编程只读存储器(EPROM)、卡、棒、键驱动)。此外，本文描述的各种存储介质能代表用于存储信息的一个或多个设备和/或其它机器可读介质。术语“机器可读介质”可以包括但不限于能存储、包含和/或承载代码和/或指令和/或数据的无线信道和各种其它介质(和/或存储介质)。
应该理解，上文所描述的实施例仅是示意。本文描述的实施例可在硬件、软件、固件、中间件、微码或者其任意组合中实现。对于硬件实现，处理器可以在一个或者多个特定用途集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理设备(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器和/或设计为执行本文所述功能的其它电子单元或者其结合内实现。
本申请的一些方面可以完全由硬件执行、可以完全由软件(包括固件、常驻软件、微码等)执行、也可以由硬件和软件组合执行。以上硬件或软件均可被称为“数据块”、“模块”、“引擎”、“单元”、“组件”或“***”。处理器可以是一个或多个专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理器件(DAPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器或者其组合。此外，本申请的各方面可能表现为位于一个或多个计算机可读介质中的计算机产品，该产品包括计算机可读程序编码。例如，计算机可读介质可包括，但不限于，磁性存储设备(例如，硬盘、软盘、磁带……)、光盘(例如，压缩盘CD、数字多功能盘DVD……)、智能卡以及闪存设备(例如，卡、棒、键驱动器……)。
计算机可读介质可能包含一个内含有计算机程序编码的传播数据信号，例如在基带上或作为载波的一部分。该传播信号可能有多种表现形式，包括电磁形式、光形式等等、或合适的组合形式。计算机可读介质可以是除计算机可读存储介质之外的任何计算机可读介质，该介质可以通过连接至一个指令执行***、装置或设备以实现通讯、传播或传输供使用的程序。位于计算机可读介质上的程序编码可以通过任何合适的介质进行传播，包括无线电、电缆、光纤电缆、射频信号、或类似介质、或任何上述介质的组合。
上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述申请披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请示范实施例的精神和范围。
同时，本申请使用了特定词语来描述本申请的实施例。如“一个实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。
上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述发明披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请示范实施例的精神和范围。
Claims (7)
1.一种机械臂的在线轨迹规划方法，其特征在于，包括：
接收每个采样周期内的主手末端位姿，将所述主手末端位姿转换为机械臂目标位姿矩阵，根据所述机械臂目标位姿矩阵得到原始途径点序列；
对所述原始途径点序列进行增量式处理，得到目标途径点序列；
根据所述目标途径点序列计算出光滑B样条的控制点序列；
对所述控制点序列进行重采样得到第一控制点序列；
将所述第一控制点序列输入B样条近似滤波器得到规划轨迹，将所述规划轨迹发送给机械臂；
其中，根据所述目标途径点序列计算出光滑B样条的控制点序列包括：根据所述目标途径点序列构造离散B样条曲线，对所述离散B样条曲线进行Z变换得到目标途径点的近似函数，根据所述近似函数计算出光滑B样条的控制点序列，所述近似函数为如下形式：
其中，是第k-M个控制点，/>是第k-n个目标途径点，k是当前离散时间点，M是B样条离散近似过程的近似阶数，/>是脉冲响应系数；
其中，所述B样条近似滤波器的Z域传递函数为P个均值滤波器M(z)的串联，P为B样条阶次，其中均值滤波器M(z)为如下形式：
其中，N为两个目标途径点之间所需插值点的数量。
2.如权利要求1所述的方法，其特征在于，对所述原始途径点序列进行增量式处理的步骤包括：
将所述机械臂目标位姿矩阵离散化为第一增量矩阵；
根据所述第一增量矩阵计算试探步位姿矩阵；
对所述试探步位姿矩阵进行逆运动学计算，获取机械臂角关节值；
判断所述机械臂角关节值是否超过角关节的工作范围，如果是，则让机械臂暂停并保持当前位置。
3.如权利要求1所述的方法，其特征在于，还包括：
根据机械臂当前位姿计算笛卡尔空间下速度和加速度的上限；
根据所述目标途径点序列计算规划速度和规划加速度；
判断所述规划速度和规划加速度是否超过所述上限，如果是，则让机械臂暂停并保持当前位置。
4.一种机械臂的在线轨迹规划装置，其特征在于，包括：
输入单元，配置为接收每个采样周期内的主手末端位姿，将所述主手末端位姿转换为机械臂目标位姿矩阵，根据所述机械臂目标位姿矩阵得到原始途径点序列；
计算单元，配置为对所述原始途径点序列进行增量式处理，得到目标途径点序列，根据所述目标途径点序列计算出光滑B样条的控制点序列，包括根据所述目标途径点序列构造离散B样条曲线，对所述离散B样条曲线进行Z变换得到目标途径点的近似函数，根据所述近似函数计算出光滑B样条的控制点序列，其中，所述近似函数为如下形式：
其中，是第k-M个控制点，/>是第k-n个目标途径点，k是当前离散时间点，M是B样条离散近似过程的近似阶数，/>是脉冲响应系数；
重采样单元，配置为对所述控制点序列进行重采样得到第一控制点序列；
B样条近似滤波器，配置为接收所述第一控制点序列，根据所述第一控制点序列得到规划轨迹，其中，所述B样条近似滤波器的Z域传递函数为P个均值滤波器M(z)的串联，P为B样条阶次，其中均值滤波器M(z)为如下形式：
其中，N为两个目标途径点之间所需插值点的数量；
输出单元，配置为将所述规划轨迹发送给机械臂。
5.如权利要求4所述的装置，其特征在于，所述计算单元还配置为将所述机械臂目标位姿矩阵离散化为第一增量矩阵，根据所述第一增量矩阵计算试探步位姿矩阵；对所述试探步位姿矩阵进行逆运动学计算，获取机械臂角关节值；判断所述机械臂角关节值是否超过角关节的工作范围，如果是，则让机械臂暂停并保持当前位置。
6.如权利要求4所述的装置，其特征在于，所述计算单元还配置为根据机械臂当前位姿计算笛卡尔空间下速度和加速度的上限；根据所述目标途径点序列计算规划速度和规划加速度；判断所述规划速度和规划加速度是否超过所述上限，如果是，则让机械臂暂停并保持当前位置。
7.一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如权利要求1-3任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211640360.9A CN115781683B (zh) | 2022-12-20 | 2022-12-20 | 机械臂的在线轨迹规划方法、装置及计算机可读介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211640360.9A CN115781683B (zh) | 2022-12-20 | 2022-12-20 | 机械臂的在线轨迹规划方法、装置及计算机可读介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115781683A CN115781683A (zh) | 2023-03-14 |
| CN115781683B true CN115781683B (zh) | 2024-05-24 |
Family
ID=85427314
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211640360.9A Active CN115781683B (zh) | 2022-12-20 | 2022-12-20 | 机械臂的在线轨迹规划方法、装置及计算机可读介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115781683B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108000501A (zh) * | 2017-11-22 | 2018-05-08 | 湖北工业大学 | 一种用于串联机器人的新型轨迹规划方法 |
| WO2021248798A1 (zh) * | 2020-06-08 | 2021-12-16 | 苏州艾吉威机器人有限公司 | 路径跟踪方法、***、装置及计算机可读存储介质 |
| CN114227672A (zh) * | 2021-11-26 | 2022-03-25 | 北京工业大学 | 机械臂安全碰撞轨迹规划方法、装置、存储介质及设备 |
| CN114633258A (zh) * | 2022-04-24 | 2022-06-17 | 中国铁建重工集团股份有限公司 | 一种隧道环境下机械臂运动轨迹的规划方法及相关装置 |
| CN114952868A (zh) * | 2022-07-26 | 2022-08-30 | 之江实验室 | 7自由度srs型机械臂控制方法及装置、弹琴机器人 |
| CN114952838A (zh) * | 2022-05-26 | 2022-08-30 | 哈尔滨工业大学 | 一种基于末端测量反馈的机械臂关节轨迹规划方法 |
| WO2022193639A1 (zh) * | 2021-03-18 | 2022-09-22 | 深圳市优必选科技股份有限公司 | 机械臂及其轨迹规划方法和装置 |
| CN115302502A (zh) * | 2022-07-08 | 2022-11-08 | 浙江大学 | 一种结合抓取网络和抓取位姿选择与调整的机械臂轨迹规划方法 |
-
2022
- 2022-12-20 CN CN202211640360.9A patent/CN115781683B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108000501A (zh) * | 2017-11-22 | 2018-05-08 | 湖北工业大学 | 一种用于串联机器人的新型轨迹规划方法 |
| WO2021248798A1 (zh) * | 2020-06-08 | 2021-12-16 | 苏州艾吉威机器人有限公司 | 路径跟踪方法、***、装置及计算机可读存储介质 |
| WO2022193639A1 (zh) * | 2021-03-18 | 2022-09-22 | 深圳市优必选科技股份有限公司 | 机械臂及其轨迹规划方法和装置 |
| CN114227672A (zh) * | 2021-11-26 | 2022-03-25 | 北京工业大学 | 机械臂安全碰撞轨迹规划方法、装置、存储介质及设备 |
| CN114633258A (zh) * | 2022-04-24 | 2022-06-17 | 中国铁建重工集团股份有限公司 | 一种隧道环境下机械臂运动轨迹的规划方法及相关装置 |
| CN114952838A (zh) * | 2022-05-26 | 2022-08-30 | 哈尔滨工业大学 | 一种基于末端测量反馈的机械臂关节轨迹规划方法 |
| CN115302502A (zh) * | 2022-07-08 | 2022-11-08 | 浙江大学 | 一种结合抓取网络和抓取位姿选择与调整的机械臂轨迹规划方法 |
| CN114952868A (zh) * | 2022-07-26 | 2022-08-30 | 之江实验室 | 7自由度srs型机械臂控制方法及装置、弹琴机器人 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115781683A (zh) | 2023-03-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP7203058B2 (ja) | 機械システムへの入力を生成する方法、及び複数のロボットジョイントを含むロボット | |
| EP0433375B1 (en) | Shaping command input to minimize unwanted dynamics | |
| Mohamed et al. | Command shaping techniques for vibration control of a flexible robot manipulator | |
| CA2735262C (en) | Inverse kinematics | |
| WO2002023286A9 (en) | Time domain passivity control of haptic interfaces | |
| CN111249005A (zh) | 一种穿刺手术机器人柔顺控制*** | |
| Chen et al. | Neural learning enhanced variable admittance control for human–robot collaboration | |
| Wu et al. | Development and hybrid force/position control of a compliant rescue manipulator | |
| CN109571432A (zh) | 一种基于力传感器的机器人直接示教方法 | |
| CN111702767A (zh) | 一种基于反演模糊自适应的机械手阻抗控制方法 | |
| Su et al. | Hybrid adaptive/robust motion control of rigid-link electrically-driven robot manipulators | |
| Ghorbani et al. | Comparison of various input shaping methods in rest-to-rest motion of the end-effecter of a rigid-flexible robotic system with large deformations capability | |
| CN115202293B (zh) | 一种工业机器人两段式速度规划方法 | |
| CN114179089A (zh) | 一种机械臂的鲁棒区域跟踪控制方法 | |
| CN115781683B (zh) | 机械臂的在线轨迹规划方法、装置及计算机可读介质 | |
| CN113143465B (zh) | 一种基于关节力矩的机械臂拖动引导定位的方法 | |
| CN114131617A (zh) | 一种工业机器人的智能柔顺控制方法和装置 | |
| Thomsen et al. | Smooth online time-varying input shaping with fractional delay FIR filtering | |
| CN116736748A (zh) | 构建机器人的控制器的方法和机器人 | |
| Flash et al. | Communication: Discrete trajectory planning | |
| Vossoughi et al. | Robot finger stiffness control in the presence of mechanical nonlinearities | |
| Kuznetcova et al. | DESIGN AND SIMULATION ANALYSIS OF HAPTIC DEVICE WITH PARALLEL KINEMATIC. | |
| Tiwari et al. | Improved dual haptic controller with extra virtual damper for Z-width enhancement | |
| CN109634406B (zh) | 一种仿生两手指运动控制方法、装置及设备 | |
| Yang et al. | Velocity Constraints Based Online Trajectory Planning for High-Speed Parallel Robots |
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 |