CN112873208A - 一种抗噪与动力学约束的机器人实时运动规划方法及装置 - Google Patents

一种抗噪与动力学约束的机器人实时运动规划方法及装置 Download PDF

Info

Publication number
CN112873208A
CN112873208A CN202110125245.7A CN202110125245A CN112873208A CN 112873208 A CN112873208 A CN 112873208A CN 202110125245 A CN202110125245 A CN 202110125245A CN 112873208 A CN112873208 A CN 112873208A
Authority
CN
China
Prior art keywords
robot
joint
theta
derivative
angular acceleration
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
CN202110125245.7A
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.)
Foshan Shuke Intelligent Robot Technology Co ltd
Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
Original Assignee
Foshan Shuke Intelligent Robot Technology Co ltd
Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
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 Foshan Shuke Intelligent Robot Technology Co ltd, Institute of Intelligent Manufacturing of Guangdong Academy of Sciences filed Critical Foshan Shuke Intelligent Robot Technology Co ltd
Priority to CN202110125245.7A priority Critical patent/CN112873208A/zh
Publication of CN112873208A publication Critical patent/CN112873208A/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
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种抗噪与动力学约束的机器人实时运动规划方法及装置，其中，所述方法包括：读取当前时刻机器人的关节角度、关节角速度的反馈；基于当前的关节角和关节角加速度与机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；获得机器人的期望轨迹，计算机器人的跟踪误差及其对应的导数；在机器人的控制系统存在噪声情况下，设置机器人在实现轨迹跟踪时的加速度等式约束条件；计算机器人角加速度以及辅助变量的变化率；基于机器人角加速度以及辅助变量的变化率更新机器人的角加速度指令。在本发明实施例中，能够在控制系统存在噪声的情况下，实现机器人末端对期望轨迹的高精度轨迹跟踪。

Description

一种抗噪与动力学约束的机器人实时运动规划方法及装置
技术领域
本发明涉及机器人控制技术领域，尤其涉及一种抗噪与动力学约束的机器人实时运动规划方法及装置。
背景技术
机器人运动规划是机器人高精度运动控制的重要前提。其核心问题是针对机器人末端执行器的期望运动，从机器人关节空间规划相应的运动指令，是机器人末端执行器在遵循预设轨迹的同时，保证机器人自身的固有约束。
目前机器人的运动规划可以离线运动规划与在线运动规划分为两大类。顾名思义，离线规划无法在机器人作业的过程中同时进行规划，效率略低。目前的在线运动规划以雅克比矩阵求逆法为主，其做法基本都是将问题的解描述为一个最小范数解加上一个同类解，即解析解的形式。对实际机器人系统而言，机器人的物理约束是多层面的，这些约束不仅包括角度、角速度，更包括角加速度与驱动力矩，这是机器人机电系统的本质决定的。此外，机器人系统必然受到外界测量噪声的影响，如何在系统受到诸多约束与噪声影响下，设计机器人的高效在线运动规划方法，是一个待解决的重要难题。
发明内容
本发明的目的在于克服现有技术的不足，本发明提供了一种抗噪与动力学约束的机器人实时运动规划方法及装置，能够在控制系统存在噪声的情况下，实现机器人末端对期望轨迹的高精度轨迹跟踪。
为了解决上述技术问题，本发明实施例提供了一种抗噪与动力学约束的机器人实时运动规划方法，所述方法包括：
读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；
基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；同时，
获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；
在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；
基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；
基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
2、根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述方法还包括：
建立机器人数学模型，并基于机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立机器人的物理约束模型；
基于所述机器人数学模型及所述物理约束模型建立与角加速度相关的综合物理不等式模型。
3、根据权利要求2所述的机器人实时运动规划方法，其特征在于，所述机器人数学模型如下：
Figure BDA0002923378050000021
所述物理约束模型如下：
Figure BDA0002923378050000022
其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；
Figure BDA0002923378050000023
为r的一阶导数，
Figure BDA0002923378050000024
为r的二阶导数；
Figure BDA0002923378050000025
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000031
为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000032
为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000033
表示
Figure BDA0002923378050000034
下限，
Figure BDA0002923378050000035
表示
Figure BDA0002923378050000036
上限；
Figure BDA0002923378050000037
表示
Figure BDA0002923378050000038
下限，
Figure BDA0002923378050000039
表示
Figure BDA00029233780500000310
上限；τ-表示τ下限，τ+表示τ上限。
4、根据权利要求2所述的机器人实时运动规划方法，其特征在于，所述综合物理不等式模型如下：
Figure BDA00029233780500000311
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500000312
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500000313
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500000314
为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500000315
表示
Figure BDA00029233780500000316
下限，
Figure BDA00029233780500000317
表示
Figure BDA00029233780500000318
上限；
Figure BDA00029233780500000319
表示
Figure BDA00029233780500000320
下限，
Figure BDA00029233780500000321
表示
Figure BDA00029233780500000322
上限；τ-表示τ下限，τ+表示τ上限。
5、根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述加速度等式约束条件如下：
Figure BDA00029233780500000323
其中，θ为机器人关节角度，
Figure BDA00029233780500000324
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500000325
为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；
Figure BDA00029233780500000326
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500000327
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
6、根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率，还包括：
建立所述加速度等式约束条件和综合物理不等式模型与加速度优化指标的问题描述模型；
基于所述问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率。
7、根据权利要求6所述的机器人实时运动规划方法，其特征在于，所述问题描述模型如下：
min
Figure BDA0002923378050000041
s.t.
Figure BDA0002923378050000042
Figure BDA0002923378050000043
Figure BDA0002923378050000044
Figure BDA0002923378050000045
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA0002923378050000046
均为正控制常数；θ为机器人关节角度，
Figure BDA0002923378050000047
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000048
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA0002923378050000049
为
Figure BDA00029233780500000410
的转置；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500000411
表示
Figure BDA00029233780500000412
下限，
Figure BDA00029233780500000413
表示
Figure BDA00029233780500000414
上限；
Figure BDA00029233780500000415
表示
Figure BDA00029233780500000416
下限，
Figure BDA00029233780500000417
表示
Figure BDA00029233780500000418
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500000419
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500000420
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
8、根据权利要求6所述的机器人实时运动规划方法，其特征在于，所述迭代的机器人控制器求解模型如下：
Figure BDA00029233780500000421
Figure BDA00029233780500000422
Figure BDA00029233780500000423
Figure BDA00029233780500000424
其中，∈>0，λ1、λ2、λ3均为辅助变量，PΩ为饱和函数，该函数的上限为
Figure BDA00029233780500000425
下限为
Figure BDA00029233780500000426
Figure BDA0002923378050000051
分别为λ1、λ2、λ3的一阶导数；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA0002923378050000052
均为正控制常数；θ为机器人关节角度，
Figure BDA0002923378050000053
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000054
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA0002923378050000055
为θ的三阶导数；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000056
表示
Figure BDA0002923378050000057
下限，
Figure BDA0002923378050000058
表示
Figure BDA0002923378050000059
上限；
Figure BDA00029233780500000510
表示
Figure BDA00029233780500000511
下限，
Figure BDA00029233780500000512
表示
Figure BDA00029233780500000513
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500000514
为J的一阶导数，JT为J的转置；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500000515
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
9、根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令，包括：
将所述机器人角加速度以及辅助变量的变化率反馈至所述机器人的控制系统中，在所述机器人的控制系统内根据所述机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
10、一种抗噪与动力学约束的机器人实时运动规划装置，其特征在于，所述装置包括：
读取模块：用于读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；
推导模块：用于基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；同时，
第一计算模块：用于获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；
设置模块：用于在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；
第二计算模块：用于基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；
更新模块：用于基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
在本发明实施例中，解决在控制系统存在关节角度约束、关节角速度约束、关节加速度约束、力矩约束以及受到噪声影响的情况下，实时规划机器人的实时加速度指令，从而实现机器人对给定末端执行器操作轨迹的高精度跟踪；并且能够在控制系统存在噪声的情况下，实现机器人末端对期望轨迹的高精度轨迹跟踪；能够避免机器人的关节角与角速度超限，同时能够有效避免机器人的加速度与驱动力矩过大，这对于实现全过程的机器人高精度控制具有重要意义。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明实施例中的抗噪与动力学约束的机器人实时运动规划方法的流程示意图；
图2是本发明实施例中的抗噪与动力学约束的机器人实时运动规划装置的结构组成示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
实施例
请参阅图1，图1是本发明实施例中的抗噪与动力学约束的机器人实时运动规划方法的流程示意图。
如图1所述，一种抗噪与动力学约束的机器人实时运动规划方法，所述方法包括：
S11：建立机器人数学模型，并基于机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立机器人的物理约束模型；
在本发明具体实施过程中，所述机器人数学模型如下：
Figure BDA0002923378050000071
所述物理约束模型如下：
Figure BDA0002923378050000072
其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；
Figure BDA0002923378050000073
为r的一阶导数，
Figure BDA0002923378050000074
为r的二阶导数；
Figure BDA0002923378050000075
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000076
为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000077
为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000078
表示
Figure BDA0002923378050000079
下限，
Figure BDA00029233780500000710
表示
Figure BDA00029233780500000711
上限；
Figure BDA00029233780500000712
表示
Figure BDA00029233780500000713
下限，
Figure BDA00029233780500000714
表示
Figure BDA00029233780500000715
上限；τ-表示τ下限，τ+表示τ上限。
具体的，需要建立机器人数学模型，该机器人数学模型是根据机器人的关节角度、机器人末端执行器的坐标、机器人的前向运动学模型；机器人的雅克比矩阵、机器人的关节力矩、机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩建立的，具体如下：
Figure BDA00029233780500000716
在建立机器人数学模型之后，还根据机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立的物理约束模型，具体如下：
Figure BDA0002923378050000081
其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；
Figure BDA0002923378050000082
为r的一阶导数，
Figure BDA0002923378050000083
为r的二阶导数；
Figure BDA0002923378050000084
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000085
为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000086
为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000087
表示
Figure BDA0002923378050000088
下限，
Figure BDA0002923378050000089
表示
Figure BDA00029233780500000810
上限；
Figure BDA00029233780500000811
表示
Figure BDA00029233780500000812
下限，
Figure BDA00029233780500000813
表示
Figure BDA00029233780500000814
上限；τ-表示τ下限，τ+表示τ上限。
S12：基于所述机器人数学模型及所述物理约束模型建立与角加速度相关的综合物理不等式模型；
在本发明具体实施过程中，所述综合物理不等式模型如下：
Figure BDA00029233780500000815
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500000816
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500000817
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500000818
为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500000819
表示
Figure BDA00029233780500000820
下限，
Figure BDA00029233780500000821
表示
Figure BDA00029233780500000822
上限；
Figure BDA00029233780500000823
表示
Figure BDA00029233780500000824
下限，
Figure BDA00029233780500000825
表示
Figure BDA00029233780500000826
上限；τ-表示τ下限，τ+表示τ上限。
具体的，需要根据机器人数学模型和物理约束模型来建立与角加速度相关的综合物理不等式模型，该综合物理不等式模型如下：
Figure BDA00029233780500000827
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500000828
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500000829
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000091
为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000092
表示
Figure BDA0002923378050000093
下限，
Figure BDA0002923378050000094
表示
Figure BDA0002923378050000095
上限；
Figure BDA0002923378050000096
表示
Figure BDA0002923378050000097
下限，
Figure BDA0002923378050000098
表示
Figure BDA0002923378050000099
上限；τ-表示τ下限，τ+表示τ上限。
S13：读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；
在本发明具体实施过程中，通过设置在该机器人各个关节上的传感器设备来读取当前时刻机器人的关节角度、关节角速度的反馈，从而获得机器人的关节角度和关节角加速度。
S14：基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；
在本发明具体实施过程中，在得到当前的关节角和关节角加速度之后，结合机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩即可推导当前角加速度的上下限。
S15：获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；
在本发明具体实施过程中，该机器人的期望轨迹一般由用户设置，机器人当前的运动轨迹可以根据当前机器人的实时状态获得，即可通过机器人的当前运动轨迹与期望轨迹计算比较，得到跟踪误差，然后对该跟踪误差进行求导，即可得到跟踪误差的导数。
S16：在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；
在本发明具体实施过程中，所述加速度等式约束条件如下：
Figure BDA00029233780500000910
其中，θ为机器人关节角度，
Figure BDA00029233780500000911
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500000912
为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；
Figure BDA00029233780500000913
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500000914
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
具体的，在设置具有抗噪能力的机器人跟踪条件，需要在机器人控制系统存在噪声的情况下，根据跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；该加速度等式约束条件如下：
Figure BDA0002923378050000101
其中，θ为机器人关节角度，
Figure BDA0002923378050000102
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000103
为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000104
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA0002923378050000105
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
S17：基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；
在本发明具体实施过程中，所述基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率，还包括：建立所述加速度等式约束条件和综合物理不等式模型与加速度优化指标的问题描述模型；基于所述问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率。
进一步的，所述问题描述模型如下：
min
Figure BDA0002923378050000106
s.t.
Figure BDA0002923378050000107
Figure BDA0002923378050000108
Figure BDA0002923378050000109
Figure BDA00029233780500001010
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500001011
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500001012
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001013
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA00029233780500001014
为
Figure BDA00029233780500001015
的转置；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001016
表示
Figure BDA00029233780500001017
下限，
Figure BDA00029233780500001018
表示
Figure BDA00029233780500001019
上限；
Figure BDA00029233780500001020
表示
Figure BDA00029233780500001021
下限，
Figure BDA00029233780500001022
表示
Figure BDA00029233780500001023
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001024
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001025
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
进一步的，所述迭代的机器人控制器求解模型如下：
Figure BDA0002923378050000111
Figure BDA0002923378050000112
Figure BDA0002923378050000113
Figure BDA0002923378050000114
其中，∈>0，λ1、λ2、λ3均为辅助变量，PΩ为饱和函数，该函数的上限为
Figure BDA0002923378050000115
下限为
Figure BDA0002923378050000116
Figure BDA0002923378050000117
分别为λ1、λ2、λ3的一阶导数；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA0002923378050000118
均为正控制常数；θ为机器人关节角度，
Figure BDA0002923378050000119
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001110
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA00029233780500001111
为θ的三阶导数；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001112
表示
Figure BDA00029233780500001113
下限，
Figure BDA00029233780500001114
表示
Figure BDA00029233780500001115
上限；
Figure BDA00029233780500001116
表示
Figure BDA00029233780500001117
下限，
Figure BDA00029233780500001118
表示
Figure BDA00029233780500001119
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001120
为J的一阶导数，JT为J的转置；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001121
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
具体的，在得到加速度等式约束条件和综合物理不等式模型之后，需要根据加速度等式约束条件和综合物理不等式模型与加速度优化指标建立问题描述模型，该问题描述模型如下：
min
Figure BDA00029233780500001122
s.t.
Figure BDA00029233780500001123
Figure BDA00029233780500001124
Figure BDA00029233780500001125
Figure BDA00029233780500001126
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500001127
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500001128
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001129
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA00029233780500001130
为
Figure BDA00029233780500001131
的转置；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001132
表示
Figure BDA00029233780500001133
下限，
Figure BDA00029233780500001134
表示
Figure BDA00029233780500001135
上限；
Figure BDA00029233780500001136
表示
Figure BDA00029233780500001137
下限，
Figure BDA00029233780500001138
表示
Figure BDA00029233780500001139
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000121
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA0002923378050000122
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
然后通过该问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率；该迭代的机器人控制器求解模型如下：
Figure BDA0002923378050000123
Figure BDA0002923378050000124
Figure BDA0002923378050000125
Figure BDA0002923378050000126
其中，∈>0，λ1、λ2、λ3均为辅助变量，PΩ为饱和函数，该函数的上限为
Figure BDA0002923378050000127
下限为
Figure BDA0002923378050000128
Figure BDA0002923378050000129
分别为λ1、λ2、λ3的一阶导数；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500001210
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500001211
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001212
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA00029233780500001213
为θ的三阶导数；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001214
表示
Figure BDA00029233780500001215
下限，
Figure BDA00029233780500001216
表示
Figure BDA00029233780500001217
上限；
Figure BDA00029233780500001218
表示
Figure BDA00029233780500001219
下限，
Figure BDA00029233780500001220
表示
Figure BDA00029233780500001221
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001222
为J的一阶导数，JT为J的转置；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001223
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
S18：基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
在本发明具体实施过程中，所述基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令，包括：将所述机器人角加速度以及辅助变量的变化率反馈至所述机器人的控制系统中，在所述机器人的控制系统内根据所述机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
具体的，通过将该机器人角加速度以及辅助变量的变化率反馈至所述机器人的控制系统中，在该机器人控制系统接收到机器人角加速度以及辅助变量的变化率后，根据机器人角加速度以及辅助变量的变化率更新该机器人的角加速度指令，最终通过该角加速度指令实现对该机器人的控制。
在本发明实施例中，解决在控制系统存在关节角度约束、关节角速度约束、关节加速度约束、力矩约束以及受到噪声影响的情况下，实时规划机器人的实时加速度指令，从而实现机器人对给定末端执行器操作轨迹的高精度跟踪；并且能够在控制系统存在噪声的情况下，实现机器人末端对期望轨迹的高精度轨迹跟踪；能够避免机器人的关节角与角速度超限，同时能够有效避免机器人的加速度与驱动力矩过大，这对于实现全过程的机器人高精度控制具有重要意义。
实施例
请参阅图2，图2是本发明实施例中的抗噪与动力学约束的机器人实时运动规划装置的结构组成示意图。
如图2所示，一种抗噪与动力学约束的机器人实时运动规划装置，所述装置包括：
第一建立模块21：用于建立机器人数学模型，并基于机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立机器人的物理约束模型；
在本发明具体实施过程中，所述机器人数学模型如下：
Figure BDA0002923378050000131
所述物理约束模型如下：
Figure BDA0002923378050000132
其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；
Figure BDA0002923378050000133
为r的一阶导数，
Figure BDA0002923378050000134
为r的二阶导数；
Figure BDA0002923378050000135
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000136
为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000141
为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000142
表示
Figure BDA0002923378050000143
下限，
Figure BDA0002923378050000144
表示
Figure BDA0002923378050000145
上限；
Figure BDA0002923378050000146
表示
Figure BDA0002923378050000147
下限，
Figure BDA0002923378050000148
表示
Figure BDA0002923378050000149
上限；τ-表示τ下限，τ+表示τ上限。
具体的，需要建立机器人数学模型，该机器人数学模型是根据机器人的关节角度、机器人末端执行器的坐标、机器人的前向运动学模型；机器人的雅克比矩阵、机器人的关节力矩、机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩建立的，具体如下：
Figure BDA00029233780500001410
在建立机器人数学模型之后，还根据机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立的物理约束模型，具体如下：
Figure BDA00029233780500001411
其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；
Figure BDA00029233780500001412
为r的一阶导数，
Figure BDA00029233780500001413
为r的二阶导数；
Figure BDA00029233780500001414
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001415
为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001416
为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001417
表示
Figure BDA00029233780500001418
下限，
Figure BDA00029233780500001419
表示
Figure BDA00029233780500001420
上限；
Figure BDA00029233780500001421
表示
Figure BDA00029233780500001422
下限，
Figure BDA00029233780500001423
表示
Figure BDA00029233780500001424
上限；τ-表示τ下限，τ+表示τ上限。
第二建立模块22：用于基于所述机器人数学模型及所述物理约束模型建立与角加速度相关的综合物理不等式模型；
在本发明具体实施过程中，所述综合物理不等式模型如下：
Figure BDA00029233780500001425
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA0002923378050000151
均为正控制常数；θ为机器人关节角度，
Figure BDA0002923378050000152
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000153
为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000154
表示
Figure BDA0002923378050000155
下限，
Figure BDA0002923378050000156
表示
Figure BDA0002923378050000157
上限；
Figure BDA0002923378050000158
表示
Figure BDA0002923378050000159
下限，
Figure BDA00029233780500001510
表示
Figure BDA00029233780500001511
上限；τ-表示τ下限，τ+表示τ上限。
具体的，需要根据机器人数学模型和物理约束模型来建立与角加速度相关的综合物理不等式模型，该综合物理不等式模型如下：
Figure BDA00029233780500001512
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500001513
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500001514
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001515
为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001516
表示
Figure BDA00029233780500001517
下限，
Figure BDA00029233780500001518
表示
Figure BDA00029233780500001519
上限；
Figure BDA00029233780500001520
表示
Figure BDA00029233780500001521
下限，
Figure BDA00029233780500001522
表示
Figure BDA00029233780500001523
上限；τ-表示τ下限，τ+表示τ上限。
读取模块23：用于读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；
在本发明具体实施过程中，通过设置在该机器人各个关节上的传感器设备来读取当前时刻机器人的关节角度、关节角速度的反馈，从而获得机器人的关节角度和关节角加速度。
推导模块24：用于基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；
在本发明具体实施过程中，在得到当前的关节角和关节角加速度之后，结合机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩即可推导当前角加速度的上下限。
第一计算模块25：用于获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；
在本发明具体实施过程中，该机器人的期望轨迹一般由用户设置，机器人当前的运动轨迹可以根据当前机器人的实时状态获得，即可通过机器人的当前运动轨迹与期望轨迹计算比较，得到跟踪误差，然后对该跟踪误差进行求导，即可得到跟踪误差的导数。
设置模块26：用于在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；
在本发明具体实施过程中，所述加速度等式约束条件如下：
Figure BDA0002923378050000161
其中，θ为机器人关节角度，
Figure BDA0002923378050000162
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000163
为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000164
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA0002923378050000165
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
具体的，在设置具有抗噪能力的机器人跟踪条件，需要在机器人控制系统存在噪声的情况下，根据跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；该加速度等式约束条件如下：
Figure BDA0002923378050000166
其中，θ为机器人关节角度，
Figure BDA0002923378050000167
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000168
为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000169
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001610
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
第二计算模块27：用于基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；
在本发明具体实施过程中，所述基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率，还包括：建立所述加速度等式约束条件和综合物理不等式模型与加速度优化指标的问题描述模型；基于所述问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率。
进一步的，所述问题描述模型如下：
min
Figure BDA0002923378050000171
s.t.
Figure BDA0002923378050000172
Figure BDA0002923378050000173
Figure BDA0002923378050000174
Figure BDA0002923378050000175
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA0002923378050000176
均为正控制常数；θ为机器人关节角度，
Figure BDA0002923378050000177
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000178
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA0002923378050000179
为
Figure BDA00029233780500001710
的转置；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001711
表示
Figure BDA00029233780500001712
下限，
Figure BDA00029233780500001713
表示
Figure BDA00029233780500001714
上限；
Figure BDA00029233780500001715
表示
Figure BDA00029233780500001716
下限，
Figure BDA00029233780500001717
表示
Figure BDA00029233780500001718
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001719
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001720
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
进一步的，所述迭代的机器人控制器求解模型如下：
Figure BDA00029233780500001721
Figure BDA00029233780500001722
Figure BDA00029233780500001723
Figure BDA00029233780500001724
其中，∈>0，λ1、λ2、λ3均为辅助变量，PΩ为饱和函数，该函数的上限为
Figure BDA00029233780500001725
下限为
Figure BDA00029233780500001726
Figure BDA00029233780500001727
分别为λ1、λ2、λ3的一阶导数；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500001728
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500001729
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001730
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA00029233780500001731
为θ的三阶导数；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001732
表示
Figure BDA00029233780500001733
下限，
Figure BDA00029233780500001734
表示
Figure BDA00029233780500001735
上限；
Figure BDA00029233780500001736
表示
Figure BDA00029233780500001737
下限，
Figure BDA00029233780500001738
表示
Figure BDA00029233780500001739
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001740
为J的一阶导数，JT为J的转置；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001741
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
具体的，在得到加速度等式约束条件和综合物理不等式模型之后，需要根据加速度等式约束条件和综合物理不等式模型与加速度优化指标建立问题描述模型，该问题描述模型如下：
min
Figure BDA0002923378050000181
s.t.
Figure BDA0002923378050000182
Figure BDA0002923378050000183
Figure BDA0002923378050000184
Figure BDA0002923378050000185
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA0002923378050000186
均为正控制常数；θ为机器人关节角度，
Figure BDA0002923378050000187
为θ的一阶导数，表示机器人关节角速度，
Figure BDA0002923378050000188
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA0002923378050000189
为
Figure BDA00029233780500001810
的转置；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA00029233780500001811
表示
Figure BDA00029233780500001812
下限，
Figure BDA00029233780500001813
表示
Figure BDA00029233780500001814
上限；
Figure BDA00029233780500001815
表示
Figure BDA00029233780500001816
下限，
Figure BDA00029233780500001817
表示
Figure BDA00029233780500001818
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA00029233780500001819
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001820
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
然后通过该问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率；该迭代的机器人控制器求解模型如下：
Figure BDA00029233780500001821
Figure BDA00029233780500001822
Figure BDA00029233780500001823
Figure BDA00029233780500001824
其中，∈>0，λ1、λ2、λ3均为辅助变量，PΩ为饱和函数，该函数的上限为
Figure BDA00029233780500001825
下限为
Figure BDA00029233780500001826
Figure BDA00029233780500001827
分别为λ1、λ2、λ3的一阶导数；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure BDA00029233780500001828
均为正控制常数；θ为机器人关节角度，
Figure BDA00029233780500001829
为θ的一阶导数，表示机器人关节角速度，
Figure BDA00029233780500001830
为θ的二阶导数，表示机器人关节角加速度，
Figure BDA00029233780500001831
为θ的三阶导数；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure BDA0002923378050000191
表示
Figure BDA0002923378050000192
下限，
Figure BDA0002923378050000193
表示
Figure BDA0002923378050000194
上限；
Figure BDA0002923378050000195
表示
Figure BDA0002923378050000196
下限，
Figure BDA0002923378050000197
表示
Figure BDA0002923378050000198
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure BDA0002923378050000199
为J的一阶导数，JT为J的转置；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure BDA00029233780500001910
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
更新模块28：用于基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
在本发明具体实施过程中，所述基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令，包括：将所述机器人角加速度以及辅助变量的变化率反馈至所述机器人的控制系统中，在所述机器人的控制系统内根据所述机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
在本发明实施例中，解决在控制系统存在关节角度约束、关节角速度约束、关节加速度约束、力矩约束以及受到噪声影响的情况下，实时规划机器人的实时加速度指令，从而实现机器人对给定末端执行器操作轨迹的高精度跟踪；并且能够在控制系统存在噪声的情况下，实现机器人末端对期望轨迹的高精度轨迹跟踪；能够避免机器人的关节角与角速度超限，同时能够有效避免机器人的加速度与驱动力矩过大，这对于实现全过程的机器人高精度控制具有重要意义。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。
另外，以上对本发明实施例所提供的一种抗噪与动力学约束的机器人实时运动规划方法及装置进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种抗噪与动力学约束的机器人实时运动规划方法，其特征在于，所述方法包括：
读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；
基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；同时，
获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；
在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；
基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；
基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
2.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述方法还包括：
建立机器人数学模型，并基于机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立机器人的物理约束模型；
基于所述机器人数学模型及所述物理约束模型建立与角加速度相关的综合物理不等式模型。
3.根据权利要求2所述的机器人实时运动规划方法，其特征在于，所述机器人数学模型如下：
Figure FDA0002923378040000021
所述物理约束模型如下：
Figure FDA0002923378040000022
其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；
Figure FDA0002923378040000023
为r的一阶导数，
Figure FDA0002923378040000024
为r的二阶导数；
Figure FDA0002923378040000025
为θ的一阶导数，表示机器人关节角速度，
Figure FDA0002923378040000026
为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；
Figure FDA0002923378040000027
为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ-表示θ下限，θ+表示θ上限；
Figure FDA0002923378040000028
表示
Figure FDA0002923378040000029
下限，
Figure FDA00029233780400000210
表示
Figure FDA00029233780400000211
上限；
Figure FDA00029233780400000212
表示
Figure FDA00029233780400000213
下限，
Figure FDA00029233780400000214
表示
Figure FDA00029233780400000215
上限；τ-表示τ下限，τ+表示τ上限。
4.根据权利要求2所述的机器人实时运动规划方法，其特征在于，所述综合物理不等式模型如下：
Figure FDA00029233780400000216
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、k2、
Figure FDA00029233780400000217
均为正控制常数；θ为机器人关节角度，
Figure FDA00029233780400000218
为θ的一阶导数，表示机器人关节角速度，
Figure FDA00029233780400000219
为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure FDA00029233780400000220
表示
Figure FDA00029233780400000221
下限，
Figure FDA00029233780400000222
表示
Figure FDA00029233780400000223
上限；
Figure FDA00029233780400000224
表示
Figure FDA00029233780400000225
下限，
Figure FDA00029233780400000226
表示
Figure FDA00029233780400000227
上限；τ-表示τ下限，τ+表示τ上限。
5.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述加速度等式约束条件如下：
Figure FDA0002923378040000031
其中，θ为机器人关节角度，
Figure FDA0002923378040000032
为θ的一阶导数，表示机器人关节角速度，
Figure FDA0002923378040000033
为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；
Figure FDA0002923378040000034
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure FDA0002923378040000035
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
6.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率，还包括：
建立所述加速度等式约束条件和综合物理不等式模型与加速度优化指标的问题描述模型；
基于所述问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率。
7.根据权利要求6所述的机器人实时运动规划方法，其特征在于，所述问题描述模型如下：
Figure FDA0002923378040000036
Figure FDA0002923378040000037
Figure FDA0002923378040000038
Figure FDA0002923378040000039
Figure FDA00029233780400000310
其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure FDA00029233780400000311
均为正控制常数；θ为机器人关节角度，
Figure FDA00029233780400000312
为θ的一阶导数，表示机器人关节角速度，
Figure FDA00029233780400000313
为θ的二阶导数，表示机器人关节角加速度，
Figure FDA00029233780400000314
为
Figure FDA00029233780400000315
的转置；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure FDA00029233780400000316
表示
Figure FDA00029233780400000317
下限，
Figure FDA00029233780400000318
表示
Figure FDA00029233780400000319
上限；
Figure FDA00029233780400000320
表示
Figure FDA00029233780400000321
下限，
Figure FDA00029233780400000322
表示
Figure FDA00029233780400000323
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure FDA00029233780400000324
为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure FDA0002923378040000041
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
8.根据权利要求6所述的机器人实时运动规划方法，其特征在于，所述迭代的机器人控制器求解模型如下：
Figure FDA0002923378040000042
Figure FDA0002923378040000043
Figure FDA0002923378040000044
Figure FDA0002923378040000045
其中，∈>0，λ1、λ2、λ3均为辅助变量，PΩ为饱和函数，该函数的上限为
Figure FDA0002923378040000046
下限为
Figure FDA0002923378040000047
Figure FDA0002923378040000048
Figure FDA0002923378040000049
分别为λ1、λ2、λ3的一阶导数；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ1、κ2、
Figure FDA00029233780400000410
均为正控制常数；θ为机器人关节角度，
Figure FDA00029233780400000411
为θ的一阶导数，表示机器人关节角速度，
Figure FDA00029233780400000412
为θ的二阶导数，表示机器人关节角加速度，
Figure FDA00029233780400000413
为θ的三阶导数；τ为机器人的关节力矩；θ-表示θ下限，θ+表示θ上限；
Figure FDA00029233780400000414
表示
Figure FDA00029233780400000415
下限，
Figure FDA00029233780400000416
表示
Figure FDA00029233780400000417
上限；
Figure FDA00029233780400000418
表示
Figure FDA00029233780400000419
下限，
Figure FDA00029233780400000420
表示
Figure FDA00029233780400000421
上限；τ-表示τ下限，τ+表示τ上限；J表示机器人的雅克比矩阵；
Figure FDA00029233780400000422
为J的一阶导数，JT为J的转置；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-rd，
Figure FDA00029233780400000423
为e的一阶导数，r为机器人的实际轨迹，rd为机器人的期望轨迹；t表示时间。
9.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令，包括：
将所述机器人角加速度以及辅助变量的变化率反馈至所述机器人的控制系统中，在所述机器人的控制系统内根据所述机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
10.一种抗噪与动力学约束的机器人实时运动规划装置，其特征在于，所述装置包括：
读取模块：用于读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；
推导模块：用于基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；同时，
第一计算模块：用于获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；
设置模块：用于在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；
第二计算模块：用于基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；
更新模块：用于基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。
CN202110125245.7A 2021-01-29 2021-01-29 一种抗噪与动力学约束的机器人实时运动规划方法及装置 Pending CN112873208A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110125245.7A CN112873208A (zh) 2021-01-29 2021-01-29 一种抗噪与动力学约束的机器人实时运动规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110125245.7A CN112873208A (zh) 2021-01-29 2021-01-29 一种抗噪与动力学约束的机器人实时运动规划方法及装置

Publications (1)

Publication Number Publication Date
CN112873208A true CN112873208A (zh) 2021-06-01

Family

ID=76053518

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110125245.7A Pending CN112873208A (zh) 2021-01-29 2021-01-29 一种抗噪与动力学约束的机器人实时运动规划方法及装置

Country Status (1)

Country Link
CN (1) CN112873208A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023024278A1 (zh) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106426164A (zh) * 2016-09-27 2017-02-22 华南理工大学 一种冗余度双机械臂的多指标协调运动规划方法
CN106625680A (zh) * 2017-02-07 2017-05-10 华侨大学 一种冗余度机械臂加速度层容噪控制方法
CN106737670A (zh) * 2016-12-15 2017-05-31 华侨大学 一种具有抗噪特性的冗余度机械臂重复运动规划方法
CN108714894A (zh) * 2018-05-03 2018-10-30 华南理工大学 一种求解双冗余机械臂互相碰撞的动力学方法
US20190184560A1 (en) * 2017-01-19 2019-06-20 Beijing University Of Technology A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error
CN110977989A (zh) * 2019-12-27 2020-04-10 广东省智能制造研究所 一种协作机器人的运动规划与实时避障方法及系统
CN111037560A (zh) * 2019-12-25 2020-04-21 广东省智能制造研究所 一种协作机器人柔顺力控制方法及系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106426164A (zh) * 2016-09-27 2017-02-22 华南理工大学 一种冗余度双机械臂的多指标协调运动规划方法
CN106737670A (zh) * 2016-12-15 2017-05-31 华侨大学 一种具有抗噪特性的冗余度机械臂重复运动规划方法
US20190184560A1 (en) * 2017-01-19 2019-06-20 Beijing University Of Technology A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error
CN106625680A (zh) * 2017-02-07 2017-05-10 华侨大学 一种冗余度机械臂加速度层容噪控制方法
CN108714894A (zh) * 2018-05-03 2018-10-30 华南理工大学 一种求解双冗余机械臂互相碰撞的动力学方法
CN111037560A (zh) * 2019-12-25 2020-04-21 广东省智能制造研究所 一种协作机器人柔顺力控制方法及系统
CN110977989A (zh) * 2019-12-27 2020-04-10 广东省智能制造研究所 一种协作机器人的运动规划与实时避障方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
XUEFENG ZHOU: "Collision-Free Compliance Control for Redundant Manipulators: An Optimization Case", <FRONTIERS IN NEUROROBOTICS> *
程韬波: "基于递归神经网络的多机器人智能协同控制", 《机电工程技术》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023024278A1 (zh) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人

Similar Documents

Publication Publication Date Title
CN109159151B (zh) 一种机械臂空间轨迹跟踪动态补偿方法和系统
CN104339349B (zh) 机器人装置及机器人控制方法
CA2735262C (en) Inverse kinematics
CN108638068B (zh) 一种携带冗余度机械臂的飞行机器人控制系统设计方法
CN110850895B (zh) 一种路径跟踪方法、装置、设备及存储介质
JP2018149606A (ja) 生産ラインにおける運用を考慮した学習制御を行うロボット及びその制御方法
CN114952868B (zh) 7自由度srs型机械臂控制方法及装置、弹琴机器人
CN111522351B (zh) 水下机器人三维编队及避障方法
CN109445274B (zh) 一种柔性空间机械臂振动控制方法及系统
CN107363832B (zh) 一种工业机器人前馈力矩实时计算方法
CN108267952B (zh) 一种水下机器人自适应有限时间控制方法
CN112809666B (zh) 一种基于神经网络的5-dof机械臂力位跟踪算法
CN106708078A (zh) 一种适用于空间机器人执行器故障下的快速姿态稳定方法
CN114523477A (zh) 关节位姿的校准方法、系统及存储介质
CN112847373B (zh) 一种机器人轨迹同步控制方法、计算机可读存储介质
CN112873208A (zh) 一种抗噪与动力学约束的机器人实时运动规划方法及装置
CN115480583A (zh) 飞行作业机器人的视觉伺服跟踪与阻抗控制方法
CN112596389A (zh) 基于闭环交叉耦合迭代学习的水晶研磨控制方法与系统
WO2023165174A1 (zh) 构建机器人的控制器的方法、机器人的运动控制方法、装置以及机器人
WO2023116129A1 (zh) 一种协作机器人的柔顺力控制方法及系统
CN112894819A (zh) 一种基于双神经网络的机器人动力学运动控制方法及装置
Shang et al. Finite-time tracking for UUV with system uncertainty using fast adaptive nonsingular terminal high-order sliding mode control
CN115268480B (zh) 一种基于李代数的欠驱动航天器姿态控制方法、设备和介质
WO2023165192A1 (zh) 机器人控制方法、装置、机器人以及计算机可读存储介质
CN113787514B (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